1.代码分析

1.1 G2oTypes.h

G2oTypes.h的主要结构如下(对于类的作用已注释）：
在这里插入图片描述

1.2 G2oTypes.cc

局部地图中imu的局部地图优化（此时已经初始化完毕不需要再优化重力方向与尺度）

EdgeInertial::EdgeInertial(IMU::Preintegrated *pInt):JRg(Converter::toMatrix3d(pInt->JRg)),JVg(Converter::toMatrix3d(pInt->JVg)), JPg(Converter::toMatrix3d(pInt->JPg)), JVa(Converter::toMatrix3d(pInt->JVa)),JPa(Converter::toMatrix3d(pInt->JPa)), mpInt(pInt), dt(pInt->dT)
{// 准备工作，把预积分类里面的值先取出来，包含信息的是两帧之间n多个imu信息预积分来的// This edge links 6 vertices// 6元边resize(6);// 1. 定义重力g << 0, 0, -IMU::GRAVITY_VALUE;// 2. 读取协方差矩阵的前9*9部分的逆矩阵，该部分表示的是预积分测量噪声的协方差矩阵cv::Mat cvInfo = pInt->C.rowRange(0, 9).colRange(0, 9).inv(cv::DECOMP_SVD);// 转成eigen Matrix9dMatrix9d Info;for (int r = 0; r < 9; r++)for (int c = 0; c < 9; c++)Info(r, c) = cvInfo.at<float>(r, c);// 3. 强制让其成为对角矩阵Info = (Info + Info.transpose()) / 2;// 4. 让特征值很小的时候置为0，再重新计算信息矩阵（暂不知这么操作的目的是什么，先搞清楚操作流程吧）Eigen::SelfAdjointEigenSolver<Eigen::Matrix<double, 9, 9>> es(Info);Eigen::Matrix<double, 9, 1> eigs = es.eigenvalues(); // 矩阵特征值for (int i = 0; i < 9; i++)if (eigs[i] < 1e-12)eigs[i] = 0;// asDiagonal 生成对角矩阵Info = es.eigenvectors() * eigs.asDiagonal() * es.eigenvectors().transpose();setInformation(Info);
}

计算误差

void EdgeInertial::computeError()
{// 计算残差// TODO Maybe Reintegrate inertial measurments when difference between linearization point and current estimate is too bigconst 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 Eigen::Matrix3d dR = Converter::toMatrix3d(mpInt->GetDeltaRotation(b1));const Eigen::Vector3d dV = Converter::toVector3d(mpInt->GetDeltaVelocity(b1));const Eigen::Vector3d dP = Converter::toVector3d(mpInt->GetDeltaPosition(b1));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;
}