论文笔记-LIO-SAM: Tightly-coupled Lidar Inertial Odometry via Smoothing and Mapping

Abstract

关键词：紧耦合，gtsam

1. IMU在此处的效果：使用imu的结果对点云去畸变；对Lidar Odometry提供较好的初值（取消了帧间里程计）；lidar odometry得到的结果也可以用来估计imu的零偏（紧耦合的体现）

2. 为了保证实时性：
   在位姿优化时边缘化旧的Lidar帧，而不是用Lidar帧和全局地图去匹配。在局部范围的帧匹配（不是全局范围）极大的提高了实时性。这个局部的地图是由选取的关键帧组成的，用的是一种滑窗的方法，让新的关键帧和固定大小的”sub-keyframes”进行配准。

I. Introduction

1. 这里点出了视觉SLAM的优势，比较有利于进行回环方面的检测（比如词袋或者深度学习的方法），但是对光照敏感，而且对于单目相机，尺度是不可观的。而对于激光雷达的话，随着目前技术发展，128线的Lidar已经可以得到环境的很多细节。
2. 作者还指出了LOAM的局限： 由于它的数据是存在全局体素地图中，这样是难以进行回环检测的，因为触发了回环检测之后，通常会进行位姿图优化（只优化位姿，不优化定），这对于loam是不太行的；同时，也难以与其他的传感器进行配合，比如GPS（用于修正位姿）。作为一个里程计，累计误差一定会存在。
3. 不同传感器的结果都可以放入因子图进行联合优化。

II. RELATED WORK

这里主要是提及了使用imu进行松耦合和紧耦合：

• 松耦合（如LOAM，LeGO-SLAM）：
  使用了IMU进行去点云畸变以及为帧间匹配提供初始估计，但是imu没有进入算法的优化过程，作者还举了EKF（滤波方法一般用松耦合框架）用在机器人状态估计的例子，也是松耦合。
• 紧耦合：
  在一些论文中，会使用预积分的imu测量值对点云做运动补偿。对于基于滤波方法的紧耦合，不太好加回环。而LIO-MAPING是基于优化的紧耦合，但是实时性较差（和VINS-MONO类似）。

III. LIDAR INERTIAL ODOMETRY VIA SMOOTHING AND MAPPING

A. 系统概述

符号定义：
世界坐标系$W$，机器人坐标系$B$，把IMU的坐标系就认为是$B$系（在紧耦合框架中是常用做法）。机器人的状态量$x$就可以写成：
$$x=[R^T,p^T,v^T,b^T{]}^T(1)$$
此处有$R\in SO(3)$表旋转矩阵 $p\in {\mathbb{R}}^3$为位移，$v$是速度，$b$是imu的bias。从$B$系到$W$系的变换矩阵$T\in SE(3)$可以被表示为：$T=[R|p]$.

问题的提出：
这里就是用Lidar，imu，GPS（可选）几种数据的观测值，来求得机器人的位姿。这个问题可以可以被构建成最大后验问题，作者使用因子图来对该问题进行建模（论文中说：factor graph is better suited to perform inference when compared with Bayes nets，也是isam的优点(？ 不太理解原因)）。基于高斯噪声的假设，这个问题的MAP inference就相当于是来解决非线性最小二乘优化问题
<怎么把slam问题转化成最小二乘问题见slam十四讲>

四个因子：imu预积分，激光雷达里程计，GPS，回环
因子图的变量：机器人的状态量
什么时候把新的机器人状态量节点加入因子图？
答：当且仅当机器人的位姿变化超过了用户定义的阈值时（此时认为他是关键帧，只有关键帧才被加入因子图）。使用isam2插入新节点对因子图进行优化。

B.IMU预积分因子

作用：提供两帧间的相对约束关系
LIO-SAM中是使用的9轴的imu，除了加速度计和陀螺仪（角速度），还有三个自由度的姿态信息（朝向）。预积分只用到前六个信息。注意加速度计中要考虑到重力的影响（如果是在本地做实验，重力加速度的变化忽略不计）。
imu的观测值(在B系)：
$$\begin{gathered} {\hat{\omega }}_t={\omega }_t+b_t^{\omega }+n_t^{\omega } \\ {\hat{a}}_t=R_t^{BW}(a_t-g)+b_t^a+n_t^a \end{gathered}$$
即测量值除了真值还包括了零偏和噪声，重力g在世界坐标系下不变。
可以利用imu的测量结果来推测机器人的运动，可以求得世界坐标系下的速度，位置，旋转，这里代入的就是imu系下加速度的真值：
$$\begin{gathered} v_{t+\Delta t}=v_t+g\Delta t+R_t^{WB}({\hat{a}}_t-b_t^a-n_t^a)\Delta t \\ {p}_{t+\Delta t}=p_t+v_t\Delta t+\frac{1}{2}g\Delta t^2+\frac{1}{2}{R}_t^{WB}({\hat{a}}_t-b_t^a-n_t^a)\Delta t^2 \\ {R}_{t+\Delta t}=R_{t}exp(({\omega }_t-b_t^{\omega }-n_t^{\omega })\Delta t) \end{gathered}$$（最后的式子是把角度做指数映射变旋转矩阵）
由于$\Delta t$时间特别短，所以假定这段时间内加速度,角速度和零偏是定值。

然后使用imu预积分来获得两帧之间的相对运动，在时间i和j之间的预积分的测量值$\Delta v_{ij}$, $\Delta p_{ij}$和$\Delta R_{ij}$可以按照如下计算：
$$\begin{gathered} \Delta v_{ij}=R_i^T(v_j-v_i-g\Delta t_{ij}) \\ {p}_{ij}=R_i^T(p_j-p_i-v_i\Delta t_{ij}-\frac{1}{2}g\Delta t_{ij}^2) \\ \Delta R_{ij}=R_i^T{R}_j \end{gathered}$$

$\ast$预积分论文 On-Manifold Preintegration for Real-Time Visual-Inertial Odometry

C.Lidar里程计

类似于LOAM，使用边缘点和面特征点。
但在这里，作者考虑到如果用每一帧来计算和增加因子，计算量会很大，所以这里引入了视觉slam中“关键帧”方法。被选择为关键帧的条件是：如果和之前的状态$x_i$相比，机器人位姿的变化超过了设定的阈值（平移1m/角度10°），那么就把当前帧设置为关键帧${\mathbb{F}}_{i+1}$。新保存的这个关键帧在因子图中和一个新的机器人状态节点$x_{i+1}$关联，两关键帧之间的帧会被舍弃。

使用关键帧的好处： 不仅是对地图密度和内存的平衡，也有利于保持相对稀疏的因子图（才能做实时非线性优化）。

如何生成Lidar里程计因子？
1）构建局部地图$M_i$：选离当前帧最近的25个关键帧，使用变换矩阵并把他们转到世界坐标系下拼成一个局部地图；由于提取了边缘点和面点两种特征，所以这里的局部地图也是分成了两个，最后还要分别对他两进行下采样，对$M_i^e$下采样分辨率为0.2m,$M_i^p$的为0.4m。
2）帧匹配：此时就是要把当前帧${\mathbb{F}}_{i+1}$，即两个特征点云 { F i + 1 e , F i + 1 p } \{ F_{i+1}^e, F_{i+1}^p\} {Fi+1e​,Fi+1p​}和局部地图$M_i$进行匹配，这里还是使用了LOAM中的方法。先把当前帧的特征点转到世界坐标系，这个初始的转换矩阵${\tilde{T}}_{i+1}$是从imu的数据得来，然后在地图中用KD树找到当前关键帧中对应的特征点，用LOAM中的点到线&点到面距离最小化得到优化后的位姿。
3）相对变换：此时可以得到i+1时刻的位姿$T_{i+1}$, 又因为已知$T_i$, 所以可求$\Delta T_{i,i+1}=T_i^T{T}_{i+1}$。一方面，imu预积分会对第i和第i+1帧进行约束，同时帧匹配的结果也会对这两帧的旋转和平移进行约束。

D. GPS

如果只用imu和lidar，系统还是会在长期任务中有漂移，所以要有提供全局测量信息的传感器消除漂移，比如高度仪，指南针，GPS。
作者认为：没有必要频繁的插入GPS的观测，因为lidar inertial odometry的漂移是十分缓慢的，所以只在估计的位姿不确定度大于收到的GPS位姿不确定度时，采增加GPS因子。

具体方法： 首先，先把GPS的测量值（经度和纬度）转换到本地的笛卡尔坐标系。一旦因子图中增加了新的节点，就要把一个新的GPS因子关联到这个节点。如果GPS信号没有和lidar做硬件同步，就要在lidar帧时间戳的基础上对GPS的测量值进行线性插值。

E.回环因子

具体方法： 基于欧氏距离的回环检测方法 + 帧到局部地图的ICP匹配。
当一个新的状态$x_{i+1}$加入了因子图时，首先去找在欧几里得空间中靠近$x_{i+1}$的先验状态（15m），如下图所示，比如说$x_3$是返回的候选，接下来就尝试用${\mathbb{F}}_{i+1}$去和 { F 3 − m , . . . , F 3 , . . . , F 3 + m } \{ \mathbb{F_{3-m}},...,\mathbb{F_3},...,\mathbb{F_{3+m}}\} {F3−m​,...,F3​,...,F3+m​}进行匹配（文中m=12）。在匹配之前，先把当前关键帧和局部地图投影到世界坐标系。在获得相对变换$\Delta T_{3,i+1}$之后将它作为回环因子加入图中。

实际中，作者发现使用回环因子对于修正机器人在高度方向的漂移是非常有用的，因为GPS的高度的测量是非常不准确的（作者的代码中只采用了GPS的x和y方向的信息）。作者说没有回环的情况下，仅用GPS高度误差能到100m这种离谱程度。

IV. EXPERIMENTS

实验设备：16线lidar，9轴imu，GPS
数据集：5个场景，包括“Rotation”， “Walking”， “Campus”，“Park”， “Amsterdam” 。

• 数据集Rotation
  这个数据集只使用了imu预积分和lidar里程计因子
  
  LIO-SAM的结构细节更多，因为在快速旋转时，它的lidar帧在$SO(3)$匹配更准。

• 数据集walking
  用来评价系统在大位移大旋转SE(3)时的表现。
  
  LIOM会处理所有的测量值，所以达不到实时的表现，而LOAM和LIO-SAM都可以实时。这个测试中也没有用上GPS和回环因子

• 数据集Campus
  这个测试是为了看引入GPS和回环因子的好处。关于这个场景，论文有说校园里有很多树，所以GPS的结果多数时候不太准。GPS有效的时间看下面的轨迹图。
  
  

• 数据集Park
  LIOM依然不能实时运行。在这个数据集，LIO-GPS虽然在俯视图回到了远点，但是高度漂移太大了，导致不能回环。
  

• 数据集Amsterdam
  这个数据集的挑战性：

  1. 运河上的桥梁构成了退化的场景，因为当船在桥下时，几乎没有什么有用的特征，类似于在一个长的、没有特征的走廊上移动。此时平面特征的数量明显减少，因为没有地面。
  2. 当阳光直射在传感器的视野中时，激光雷达会出现许多错误的检测，这在数据收集期间大约有20%的时间发生。
  3. 由于头顶上有桥梁和城市建筑，我们也只能获得间歇性的GPS接收。

  由于这些挑战，LOAM、LIOM和LIO-odom
  在这个测试中都没有产生有意义的结果。与在公园数据集中遇到的问题类似，LIO-GPS在返回机器人的初始位置时，由于高度的漂移而无法闭环，这进一步促使作者在LIO-SAM中使用闭环因子。

F. Benchmarking Results

这里作者提到，运行速度主要是受环境中特征点云的密度的影响，受因子图节点数量和因子数量的影响更小。

---

代码整体框架

四个节点，节点之间用topic传输数据。预积分节点中，进行图优化时，使用的是lidar里程计和imu积分的结果。