LIO-SAM源码解析（二）：代码结构
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匹配，得到位姿变换，构建闭环因子数据，加入因子图优化。
2.4.2. 订阅
- 订阅当前激光帧点云信息，来自FeatureExtraction；
- 订阅GPS里程计；
- 订阅来自外部闭环检测程序提供的闭环数据，本程序没有提供，这里实际没用上。
2.4.3. 发布
- 发布历史关键帧里程计；
- 发布局部关键帧map的特征点云；
- 发布激光里程计，rviz中表现为坐标轴；
- 发布激光里程计；
- 发布激光里程计路径，rviz中表现为载体的运行轨迹；
- 发布地图保存服务；
- 发布闭环匹配局部关键帧map；
- 发布当前关键帧经过闭环优化后的位姿变换之后的特征点云；
- 发布闭环边，rviz中表现为闭环帧之间的连线；
- 发布局部map的降采样平面点集合；
- 发布历史帧（累加的）的角点、平面点降采样集合；
- 发布当前帧原始点云配准之后的点云；
参考文献
LIO-SAM源码解析：准备篇 - 知乎