ORB SLAM3——IMU优化部分精读-VertexPose(ImuCamPose)的更新量到底是什么？

qq_41706238

已于 2022-03-02 01:04:37 修改

阅读量554

点赞数 1

文章标签：机器人 c++ 自动驾驶

于 2022-03-02 00:43:03 首次发布

本文链接：https://blog.csdn.net/qq_41706238/article/details/123219440

版权

ORB SLAM3——IMU优化部分精读-VertexPose(ImuCamPose)的更新量到底是什么？

先说答案

void ImuCamPose::Update(const double *pu)
{
   
    Eigen::Vector3d ur, ut;
    ur << pu[0], pu[1], pu[2];
    ut << pu[3], pu[4], pu[5];

    // Update body pose
    twb += Rwb*ut;
    Rwb = Rwb*ExpSO3(ur);

ur是 $\phi_{wb}$ ,ut是 $t_{bw}$

相信有很多小伙伴在看ORB-SLAM的代码的时候发出了和我一样的疑惑，在此记录一下。
我的疑惑始于这段代码

void EdgeMonoOnlyPose::linearizeOplus()
{
   
    const VertexPose* VPose = static_cast<const VertexPose*>(_vertices[0]);

    const Eigen::Matrix3d &Rcw = VPose->estimate().Rcw[cam_idx];
    const Eigen::Vector3d &tcw = VPose->estimate().tcw[cam_idx];
    const Eigen::Vector3d Xc = Rcw*Xw + tcw;
    const Eigen::Vector3d Xb = VPose->estimate().Rbc[cam_idx]*Xc+VPose->estimate().tbc[cam_idx];
    const Eigen::Matrix3d &Rcb = VPose->estimate().Rcb[cam_idx];

    Eigen::Matrix<double,2,3> proj_jac = VPose->estimate().pCamera[cam_idx]->projectJac(Xc);

    Eigen::Matrix<double,3,6> SE3deriv;
    double x = Xb(0);
    double y = Xb(1);
    double z = Xb(2);
    SE3deriv << 0.0, z,   -y, 1.0, 0.0, 0.0,
            -z , 0.0, x, 0.0, 1.0, 0.0,
            y ,  -x , 0.0, 0.0, 0.0, 1.0;
    _jacobianOplusXi = proj_jac * Rcb * SE3deriv; // symbol different becasue of update mode
}