orb-slam3：优化状态量是camera坐标系下RPV还是body坐标系下RPV探究

dueen1123

已于 2022-11-16 17:39:07 修改

阅读量499

点赞数 1

文章标签：人工智能

于 2022-11-16 17:31:18 首次发布

本文链接：https://blog.csdn.net/dueen1123/article/details/127888936

版权

ORB-SLAM3的对于状态量RPV的优化，添加的残差块主要是：
（1）IMU残差（预积分论文）
（2）视觉残差（SLAM14讲-P187）

状态量优化：RPV = RPV + J*△RPV
更新状态是通过J(e/RPV)*△RPV，其中雅可比J(e/RPV)是残差e对于状态量(RPV)的导数

那么优化的状态量RPV究竟是body坐标系的还是camera坐标系的呢？
答案：body系

优化的状态变量是在body坐标系下，从以下几个方面得以验证：

1. 关于状态变量的更新

对于优化变量VertexPose* VP，其更新函数Update()如下
可以看出是Twb进行直接更新的，Tbw以及Tcw是通过Twb进行间接更新的

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);

    // Update camera poses
    const Eigen::Matrix3d Rbw = Rwb.transpose();
    const Eigen::Vector3d tbw = -Rbw * twb;

    for(int i=0; i<pCamera.size(); i++)
    {
        Rcw[i] = Rcb[i] * Rbw;
        tcw[i] = Rcb[i] * tbw + tcb[i];
    }

}

2. IMU残差计算

残差计算都是关于Rwb或者twb的，也就是body坐标系

const Eigen::Vector3d er = LogSO3(dR.transpose()*VP1->estimate().Rwb.transpose()*VP2->estimate().Rwb);
const Eigen::Vector3d ev = VP1->estimate().Rwb.transpose()*(VV2->estimate() - VV1->estimate() - g*dt) - dV;
const Eigen::Vector3d ep = VP1->estimate().Rwb.transpose()*(VP2->estimate().twb - VP1->estimate().twb
                                                               - VV1->estimate()*dt - g*dt*dt/2) - dP;

雅可比也是关于body坐标系下求导

    // Jacobians wrt Pose 1
    //顺序: er ev ep 分别对 VPk(VP1)求导 
    
    _jacobianOplus[0].setZero();                   // er/Pi = 0  全部设置为0
    // rotation                                    // ev/Pi = 0  
    _jacobianOplus[0].block<3,3>(0,0) = -invJr * Rwb2.transpose() * Rwb1; 
    _jacobianOplus[0].block<3,3>(3,0) = Sophus::SO3d::hat(Rbw1*(VV2->estimate() - VV1->estimate() - g*dt));// er/Ri = -J(dR)'*Rj_T*Ri
    _jacobianOplus[0].block<3,3>(6,0) = Sophus::SO3d::hat(Rbw1*(VP2->estimate().twb - VP1->estimate().twb  // ev/Ri = (Ri_T*(Vj-Vi-g*t))^
                                                   - VV1->estimate()*dt - 0.5*g*dt*dt));                   // ep/Ri = (Ri_T*(Pj-Pi-Vi*t-1/2*g*t*t))^
    // translation                                 
    _jacobianOplus[0].block<3,3>(6,3) = -Eigen::Matrix3d::Identity();        // ep/Pi = -I(3x1)

3. 视觉重投影误差计算

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;
    //相机坐标系转换为body坐标系
    const Eigen::Vector3d Xb = VPose->estimate().Rbc[cam_idx]*Xc + VPose->estimate().tbc[cam_idx];
    const Eigen::Matrix3d &Rcb = VPose->estimate().Rcb[cam_idx];

    //(残差)对(相机坐标系下的P)求导
    Eigen::Matrix<double,2,3> proj_jac = VPose->estimate().pCamera[cam_idx]->projectJac(Xc);
    //(body坐标系下的P)对(旋转李代数)求导
    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;
    //链式法则：(残差)对(旋转李代数)求导 = (残差)对(相机坐标系下的P)求导 * (相机坐标系下的P)对(body坐标系下的P)求导 * (body坐标系下的P)对(旋转李代数)求导
    _jacobianOplusXi = proj_jac * Rcb * SE3deriv; // symbol different becasue of update mode
}