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
}
雅可比公式(2x6维)_jacobianOplusXi = proj_jac * Rcb * SE3deriv
的推导