LIO-SAM论文阅读笔记

LOAM存在的问题

（1）地图的存储方式导致难以检测回环以及增加其他传感器信息

（2）在大场景情况下会漂移

（3）在特征丰富环境，voxel_map 会变得稠密，导致优化没有效率

核心思想

将激光里程计，IMU 预积分，GPS，回环等约束添加到因子图中，从而实现紧耦合的优化

软件系统框图

四种因子

IMU 预积分

IMU 预积分可以用来 de-skews 当前帧点云

激光里程计

（1）与 LOAM 或者 Lego-LOAM 一样提取线面特征

（2）利用局部关键帧构建 voxel map：选择距离当前关键帧最近的 n 个关键帧，将其线面特征进行合并形成地图

（3）与 LOAM 一样寻找当前关键帧与局部地图的匹配关系，并进行优化求解(利用 IMU 预积分作为初始值)，从而获得与上一个关键帧的相对位姿

（4）根据位姿变化选取关键帧(保持因子图的稀疏)

注：激光里程计可以用于估计 IMU 的 bias

GPS

将 GPS 数据转换到局部笛卡尔坐标系

注：这里作者发现误差变化缓慢，所以只是在位姿估计的协方差大于 GPS 协方差时才添加 GPS 因子

回环

利用欧式距离检测回环，但也可以用其他方法

选择距离当前关键帧15m内的关键帧作为回环帧，然后选择回环帧前后各12个关键帧作为 voxel map 进行 ICP

由于 GPS 在 altitude 上的测量是非常不精确的，导致估计出来的位姿在 altitude 上有很大的误差，但加入回环后可以减小这种误差

实验

作者在实验中发现最影响运行时间的是特征地图的稠密度而不是因子图的规模

代码流程图

featureExtraction.cpp

1.计算当前帧点云的曲率

2.标记不被提取的点(LOAM中提到的两种特殊情况)

3.提取特征(跟LOAM类似，但只提取两种类型的点，不细分成四种类型)

4.发布相关数据

imageProjection.cpp

1.缓存点云

2.获取运动补偿所需的信息

imu去畸变

(1)获得当前帧点云开始时的姿态，(2)利用IMU角速度获得当前帧点云覆盖下所有imu数据的角度

odom去畸变(IMU预积分出来的)

(1)获得当前帧点云开始时的位姿，(2)获得当前帧点云结束时的位姿，(3)计算delta pose

3.投影点云

其中包含了对点云进行运动补偿(利用IMU补偿到第一个点对应时刻的位姿)，这里作者只是利用旋转对点云进行去畸变，因为考虑运动比较缓慢

4.记录点云的信息(距离，3d坐标，列坐标等)

5.发布相关数据

imuPreintegration.cpp

1.实现IMU预积分以及在因子图中加入IMU约束进行优化(这里是第二个因子图，利用另一个节点优化后的位姿作为先验)

2.融合IMU增量位姿和全局一致的位姿

mapOptimization.cpp

检测回环(空间和时间)，构建回环帧的局部地图，并利用ICP进行匹配(使用ICP得分作为噪声项)

1.更新当前帧的初始位姿

如果是第一帧，就用IMU的初始姿态，如果不是第一帧，则用IMU的旋转信息更新或者odom信息更新(计算delta pose，然后乘以上一帧的最佳位姿)

2.提取相关的关键帧并且构建点云局部地图(时间和空间)

3.对当前帧进行下采样

4.构建优化问题求解点云配准

对于线特征，找局部地图里面距离最近的5个线特征，判断这5个点是否构成线，然后计算点到线的距离以及雅克比方向

对于面特征，找局部地图里面距离最近的5个面特征，判断这5个点是否构成面，然后计算点到面的距离以及雅克比方向，这里是如何判断面的方向是哪一边，这关系到雅克比的方向???

构建增量方程求解增量，这里求解的是rpy，xyz六维的增量。同时这里会对H矩阵进行特征值分解从而判断某个维度是否会退化(退化方向不更新)

5.根据配准结果判断关键帧(旋转和平移)并添加相关因子进行优化

6.调整全局轨迹(有回环情况下)

7.发布相关数据