LIO-SAM代码简明分析_liosam关键帧

LIO-SAM代码简明分析

0.IMU预积分

目的

1.IMU通过加速度计和陀螺仪测出的是加速度和角速度，通过积分获得两帧之间的旋转和位移的变换；
2.在后端非线性优化的时候，需要优化位姿，每次调整位姿都需要在它们之间重新传递IMU测量值，需要重新积分，这将非常耗时，为了避免重新传递测量值，所以采取预积分策略。

一.ImageProjection：接受来自IMUPreintegration的高频激光里程计odom，发布经过运动畸变校正的点云。

1.点云去除运动畸变

(1).首帧imu姿态角赋予lidar帧做初始姿态角，之后令第一个imu帧旋转角为0，则之后每一个IMU时刻的旋转角都是基于当前IMU角速度和之前旋转角的累加。
(2).通过匀速模型计算之后每个imu数据帧的旋转角。
(3)获得首尾激光里程计之间的位姿变换
(4).寻找每个激光点云点最近的两个相邻imu帧，通过两个相邻imu帧之间的旋转，线性插值获得每个激光点云点的旋转。
(5).通过获得当前时刻与初始时刻之间的位姿变换，计算每个激光点云点与初始时刻的位姿变换，并且将每个激光点云点补偿到第一个点对应的时刻。

二.IMUPreintegration：接受imu原始数据与mapOptmization发出的里程计数据，发出经过imu预积分优化之后的里程计数据。

1.imu预积分以及更新步骤(imuHandler与odometryHandler结合观看的）

1. 第一次优化：

• 传入imu数据，传入mapOp发布的激光里程计数据；
• 系统初始化：获得先验odom位姿，重置imu积分器和opt积分器；
• 提取两帧imu数据，放入opt积分器进行预积分，构建imu因子；
• 然后把Xkey-1 到Xkey之间，加入这个imu因子以及 激光里程计提供的pose因子，整体做一个优化。优化的结果就是bias，以及“第二帧开始”这个时刻的系统位姿；
• 把优化的结果（主要是bias），重置opt积分器和imu积分器。 然后把当前帧（上面的“第二帧开始”）之前的数据给删掉，用imu积分器，从“第二帧开始”这里开始往后积分。（我们需要明确一点，在这个处理过程中，imu队列也在持续的进数据，（即1.1的imuhandle中）），这里处理完，那么就置donefirst=True，这样imuHandler部分，就可以无缝衔接接着在这里的基础上对imu积分器继续积分。

2.imu预积分节点中最重要的也是LIO-SAM相对于LeGO-LOAM、LOAM的主要区别之一．这部分通过订阅通过地图优化节点优化后的雷达里程计信息对IMU的偏置进行矫正，并对矫正后的预积分数据重新进行预积分，有效的抑制了imu预积分的漂移，明显的提高预积分的精度．预积分精度提高就能够提供扫描匹配较精确的初始估计值，从而提高地图优化节点中的匹配精度．

三.mapOptmization：接受featureExtraction发布的点云，发布经过imu数据加权过后的激光里程计数据，发布经过优化过后的lidarodometry。

1.updateInitialGuess：

分为三种情况，初始帧、里程计信息可用帧、里程计信息不可用imu信息可用帧。transformTobeMap第一次变化，初始化。

初始化位姿变换步骤：

• 利用imu数据初始化最佳位姿transformTobeMap；
  （1）odomAvailable == true
• 利用来自于imageProjection的initialGuess初始化当前帧位姿，随激光帧变化；
• 利用下一帧initialGuess计算位姿变换；
• 利用位姿变化更新transformTobeMap；
  （2）imuAvailable == true
• 单纯使用imu无法得到靠谱的平移信息，因此，平移直接为0；
• 利用imu的位姿变化更新transformTobeMap；

2.extractSurroundingKeyFrames：

关键帧选择与局部地图构建：

1.第一帧关键帧由初始因子图优化获得cloudKeyPoses3D；
2.通过kdtree搜索出最新关键帧一定范围的关键帧；surroundingKeyPosesDS
3.最近10秒的关键帧也保存下来；surroundingKeyPosesDS
4.筛选surroundingKeyPosesDS符合条件的加入局部地图laserCloudFromMap。

3.downsampleCurrentScan：

对整体点云进行下采样，存储

4.scan2MapOptimization：

cornerOptimization：SΔABC=1/2a∗b∗sin(C)
                    SΔABC=1/2c∗h
                    a∗b∗sin(C)=∣a×b∣
                     h=∣a×b∣/∣c∣
1
2
3
4

h为点到直线的距离。

                  pa /= ps; pb /= ps; pc /= ps; pd /= ps;
                  dH=pa2+pb2+pc2pa⋅x+pb⋅y+pc⋅z+pd
1
2

代码中首先利用面点地图中与面特征点最近的5个特征点构建超定方程，然后通过QR分解计算面特征点对应的面特征 p a ⋅ x + p b ⋅ y + p c ⋅ z + p d = 0。

combineOptimizationCoeffs：

将laserCloudOriCornerFlag为true的取出存储。

LMOptimization：

雅可比矩阵分为残差对平移求导和对旋转求导：

J = [ J t , J r ] T；
Jt=∂D(⋅)/∂G(⋅)=coff；
Jr=[Jrx,Jry,Jrz]T=∂D(⋅)/∂G(⋅) x （旋转矩阵对RPY求导）X 点的坐标
1
2
3

采用G-N优化求解deltaX，然后更新transformTobeMap

transformUpdate：

将transformTobeMap中roll’与pitch与imu数据进行融合并且更新transformTobeMap

5.saveKeyFramesAndFactor

addOdomFactor：

添加里程计因子，添加帧间约束。

addGPSFactor：

添加gps因子。

addLoopFactor：

添加回环因子
根据优化出来的预测位姿对transformTobeMap进行更新。cornerCloudKeyFrames代表关键帧位置处的角点点云，surfCloudKeyFrames代表关键帧位置处的平面点点云。这俩东西就是上面1.2处extractSurroundingKeyFrames用到的内容，cornerCloudKeyFrames通过cloudKeyPoses6D变换到世界系下，被存到laserCloudCornerFromMap里面，这个FromMap又在scan2MapOptimization函数中被设置到kdtreeCornerFromMap这个Kd树里，在cornerOptimization函数里，就是把当前帧的激光点云依据1.1的初值transformTobeMapped，变换到世界坐标系下，再用kdtreeCornerFromMap进行kd搜索，建立匹配关系，优化transformTobeMapped。

6.correctPoses

若发现回环，则将所有的关键帧信息都更新一遍

7.publishOdometry

发布激光里程计信息、发布TF变换、发布与imu融合过后的激光里程计信息。

四.loopClosureThread

构建关键帧，将关键帧的位姿存储。以固定频率进行回环检测。每次处理最新的关键帧，通过kdtree寻找历史关键帧中距离和时间满足条件的一个关键帧。然后就认为形成了回环。
形成回环后，历史帧周围25帧，构建局部地图，与当前关键帧进行icp匹配求解位姿变换。
lio-sam 认为里程计累计漂移比较小，所以通过距离与时间这两个概念进行的关键帧的回环检测闭环的时候没有立即更新当前帧的位姿，而是添加闭环因子，让图优化去更新位姿。

最后附上个人在学习LIO-SAM中整理的思维导图：
参考文章：
https://blog.csdn.net/zkk9527/article/details/117957067
https://blog.csdn.net/weixin_37835423/article/details/111587379
https://blog.csdn.net/iwanderu/article/details/123172321