ORB-SLAM3源码学习：G2oTypes.cc: void EdgeInertial::linearizeOplus计算残差对状态增量的雅克比矩阵

前言

这个函数和计算残差函数一样需要学习g2o和IMU相关的公式。

1.函数声明

void EdgeInertial::linearizeOplus()

2.函数定义

可以用下面这样的表格来表示这些雅可比矩阵的关系

涉及到的IMU公式：

未提及的雅可比矩阵则在表格对应处为0矩阵。

// 计算残差对状态增量的雅克比矩阵
void EdgeInertial::linearizeOplus()
{// 获取因子图的顶点与计算残差函数的顶点对应const VertexPose* VP1 = static_cast<const VertexPose*>(_vertices[0]);const VertexVelocity* VV1= static_cast<const VertexVelocity*>(_vertices[1]);const VertexGyroBias* VG1= static_cast<const VertexGyroBias*>(_vertices[2]);const VertexAccBias* VA1= static_cast<const VertexAccBias*>(_vertices[3]);const VertexPose* VP2 = static_cast<const VertexPose*>(_vertices[4]);const VertexVelocity* VV2= static_cast<const VertexVelocity*>(_vertices[5]);const IMU::Bias b1(VA1->estimate()[0],VA1->estimate()[1],VA1->estimate()[2],VG1->estimate()[0],VG1->estimate()[1],VG1->estimate()[2]);const IMU::Bias db = mpInt->GetDeltaBias(b1);Eigen::Vector3d dbg;dbg << db.bwx, db.bwy, db.bwz;const Eigen::Matrix3d Rwb1 = VP1->estimate().Rwb;  // Riconst Eigen::Matrix3d Rbw1 = Rwb1.transpose();     // Ri.t()const Eigen::Matrix3d Rwb2 = VP2->estimate().Rwb;  // Rjconst Eigen::Matrix3d dR = mpInt->GetDeltaRotation(b1).cast<double>();const Eigen::Matrix3d eR = dR.transpose()*Rbw1*Rwb2;        // r△Rijconst Eigen::Vector3d er = LogSO3(eR);                      // r△φijconst Eigen::Matrix3d invJr = InverseRightJacobianSO3(er);  // Jr^-1(log(△Rij))// _jacobianOplus个数等于边的个数，里面的大小等于观测值维度（也就是残差）× 每个节点待优化值的维度// Jacobians wrt Pose 1// _jacobianOplus[0] 9*6矩阵 总体来说就是三个残差分别对pose1的旋转与平移（p）求导_jacobianOplus[0].setZero();// rotation// (0,0)起点的3*3块表示旋转残差对pose1的旋转求导_jacobianOplus[0].block<3,3>(0,0) = -invJr*Rwb2.transpose()*Rwb1;// (3,0)起点的3*3块表示速度残差对pose1的旋转求导_jacobianOplus[0].block<3,3>(3,0) = Sophus::SO3d::hat(Rbw1*(VV2->estimate() - VV1->estimate() - g*dt));// (6,0)起点的3*3块表示位置残差对pose1的旋转求导_jacobianOplus[0].block<3,3>(6,0) = Sophus::SO3d::hat(Rbw1*(VP2->estimate().twb - VP1->estimate().twb- VV1->estimate()*dt - 0.5*g*dt*dt));// translation// (6,3)起点的3*3块表示位置残差对pose1的位置求导_jacobianOplus[0].block<3,3>(6,3) = -Eigen::Matrix3d::Identity();// Jacobians wrt Velocity 1// _jacobianOplus[1] 9*3矩阵 总体来说就是三个残差分别对pose1的速度求导_jacobianOplus[1].setZero();_jacobianOplus[1].block<3,3>(3,0) = -Rbw1;_jacobianOplus[1].block<3,3>(6,0) = -Rbw1*dt;// Jacobians wrt Gyro 1// _jacobianOplus[2] 9*3矩阵 总体来说就是三个残差分别对陀螺仪偏置的速度求导_jacobianOplus[2].setZero();_jacobianOplus[2].block<3,3>(0,0) = -invJr*eR.transpose()*RightJacobianSO3(JRg*dbg)*JRg;_jacobianOplus[2].block<3,3>(3,0) = -JVg;_jacobianOplus[2].block<3,3>(6,0) = -JPg;// Jacobians wrt Accelerometer 1// _jacobianOplus[3] 9*3矩阵 总体来说就是三个残差分别对加速度计偏置的速度求导_jacobianOplus[3].setZero();_jacobianOplus[3].block<3,3>(3,0) = -JVa;_jacobianOplus[3].block<3,3>(6,0) = -JPa;// Jacobians wrt Pose 2// _jacobianOplus[4] 9*6矩阵 总体来说就是三个残差分别对pose2的旋转与平移（p）求导_jacobianOplus[4].setZero();// rotation_jacobianOplus[4].block<3,3>(0,0) = invJr;// translation_jacobianOplus[4].block<3,3>(6,3) = Rbw1*Rwb2;// Jacobians wrt Velocity 2// _jacobianOplus[5] 9*3矩阵 总体来说就是三个残差分别对pose2的速度求导_jacobianOplus[5].setZero();_jacobianOplus[5].block<3,3>(3,0) = Rbw1;
}