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

0.前言

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

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

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

1.imuPreintergration.cpp

1.1.IMUPreintegration类

1.1.1.数据流

后端增量里程计，话题lio_sam/mapping/odometry_incremental，注意这里的增量里程计其实就是单纯的里程计，也就是相邻两帧之间的坐标增量，因为对于前端的IMU预积分节点来说，要求这个发来的里程计增量必须是平滑的。这个增量里程计部分还是有疑问，具体看后面mapOptmization.cpp里面的问题。

最新理解：
- 首先后端的增量里程计部分没有问题，详细可以看后面的讲解。
- 这里为什么要求增量里程计而不是全局里程计，我觉得有两个原因：
  
  （a）如果全局优化后结果和优化前结果相差太大，那么传给前端IMU优化的话，很有可能导致前端的优化崩溃，因为无法同时满足全局里程计位姿的先验要求和IMU预积分的帧间约束要求；
  
  （b）如果全局优化后结果和优化前相差不是特别大，不会导致前端优化崩溃（应该最常见的是这种情况）。那么这时候前端IMU优化会把全局误差很大程度上矫正到IMU的零偏上，这就引出了IMU前端优化的目的——矫正IMU的零偏。所以我们自然是想用一个增量的LiDAR点云配准的里程计来校准这个零偏，而不是用矫正掉之前所有的累计误差的全局位姿来矫正最近一段时间的IMU积分中的零偏，这样反而会让IMU零偏变得更加不准确。
IMU原始消息，话题imuTopic

发布：IMU频率的增量里程计，话题odomTopic + "_incremental"

1.1.2.回调函数

1.1.2.1.odometryHandler

概述：这个回调里面进行的因子图优化，主要是为了使用后端的LiDAR odom的位姿和IMU预积分之间的约束进行融合，从而矫正IMU的零偏。

步骤：

如果是第1次加入因子图（刚开始或者因子图过大上一次清空了），那么就添加最新的odom作为因子图中第0个位姿的先验
先把两帧odom之间的IMU进行预积分
把预积分作为帧间约束加入到因子图中
把最新的odom位姿作为位姿先验加入因子图中，当然注意这里要把LiDAR odom位姿转为IMU位姿
在之前优化的最优位姿基础上，利用第1步的IMU预积分得到当前位姿的先验估计，作为优化状态变量的初值加入因子图
优化
把优化后的当前帧最优位姿存储到上一次的最优位姿上，为下一次循环的第4步做准备
由于优化后最新的IMU零偏改变了，所以就把最新odom之后的那些IMU利用新的零偏重新进行预积分

1.1.2.2.imuHandler

概述：这个回调主要是为了实现实时的IMU频率的odom功能，每来一个IMU数据都进行一积分和状态预测，然后发布最新的状态

步骤：

IMU测量值转换，这个是和作者使用的特殊的IMU有关的（坐标系不是正常的前左上坐标系）
把IMU数据放入imuQueOpt和imuQueImu，前者用于odometryHandler回调函数中计算帧间约束进行优化，后者用于在odometryHandler回调函数中当优化完成后IMU零偏改变重新进行预积分。Q：我感觉这里imuQueImu可以用imuQueOpt来代替？
把最新的IMU数据进行预积分，并且基于上一次odometryHandler中优化得到的最新的状态得到当前最新的状态
把第2步得到的IMU频率的位姿转到LiDAR坐标系发布出去，可见这是一个十分高频的发布，但是注意这里并不是为了可视化使用的，而是为了给下一个TransformFusion类用来融合后端全局里程计使用的。

这里还有一个比较值得思考的细节，就是在上一帧优化得到最优位姿之后，在odom发来这帧位姿之前，用的都是上一帧得到的最优bias。这帧odom发来之后进行优化，bias改变重新进行预计分，但是虽然重新预积分了，但是之前的IMU频率的位姿已经发出去了不能改变了，所以只能改变后面的位姿了。

1.2.TransformFusion类

1.2.1.数据流

后端全局里程计，话题lio_sam/mapping/odometry，这里的全局里程计不知道该怎么翻译更好，总之就是它发布的是包含回环、GPS的图优化之后最准确的那个最新的位姿。
上面imuhandler中发布的IMU频率的、在后端增量里程计基础上递推的里程计，话题odomTopic + "_incremental"

发布：

IMU频率的、在后端全局里程计基础上递推的里程计，话题odomTopic

1.2.2.回调函数

1.2.2.1.lidarOdometryHandler

概述：把后端发来的全局里程计存储起来

1.2.2.2.imuOdometryHandler

概述：这个回调主要是为了实现实时的IMU频率的odom功能，每来一个IMU数据都进行一积分和状态预测，然后发布最新的状态

步骤：

把IMU回调中发出来的IMU频率的、在后端增量里程计基础上递推的最新状态存入imuOdomQueue
找到imuOdomQueue中在后端全局里程计时间戳之后的第一个（最老的）imuOdom，然后和imuOdomQueue中最后一个（最新的）里程计求相对运动量，并且把这个相对运动量补偿到后端发来的全局里程计上，得到IMU频率的、在后端全局里程计基础上递推得到的里程计。
把第1步得到的里程计降频，以10HZ频率发送给rviz显示。并且把这些里程计放到一段小的path中发布，这个就是在rviz中最新姿态附近的那一段紫色的轨迹。

2.imageProjection.cpp

2.1.ImageProjection类

2.1.1.数据流

IMU原始数据，话题imuTopic
前端IMU发布的IMU频率的、在后端增量里程计基础上递推的里程计，话题odomTopic + "_incremental"
LiDAR点云原始数据，话题pointCloudTopic

发布：

运动补偿后的标准ros点云消息格式，话题"lio_sam/deskew/cloud_deskewed"
自定义的点云类型消息，包含除了点云之外的其他很多信息，比如角度、线束等，话题"lio_sam/deskew/cloud_info"

2.1.2.回调函数

2.1.2.1.imuHandler

概述：把IMU原始数据放进imuQueue队列

2.1.2.2.odometryHandler

概述：把前端IMU发来的、IMU频率的、在后端增量里程计基础上递推的里程计结果放进odomQueue队列

2.1.2.3.cloudHandler

概述：接受一帧原始点云消息，按照线束提取确定每一帧提取特征点的起止位置（首尾5个不用），然后把点云放到自定义点云消息中发布给featureExtraction.cpp。Q：目前感觉这个函数的存在的作用实在是鸡肋，完全可以调整到featureExtraction.cpp中，并且很多操作都是从LeGO-LOAM继承来的，并没有作用了，但是没有删掉

步骤：

把发来的点云放进cloudQueue队列，同时判断点云是否有线束、时间戳等信息，为后面去畸变做准备。另外一个很重要的是记录这帧点云的起止时间，其中把第一个点的时间认为是这帧点云的时间
准备IMU和odom的运动补偿信息：
- 首先判断IMU时间是否能够覆盖这帧点云的时间，不能覆盖直接返回false，从而点云回调这个函数也直接就退出了。Q：有风险吧？如果如下情况，岂不是这帧点云就废了？要在程序中打印看是否会出现这个问题

处理IMU运动补偿信息：

（1）把距离点云起始点时间最近的IMU数据拿出来，因为是9轴IMU，所以可以得到三轴欧拉角，存进cloudInfo.imuRollInit/PitchInit/YawInit等变量中；

（2）把这帧LiDAR对应的所有IMU数据依次拿出来，利用原始的IMU陀螺仪数据进行欧拉积分，得到每个时刻IMU的欧拉角，为后面点云去畸变使用。Q：直接用原始IMU数据，没有减去零偏，不太好吧？

（3）全部处理完，置位cloudInfo.imuAvailable 标志位，表示可以利用IMU数据进行旋转运动的去畸变。
处理odom运动补偿信息：

（1）把早于当前LiDAR点云起始点的IMU里程计全部丢掉，然后寻找距离当前点云起始时间最近的那个IMU里程计

（2）把（1）中找到的点云起始时间的那个IMU里程计位姿作为后端点云匹配优化的时候的初始位姿，存入cloudInfo.initialGuess中，并置位cloudInfo.odomAvailable标志，表明IMU里程计提供了这帧点云的初始位姿估计

（3）判断IMU里程计是否覆盖当前帧点云的最后时间，这里有个问题问题同上面IMU处理的（2），就是离得很近但是还没超过，这样也认为不满足要求了。如果这里满足要求，那么就用IMU里程计的结果计算这一帧点云的平移增量，然后在后面点云去畸变的时候就对平移增量进行线性插值。Q：离大谱！有IMU每一时刻的位置，你为啥非要线性插值？这样假设了匀速运动，效果肯定没有使用IMU里程计每个时刻的结果好啊。可能是因为作者发现平移对畸变影响不大，甚至它最后都没有补偿平移畸变，所以在这里为了降低计算量对平移畸变利用线性插值计算？

（4）如果（3）中满足要求，置位 odomDeskewFlag，表示可以用IMU里程计来做点云的平移运动补偿

小结：真特么鸡肋，用IMU里程计的数据不香吗？人家计算的欧拉角是减去零偏的，这里非要从原始数据再自己计算，服了。

把这帧点云投影到矩形中，最后保存到最终的自定义couldInfo消息里面（1.注意这里遍历了整个点云）：

（1）根据点的xy位置计算它在水平1800个位置上属于那个位置，并且把点的距离放到对应位置的矩形二维数组中

（2）对当前这个点去除旋转畸变，就是把它对齐到这一帧点云的第一个点上
遍历每一个scan，记录可以被提取特征点的那些点的信息（2.注意这里又遍历了整个点云？）：

（1）计算这个点在水平方向属于1800的那个位置，放入cloudInfo.pointColInd。注意这个信息是有用的，后面在判断遮挡点、以及已经被选为特征点的点周围标记不再选择为特征点（继承LOAM）的时候有一些特殊情况，都用到了这个变量。

（2）计算这个点的距离，放入cloudInfo.pointRange，注意这个是最后计算曲率直接使用的，跟LOAM中计算坐标不同。

（3）存储这个点的3D坐标

（4）记录当前这个scan可以提取特征点的索引，就是起止排除5个点。
发布点云：

（1）看是否有订阅3中去除畸变后的点云的节点，如果有就把这个点云发出去

（2）把去畸变的点云放到自定义的couldInfo消息中发布，注意这个才是真正点云提取模块用到的消息（注意这里又把点云通过ros发出去，这么大的数据量，又带来了延时）
一些参数复位清零

总结：这个文件存在的意义真的是服了，纯纯的没事找事，访问了两遍点云不说，还转发了一次基本上是原始的点云消息，这不纯找事？

之前写的一些问题，基本上在上面包括了：

投影到一个图像（准确来说是一个二维矩阵）上有什么用？实在没发现有啥用，反而是把点云在这里访问了一遍，然后到下面的cloudExtraction()函数中又访问了一遍
去畸变的时候，旋转部分直接就是用的原始的陀螺仪读数计算的，没有减零偏？
去畸变的时候，平移部分使用odom的话本质上来说其实也是IMU积分算出来的，但是因为在IMU积分的过程中是减去零偏进行计算的，所以更加准确。但是作者又注释掉了，说步行的速度平移带来的畸变并不严重。What the fuck? 不确定后面scan to map的时候这样没有去平移的畸变不会有影响吗？
同理这里可以想到，既然都有odom发来的减掉零偏的IMU积分值了，为啥旋转不也是用这个结果来进行去畸变？而要使用IMU原始数据自己再算一遍？
降采样直接按照线束降采样？这样操作可行吗？