《LIO-SAM阅读笔记》1.IMU预积分模块

前言：

        LIO-SAM是一个多传感器融合的紧耦合SLAM框架，融合的传感器类型有雷达、IMU和GPS，其中雷达和IMU在LIO-SAM框架中必须使用的。LIO-SAM的优化策略采用了GTSAM库，GTSAM库采用了因子图的优化方法，其提供了一些列C++的外部接口，以便用户方便传入参数等进行优化。特别的是GTSAM库专门单独设计关于IMU计算与优化的接口。

        IMU预积分模块在LIO-SAM源码中写在了imuPreintegration.cpp文件中，其中预积分模块的功能使用class IMUPreintegration来实现，IMUPreintegration类中构造函数中最主要的两个部分分别是imu的回调函数imuHandler和odom的回调函数odometryHandler。

        特别注意的是，在本文中需要注意到IMU预积分和IMU里程计是不同的两个概念，预积分量是一段时间内的相对位姿变换，IMU里程计是实时的全局位姿。在此模块中IMU预计分的计算是基于后端提供的增量式odom计算的，IMU里程计的发布是基于后端提供的优化后的odom计算的。
 

数据来源：

从上面系统流程图可以看出，预积分模块接受的数据来源有两个，一个是IMU的原始数据，另外是后端模块发送的lidar里程计数据。值得注意的是，从流程图上并不能看出，其实后端模块发送了两个lidar里程计，一个是增量式里程计，在IMUPreintegration类里接收使用，另一个是优化后的里程计，在TransformFusion类内部接收。

使用增量式里程计的原因：增量式里程计是一个平滑的结果不会有大幅度的位姿跳跃，适用于因子图优化时，帧间的位姿变换对imu预积分的约束。

关于为什么需要使用增量式里程计可以参考以下文章：

《LIO-SAM阅读笔记》-为何要引入增量式里程计？-CSDN博客

一：IMUPreintegration类

1.IMU回调函数imuHandler

在imuHandler中主要分为了3个部分，分别是将imu数据放入缓存队列，计算计算增量式imu里程计，发布增量式imu里程计。

1.1 将imu数据放入缓存队列

在回调函数中，首先将接受到的imu原始数据存放进两个队列中，第一个队列用来预积分与优化，第二个队列用来更新imu状态量（进行优化后的位姿传播），这两个队列都是在odom的回调函数odometryHandler中使用。

注：整个模块中一共两个预积分器imuIntegratorOpt_和imuIntegratorImu_，其中imuIntegratorOpt_用来求解两帧点云之间的预积分量并用于因子图优化，imuIntegratorImu_用来求解两帧imu数据之间的预积分量并用于imu里程计求解（因子图之后的重传播）。其中imuIntegratorOpt_和因子图优化为imuIntegratorImu_提供更新后的偏置。

1.2 计算增量式imu里程计

       此部分是将2.3部分联系在一起的，利用最近一次的经过联合IMU预积分因子图优化后的增量式雷达odom里程计作为起点利用imu预积分来计算每一帧imu数据时刻的位姿。

        2.3小节（重传播）将imu队列中当前帧odom之后的imu信息加入到预积分器中，在此小节中是将新到的每帧imu信息到预积分器，以及预积分器的推理。

1.3 发布增量式imu里程计

imu里程计位姿转到lidar系，发布增量式imu里程计，由同文件中的TransformFusion类接收。

2.odom回调函数odometryHandler

在odometryHandler回调函数中主要进行了imu数据和增量式lidar的里程计数据联合进行因子图优化的操作。

因子图优化的是什么?

此处因子图优化的是来自后端提供的增量式里程计，增量式里程计没有经过后端的闭环、gps等联合优化。

2.1 odometryHandler中进行的主要操作

• Step 0. 系统初始化，第一帧
• Step 1. 计算前一帧与当前帧之间的imu预积分量，用前一帧状态施加预积分量得到当前帧初始状态估计，添加来自mapOptimization的当前帧位姿，进行因子图优化，更新当前帧状态
• Step 2. 优化之后，执行重传播；优化更新了imu的偏置，用最新的偏置重新计算当前激光里程计时刻之后的imu预积分，这个预积分用于计算每时刻位姿。
• Step 3. 每隔100帧激光里程计，重置ISAM2优化器，保证优化效率

2.2 因子图优化的步骤

• 1.添加imu预积分因子

// 上面imu预积分的结果
const gtsam::PreintegratedImuMeasurements &preint_imu = dynamic_cast<const gtsam::PreintegratedImuMeasurements &>(*imuIntegratorOpt_);
// 参数：前一帧位姿，前一帧速度，当前帧位姿，当前帧速度，前一帧偏置，预计分量   //？：此处的当前帧位姿和当前帧速度是哪里得到的？此处是否是待求量？
gtsam::ImuFactor imu_factor(X(key - 1), V(key - 1), X(key), V(key), B(key - 1), preint_imu);
graphFactors.add(imu_factor);

• 2.添加imu偏置因子，前一帧偏置，当前帧偏置，观测值，噪声协方差；deltaTij()是积分段的时间

graphFactors.add(gtsam::BetweenFactor<gtsam::imuBias::ConstantBias>(B(key - 1), B(key), gtsam::imuBias::ConstantBias(),
                                                                            gtsam::noiseModel::Diagonal::Sigmas(sqrt(imuIntegratorOpt_->deltaTij()) * noiseModelBetweenBias)));

• 3.添加位姿因子

gtsam::Pose3 curPose = lidarPose.compose(lidar2Imu);
gtsam::PriorFactor<gtsam::Pose3> pose_factor(X(key), curPose, degenerate ? correctionNoise2 : correctionNoise);
graphFactors.add(pose_factor);
// 用前一帧的状态、偏置，施加imu预计分量，得到当前帧的状态  // note： 前一帧的状态是经过上一次优化后的结果
gtsam::NavState propState_ = imuIntegratorOpt_->predict(prevState_, prevBias_);

• 4.变量节点赋初值

graphValues.insert(X(key), propState_.pose());
graphValues.insert(V(key), propState_.v());
graphValues.insert(B(key), prevBias_);

        变量节点就是待优化变量 

• 5.优化

optimizer.update(graphFactors, graphValues);
optimizer.update();
graphFactors.resize(0);
graphValues.clear();
// 优化结果
gtsam::Values result = optimizer.calculateEstimate();

注意： 每优化完成一次后，就会清空因子图和变量，优化器是每100帧重置一次。因此每次向优化器内添加的因子图和变量是一一对应的。

• 6.利用优化结果更新状态量

// 更新当前帧位姿、速度
prevPose_ = result.at<gtsam::Pose3>(X(key));
prevVel_ = result.at<gtsam::Vector3>(V(key));
// 更新当前帧状态
prevState_ = gtsam::NavState(prevPose_, prevVel_);
// 更新当前帧imu偏置
prevBias_ = result.at<gtsam::imuBias::ConstantBias>(B(key));

 通过优化结果可以看出：此因子图优化可以优化当前帧位姿和imu零偏。

• 7.重置预积分器，设置新的偏置，这样下一帧激光里程计进来的时候，预积分量就是两帧之间的增量

imuIntegratorOpt_->resetIntegrationAndSetBias(prevBias_);

2.3 因子图优化之后的重传播步骤

        这里用一张示意图来表达，这一部操作最主要的原因是：imu接受数据的频率大于odom里程计的数据，因此每来一个odom数据，队列中已经有多个imu数据，而因子图优化的频率是按照odom里程计的频率来进行的，因此如果想要得到每一个imu数据时刻的位姿估计就要以最近的odom时刻的位姿为初始值，通过每个imu数据时刻的预积分进行位姿的传播。

        这里的重传播是

效果展示：此处一小段粉红色的轨迹就是通过经过因子图优化后的重传播（IMU预积分）预测出的轨迹，前面蓝色的轨迹是因子图优化得到的轨迹。

二：TransformFusion类

TransformFusion类主要功能是订阅激光里程计（来自MapOptimization优化后的激光里程计）和IMU里程计（IMU预积分中），根据前一时刻激光里程计，和该时刻到当前时刻的IMU里程计变换增量，计算当前时刻IMU里程计；rviz展示IMU里程计轨迹（局部）。

概括：TransformFusion类是为了将以增量式里程计为起点的IMU里程计转化到，以后端优化后的激光里程计为起点的IMU里程计并发布出出去。

重点：此处发布的imu里程计和IMU预积分中发布的IMU里程计是不同的，MapOptimization中会发送两个激光里程计话题。其中一个是增量式激光里程计，在IMU预积分订阅并使用。另外一个是后端优化后的激光里程计，在TransformFusion订阅并使用。最终我们发布的轨迹信息是基于优化后激光里程计信息+IMU里程计信息。之所以还发布增量式的激光里程计信息是为了方便IMU预积分环节使用。