修改网站模板详解,国外装修网站模板,靖江网站推广,网站开发人员工工资前言 这部分函数涉及了g2o的内容以及IMU相关的推导内容#xff0c;需要你先去进行这部分的学习。
1.函数声明
void EdgeInertial::computeError()
2.函数定义
涉及到的IMU的公式#xff1a; {// TODO Maybe Reintegrate inertial measurments when difference between …前言 这部分函数涉及了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_castconst VertexPose*(_vertices[0]); //位姿Ticonst VertexVelocity* VV1 static_castconst VertexVelocity*(_vertices[1]); //速度viconst VertexGyroBias* VG1 static_castconst VertexGyroBias*(_vertices[2]); //零偏Bgiconst VertexAccBias* VA1 static_castconst VertexAccBias*(_vertices[3]); //零偏Baiconst VertexPose* VP2 static_castconst VertexPose*(_vertices[4]); //位姿Tjconst VertexVelocity* VV2 static_castconst 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).castdouble();const Eigen::Vector3d dV mpInt-GetDeltaVelocity(b1).castdouble();const Eigen::Vector3d dP mpInt-GetDeltaPosition(b1).castdouble();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;
}
结束语
以上就是我学习到的内容如果对您有帮助请多多支持我如果哪里有问题欢迎大家在评论区积极讨论我看到会及时回复。
