LIO-SAM代码笔记

lio-sam ：

imuPreintegration.cpp：
odometryHandler：获取后端优化的位姿，使用imuQueOpt的预积分积分作为每次获取到的位姿与上一次的帧间约束，优化后得到新的位姿
imuHandler：在odometryHandler的基础上继续积分，从而使用imu外推得到最新预测位姿，发布到imageProjection.cpp（odomTopic + "_incremental"）、imuPreintegration.cpp的TransformFusion（用于显示）

imageProjection.cpp:
1、接收imu和点云数据，以及来自imuPreintegration.cpp通过预计积分得到的位姿
2、点云处理：
（1）使用与第一个点最近的imu的姿态作为点云的姿态imuRollInit，使用角速度积分，记录每时刻的姿态
（2）将接受imuPreintegration.cpp的位姿与点云第一个点最近的赋值到initialGuessX，找到与点云最后一个点最近的位姿，并计算两者间的相对位姿变换odomIncreX, odomIncreY, odomIncreZ, rollIncre, pitchIncre, yawIncre
（3）将lidar坐标下的点云以线束为行，水平角为列，距离为值映射到Mat上，将imu积分得到每个点的姿态，使用（2）的数据插值得到位置，从而去畸变，并将Mat沿行的顺序记录去畸变后点（位姿是相对于第一个点的）的信息（Mat作用：将点云按线束和角度大小排序）
（4）记录点云数据和曲率的起点和终点到cloudInfo
（5）带有imu估计的位姿的点云给featureExtraction.cpp（"lio_sam/deskew/cloud_info"），rviz（"lio_sam/deskew/cloud_deskewed"）

featureExtraction.cpp：
1、接受来自imageProjection.cpp已经去畸变的点云
2、计算曲率，使用前5个点与后5个点减去中间点的平方为曲率
3、标记遮挡点：相连点距离较大；前后两个点与中间点距离较大，代表着方向指向lidar，可能下一次就看不见了cloudKeyPoses3D点，因此面点数量较多，需要下采样
5、将角点和面点加入cloudInfo，并发送到mapOptmization.cpp（"lio_sam_6axis/feature/cloud_info"）

mapOptmization.cpp：
1、接收来自featureExtraction.cpp的特征点云，并比较时间，避免高频率处理
2、第一次接收到点云，位姿的imu预测值取为点云的初始值
3、预测当前帧位姿，这里需要注意的是，因为回环检测和GPS的存在，前端位姿和后端位姿不一定连贯，但是前端短期内的变换是正确的，因此使用前端的位姿变换加上后端上一帧的位姿得到当前位姿，如果没收到前端预积分发送过来的位姿，而是只有imu直接积分的位姿，则只更新姿态。
4、基于上一次的关键帧构建局部地图：使用kd树寻找距离与上一帧关键帧距离小于一定阈值的关键帧，并滤波减少数量；寻找时间比较近的关键帧；将这些关键帧合并为角点laserCloudCornerFromMap 、面点laserCloudSurfFromMap （不需要下采样，因为添加关键帧时，已经下采样过了）
5、对当前帧下采样：laserCloudSurfFromMap 、 laserCloudSurfLastDS
6、点到线：在局部地图上，遍历当前帧点，找到5个最近的点，计算这5个点的方差，对其进行特征值分解（如果点云数据大致分布在一条直线上，那么数据变化最大的方向实际上是这条直线本身的方向。因为点云数据在该直线方向上是有组织、有规律地分布的，所以沿着直线方向的方差（或对应的最大特征值）实际上反映了数据的主要变化趋势，这是数据点分布最集中、而非最分散的方向。相反，在垂直于这条直线的维度上，理论上数据的变化应该是最小的，因为如果所有点完美地落在同一直线上，那么垂直方向上的方差应为0，对应特征值也应为0（表明没有变化）），最大特征值应远大于次大特征值，其对应特征向量是直线方向，从而求出当前点和局部地图对应的直线的两个点。使用外积计算三个点的面积，两点距离为底，则可求出高（残差），方向则是既垂直底，又垂直外积矢量，再归一化，即是雅可比
7、点到面：在局部地图上，遍历当前帧点，找到5个最近的点，根据Ax+By+Cz+1=0;来求解超定方程，之后再将5个点分别代进去，距离过大则舍弃。将当前点代入方程得到残差，A、B、C则是点到面的方向，即残差对位置的雅可比
8、将点到线和点到面的残差、雅可比合在一起处理。
9、之前点云是在lidar坐标系的，现在需要换到相机系，方便套用代码，其实只是坐标轴不对应而已。6-7步已经求出了残差、残差对位置的导数，还需要求出残差对欧拉角的导数。残差对欧拉角 = 残差对位置的导数 * 位置对欧拉角的导数。位置对欧拉角的导数 = (R*p+t)对欧拉角求导，R = 由三角函数构成的旋转矩阵（旋转顺序Y-X-Z）
10、求解增量：第9步求出雅可比矩阵J，对海塞矩阵H特征值分解（Hessian矩阵的某个特征值接近于0或负值，这通常意味着在该特征向量方向上函数的变化非常小，可能存在退化现象。接近0的特征值表明函数在此方向上几乎是平坦的，没有足够的信息用于确定最优解。负特征值则可能意味着局部极小值点不存在，或是在非凸问题中。），将其对应的特征向量置0，不更新这个方向。
11、因为姿态很重要，因此将预测的roll、pitch和磁力计的测量值进行加权平均
12、关键帧判断（符合关键帧才进行后端优化）：姿态或者位移要有一个大于一定值，否则不优化
13、添加里程计因子：第一次直接添加位姿的先验因子；第二次添加帧间约束，上一帧与当前帧之间约束
14、添加GPS观测因子：时间需与点云相近，协方差需要较小，两次GPS位置不能太小，增加GPS观测位姿因子
15、增加回环因子：获取loopClosureThread得到的回环帧、对应关键帧、帧间变换位姿、匹配分数作为帧间约束
16、优化得到当前帧位姿，添加到cloudKeyPoses3D、cloudKeyPoses6D（intensity为添加顺序的索引）
17、更新最新位姿为transformTobeMapped
18、将lidar坐标下的点云记录到cornerCloudKeyFrames、surfCloudKeyFrames
19、globalPath.poses记录下每次优化后的位姿
20、如果存在回环或GPS因子，则之前的位姿也变动了，需要更新：遍历优化得到的位姿重新赋值到cloudKeyPoses3D、cloudKeyPoses6D，并记录到globalPath.poses
21、发送当前位姿给rviz和 imuPreintegration.cpp的TransformFusion（"lio_sam/mapping/odometry"）
22、发布tf，把当前位姿发布为odometryFrame（odom）到 "lidar_link"
23、发送里程计位姿给imuPreintegration.cpp（"lio_sam/mapping/odometry_incremental"），这里的里程计位姿有特殊处理，如果直接发送经过回环或GPS因子图优化后的位姿，这个位姿可能变化较大，那imuPreintegration.cpp收到的位姿对于预积分约束来说是有问题的，因此发送的是上一次发送给imuPreintegration.cpp的位姿+当前帧点云匹配后，因子图优化前的位姿与上一帧因子图优化过的最佳位姿之差，除此之外还与imu直接积分的roll和pitch角进行一个加权平均
24、发布可视化话题：
（1）发布轨迹，即所有世界坐标下的关键帧位姿到rviz("lio_sam/mapping/trajectory")
（2）发布世界坐标下的面点到rviz（"lio_sam/mapping/map_local"）
（3）发送转到世界坐标系的当前帧特征点到rviz（"lio_sam/mapping/cloud_registered"）
（4）发送所有去畸变并转到世界坐标系下的当前帧点云到rviz("lio_sam/mapping/cloud_registered_raw")
（5）发送至今为止世界坐标下所有关键帧的位姿globalPath到rviz("lio_sam/mapping/path")

loopClosureThread：
1、使用kd树寻找与最新关键帧距离最近，但是时间较大的回环帧（需要避免静止情况）
2、取出最新关键帧的全部点云，取出回环帧以及其旁边的点云
3、发布世界坐标系下的回环帧给rviz（"lio_sam/mapping/icp_loop_closure_history_cloud"）
3、使用pcl的icp进行匹配，得到目标点云（回环帧）指向源点云（最新关键帧）的变换位姿
4、发送找到回环帧的并且转到世界坐标系下的最新关键帧给rviz（"lio_sam/mapping/icp_loop_closure_corrected_cloud"）
5、获取最新关键帧经过icp后，真实的位姿tCorrect（tCorrect = correctionLidarFrame * tWrong，Twc * p= Tts * Tww * p，首先将点转到世界坐标，再修正误差转到回环帧下）
6、记录最新关键帧和回环帧的索引以及相对位姿变换T（修正后指向修正前），给后端优化时作为帧间回环约束
7、发送所有记录的回环约束到rviz（"/lio_sam/mapping/loop_closure_constraints"）

visualizeGlobalMapThread：
1、发布转到全局位姿的当前关键帧以及旁边关键帧的点云到rviz（"lio_sam/mapping/map_global"）

改进：
1、六轴IMU，引入使用重力对齐
2、欧拉角换成四元数
3、回环检测的条件是距离较小，时间较长，但未考虑静止不动情况
4、回环匹配可以使用ndt