精读Stereo Visual Inertial LiDAR Simultaneous Localization and Mapping

文献信息
标题：Stereo Visual Inertial LiDAR Simultaneous Localization and Mapping
作者：Weizhao Shao, Srinivasan Vijayarangan∗, Cong Li∗, and George Kantor
论文期刊：IROS2019
参考CSDN写作
知乎大佬
代码链接

1：Abstract

同时定位和地图绘制（SLAM）是移动和空中机器人技术的一项基本任务。 与基于视觉的系统相比，基于LiDAR的系统在准确性和鲁棒性已被证明是更优的。但是基于纯LiDAR的系统在某些退化的情况下仍无法正常工作，例如穿越隧道。我们提出了双目视觉惯性激光雷达（VIL）SLAM，在这些退化情况下性能更好，并且在所有其他情况下具有良好的性能。 VIL-SLAM通过将紧耦合的双目视觉惯性里程计（VIO）与LiDAR建图和LiDAR增强的视觉回环检测结合在一起，实现了这一目标。该系统实时生成闭环校正的6自由度LiDAR姿态，并实时生成1cm体素密集图。与最优的LiDAR方法相比，VIL-SLAM展示出更高的准确性和鲁棒性。

2：INTRODUCTION

• 基于纯LiDAR的系统有其不足之处。
  它们在具有重复结构（如隧道或走廊）的环境中会失败。这些环境很难建图和定位，因此必须部署能够利用所有传感器模式优势的系统才能成功。

• 我们建议使用VIM-SLAM，它使用IMU，立体摄像机和LiDAR，并共同开发其优势。 VILSLAM通过将双目VIO和LiDAR映射与闭环集成在一起来实现这一目标。据我们所知，这是此类工作的第一个。此外，我们介绍了一种使用ToF激光扫描仪（Faro）评估地图结果的方法。我们还将在EuRoC MAV数据集上提供VIO验证结果。

• VIL-SLAM使用
  VIL-SLAM使用一个紧耦合的双目VIO，其执行固定长度平滑图优化，使用稀疏3D特征用于地图配准的LiDAR建图，和融合稀疏点云对齐和视觉回环检测的闭环。回环检测优化一个全局位姿图使用一个增量求解器。

• VIL-SLAM旨在在不同环境中长期稳定运行。
  高频IMU测量产生的估计值对于较短的时间间隔是合理的，但会很快漂移。当受双目视觉测量的约束时，我们可以纠正偏差并估计准确的相对运动（称为VIO）。
  相对运动估计被用于辅助LiDAR扫描匹配，其然后累计高保真度的3D点云来形成一个准确的地图。

• 闭环
  机器人的状态估计在长期运动中累积误差。闭环解决了该问题通过识别重新访问的区域使用视觉或者LiDAR方法。视觉方法包括BoW[2]来识别地点和PnP算法来估计运动修正。在LiDAR方法中，场景被识别使用基于分割的方法如SegMatch[3]，并且位姿修正被估计使用ICP[4]算法。虽然BoW方法很快很通用，它缺乏很慢但是很鲁棒的使用ICP的LiDAR方法的准确性。VIL-SLAM使用一个混合方法，其中首先它使用BoW技术寻找闭环候选，使用PnP算法生成一个粗糙的位姿修正估计，然后使用ICP精修该粗糙估计。

• VIL-SLAM使用一种混合方法，
  首先使用Bagof-Words技术找到闭环候选者，
  然后使用Perspective-n-Point（PnP）算法生成姿态校正的粗略估计，
  最后使用ICP细化粗略估计。

3：RELATED WORK

当前的VIO文献介绍了各种公式，以整合视觉和惯性数据。文献中有紧耦合（其中视觉信息和惯性测量被共同优化）或松耦合系统（其中IMU是一个单独的模块，与仅视觉状态估计器相融合)。这些方法可以进一步分为基于滤波的或基于图优化。基于紧密耦合的基于优化的方法，利用迭代地最小化残差的优势，通常可以以更高的计算成本获得更好的精度和鲁棒性。在我们的工作中，我们通过以无结构的方式表达路标(landmarks)，并仅针对固定大小的姿态图进行优化，以实现实时性能来限制计算成本。仅使用激光扫描仪的当前最先进的SLAM系统是[19] – [23]，其中需要运动模型，即恒速模型或高斯过程。文献[24]中的方法结合了立体相机和激光扫描仪。它具有从视觉测距法（VO）生成的运动估计，并通过匹配激光扫描进行完善。我们系统的区别在于它们使用多分辨率网格地图表示，而我们使用稀疏点云来定位和输出稠密点云。而且，与VO相比，VIO通常更健壮和准确[25]。使用IMU，单眼相机和激光扫描仪的VLOAM [26]与我们的系统最相似。一个区别是，我们使用紧密耦合的VIO作为运动模型来初始化LiDAR mapping算法，而VLOAM使用松散耦合的IMU和摄像机。尽管我们的VIO更加强大，但VLOAM拥有一个更具交互性的系统，可以将来自摄像头和LiDAR模块的信息用于IMU偏差校正。VIL-SLAM具有的另一项功能是LiDAR增强的环路闭合功能。

4：SYSTEM OVERVIEW

系统具有四个模块
1：视觉前端取自双目相机的双目对，它执行帧到帧跟踪（Frame-to-frame KLT tracking）和双目匹配（Stereo ORB descriptor matching）并输出双目匹配作为视觉的测量。
2：双目VIO取双目匹配和IMU测量，执行IMU预积分和紧耦合的固定滞后平滑在一个位姿图中（Fixed-lag smoothing）该模块以IMU速率和相机速率输出VIO位姿。
3：LIDAR mapping 模块使用来自VIO的运动估计并执行LiDAR点云去畸变（LIDAR scan dewarping）和帧图配准（Scan to map registration）
4:回环模块执行视觉回环检测和初始的回环约束估计，其进一步通过一个稀疏点云ICP对齐来精修。一个约束了所有LiDAR位姿的全局位姿图被增量优化来获得全局修正的轨迹和一个实时的LiDAR位姿修正。它们被返回LiDAR建图模块用于地图更新和重定位。在后处理中，我们使用最优估计的LiDAR位姿缝补去畸变的LiDAR帧来得到稠密点云结果。

5：VISUAL FRONTEND

视觉前端接收一个双目对，并执行帧帧跟踪和双目匹配用于一组双目匹配的稀疏特征点的生成，即双目匹配。本文使用Kanade Lucas Tomasi（KLT）特征跟踪器来跟踪先前双目匹配中的所有特征点，在左或者在右相机中。只有当它们被同时跟踪时，我们有一个跟踪的双目匹配并且它被放入到输出中。宽双目极线有助于尺度估计并减少由较远特征导致的退化问题。我们使用基于特征的方法，比KLT更适合处理长基线。如果跟踪的立体匹配数低于阈值，使用Shi-Tomashi角点检测器进行特征提取，然后进行特征消除过程，删除与任何小于阈值的现有特征的像素距离的特征。ORB描述子后计算所有幸存特征，然后进行暴力立体匹配以获得新的匹配结果。系统通过对第一帧立体视觉进行立体匹配来初始化系统。

6：STEREO VISUAL INERTIAL ODOMETRY

双目VIO的目标是在相对较高的频率上提供实时准确的状态估计，作为LiDAR建图算法的运动模型。在位姿图上运行的紧耦合固定滞后平滑器在精度和效率之间是一个很好的权衡。基于优化的方法通常允许多次线性化以接近全局最小值。固定滞后位姿图优化器进一步限定了变量的最大数目，因此计算成本是有界的。由于糟糕的视觉测量会导致收敛问题，对视觉测量强制执行严格的异常值拒绝机制。系统通过检查平均重投影误差来消除异常值。

该算法以IMU预积分因子和无结构视觉因子作为约束条件，对窗口内部的状态进行优化。将最近N个状态作为状态估计窗口大小。
紧耦合固定滞后平滑器在精度和效率之间取得了平衡，VIO包括三个过程：IMU预积分、无结构视觉因子、优化和边缘化。

A. IMU pre-integration factor

使用IMU预积分方法生成${\mathbf{S}}_i$和${\mathbf{S}}_j$之间的相对IMU测量。重新线性化能够在优化过程中被高效地执行。IMU预积分因子表示的残差是${\mathbf{r}}_{ij}^I$，它由三部分组成，分别为位姿的残差${\mathbf{r}}_{\Delta \xi ij}$，速度残差${\mathbf{r}}_{\Delta \mathbf{v}ij}$和偏差${\mathbf{r}}_{\Delta \mathbf{b}ij}$。

B. Structureless vision factor

${\xi }_{k,lc}$相机的位姿，$p$是路标，${\mathbf{x}}_p\in {\mathbb{R}}^3$是全局坐标系中的位置。被多个状态观测到，表示观测到 $p$的状态集合为 { S } p \{\mathbf{S}\}_{p} { S}p​。对于 { S } p \{\mathbf{S}\}_{p} { S}p​ 中的任何状态 ${\mathbf{S}}_k$ ，表示通过在左图片中测量 $p$构建的残差为${\mathbf{r}}_{{\xi }_{k,ic},p}^V$（ ${\xi }_{k,lc}$是左相机位姿，被获得通过应用一个IMU-相机变换到${\xi }_k$ ）
$${\mathbf{r}}_{{\xi }_{k,lc},p}^V={\mathbf{z}}_{{\xi }_{k,lc},}$$