简介

LIO-SAM 的完整注释代码以及流程图见 LIO-SAM With Chinese Comments

整体思路

在整个 LIO-SAM 框架中，会发布两类里程计信息，每一类里程计又包含高频和低频两种：

• 基于增量信息的里程计：这个里程计不会考虑回环检测，只会进行局部的匹配，这样可以保持轨迹的平滑性
• 基于全局信息的里程计：这个里程计会进行回环检测，因此轨迹在某些地方可能不会很平滑，但总体而言精度会高于基于增量的里程计

每一类里程计分为以下两类：

• IMU 里程计：频率和 IMU 消息频率一致，主要利用惯导信息进行状态解算获得位姿
• Lidar 里程计：频率和 Lidar 消息频率一致，主要方法是将点云和局部地图匹配获得位姿变化信息

imuPreintegration.cpp 中主要包含两个类，分别是 IMUPreintegration 和 TransformFusion。其作用分别为：

• IMUPreintegration
  • 维护一个因子图，每接收一个 Lidar 增量里程计消息进行一次更新以及优化
    • 变量为每一个 Lidar 关键帧的状态（包括位姿、速度以及 IMU 零偏）
    • 因子包括 Lidar 增量里程计对每一帧的状态估计作为先验因子，以及每两帧之间的预积分观测作为约束因子，对两时刻之间的零偏也进行了约束
  • 作为惯性导航里程计，利用 IMU 信息进行位姿解算，获得基于增量信息的高频 IMU 里程计估计
• TransformFusion
  • 接收 Lidar 全局里程计消息和高频 IMU 增量里程计估计，用 Lidar 里程计消息时刻到最新 IMU 里程计消息时刻的相对位姿变换来更新 Lidar 全局里程计的预测，作为基于全局信息的高频 IMU 里程计估计，频率和 IMU 频率一致

整体的代码逻辑可以参考下图：

IMUPreintegration 类

IMUPreintegration 类的整体思路参考下图：

例子中我们假设 FX 是从地图优化节点传来的基于增量信息的 Lidar 里程计消息，MX 是 IMU 测量值。其中接收 F3 时，缓存队列中已经包括 M1 和 M2。M3 则是最新一个 IMU 测量值。

接收 IMU 消息放入两个缓存队列中，分别为：imuOpt 和 imu。其中：

• imu 队列被直接用于计算状态解算，每当收到一个新的 IMU 测量值 M3，会将其加入缓存队列中，并利用优化后的上一个 Lidar关键帧附近的 IMU 状态 （这里的例子是 M0）以及队列中所有的 IMU 测量值（M1, M2, M3）进行积分得到当前时刻 IMU 状态。这里注意，由于接收 F3 时，M1, M2 已经在队列里，因此实际上对 M1, M2 的积分在那个时刻已经完成。
• imuOpt 中的测量值会被用于和进行优化： IMUPreintegration 类中维护一个因子图优化器，每当接收一个新的基于增量的 Lidar 里程计位姿 F3 时，首先会计算 F2 到 F3 时刻之间的 IMU 测量值的预积分。然后利用惯性导航解算预测 F3 时刻（F3 左边的 IMU 测量值）对应的状态估计作为该变量节点的初始值，并且构造两个与其相关的约束因子，其中一个是来自 F3 的基于增量信息的 Lidar 里程计的位姿估计作为先验因子，另一个就是 F2 到 F3 之间 IMU 预积分的约束因子。接下来进行一次优化，这次优化会将 F3 之前所有 IMU 时刻对应的状态进行优化，包括位姿、速度以及零偏。在完成优化之后，需要对 imu 中的测量值重新计算预积分，因此重置 imu 预积分器中的零偏并清空状态，从 M1 开始进行积分到 M2。此时 imu 积分器中更新到 M2 对应的状态。

TransformFusion 类

这个类的思路比较简单，接收来自后端的基于全局信息的 Lidar 里程计 L 和 IMUPreintegration 类基于增量的 imu 里程计消息 I。这里作者想要做的就是发布一个基于全局信息（含回环）的，但同时又是高频的里程计。因此思路很简单，利用 L 对应时刻的 imu 里程计消息到最新一个 imu 里程计消息的相对位姿变换，并用其作用 L 即可。实现思路参考上面流程图和代码注释。