LIO-SAM代码解读
笔者初学，文章仅供参考，有不同于网上注释的版本的地方，如有错误请多包含。
该开源算法的作者是Ti Xiaoshan，开源过lego-loam、lio-sam、vins-mono算法。
系统架构
整体来看：输入是imu、lidar观测数据，GPS可选，输出是小车的位姿
对于紧耦合的理解为：雷达里程计信息用来计算imu偏差，得到imu里程计；imu里程计用来作为当前帧点云的初始位姿估计；imu信息对雷达点云去畸变；去畸变的雷达点云与局部地图匹配得到雷达里程计。
下面我会依次展开各个模块话题订阅与发布
featureExtraction.cpp
这个模块代码量最少，也最简单。
结合代码来看，话题包含信息如下
lio_sam/deskew/cloud_info：
cloudInfo.cloud_deskewed：去畸变之后的有效点云
cloudInfo.initialGuessX等：坐标初始估计，来自imuOdom坐标，用于mapOptimization
cloudInfo.initialGuessRoll等：欧拉角初始估计，来自imuOdom四元数，用于mapOptimization
cloudInfo.imuRollInit等：欧拉角初始化，来自imu原始数据，用于mapOptimization
cloudInfo.startRingIndex[i]：i范围0~15，本身与count大小有关，去掉前5个点的起始索引
cloudInfo.endRingIndex[i]：i范围0~15，本身与count大小有关，去掉后5个点的终止索引
count范围：0~（16-1）*（1800-1）
cloudInfo.pointColInd[count]：记录索引，本身范围0~1799
cloudInfo.pointRange[count]：记录点的距离
cloudInfo.imuAvailable：判断imu是否可用，处理完imu队列后，变为true
cloudInfo.odomAvailable：判断里程计是否可用，更新完初始坐标、欧拉角后变为true
lio_sam/feature/cloud_info:
添加的内容如下：
cloudInfo.cloud_corner：边缘点
cloudInfo.cloud_surface：面点
这个cpp订阅刚刚在imageProjection.cpp部分中发布的lio_sam/deskew/cloud_info消息，然后提取边缘点（角点），平面点，然后填充cloud_info的cloud_corner和cloud_surface字段，重新以"lio_sam/feature/cloud_info"的形式发布出去。
calculateSmoothness()：计算曲率。
比方说提取的点云数量(extractedCloud)有200个，要计算第6个点~第195个点的曲率，其实计算的是点云距离差的平方，以第6个点为例：求第1个点与第六个点的距离差+第2个点与第六个点的距离差+…+第5个点与第六个点的距离差+第7个点与第六个点的距离差+…+第11个点与第六个点的距离差，然后将这个值求平方，来表示第6个点的曲率。这些计算曲率的点未被标记（cloudNeighborPicked[i] = 0）标签为0（cloudLabel[i] = 0），保存曲率到cloudSmoothness[i].value，保存索引到cloudSmoothness[i].ind ，以我们举的例子为例，i的范围从6~195.
markOccludedPoints():标记被遮挡的点和平行点，标记后会有相关的点被比较为1(cloudNeighborPicked[i] = 1)。
现讨论遮挡点和平行点的判断条件，还是以200个点举例
遮挡点：
从第6个点开始，得到第6、7个点的距离（cloudInfo.pointRange[6(7)]），求6、7点的索引(cloudInfo.pointColInd)的差的绝对值，对于同一ring这个差值不会很大，大部分情况下应该为1，不同的ring这个差值就比较大。所以这个目的应该是处理同一ring下的遮挡点提取。if (columnDiff < 10)这个条件也可以理解为0.2°的分辨率×10=2°，也就是2°范围内（水平），再判断第6、7个点的距离差，如果大于0.3米就认为有遮挡情况，这里画图讲一下我的理解：
如图所示：如果出现这样的情况，就认为6点有可能遮住7点，甚至后边的8、9、10、11点，当然8、9、10、11点的位置我们没去判断，这里作者可能认为8、9、10、11这几个点的距离都比7点要大？其实仔细想一想这里有点问题？所以就把7、8、9、10、11这5个点标记为1，后边的平行点以及角点和面点就不再考虑这些了。
同理，如果反过来也是一样。
平面点
以第6个点举例来说明：求第5个点与第6个的差的绝对值diff1，第7个点与第6个的差的绝对值diff2，剩下的看下图
先看黑色笔迹，如果这样的话，就认为点6是平行点，5、6、7三个点在一个平面S上（或者这样说：56直线与67直线平行），从而把点6标记为1（cloudNeighborPicked[6] = 1），不知道这样理解是否有问题？再看另一种情况就是棕色5点，当同时满足这两个条件时，是不是差不多可以认为56直线与67直线平行？这种情况从激光雷达扫描的实际情况来想一想确实以后不需要再考虑6点了，我想不到是什么现实环境会导致这样的情况。
extractFeatures()提取角点、面点
以16线(ring)为例，一条一条的来提取角点、面点，这是最外面的for循环。
首先把水平的360°区域分成六个部分，每个部分提取20个角点和不限数量的面点。
先讨论6个区域的划分（好像没什么意义）
int sp = (cloudInfo.startRingIndex[i] * (6 - j) + cloudInfo.endRingIndex[i] * j) / 6;
int ep = (cloudInfo.startRingIndex[i] * (5 - j) + cloudInfo.endRingIndex[i] * (j + 1)) / 6 - 1;
j从0~5,i在这里当做常量不变化。
这段代码应该最终化简结果，参考其他大佬的注释，中间过程其实是这样，化简之后就是代码那样
其中startRingIndex顺时针到endRingIndex这个小的锐角之间大概有10个点没参与，这点数量与剩下的1790个点相比可以忽略，红色区域是对应j=0时划分的第一个区域。剩下的5个区域同理。
角点、面点判断条件
前面判断遮挡点与平行点没有被挑选(cloudNeighborPicked[i] == 0)的点且曲率大于edgeThreshold被选为角点，每个区域（不到300个点）只找20个，标签cloudLabel为1，放入cornerCloud，假如说第66个点被选为角点，则61~71个点都标为选中，目的是不让特征选的过于密集？
前面判断遮挡点与平行点没有被挑选且没被选为角点(cloudNeighborPicked[i] == 0)的点且曲率小于surfThreshold被选为平面点，每个区域（不到300个点）数量不限，标签cloudLabel为-1，放入surfaceCloudScan，假如说第166个点被选为平面点，则161~171个点都标为选中，目的是不让特征选的过于密集？然后对于平面点降采样。存到surfaceCloudScanDS。
imageProjection.cpp
这个模块是特征提取的上一个模块，比特征提取模块稍难
/imu_corect包含的信息有：欧拉角、角速度、线加速度、时间戳
/odometry/imu_incremental包含的信息有：雷达坐标系下的三维坐标、四元数、“中间系”下的线速度、角速度
/point_raw包含的信息有：time、ring、intensity、三维坐标
lio_sam/deskew/cloud_deskewed:
lio_sam/deskew/cloud_info：
cloudInfo.cloud_deskewed：去畸变之后的有效点云
cloudInfo.initialGuessX等：坐标初始估计，来自imuOdom坐标，用于mapOptimization
cloudInfo.initialGuessRoll等：欧拉角初始估计，来自imuOdom四元数，用于mapOptimization
cloudInfo.imuRollInit等：欧拉角初始化，来自imu原始数据，用于mapOptimization
cloudInfo.startRingIndex[i]：i范围0~15，本身与count大小有关，去掉前5个点的起始索引
cloudInfo.endRingIndex[i]：i范围0~15，本身与count大小有关，去掉后5个点的终止索引
count范围：0~（16-1）*（1800-1）
cloudInfo.pointColInd[count]：记录索引，本身范围0~1799
cloudInfo.pointRange[count]：记录点的距离
cloudInfo.imuAvailable：判断imu是否可用，处理完imu队列后，变为true
cloudInfo.odomAvailable：判断里程计是否可用，更新完初始坐标、欧拉角后变为true
斜体在特征提取模块时候会广泛用到
对于imu原始数据：与雷达时间对齐，利用第一帧数据初始化cloudInfo.imuRollInit、cloudInfo.imuPitchInit、cloudInfo.imuYawInit，其余帧imu数据初始化各个时刻角度imuRotX、imuRotY、imuRotZ
对于imu里程计数据：与雷达时间对齐，利用第一帧数据初始化位姿cloudInfo.initialGuessX、cloudInfo.initialGuessY、cloudInfo.initialGuessZ、cloudInfo.initialGuessRoll、cloudInfo.initialGuessPitch、cloudInfo.initialGuessYaw，根据首尾两帧得到位姿增量odomIncreX、odomIncreY、odomIncreZ、rollIncre、pitchIncre、yawIncre
对于lidar数据：根据对应时间的imu原始数据，得到每个筛选后的点去畸变位姿
订阅imu、lidar原始数据，来自imuPreintegration.cpp的imu里程计，对lidar的每帧点云数据去畸变。发布lio_sam/deskew/cloud_info给featureExtraction.cpp进行特征提取，发布lio_sam/deskew/cloud_deskewed（只有点云信息）用于rviz展示？
imuDeskewInfo()：初始化当前帧激光起止时间内的欧拉角
在imu序列里找到能够覆盖当前帧激光起止时间的最小时间区间，如下图起点b，根据起点b初始化cloudInfo.imuRollInit, cloudInfo.imuPitchInit, cloudInfo.imuYawInit，imuRotX[0] = 0; imuRotY[0] = 0; imuRotZ[0] = 0;
imuTime[0] = currentImuTime;以此为基础初始化这段时间内相应点的imuRotX、imuRotY、imuRotZ、imuTime，当然还有角速度angular_x、angular_y、angular_z，最后cloudInfo.imuAvailable = true;
odomDeskewInfo():里程计信息处理
利用起始时刻startOdomMsg来初始化cloudInfo.initialGuessX、initialGuessY、initialGuessZ、initialGuessRoll、initialGuessPitch、initialGuessYaw；只针对起止时刻来得到变换矩阵transBt，再得到位姿增量（odomIncreX, odomIncreY, odomIncreZ, rollIncre, pitchIncre, yawIncre），背后的数学原理懵懵懂懂。
与imu一样，同样要求odom时间范围覆盖当前帧Lidar起止时间。
这里会不会有一个伏笔：这个时间段内有这么多时刻，而变换矩阵仅仅只使用了起止两个时刻？可能作者这样做是为了简单，后续增量是用比例来乘的。imu的采样频率远大于lidar采样频率，若每一帧都考虑，计算量会很大。
projectPointCloud():遍历所有雷达点云
这里根据点云的距离、ring筛选掉一些点，将columnIdn（列号）的范围整到（0，1800），且与360个角度有一一对应关系，如果我没梳理错的话应该是这样的关系。
然后就是将所有的雷达点云转换到当前帧雷达数据第一个点所在的坐标系。再填入深度（距离），索引（featureExtraction.cpp会用到）。这里有一个比较细的细节问题：就是前面可能会因为距离等因素筛选掉一些点云点，比如筛选掉500个，还剩下1800-500=1300个，这样子的话，第一个ring的index只能到1299，第二个ring的index会从0+1*1800开始计数，我要表达的意思也就是index也许不是一个连续整数。
最后，将坐标系转换后点云保存到fullCloud中。
PointType deskewPoint(PointType *point, double relTime)
findRotation（）前后时刻差值计算旋转增量
这里的一个关键是在imu的序列中找到一个点，使得这个点的时间imuTime[imuPointerFront]大于当前激光点的时间pointTime，然后根据下图计算旋转量。
findPosition（）在这里没用，根据注释掉的内容，这里要使用到imuOdom得到的增量odomIncreX、odomIncreY、odomIncreZ。
再根据变换矩阵将当前点的三维坐标变换到第一个点所在的坐标系。这里操作类似
odomDeskewInfo()。
cloudExtraction（）
一个ring一个ring的开始，只要这个点有距离值，保存索引到pointColInd[count]，其中count值0180018002…18005，其内容是0~1800；保存距离信息到pointRange[count]；将fullCloud相应的点提取到extractedCloud。最后就是发布。
imuPreintegration.cpp
这个模块设计预积分模块，较难
对imu预积分简单的理解
imu预积分的必要性，即由于imu本身存在bias，这个量会随着积分使得位姿飘得越来越严重；再就是位姿更新时，按照正常的思路需要重新积分关键帧之间的imu数据，这样的话计算量就会很大。
我们主要留意三个等式积分号内的 qwbt ,当优化位姿的时候，优化的其实是b系在世界系w下的位姿，这个东西是会变的。也就是说，每优化一次，每两个时刻之间的imu位姿增量积分部分都要重新进行计算。
此时的积分项就变成了相对于i时刻的状态而不是相对于世界坐标系。当i时刻相对于世界坐标系(
qwbi )变化的时候，当前时刻相对于i时刻(比如qbibt)是不会变化的，后者就只与imu的数据有关,imu当前时刻测得数据是多少就是多少。
lio-sam预积分具体使用GTSAM来实现的，目前仅了解API调用。
/imu_corect包含的信息有：欧拉角、角速度、线加速度、时间戳
lio_sam/mapping/odometry包含：时间戳、id、三维坐标、四元数
lio_sam/mapping/odometry_incremental包含：时间戳、id、三维坐标、四元数
/odometry/imu_incremental包含的信息有：雷达坐标系下的三维坐标、四元数、“中间系”下的线速度、角速度
/odometry/imu:雷达坐标系下的三维坐标、欧拉角
lio_sam/imu/path:雷达坐标系下的位姿、id等
TransformFusion类
这个类主要做的事情就是融合，融合lidarOdom和imuOdom发布TF与imu/path
lidarOdometryHandler():
代码比较简单，锁雷达里程计数据，得到变换矩阵lidarOdomAffine以及时间戳lidarOdomTime，imuOdometryHandler()会用到
imuOdometryHandler():计算当前时刻imu里程计位姿，发布话题
Q:这里有一个关于tf发布的坑：map_to_odom这个变换矩阵有啥用？tfMap2Odom还有这个变量。odom_2_baselink
时间同步：保证雷达里程计起始时间小于imu里程计队列imuOdomQueue起始时间
当前时刻imu里程计位姿=最近的一帧激光里程计位姿 * imu里程计增量位姿变换，将其转换为平移和欧拉角，并发布"odometry/imu"、发布tf、发布imu/path（未看）
odometry/imu为imuOdom与lidarOdom融合之后的里程计，TF与imu/path都要用
这里才把imu里程计数据与lidar里程计数据进行融合，从而发布到tf用于建图
IMUPreintegration类
这个类主要做的事情就是预积分
将imu时间戳与lidarOdom时间戳对齐，来对imu数据进行预积分，发布/odometry/imu_incremental
odometryHandler():对雷达里程计和imu数据进行处理
这个回调函数主要是利用因子图优化得到优化后的状态和bias传递给imuHandler()进行预积分
这里直接用流程图来说明吧。
第一个是总体的流程图
初始化流程图
第101帧lidar里程计信息处理流程图
其他帧雷达里程计数据处理
其中的图优化处理应该是这样的
将优化好的prevState_(包含pose、Vel)、prevBias_传递给imuHandler来预测当前状态
imuHandler()对原始imu数据进行处理
转换到雷达坐标系，放到imuQueOpt和imuQueImu队列中
imu预积分器添加一帧imu数据，得到预积分量，根据这个预积分量和上一帧激光里程计的状态偏置预测得到目前的状态
再将目前imu位姿的状态转到雷达坐标系下，发布imu里程计信息（包含了位置、旋转四元数、线速度、角度）
详细流程如下：
mapOptimization.cpp
这个代码量最多，涉及的知识也最杂，目前读的还不是很明白。
这个模块有三个线程：
1.雷达里程计以及后端优化线程：主要做两件事情，发布雷达里程计；优化关键帧位姿。
再具体一点，执行scan-mapping，得到雷达里程计；将lidarOdom、回环检测因子加入因子图优化，得到全局优化的关键帧位姿
2.回环检测线程，使用ICP算法，检测合适回环因子。
3.全局地图可视化线程，把历史关键帧的点云根据最新优化的位姿转换到世界坐标系，将融合之后的全局点云地图发布出去。
雷达里程计以及后端优化线程
点云匹配使用高斯牛顿算法优化当前帧点云到局部地图的距离
流程图如下
updateInitialGuess()：
大部分情况下利用imu里程计更新transformTobeMapped[]，其中初始值更新时参考如下
extractSurroundingKeyFrames()：这里C++不会的知识点和库比较多，看不太懂。
大致应该是提取距离以及时间比较近的点，降采样；将角点、面点加入局部地图
downsampleCurrentScan()：来自特征提取模块角点、面点（与上一步角点、面点不同）降采样
scan2MapOptimization()：
cornerOptimization()：边缘点匹配优化。计算点到直线距离，如果这个距离小于10cm就加入边缘点
判断成为直线的条件与论文不太一致，这里牵扯到数学均值、协方差、特征向量相关
surfOptimization()：同上，距离一致
combineOptimizationCoeffs():…
回环检测线程
比如这里的x3状态与xi+1状态能够构成回环，可以对这两个状态添加约束，使得整体的位姿估计达到全局最优的状态。在作者演示的视频里，效果是这样的(下图)
由于ICP算法较为费时，故该线程执行频率时1Hz。回环点的选择：1.距离比较近2.时间比较远
全局地图可视化线程
为了可视化，这个过程也比较费时，执行频率5Hz。
参考链接
https://blog.csdn.net/qq_42938987/article/details/108434290
https://blog.csdn.net/unlimitedai/article/details/107378759#t1
https://blog.csdn.net/So____?type=blog
http://49.235.237.45/2020/08/19/51liosam/
https://blog.csdn.net/zkk9527/article/details/117957067#t5
https://www.bilibili.com/video/BV1FW411C7R3/?spm_id_from=333.337.search-card.all.click
https://blog.csdn.net/weixin_37835423/article/details/111587379
309
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
