1. 代码整体框架

首先看看工程目录结构，主要有五个文件，分别是utility.h，featureExtraction.cpp， imageProjection.cpp，imuPreintegration.cpp，mapOptmization.cpp

LIO-SAM/
  config/
    params.yaml             # 参数配置
  include/
    utility.h               # 读取参数，提供一些工具方法
  launch/
    run.launch              # 启动文件
  src/
    featureExtraction.cpp   # 点云计算曲率，提取特征（角点、平面点）
    imageProjection.cpp     # 激光点云运动畸变校正
    imuPreintegration.cpp   # imu预积分因子图优化，计算每时刻imu里程计
    mapOptmization.cpp      # scan-to-map匹配，因子图优化，闭环优化

对照LIO-SAM作者给出的下面这张系统架构图，我们首先获取一个整体上的印象。

首先对于一个SLAM系统，后端优化是一个核心模块，有较早的卡尔曼滤波器、现在流行的图优化、因子图优化。LIO-SAM则采用因子图优化方法，包含四种因子。

LIO-SAM因子：IMU预积分因子，激光里程计因子，GPS因子，闭环因子。

下图是LIO-SAM的因子图结构，变量节点是关键帧。相邻的关键帧之间，通过IMU数据计算预积分，获得位姿变换，构建IMU预积分因子。每个关键帧还有对应的GPS数据参与校正。如果有闭环出现，闭环帧之间可以构建约束。关键帧之间有若干普通帧，这些帧不参与图优化，但是会执行scan-to-map的配准，优化每帧位姿。

2. 整体流程

激光运动畸变校正。利用当前帧起止时刻之间的IMU数据、IMU里程计数据计算预积分，得到每一时刻的激光点位姿，从而变换到初始时刻激光点坐标系下，实现校正。
提取特征。对经过运动畸变校正之后的当前帧激光点云，计算每个点的曲率，进而提取角点、平面点特征。
scan-to-map匹配。提取局部关键帧map的特征点，与当前帧特征点执行scan-to-map匹配，更新当前帧的位姿。
因子图优化。添加激光里程计因子、GPS因子、闭环因子，执行因子图优化，更新所有关键帧位姿。
闭环检测。在历史关键帧中找候选闭环匹配帧，执行scan-to-map匹配，得到位姿变换，构建闭环因子，加入到因子图中一并优化。

下面在整体上给出这四个文件对应模块的功能，以及模块之间数据的发布-订阅关系。

2.1. ImageProjection 激光运动畸变校正

2.1.1. 功能简介

利用当前激光帧起止时刻间的imu数据计算旋转增量，IMU里程计数据（来自ImuPreintegration）计算平移增量，进而对该帧激光每一时刻的激光点进行运动畸变校正（利用相对于激光帧起始时刻的位姿增量，变换当前激光点到起始时刻激光点的坐标系下，实现校正）；
同时用IMU数据的姿态角（RPY，roll、pitch、yaw）、IMU里程计数据的的位姿，对当前帧激光位姿进行粗略初始化。

2.1.2. 订阅

订阅原始IMU数据；
订阅IMU里程计数据，来自ImuPreintegration，表示每一时刻对应的位姿；
订阅原始激光点云数据。

2.1.3. 发布

发布当前帧激光运动畸变校正之后的有效点云，用于rviz展示；
发布当前帧激光运动畸变校正之后的点云信息，包括点云数据、初始位姿、姿态角、有效点云数据等，发布给FeatureExtraction进行特征提取。

2.2. FeatureExtraction 点云特征提取

2.2.1. 功能简介

对经过运动畸变校正之后的当前帧激光点云，计算每个点的曲率，进而提取角点、平面点（用曲率的大小进行判定）。

2.2.2. 订阅

订阅当前激光帧运动畸变校正后的点云信息，来自ImageProjection。

2.2.3. 发布

发布当前激光帧提取特征之后的点云信息，包括的历史数据有：运动畸变校正，点云数据，初始位姿，姿态角，有效点云数据，角点点云，平面点点云等，发布给MapOptimization；
发布当前激光帧提取的角点点云，用于rviz展示；
发布当前激光帧提取的平面点点云，用于rviz展示。

2.3. ImuPreintegration IMU预积分

2.3.1. TransformFusion类

功能简介

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

订阅激光里程计，来自MapOptimization；

订阅imu里程计，来自ImuPreintegration。

发布

发布IMU里程计，用于rviz展示；

发布IMU里程计轨迹，仅展示最近一帧激光里程计时刻到当前时刻之间的轨迹。

2.3.2. ImuPreintegration类

功能简介

用激光里程计，两帧激光里程计之间的IMU预计分量构建因子图，优化当前帧的状态（包括位姿、速度、偏置）;

以优化后的状态为基础，施加IMU预计分量，得到每一时刻的IMU里程计。

订阅IMU原始数据，以因子图优化后的激光里程计为基础，施加两帧之间的IMU预计分量，预测每一时刻（IMU频率）的IMU里程计；

订阅激光里程计（来自MapOptimization），用两帧之间的IMU预计分量构建因子图，优化当前帧位姿（这个位姿仅用于更新每时刻的IMU里程计，以及下一次因子图优化）。

发布

发布imu里程计；

2.4. MapOptimization 因子图优化

2.4.1. 功能简介

scan-to-map匹配：提取当前激光帧特征点（角点、平面点），局部关键帧map的特征点，执行scan-to-map迭代优化，更新当前帧位姿；
关键帧因子图优化：关键帧加入因子图，添加激光里程计因子、GPS因子、闭环因子，执行因子图优化，更新所有关键帧位姿；
闭环检测：在历史关键帧中找距离相近，时间相隔较远的帧设为匹配帧，匹配帧周围提取局部关键帧map，同样执行scan-to-map匹配，得到位姿变换，构建闭环因子数据，加入因子图优化。