当前位置：首页 > news >正文

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

news 2025/2/26 6:36:21

前言

这部分函数涉及了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]);           //位姿Ticonst VertexVelocity* VV1= static_cast<const VertexVelocity*>(_vertices[1]);    //速度viconst VertexGyroBias* VG1= static_cast<const VertexGyroBias*>(_vertices[2]);    //零偏Bgiconst VertexAccBias* VA1= static_cast<const VertexAccBias*>(_vertices[3]);      //零偏Baiconst VertexPose* VP2 = static_cast<const VertexPose*>(_vertices[4]);           //位姿Tjconst 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;
}