LIO-SAM-noted

这里主要对LIO-SAM的整个Project做一个感性上的梳理，详细的注释参考仓库中的代码，关于具体原理参考我的博客。

LIO-SAM是一个激光惯导耦合的SLAM框架，包含了激光里程计、回环检测等功能模块。整个工程主要由四个cpp文件实现。

下面对四个文件的主要功能做一个简述，黑体加粗的地方即为该节点的核心功能！

imageProjection.cpp

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

2、同时用IMU数据的姿态角（roll、pitch、yaw）、IMU里程计数据的的位姿，对当前帧激光位姿进行粗略初始化。

订阅：

1、订阅原始IMU数据，把imu数据转换到lidar坐标系下，存入imuQueue；

2、订阅IMU里程计数据（来自ImuPreintegration，表示每一时刻对应的位姿），存入 odomQueue；

3、订阅原始激光点云数据，根据上面两步的数据对点云做去畸变处理。

发布：

1、发布当前帧激光运动畸变校正之后的有效点云，用于rviz展示；

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

featureExtraction.cpp

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

订阅：

1、订阅当前激光帧运动畸变校正后的点云信息，来自ImageProjection，计算曲率并进行特征点提取。

发布：

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

2、发布当前激光帧提取的角点点云，用于rviz展示；

3、发布当前激光帧提取的平面点点云，用于rviz展示。

imuPreintegration.cpp

这里定义了两个类 IMUPreintegration 和 TransformFusion

IMUPreintegration类

订阅激光里程计以及IMU话题，用激光里程计两帧之间的IMU数据构建预积分因子，优化两帧的状态（位姿、速度）和偏置。优化初值使用前一帧的状态和偏置施加imu预积分获得，优化得到当前帧的状态和偏置之后，重新对当前帧之后的IMU进行预积分。同时IMU回调中利用优化之后的状态和偏置开始进行预积分量得到一个高频的增量里程计。

订阅：这里涉及到两个回调函数

1、订阅IMU原始数据，以经因子图优化后的激光里程计的状态为基础，施加两帧之间的IMU观测量，预测每一时刻（IMU频率）的IMU里程计状态变量（这里是一个增量里程计，仅给出当前激光帧到当前IMU时刻的里程计），最后发布到话题 /odometry/imu_incremental。作者这里很多地方是直接使用了gtsam中的一些API；

2、订阅激光里程计（来自MapOptimization），用两帧之间的IMU预计分量构建因子图，优化当前帧和上一帧的状态（位姿、速度）和偏置位姿（这个位姿仅用于更新每时刻的IMU里程计，以及下一次因子图优化）。 GTSAM从4.0版本开始就在内部增加了IMU预积分相关的接口

发布：

1、发布当前帧激光运动畸变校正之后的有效点云，用于rviz展示；

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

TransformFusion类

主要功能是订阅激光里程计（来自MapOptimization）和IMU增量里程计，根据当前时刻对应的激光里程计帧（低频的），和当前时刻的IMU里程计变换增量（高频的，走在激光里程计前面），叠加计算当前时刻IMU里程计；rviz展示IMU里程计轨迹（局部）。

订阅：

1、订阅激光里程计，来自MapOptimization

2、订阅imu里程计，来自ImuPreintegration

发布：

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

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

imuPreintegration.cpp

1、scan-to-map匹配：提取当前激光帧特征点（角点、平面点），局部关键帧map的特征点**，执行scan-to-map迭代优化，更新当前帧位姿**；这部分涉及到一些原理推导可以参考我的博客：LIO-SAM中的scan_to_map原理剖析

2、关键帧因子图优化：关键帧加入因子图，添加激光里程计因子、GPS因子、闭环因子，执行因子图优化，更新所有关键帧位姿；

3、闭环检测：在历史关键帧中找距离相近，时间相隔较远的帧设为匹配帧，匹配帧周围提取局部关键帧map，同样执行scan-to-map匹配，得到位姿变换，构建闭环因子数据，加入因子图优化。

订阅：

1、订阅当前激光帧点云信息，来自FeatureExtraction；

2、订阅GPS里程计；

3、订阅来自外部闭环检测程序提供的闭环数据，本程序没有提供，这里实际没用上。

发布：

1、发布历史关键帧里程计；

2、发布局部关键帧map的特征点云；

3、发布激光里程计，rviz中表现为坐标轴；

4、发布激光里程计；

5、发布激光里程计路径，rviz中表现为载体的运行轨迹；

6、发布地图保存服务；

7、发布闭环匹配关键帧局部map；

8、发布当前关键帧经过闭环优化后的位姿变换之后的特征点云；

9、发布闭环边，rviz中表现为闭环帧之间的连线；

10、发布局部map的降采样平面点集合；

11、发布历史帧（累加的）的角点、平面点降采样集合；

12、发布当前帧原始点云配准之后的点云。

Thanks

LIO-SAM

LIO-SAM-DetailedNote