一. 相关背景介绍

LIO-SAM: Tightly-coupled Lidar Inertial Odometry via Smoothing and Mapping

作者Tixiao Shan在2018年发表过LeGO-LOAM，当时他还在史蒂文斯理工学院读博士，2019年毕业之后去了MIT做助理研究员。这篇文章LIO-SAM实际上是LeGO-LOAM的扩展版本，添加了IMU预积分因子和GPS因子，去除了帧帧匹配部分，然后更详细地描述了LeGO-LOAM帧图匹配部分的设计动机和细节。现在论文已经被IROS2020录用，作为高精度，imu，雷达，gps结合，程序还进行了开源，非常值得学习。

• 本人安装环境：ubuntu18.04 + melodic

• 作者github：https://github.com/TixiaoShan/LIO-SAM [论文在：~/LIO-SAM/blob/master/config/doc/paper.pdf

• 数据链接：链接：https://pan.baidu.com/s/1CbDJPIupCm3yROmM2qOsOw 提取码：ato7

二. 安装和运行

2.1 安装gtsam 4.0.2

本人测试这个版本可用，LIO-sam需要gtsam4.0.2的版本，而Lego_slam需要的版本为gtsam4.0.0-alpha2，这两个版本兼容性问题还没解决，目前每次使用都是来回切换版本，有些费时间，有解决的朋友可以评论或者私信一下本人，万分感激~~

如果想要学习gtsam的使用，可以访问链接。

wget -O ~/Downloads/gtsam.zip https://github.com/borglab/gtsam/archive/4.0.2.zip
cd ~/Downloads/ && unzip gtsam.zip -d ~/Downloads/
cd ~/Downloads/gtsam-4.0.2/
mkdir build && cd build
cmake -DGTSAM_BUILD_WITH_MARCH_NATIVE=OFF ..
sudo make install -j16  //根据自己的cpu核数定，越多越快，本人是16核的，注意不能超出自己电脑的核数

2.2 安装lio-sam

这个安装比较常规，下载编译，基本没什么问题。

cd ~/catkin_ws/src 
git clone https://github.com/TixiaoShan/LIO-SAM.git 
cd ..
catkin_make

2.3 运行

roslaunch lio_sam run.launch
rosbag play casual_walk_2.bag

运行结果如下：

三. 论文介绍

3.1 背景

LOAM(lidar odometry and mapping )以低漂移和实时估计的方法而被广泛使用，它在kitti的排名中长期霸榜，尽管它很成功，但是它也有一些自身的缺点。

• LOAM直接存储全局体素地图而不是局部地图，从而很难实现回环，来修正漂移。
• 没有结合GPS等外部测量进行位姿态修正。
• 同时在大规模测试中也会存在误差，因为它的核心是扫描匹配的方法。

LIO-sam采用帧-局部地图匹配的方式代替LOAM的帧-全局地图匹配的方式。属于一种紧耦合的方法。

• 松耦合
  LOAM和LeGO：使用IMU去除LiDAR点云的运动畸变，并且使用EKF整合LiDAR和IMU的测量。

• 紧耦合
  R-LINS(robocentric lidar-inertial state estimator)：使用误差状态卡尔曼滤波器迭代地修正机器人的位姿估计。
  LIOM：它是LIO-mapping的缩写，联合优化LiDAR和IMU测量。但是LIOM一次性处理所有测量，因此不能实时运行。

• 作者
  属于紧耦合的激光-惯性里程计方法，只是采用了因子图优化而不是滤波的方法。因子图优化通过iSAM2完成。

3.2 方法

• 3.2.1 Lidar和IMU坐标系

3.2.1 因子图框架介绍

• 优化四个因子

• LiDAR里程计因子(绿色)：由每个关键帧和之前n个关键帧之间的帧图匹配结果得到。
• IMU预积分因子(橙色)：两个相邻关键帧之间的IMU测量积分得到。
• GPS因子(黄色)：由每个关键帧的GPS测量得到。
• 回环因子(黑色)：由每个关键帧和候选回环关键帧的时序相邻的2m+1个关键帧之间的帧图匹配得到。

因子图模型如下所示：

论文采用MAP（maximum a posterior）来解决非线性优化问题，可以用贝叶斯网络的形式很好的表示，采用了高斯噪声模型，所以我们同样可以添加其他传感器，比如可以测量高度的测高仪、可以测航向的罗盘等。

3.2.3 IMU因子

IMU的角速度和加速度：

所以预测$t+∆t$时刻的速度、位置和旋转角：

$i,j$时刻的预积分：

预积分详细可以参考论文：

C. Forster, L. Carlone, F. Dellaert, and D. Scaramuzza, “On-Manifold Preintegration for Real-Time Visual-Inertial Odometry,” IEEE Transactions on Robotics, vol. 33(1): 1-21, 2016.

3.2.4 激光里程计因子

当新的雷达数据来了之后，我们先对它进行特征提取。通过局部区域点的粗糙(凹凸)程度来提取边和平面。

边：点的粗糙程度很大。
平面：点的粗糙程度较小。

把每一帧数据添加进因子图对于计算来说是很困难的，所以本文采用了一种图像中常用的方法：提取关键帧。非关键帧的数据则被丢弃。本文中提取关键帧的位置和旋转角度为$1m$和$10^0$。
$i$时刻提取的特征表示为 F i = { F i e , F i p } F_i=\{F_i^e,F_i^p\} Fi​={ Fie​,Fip​}。点云匹配和位姿优化过程与LOAM/LeGO-LOAM 相同 。不同的是, 这里使用过去n+1帧关键帧的组合点云而不是全局点云和最新帧进行匹配。具体的特征提取方法可以查看以下论文：

• J. Zhang and S. Singh, “Low-drift and Real-time Lidar Odometry and Mapping,” Autonomous Robots, vol. 41(2): 401-416, 2017.
• T. Shan and B. Englot, “LeGO-LOAM: Lightweight and Groundoptimized Lidar Odometry and Mapping on Variable Terrain,” IEEE/RSJInternational Conference on Intelligent Robots and Systems,
  pp. 4758-4765, 2018.

现在假设新加入节点$X_{i+1}$，它相对应的因子表示为$F_{i+1}$。所以它的步骤表示为：

• 步骤一：对子关键帧进行体素滤波

类似于滑动窗口，我们只对子关键帧 { F i − n , . . . , F i } \{F_{i−n}, ..., F_i\} { Fi−n​,...,Fi​}进行体素滤波得到相应的地图$M_i$，相应的我们会有边和平面的体素滤波地图$M_i^e$