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算法深度解析》

3.IMU预积分

和VIO常见算法不同，LIO-SAM是通过激光里程计矫正IMU的累计误差，然后对IMU原始数据进行连续积分得到关于IMU的里程计，然后根据IMU的实时积分结果对激光点云进行运动补偿，实现对点云的去畸变。

4.地图优化

• 读取特征提取模块的线特征和平面特征，以及由IMU预积分提供的初始位姿
• 基于初始位姿和局部地图进行scan-2-map的匹配得到矫正后位姿
• 将矫正后位姿加入因子图进行优化，得到里程计位姿
• 同时，会进行回环检测，检测到回环后作为新的因子加入优化，更新局部地图关键帧位姿
• 同时，如果存在GPS信息，会将其加入因子图一起进行优化

5.算法评估

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