LIO-SAM论文与代码阅读笔记（三）代码阅读：featureExtraction.cpp和mapOptmization.cpp

0.前言

本文是 LIO-SAM论文与代码阅读笔记系列文章的一部分，所有博客如下：

LIO-SAM论文与代码阅读笔记（一）论文阅读

LIO-SAM论文与代码阅读笔记（二）代码阅读：imuPreintergration.cpp和imageProjection.cpp

LIO-SAM论文与代码阅读笔记（三）代码阅读：featureExtraction.cpp和mapOptmization.cpp

3.featureExtraction.cpp

3.1.FeatureExtraction类

3.1.1.数据流

订阅：

• 上一个节点发来的已经预处理过的、自定义的cloudInfo消息，话题"lio_sam/deskew/cloud_info"

发布：

• 本次提取特征之后的、自定义的cloudInfo消息，话题"lio_sam/feature/cloud_info"
• 本次提取的角点特征、面点特征，话题"lio_sam/feature/cloud_corner"、"lio_sam/feature/cloud_surface"

3.1.2.回调函数

3.1.2.1.laserCloudInfoHandler

概述：把上一个节点预处理得到的点云计算曲率、移除遮挡，最后选择角点和面点的特征点，最后把提取的特征点发布出去

步骤：

1. 把自定义cloudInfo中的点云消息转成pcl格式，后面处理的都是这些点云消息

2. 遍历点云所有的点，在每个scan可以计算曲率的范围内直接使用三维距离计算曲率（Q：感觉有点问题？还是不如像原始LOAM一样使用坐标差计算曲率更加准确，如下图这样，计算的曲率也比较大，实际上它曲率应是0，这种面对墙的情况，LiDAR离墙越近越严重）

3. 移除遮挡点，这个一直没看，也不影响

4. 遍历每个scan，把每个scan水平方向1800分成6份数，每一份都把曲率排序，然后找曲率大的点和曲率小的点分别作为线点和面点，由于面点太多最后把面点进行降采样处理。

5. 把提取的特征点发布出去，这里的处理和上一个节点一样，如果有订阅角点和面点话题的节点就发布消息，如果没有就只把特征点放到自定义的couldInfo消息中发布给最后的mapOptimization节点处理。（Q：注意这里把原始点云也通过ROS发布了，看后面用没用到？）

4.mapOptmization.cpp

4.1.mapOptimization类

4.1.1.数据流

订阅：

• 上一个节点发来的提取过特征点的、自定义的cloudInfo消息，话题"lio_sam/feature/cloud_info"
• GPS消息，话题gpsTopic，注意这个比较有意思，订阅的消息类型是nav_msgs::Odometry，也就是他并非原始的GPS消息类型，而是经过了转化，Q：好像是运行了ros内部的已经有的节点实现的，这里要去看看。
• 手动发布的哪两帧有回环关系的消息，实际最后代码中没使用，实际意义也不大，不用管
• 保存地图功能的服务，可以在运行过程中随时保存地图，消息"lio_sam/save_map"

发布：

• 增量式的里程计，给IMU节点优化IMU零偏使用，话题"lio_sam/mapping/odometry_incremental"
• 全局的里程计，给IMU节点中的数据融合类TranstionFuison用于得到IMU频率的、在全局里程计基础上递推的里程计消息，话题"lio_sam/mapping/odometry"
• 其他消息，很多，基本都是rviz显示使用的

4.1.2.回调函数

4.1.2.1.laserCloudInfoHandler

概述：把上一个节点提取得到的当前帧点云的特征点拿出来，利用IMU估计的初始位姿匹配到局部地图中（局部地图是利用最近帧的相邻帧的地图进行构造）得到点云配准的位姿。最后判断是否有回环，如果有回环还要调整全局轨迹的位姿，最后发布里程计和点云。

步骤：

0. 记录这帧点云的时间戳（注意是第一个点的时间），并把提取的角点和面点转成pcl格式，后面都是处理这个点云

1. 利用IMU里程计结果估计当前帧的初始位姿：

   （0）和增量里程计有关：先把上一帧估计的最优位姿存到变量incrementalOdometryAffineFront里

   （1）如果这是第一帧点云，那么就利用9轴IMU的欧拉角赋值为初始位姿。同时也会判断是不是只用6轴结果，这样的话就把yaw角设置成0，这样6轴IMU只要能输出姿态也是可以使用的。

   注意：

   • 这个地方就是这样操作的，就是为了LIO的参考世界坐标系是ENU坐标系（9轴）或者至少Z轴和重力对齐（6轴）。
   • 另外我感觉这里有一个小bug，就是下图绿色字体的部分，这里在return之前，应该把当前赋值的初始位姿也赋值给上一次IMU里程计发来的位姿，这样第1帧点云过来的时候直接就可以用IMU里程计的位姿做差，然后叠加到第0帧的位姿上，得到对第1帧点云的一个初始位姿估计了

   （2）如果有前端IMU里程计提供的初始位姿猜测，那么就把它和上一次发来的初始位置猜测做差得到位姿增量，然后叠加到要优化上一次的全局位姿上，得到当前帧的全局位姿预测

2. 提取当前帧的关键帧，并且把这些关键帧的点云构成用于匹配的局部点云地图：

   （1）首先判断是否有关键帧，如果没有关键帧说明这是系统的第0帧点云，那么直接返回。后面点云到地图匹配也不会执行，然后直接进入因子图优化中，这个时候肯定没有回环，假设也没有GPS，那么因子图优化相当于不会改变初始猜测位姿，也就是第0帧的位姿就被赋值成了9轴IMU初始测量的欧拉角

   （2）从第1帧开始，就有关键帧了，那么就从这些关键帧中提取和当前帧时间、空间距离上比较近的哪些帧，然后用他们的点云构成局部地图：

   • 把关键帧的3D位置构成一个kdtree，然后寻找关键帧的最后一个位置（也就是当前这帧点云的上一帧点云的位置，肯定是离当前位置很近的，这里其实无所谓，用第1步得到的当前帧的初始猜测位置也可以，反而我觉得用初始猜测位置应该更好）50米范围内的所有关键帧，然后把这些关键帧的3D位置存入到一个点云结构中

   • 对刚才得到的关键帧3D位置点云进行一个下采样，然后把下采样的这些点再从刚才构建的关键帧kdtree中寻找最近的那个点（这是因为下采样之后得到的体素方格内所有点的重心，一般情况下不会是体素方格内原有的点），作为最后真正构成点云地图的那个点。注意这里关键帧的3D位置点云中，强度存储的实际上是它在关键帧数组中的索引。

   • 刚才提取到了空间上位置相近的关键帧，这里再提取时间上10秒之内的关键帧，二者组合最为最后的构成局部地图关键帧

   • 把上一步得到的关键帧的所有点云根据他们的位姿转到世界坐标系下，得到配准使用的角点和面点局部地图：

     （a）先把这次要提取的角点、面点局部地图清空，这样确保不会存在以往的帧用的、但是当前帧用不到的局部地图。注意这里只能这么操作，因为维护的局部地图就是一个非常大的点云，加进去之后就没法知道某个点属于哪一帧了，所以是无法按照关键帧的从属属性来剔除他对应的点云的

     （b）为了防止重复转换关键帧的点云到世界坐标系下，这里维护了一个全局的map变量laserCloudMapContainer，它的键是关键帧的索引，值就是这个关键帧对应的转换到世界坐标系下的角点和面点。这样如果当前帧选择的局部地图关键帧在这个map中，那么就可以直接从map中拿转换到世界坐标系下的角点和面点了，而不用重复转换。

     当然如果这里在map中没有找到当前帧选择的局部地图关键帧，那么就把它的角点和面点通过它的位姿转换到世界坐标系下，然后再加入到用于匹配的局部地图中。同时这里也不要忘了在全局map变量中添加这个关键帧和它转换的点云信息，为下一帧使用。

     （c）为了防止局部地图点云过于密集，还要对上一步得到的局部地图进行一个体素滤波下采样，从而得到最后真正用于scan-to-map的局部地图。

     （d）如果维护的全局map变量太大了，超过了1000个变量，那么直接清空，下一次重新构建。这看似暴力，但是其实也是可以的。首先1000帧中应该大多数都是很久之前的关键帧，能够被选择为当前帧的局部地图关键帧的所占比例应该很小了。另外直接清空之后下一帧来了之后构建局部地图的时候无非就是需要重新再把它的局部地图关键帧的点云再重新转换到世界坐标系下，这个时候对这一帧的速度来说会有影响，这个算是用时间换空间的一种权衡吧。但是我觉得一个很简单的改进方法就是按照按照局部地图关键帧来进行部分清空，因为一般LIO选的局部地图关键帧无论从时间上还是空间上都是当前帧之前的n个关键帧，那么只要判断出此时选择的局部地图关键帧的最小索引，然后在全局map变量中把小于这个索引的哪些（基本就是n帧之前的哪些）清除掉就可以了，这样达到了清内存的目的，也不会让下一帧全部从头开始转换局部地图关键帧的点云到世界坐标系下那样拖慢效率。

3. 对当前帧的角点和面点进行下采样，也是为了降低匹配时的计算量

4. scan to map匹配并优化：

   （1）把角点和面点的局部地图构建kdtree，然后在kdtree中寻找角点和面点最近的5个点，组成直线或者平面，然后计算梯度方向、残差（点到线、面距离）

   （2）把上一步得到的点到线和面的梯度、残差组合成一个大的变量

   （3）高斯牛顿迭代优化30次，直到收敛。注意里面会判断是否发生了退化，如果在某个方向上发生了退化那么这个方向上就不更新。比如在一个长走廊中，走廊这个方向（假设大概是LiDAR的x轴）上就会发生退化，那么就不更新这个方向上的分量

   （4）优化结束，如果是9轴IMU，会利用IMU的姿态进行一个融合，不过IMU姿态只占比1%，感觉可有可无。最后还会针对室内场景做一些约束，比如Z轴不会太大。

   （5）和后端增量里程计有关：赋值最后优化结果为incrementalOdometryAffineBack

5. 根据优化结果判断是否是关键帧，如果是关键帧那么继续进行因子图优化：

   （1）判断当前帧和上一个关键帧的位姿变化，只要有一个自由度超过了设置的阈值就认为当前帧是关键帧。如果不是关键帧，直接return。

   只有是关键帧，那么才进行下面的所有步骤：

   （2）把当前帧位姿作为一个因子节点加入因子图，并且把当前帧和上一帧之间的相对位姿变换作为相邻关键帧约束。==为什么不使用先验约束因子的形式？而是相邻约束因子的形式？==好像xq说如果是先验因子的形式，后面加入回环检测的话调整不太好调整？而使用相邻约束因子就容易调整一些。

   （3）加入GPS因子：里面有一堆判断，但是很简单，因为它就是对某个状态的先验因子。另外值得注意的是论文里说的是它对GPS进行了一个插值处理，但是实际上并没有，他是判断只要GPS时间戳离当前时间足够接近，那么就认为GPS位置就是当前帧的位置

   （4）加入回环因子：回环因子的协方差是在ICP求解的时候得到的得分，然后这里作为回环因子的置信度了。

   （5）所有因子都加完了，进行优化。如果说上面加入过回环或者GPS因子，那么要多优化几次。

   （6）优化完成，把优化后的当前帧的位姿加入到关键帧数组中。和后端增量里程计有关：同时更新当前帧的优化结果到transformTobeMapped变量中

   （7）把当前帧的点云保存到全局变量cornerCloudKeyFrames和surfCloudKeyFrames，注意这个点云就是最后保存地图的时候保存下来的那些点云。

   （8）最后更新当前最新的位姿到path中

6. 更新全局位姿：如果有回环或者GPS，位姿会发生变化，那么才执行下面的步骤

   （1）因为回环后很多位姿都会变化，所以维护的全局map里存储的转到世界坐标系下的点云也都要变化，这里索性直接清空，下一帧来了之后构建局部地图的时候再自己转吧

   （2）把维护的关键帧3D位置和6D位姿全部从gtsam优化结果中重新取出来，同时更新可视化的全局轨迹path

7. 发布里程计：

   （1）首先发布全局里程计，这个就是经过回环或GPS（如果有的话）矫正的位姿，当然如果没有回环或者GPS矫正，那么它就是纯后端点云匹配的结果

   （2）发布增量里程计：这个操作很有意思。

   • 首先第一次执行发布里程计的函数的时候，会把要发布的位姿increOdomAffine就赋值成全局位姿transformTobeMapped
   • 后面再发布的时候，都会在这个位姿的基础上，叠加一个纯LiDAR odom进行点云配得到的位姿的相对变化。注意之前认为这个地方有问题，现在看是没问题的。之前认为计算点云配准位姿的变化的时候，初始位姿incrementalOdometryAffineFront用的就是上一帧优化得到的全局最优位姿，而incrementalOdometryAffineBack是点云配准后的位姿，这样不又把上一帧优化的全局位姿牵扯进来了吗？其实并不是，初始位姿虽然是全局位姿，但是它只是为了让当前帧点云有一个更准确的匹配，匹配得到的结束位姿相对初始位姿仍然是一个增量位姿，这里面其实是不包含优化的全局位姿信息的。所以我们把这个位姿变化一直往第0帧上叠加，那么最后的里程计就是纯增量的里程计，不包含任何全局的位姿信息。

8. 发布当前帧的可视化信息：包括当前帧的特征点云、原始点云、全局路径path

总结：如果是关键帧，会增加哪些处理？

首先要明白即使是普通帧，也不是每一帧点云都会处理，而是为了保证实时性进行了降频处理，代码中是3帧处理2帧这种降频。

对于来的当前帧的点云，首先计算LiDAR odom，也就是点云配准是都会执行的。当执行到保存关键帧的时候，如果当前帧不是关键帧，那么首先不会执行后面的因子图优化，自然维护的全局里程计位姿TransformTobeMaped也就不会变换。因为不是关键帧自然也不会把当前帧的位姿存到关键帧数组中，也不会存储这一帧的点云。其他就没有什么不同了。

也就是说，关键帧增加的步骤就是执行一个全局因子图优化，而且这个因子图优化只有在有回环或者GPS的时候才起作用。所以可以认为，使用关键帧的目的就是为了降低后端添加回环的因子图优化的计算量。而其他帧不论是不是关键帧，都会计算一个全局里程计。

4.2.单独开的回环检测线程

• 把最新的关键帧的点云提出取来

• 在关键帧组成的kdtree中，寻找距离最新的关键帧15米范围内的其他关键帧，找到的距离最近、并且时间在最新的关键帧前30秒的那个关键帧就是回环帧。
  

• 对找到的回环帧，在它25米范围内再寻找关键帧，然后把找到的关键帧的点云转到世界坐标系下作为回环帧的局部地图。然后把最新关键帧的点云和这个局部地图进行一个ICP的点云匹配，从而得到最新关键帧应该在世界坐标系下的真实的位置。

  注意：这里是当前帧的点云和回环帧构成的局部地图进行匹配，所以结果得到的坐标变换仍然是最新关键帧相对于世界坐标系的坐标，而不是相对于回环帧的坐标。所以后面还要进行一些其他变换，才能得到最新关键帧和回环帧之间的位姿变换关系，这样才能作为一个相对位姿约束因子加入到因子图中。而ICP匹配会有一个得分，这个得分就作为这个相对位姿约束因子的置信度加入到因子图中。