ORB-SLAM3源码学习：G2oTypes.cc: void EdgeInertial::computeError 计算预积分残差

前言

 这部分函数涉及了g2o的内容以及IMU相关的推导内容，需要你先去进行这部分的学习。

1.函数声明

void EdgeInertial::computeError()

2.函数定义 

涉及到的IMU的公式： 

{
    // TODO Maybe Reintegrate inertial measurments when difference between linearization point and current estimate is too big
    // 当线性化点与当前估计之间的差异过大时，可能需要重新积分惯性测量值。
    // 在父类中使用了using，故这里可以直接使用保护成员变量
    // 获取因子图的顶点
    const VertexPose* VP1 = static_cast<const VertexPose*>(_vertices[0]);           //位姿Ti
    const VertexVelocity* VV1= static_cast<const VertexVelocity*>(_vertices[1]);    //速度vi
    const VertexGyroBias* VG1= static_cast<const VertexGyroBias*>(_vertices[2]);    //零偏Bgi
    const VertexAccBias* VA1= static_cast<const VertexAccBias*>(_vertices[3]);      //零偏Bai
    const VertexPose* VP2 = static_cast<const VertexPose*>(_vertices[4]);           //位姿Tj
    const VertexVelocity* VV2 = static_cast<const VertexVelocity*>(_vertices[5]);   //速度vj
    // 获取IMU偏置的三个分量并计算预积分的增量
    // _estimate是类中的一个成员变量，表示该顶点的当前估计值。
    const IMU::Bias b1(VA1->estimate()[0],VA1->estimate()[1],VA1->estimate()[2],VG1->estimate()[0],VG1->estimate()[1],VG1->estimate()[2]);
    const Eigen::Matrix3d dR = mpInt->GetDeltaRotation(b1).cast<double>();
    const Eigen::Vector3d dV = mpInt->GetDeltaVelocity(b1).cast<double>();
    const Eigen::Vector3d dP = mpInt->GetDeltaPosition(b1).cast<double>();

    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;

    _error << er, ev, ep;
}

结束语 

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