LIOSAM中关于gtsam的笔记——IMU预积分和MapOptimization

对于LIOSAM中的ImuPreIntegration.cpp中出现gtsam的部分代码，主要是实现基于因子图的位姿估计。参考资料不多，主要看看GTSAM官网的examples学习代码。

Imufactorexample2.cpp和imufactorsexample.cpp两个文件。介绍了IMU位姿估计的主要方法。下面主要理顺ImuPreIntegration.cpp中如何使用gtsam。

首先是这个类：IMUPreintegration

1、初始化阶段

定义先验因子

对于位姿因子而言，需要三个量：

key，这里是X0

初始化位姿，prevPose_

噪声

gtsam::PriorFactor<gtsam::Pose3> priorPose(X(0), prevPose_, priorPoseNoise);

然后添加到因子图中。以此类推，添加速度，偏置。最后初始化。并且使用ISAM优化器更新一次。

prevVel_ = gtsam::Vector3(0, 0, 0);
gtsam::PriorFactor<gtsam::Vector3> priorVel(V(0), prevVel_, priorVelNoise);
graphFactors.add(priorVel);
// initial bias
prevBias_ = gtsam::imuBias::ConstantBias();
gtsam::PriorFactor<gtsam::imuBias::ConstantBias> priorBias(B(0), prevBias_, priorBiasNoise);
graphFactors.add(priorBias);
// add values
graphValues.insert(X(0), prevPose_);
graphValues.insert(V(0), prevVel_);
graphValues.insert(B(0), prevBias_);
// optimize once
optimizer.update(graphFactors, graphValues);

GTSAM中有很多优化器，具体每一种优化器都有什么特点有待研究。

这个流程相在官网的Examples中ImuFactorsExample.cpp有解释。主要就是个框架。

初始化完成后，开始计算前一帧与当前帧之间的imu预积分量，用前一帧状态施加预积分量得到当前帧初始状态估计，添加来自mapOptimization的当前帧位姿，进行因子图优化，更新当前帧状态

 注意这里有两个优化器，功能不同，详见知乎卢涛代码分析。这里说流程

下面这一步是往里面加每个时刻的IMU信息

imuIntegratorOpt_->integrateMeasurement

添加IMU预积分因子，imu_factor。这里X(key)还没有初始化，一开始也是比较疑惑。

然后添加imu偏置因子，BetweenFactor

添加位姿因子，注意，这个是从激光里程计来的

gtsam::Pose3 curPose = lidarPose.compose(lidar2Imu);
gtsam::PriorFactor<gtsam::Pose3> pose_factor(X(key), curPose, degenerate ? correctionNoise2 : correctionNoise);

 做预积分，得到当前的状态

最后放到isam优化器中，做更新，算出当前的速度，位姿。

后面还有一步重传播，跟前面的结构差不多。

对于MapOptimization.cpp

 有一个里程计因子

if (cloudKeyPoses3D->points.empty())
        {
            noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Variances((Vector(6) << 1e-2, 1e-2, M_PI*M_PI, 1e8, 1e8, 1e8).finished()); // rad*rad, meter*meter
            gtSAMgraph.add(PriorFactor<Pose3>(0, trans2gtsamPose(transformTobeMapped), priorNoise));
            initialEstimate.insert(0, trans2gtsamPose(transformTobeMapped));
        }else{
            noiseModel::Diagonal::shared_ptr odometryNoise = noiseModel::Diagonal::Variances((Vector(6) << 1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4).finished());
            gtsam::Pose3 poseFrom = pclPointTogtsamPose3(cloudKeyPoses6D->points.back());
            gtsam::Pose3 poseTo   = trans2gtsamPose(transformTobeMapped);
            gtSAMgraph.add(BetweenFactor<Pose3>(cloudKeyPoses3D->size()-1, cloudKeyPoses3D->size(), poseFrom.between(poseTo), odometryNoise));
            initialEstimate.insert(cloudKeyPoses3D->size(), poseTo);
        }