学习笔记之——LIO-mapping

之前博文《学习笔记之——激光雷达SLAM（LOAM系列的复现与学习）》介绍了LOAM系列的作品。本博文看看LIO-mapping（Tightly Coupled 3D Lidar Inertial Odometry and Mapping）。本博文的部分内容（特别是理论部分）来源于网络，文末给出参考链接。本博文仅仅用于本人学习记录用。

目录

论文阅读

紧耦合的Lidar-IMU odometry

旋转约束的状态优化

代码复现

参考资料

---

论文阅读

在利用3D LiDAR进行SLAM计算时，有时会遇到一些问题，例如两帧之间的运动较为剧烈(大转角)可能会导致点云配准算法失效，在某些场景如长廊场景下有效测量点云较少使运动估计精度下降等，这些现象影响了SLAM算法的鲁棒性。而IMU(Inertial Measurement Unit)作为一种惯性测量单元，可以精确地测量载体的三轴加速度以及三轴角速度，在上述的场景中(大转角，长廊环境)可以为LiDAR提供额外的先验信息，从而增强LiDAR算法的鲁棒性，因此LiDAR-IMU fusioin成为一种近年来备受关注的SLAM方法。

LIO-Mapping借鉴LOAM和VINS-Mono的思想进行的一个联合状态估计问题。工程层面和VINS-Mono一样，只不过前端视觉部分换成了lidar的前端进行特征关联和odometry的位姿估计。其主要分为两个部分：tightly coupled lidar-IMU odometry与the rotation constrained refinement。其框架如下图所示。这是第一个开源的LiDAR-IMU紧耦合SLAM算法

紧耦合的Lidar-IMU odometry

看下图所示的时序图。假设当前的时间戳是i，则由于一般IMU设备的频率(100-1000Hz)会远远高于LiDAR点云数据的更新频率(10Hz)，因此在下一帧点云即时间戳j到来之前，会有大量的IMU数据读入，如上图中紫色线条所示。

可以根据这段时间内IMU的数据对IMU的状态进行估计，即流程图中的state-prediction部分，同时利用这段时间间隔内的IMU测量量进行预积分操作，即对应流程图中的Pre-integration。当到达时间戳j也即新的激光雷达数据帧过来后，由于激光雷达为连续测量，即在时间戳i到时间戳j过程中，激光雷达的测量是在不断运动且位姿不断变化的过程中测量得到的，因此会产生一定的运动畸变(motion distortion)。此时，在获得时间戳j对应的激光点云数据后，应首先根据IMU的state-prediction消除运动畸变即流程图中的De-skewing部分从而得到未畸变的点云.

在得到畸变矫正后的点云后，为减少计算量，采用了LOAM中的方法从点云提取了两种特征分别是角点以及平面特征点，即对应流程图中的Feature Extraction部分。然后就是根据局部地图 确定特征与局部地图的对应关系，也即流程图中的Find relative lidar measurements部分。所谓的局部地图，也就是将上图所示的从时间戳o到时间戳i的特征点都投影到一个坐标系下形成一个点云局部地图。在本文中，每次都是投影到中间的时间戳即p对应的坐标系下，即对应流程图中的Local map management。最后在确定了测量约束和IMU的预积分约束后，建立非线性最小二乘的目标函数并进行联合优化求解，即对应流程图中的Joint non-linear optimization部分。

 IMU and pre-integration

 一般来讲，一个三轴正交的IMU设备可以测量三轴的加速度以及角速度，由牛顿运动定律，若初始状态(位置，姿态，速度)已知，则可以根据加速度以及角速度的测量跟踪一个物体的状态(位置，姿态，速度)，因此广泛的应用于惯性导航(Inertial Navigation System)中。在涉及到IMU的导航系统中，通常IMU的状态可以建模为如下向量：

 （IMU为body frame） 

 De-skewing and feature extraction

 这一部分主要实现点云畸变矫正以及从畸变矫正的点云中提取特征，主要借鉴了LOAM的畸变矫正以及特征提取方法。由于激光雷达的测量与摄像头的测量不太相同。摄像头的拍照可以理解为one-shot，即快门曝光的时刻。而激光雷达的测量则是连续测量，也即每一帧点云中的不同点其实都是在不同时刻测量得到的。而车载的激光雷达又都是在不断运动的，这就导致该帧点云中的每一个点其实都是在不同的时刻测量得到的。因此需要根据IMU的加速度以及角速度信息(插值等方法)预测每一个激光扫描点测量时，该点对应的激光雷达的位置。 

 Relative lidar measurements

 为了给优化函数添加约束建立了一种特征点到局部地图的关联。

如下图所示，这篇论文采用了滑动窗口(sliding window)的方法，图中B表示的是IMU坐标系，L表示的是激光雷达坐标系，在运动过程中假设两者之间的相对位姿关系不发生变化(刚性连接)。蓝色的大窗口即是论文中说的sliding window，在源码中该window设置为包含了15个激光雷达帧大小的窗口，其中从o 到p为之前优化过的帧，而从p pp到i ii对应的帧为待优化的帧。

更详细的解析，参考：

Tightly Coupled LiDAR Inertial Odometry and Mapping源码解析（一）

Tightly Coupled LiDAR Inertial Odometry and Mapping源码解析（二）

Tightly Coupled LiDAR Inertial Odometry and Mapping源码解析（三）

Tightly Coupled LiDAR Inertial Odometry and Mapping源码解析（四）

Tightly Coupled LiDAR Inertial Odometry and Mapping源码解析（五）

旋转约束的状态优化

代码复现

先根据工程编译。然后运行

roslaunch lio test_indoor.launch
roslaunch lio map_4D_indoor.launch
rosbag play /home/kwanwaipang/桌面/LIO-mapping/fast1.bag

LIO-Mapping复现（3D LiDAR SLAM）

参考资料

https://sites.google.com/view/lio-mapping（数据集）

https://github.com/hyye/lio-mapping（作者github工程）

https://blog.csdn.net/wykxwyc/article/details/100731201

https://zhuanlan.zhihu.com/p/82383285

https://blog.csdn.net/qq_17693963/article/details/94406205