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

前言

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

1.函数声明

cpp 复制代码
void EdgeInertial::linearizeOplus()

2.函数定义

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

涉及到的IMU公式:

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

cpp 复制代码
// 计算残差对状态增量的雅克比矩阵
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;  // Ri
    const Eigen::Matrix3d Rbw1 = Rwb1.transpose();     // Ri.t()
    const Eigen::Matrix3d Rwb2 = VP2->estimate().Rwb;  // Rj

    const Eigen::Matrix3d dR = mpInt->GetDeltaRotation(b1).cast<double>();
    const Eigen::Matrix3d eR = dR.transpose()*Rbw1*Rwb2;        // r△Rij
    const Eigen::Vector3d er = LogSO3(eR);                      // r△φij
    const 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;
}

结束语

以上就是我学习到的内容,如果对您有帮助请多多支持我,如果哪里有问题欢迎大家在评论区积极讨论,我看到会及时回复。

相关推荐
Source.Liu1 分钟前
【BuildFlow & 筑流】品牌命名与项目定位说明
c++·qt·rust·markdown·librecad
bkspiderx9 分钟前
C++设计模式之行为型模式:访问者模式(Visitor)
c++·设计模式·访问者模式
励志不掉头发的内向程序员14 分钟前
【Linux系列】掌控 Linux 的脉搏:深入理解进程控制
linux·运维·服务器·开发语言·学习
薰衣草233322 分钟前
hot100练习-11
算法·leetcode
地平线开发者1 小时前
征程 6 | 工具链如何支持 Matmul/Conv 双 int16 输入量化?
人工智能·算法·自动驾驶
甄心爱学习1 小时前
数值计算-线性方程组的迭代解法
算法
应用市场1 小时前
OpenCV深度学习:目标检测、人脸识别与智能视频分
深度学习·opencv·目标检测
stolentime1 小时前
SCP2025T2:P14254 分割(divide) 题解
算法·图论·组合计数·洛谷scp2025
Q741_1471 小时前
C++ 面试基础考点 模拟题 力扣 38. 外观数列 题解 每日一题
c++·算法·leetcode·面试·模拟
W_chuanqi1 小时前
RDEx:一种效果驱动的混合单目标优化器,自适应选择与融合多种算子与策略
人工智能·算法·机器学习·性能优化