LIO-SAM算法解析

简介

LIO-SAM在lego-loam的基础上新增了对IMU和GPS的紧耦合，采用一个因子图对位姿进行优化，包括IMU因子，激光里程计因子，GPS因子、回环因子。

虽然在创新点上没什么让人觉得有趣的东西，但是在性能上相比Lego-Loam，因为加入了高频的IMU，在一些不好的场景下表现要更鲁棒，回环处的漂移也更小。

不过虽然加入了IMU，但似乎连松耦合都称不上，IMU预积分在LIO-SAM中的作用仅用来对激光点云做运动补偿去畸变，以及在scan-2-map时提供优化的初始位姿

综上，算是一个比较不错的入门算法吧。

参考链接： https://zhuanlan.zhihu.com/p/614039247

算法概述

算法输入： 激光点云，IMU原始数据，GPS(可选)
主要模块：

• 点云去畸变：对应imageProjection.cpp
• 特征提取：对应featureExtraction.cpp
• IMU预积分：对应imuPreintegration.cpp
• 地图优化：对应mapOptmization.cpp

1.点云去畸变

相比于Lego-Loam，这里没有对点云进行分割和聚类操作，而是直接去畸变后将点云以及对应的位姿发布给特征提取模块。

1.1 主要功能

• imageProjecttion的主要功能是订阅原始点云数据和imu数据，根据高频的imu信息对点云成像时雷达的位移和旋转造成的畸变进行校正
• 同时，在发布去畸变点云的时候加入IMU输出的角度和IMU里程计（imuPreintegration）的角度和位姿作为该帧的初始位姿，作为图优化的初始估计
• 并且，要对点云的Range进行计算，同时记录每个点的行列，以便在特征提取中被使用

1.2 主要流程

1. 接收到一帧点云
2. 从IMU原始数据队列找到该帧点云时间戳对应的数据，将IMU的roll、pitch、yaw塞进准备发布的该帧点云信息
3. 提取该帧点云的起止时间戳（激光雷达点云的每个点都有相对于该帧起始时间的时间间隔）
4. 对起止时间内的IMU数据进行角度积分，得到该帧点云每个时刻对应的旋转。
   注意，这里算法使用的是简单的角度累加，实际上是积分的近似，但是在很短的时间内，10Hz雷达对应100ms的扫描时间，近似的累加可以代替角度积分。
   猜想这里是因为点云去畸变是整个SLAM流程的入口，要保证足够的实时性，因此用累加代替真正的角度积分
5. 遍历该帧点云每个点，旋转到起始点坐标系
6. 从IMU里程计提取该帧点云对应的位姿（包括位置和旋转），塞进准备发布的该帧点云信息
7. 发布该帧点云信息

2.特征提取

1. 接收到从imageProjection中发布出的一个去畸变点云信息cloudInfo(自定义格式)
2. 对每个点计算曲率。计算时是计算周围点的平均距离用来作为曲率的替代
3. 标记遮挡点和与激光平行的点，后续这些点不能采纳为特征点
4. 特征提取。分别做角点（曲率大）和平面点（曲率小）特征点提取
5. 整合信息，发布完整数据包

过程同Lego-Loam,详细见博客《Lego-Laom算法深度解析》

过程同Lego-Loam,详细见博客《Lego-Laom算法深度解析》

3.IMU预积分

和VIO常见算法不同，LIO-SAM是通过激光里程计矫正IMU的累计误差，然后对IMU原始数据进行连续积分得到关于IMU的里程计。

• IMU原始数据的旋转积分用于去除点云的旋转畸变
• IMU里程计的平移信息用于去除点云的运动畸变
• IMU里程计的位姿信息和对应点云一起被封装，为map-2-scan提供初值

在计算IMU里程计时，使用了因子图，利用激光里程计因子和IMU因子对IMU位姿、速度和偏置进行了因子图优化。

4.地图优化

核心函数在laserCloudInfoHandler，主要流程如下：

1. 位姿初始化updateInitialGuess
2. 构建局部地图extractSurroundingKeyFrames
3. 点云降采样downsampleCurrentScan
4. map-2-scan并进行位姿优化scan2MapOptimization
5. 创建关键帧saveKeyFramesAndFactor
6. 回环校正correctPoses
7. 里程计和位姿发布

详细内容见链接：https://zhuanlan.zhihu.com/p/622003523

4.1 位姿初始化

读取特征提取模块的线特征和平面特征，以及由IMU预积分提供的初始位姿。

线特征和平面特征由特征提取模块featureExtraction发布，

• 第一帧做初始化：直接使用IMU输出roll、pitch、yaw作为当前帧的初始位姿。
• 后续帧初始化：
  • 如果IMU里程计可用（已经融合了激光里程计结果），使用其作为6D位姿初始化
  • 如果IMU里程计不可用但IMU原始角度可用，则在上一帧的位姿上叠加IMU角度的变化作为初始化

4.2 构建局部地图

挑选局部地图关键帧的方法很巧妙，每个关键帧的位置(x,y,z)坐标本质上也是一堆3D点，因此可以使用KD树搜索距离当前关键帧最近的点云，同时又对这些3D点位置进行降采样，3D点的稀疏性保证了对应的关键帧位置不会挨得太近。选出关键帧后就提取对应的点云，转到同一个坐标系然后融合为局部地图。

• 对所有关键帧3D位姿构建KD树
• 使用最后一个关键帧位姿作为索引，从KD树中找到指定半径范围内的其他关键帧
• 对找出的关键帧数量做降采样，避免关键帧位姿太过靠近
• 加上时间上相邻的关键帧
• 对所有挑选出的关键帧数量再做一次降采样，避免位置过近
• 将挑选出的关键帧点云转换到odom坐标系。（这里使用一个map缓存坐标变换后的点云，避免重复计算）
• 对局部地图的角点、平面点点云做降采样，并融合到一个局部地图中

4.3 基于scan-2-map的位姿优化

分别进行角点和平面点匹配，找到候选点后整合在一起，然后进行匹配优化。

4.3.1 基于角点的匹配优化

1. 将每个角点从雷达坐标系转换到map坐标系
2. 基于KD树搜索得到局部地图中距离当前角点最近的5个点
3. 5个点中，判断距离最远的点和当前点的距离，如果距离小于1m，认为该点有效
4. 基于5个点距离中心的协方差矩阵和特征值判断5点是否构成一条直线
   1. 对协方差矩阵进行特征值分解
   2. 如果最大的特征值要远大于第二个特征值，则认为则5个点能够构成一条直线
5. 计算点到直线的距离，如果距离小于阈值将该点作为优化目标点，计算当前帧角点到直线的距离、垂线的单位向量，存储为角点参数

4.3.2 基于平面点的匹配优化

1. 采用上述方法寻找5个平面点
2. 对5点进行平面拟合，使用最小二乘求解超定方程Ax+By+Cz+1=0
3. 计算点到平面的距离，如果距离小于阈值将该点作为优化目标点，将当前帧平面点到平面的距离、垂线的单位向量，存储为平面点参数

4.3.3 整合候选匹配点信息

将cornerOptimization和surfOptimization两个函数计算出来的边缘点、平面点到局部地图的距离、法向量集合在一起

4.3.4 基于高斯牛顿法进行迭代优化

基于点到直线、点到平面的距离构建残差方程，基于高斯牛顿法迭代求解使得残差最小时对应的位姿

求解过程是将雅格比矩阵的形式写出，然后计算高斯牛顿法的hessian矩阵，然后对Hessian进行QR分解计算特征向量来更新位姿，在达到最大迭代次数之前如果收敛则优化成功。

最后用imu原始RPY数据与scan-to-map优化后的位姿进行加权融合（只加权融合roll、pitch角），更新当前帧位姿的roll、pitch，约束z坐标

4.4 添加关键帧并进行因子图优化

在地图优化模块，scan-2-map并没有使用因子图，只是简单的点云配准问题，通过解析解的方式迭代求解局部地图匹配位姿。只有在创建关键帧时才会将新的关键帧加入到因子图，并进行因子图优化位姿。该因子图如下所示：

• 变量：每个关键帧的位姿
• 因子：激光里程计因子、GPS因子、回环检测因子
  • 虽然论文中也添加了IMU因子，但是在实现过程中并没有使用

4.5 回环纠正

实则是使用之前因子图优化后的位姿，对位姿序列相应关键帧的位姿进行替换

5.算法评估

论文中的对标基线是LOAM，下面的链接对LOAM系列的几个算法做了比较详细的评测，整体上LIO-SAM的稳定性较好因为加入了IMU，回环处的漂移较小
https://github.com/Tompson11/SLAM_comparison