SLAM中的非线性优-3D图优化之相对位姿Between Factor-四元数（十二）

本节开始讲解四元数推导相对位姿残差跟雅克比的推导，四元数推导也存在多种形式，第一种是借用李群李代数来推导，第二种是直接利用四元素性质推导，第三种直接利用如之前PNP章节，将四元数展开来求解，本文主要介绍前两种，至于第三种再看时间而定，轴角跟欧拉角的推导，暂时就不推导了，本节也是相对位姿between factor理论讲解最后一章，接下来继续使用之前图来说明，先看借助李群李代数推导的过程

完整代码https://gitee.com/zl_vslam/slam_optimizer/blob/master/3d_optimize/apps/main_ch12.cpp目前效果还比较一般，但本节方法已经再vins-mono中获得了证明，目前还不知道是什么原因导致结果未收敛，待后续继续更新吧，

1. 问题定义

2. 相对位姿的计算

3. 残差定义

3.1 平移残差

3.2 旋转残差

4. 误差对位姿的雅可比

4.1 平移残差对 ti 的雅可比

4.2 平移残差对 tj 的雅可比

4.3 平移残差对 qi ​的雅可比

4.4 平移残差对 qj 的雅可比

5. 旋转残差的雅可比

这里直接借用类似vins-mono中的残差跟雅克比，接下来看看对旋转如何求导

上述式子中第二行为残差项

5.1 相对旋转对 qwi 的扰动

该雅克比对应的代码再vins-mono中能找到对应

Eigen::Quaterniond corrected_delta_q = pre_integration->delta_q * Utility::deltaQ(dq_dbg * (Bgi - pre_integration->linearized_bg));
jacobian_pose_i.block<3, 3>(O_R, O_R) = -(Utility::Qleft(Qj.inverse() * Qi) * Utility::Qright(corrected_delta_q)).bottomRightCorner<3, 3>();

5.2 相对旋转对 qwj 的扰动

旋转残差的雅可比

同样对应的代码如下

Eigen::Quaterniond corrected_delta_q = pre_integration->delta_q * Utility::deltaQ(dq_dbg * (Bgi - pre_integration->linearized_bg));
jacobian_pose_j.block<3, 3>(O_R, O_R) = Utility::Qleft(corrected_delta_q.inverse() * Qi.inverse() * Qj).bottomRightCorner<3, 3>();

5.3 对局部参数的扰动

x_plus_delta = (q * delta_q).normalized();

要求解的是对delta_q的雅克比，局部参数只跟虚部相关，实部不相关，因此

bool PoseLocalParameterization::Plus(const double *x, const double *delta, double *x_plus_delta) const
{
    Eigen::Map<const Eigen::Vector3d> _p(x);
    Eigen::Map<const Eigen::Quaterniond> _q(x + 3);

    Eigen::Map<const Eigen::Vector3d> dp(delta);

    Eigen::Quaterniond dq = Utility::deltaQ(Eigen::Map<const Eigen::Vector3d>(delta + 3));

    Eigen::Map<Eigen::Vector3d> p(x_plus_delta);
    Eigen::Map<Eigen::Quaterniond> q(x_plus_delta + 3);

    p = _p + dp;
    q = (_q * dq).normalized();

    return true;
}
bool PoseLocalParameterization::ComputeJacobian(const double *x, double *jacobian) const
{
    Eigen::Map<Eigen::Matrix<double, 7, 6, Eigen::RowMajor>> j(jacobian);
    j.topRows<6>().setIdentity();
    j.bottomRows<1>().setZero();

    return true;
}

总结

本节总结了四元数推导相对位姿雅可比及残差，可以看到利用四元数性质求解的复杂度远远高于李群李代数求解的过程，后续章节会用实际证明，李群李代数推导的结果基本等于四元数，数值上只有微小到可忽略不计，因此选择合适的推导过程至关重要，下一讲会利用Between factor编程来求解为姿图优化问题，敬请期待。