精读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
参考写作
知乎大佬
代码链接

目录

• 1：Abstract

• 2：INTRODUCTION

• 3：RELATED WORK

• 4：SYSTEM OVERVIEW

• 5：VISUAL FRONTEND

• 6：STEREO VISUAL INERTIAL ODOMETRY

• • A. IMU pre-integration factor
  • B. Structureless vision factor
  • C. Optimization and marginalization

• 7：LIDAR MAPPING

• • A. LiDAR scan dewarping
  • B. Scan to map registration

• 8：VII. LIDAR ENHANCED LOOP CLOSURE

• • A. Loop detection
  • B. Loop constraint
  • C. Global pose graph optimization
  • D. Re-localization

• 9：EXPERIMENTAL RESULTS

• 10：CONCLUSIONS

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}_{i j}^{I}，它由三部分组成，分别为位姿的残差\mathbf{r}_{\Delta \xi i j}，速度残差\mathbf{r}_{\Delta \mathbf{v} i j}和偏差\mathbf{r}_{\Delta \mathbf{b} i j}。

B. Structureless vision factor

\xi_{k, l c}相机的位姿，p是路标，\mathbf{x}_{p} \in \mathbb{R}^{3}是全局坐标系中的位置。被多个状态观测到，表示观测到 p的状态集合为 \{\mathbf{S}\}_{p}。对于 \{\mathbf{S}\}_{p} 中的任何状态 \mathbf{S}_{k} ，表示通过在左图片中测量 p构建的残差为\mathbf{r}_{\xi_{k, i c}, p}^{V}（ \xi_{k, l c}是左相机位姿，被获得通过应用一个IMU-相机变换到\xi_{k} ）
\mathbf{r}_{\xi_{k, l c}, p}^{V}=\mathbf{z}_{\xi_{k, l c}, p}-h\left(\xi_{k, l c}, \mathbf{x}_{p}\right)
其中 \mathbf{Z}_{\xi_{k, l l}, p}是 p 在图片中的像素测量，同时h\left(\xi_{k, l c}, \mathbf{x}_{p}\right) 编码了视角投影。相同的构架被推导用于右相机图片。迭代方法被采用来优化位姿图，并且因此上述残差的线性化被获得。下面等式展示了路标p 的线性化残差。
\sum_{S_{p}}\left\|\mathbf{F}_{k p} \delta \xi_{k}+\mathbf{E}_{k p} \delta \mathbf{x}_{p}+\mathbf{b}_{k p}\right\|^{2}
其中Jacobin \mathbf{F}_{k p}, \mathbf{E}_{k p} 和残差\mathbf{b}_{k p}是通过线性化的结果并且通过\Sigma_{c}^{1 / 2}（视觉测量的协方差）进行归一化。堆叠每个独立分量到一个矩阵，我们有
\left\|\mathbf{r}_{p}^{V}\right\|_{\Sigma_{C}}^{2}=\left\|\mathbf{F}_{p} \delta \xi_{k}+\mathbf{E}_{p} \delta x_{p}+\mathbf{b}_{p}\right\|^{2}
为了避免在\mathbf{X}_{p}上进行优化，我们将残差投影到 \mathbf{E}_{p}的零空间上：每一项乘以 \mathbf{Q}_{p} \doteq \mathbf{I}- \mathbf{E}_{p}\left(\mathbf{E}_{p}^{\top} \mathbf{E}_{p}\right)^{-1} \mathbf{E}_{p}^{\top}, an orthogonal projector of \mathbf{E}_{p}[31] . 因此，对于地标p，我们有无结构视觉因子
\left\|\mathbf{r}_{p}^{V}\right\|_{\Sigma_{C}}^{2}=\left\|\mathbf{Q}_{p} \mathbf{F}_{p} \delta \xi_{k}+\mathbf{Q}_{p} \mathbf{b}_{p}\right\|^{2}

C. Optimization and marginalization

给定残差，位姿图优化是一个最大后验（MAP）问题，其最优解为
\mathbf{S}_{w}^{*}=\arg \min _{S_{w}^{*}}\left(\left\|\mathbf{r}_{0}\right\|_{\Sigma_{0}}^{2}+\sum_{i \in w}\left\|\mathbf{r}_{i(i+1)}^{I}\right\|_{\Sigma_{l}}^{2}+\sum_{p}\left\|\mathbf{r}_{p}^{V}\right\|_{\Sigma_{C}}^{2}\right)
其中 \mathbf{S}_{w}^{*} 是一组窗口内的状态变量。 \mathbf{r}_{0}和 \Sigma_{0} 是先验因子和它们相关的协方差。 \Sigma_{I}是IMU测量的协方差。我们使用LM优化器来解决非线性优化问题。最近的 N个状态变量被维护在优化器中。

7：LIDAR MAPPING

激光建图使用高频率IMU的VIO位姿作为运动先验来执行LiDAR点云去畸变和帧图配准。表示一帧\chi 为从一个完整LiDAR旋转中得到的点云。包含边缘和平面上点的几何特征从 \chi 中被提取在去畸变之前[22][26]。配准然后基于当前帧到地图的特征点（所有之前的特征点），作为一个最小化特征点的欧式距离残差优化问题被解决[22]。

A. LiDAR scan dewarping

去畸变是需要的当来自一个LiDAR帧的点的时间戳不同时。表示一帧内的任意时刻为 t_{i} 。我们去畸变所有的点到帧的结束时刻 t_{k+1} 基于IMU频率的VIO位姿。表示一个 t_{i}时刻的LiDAR点为 \mathbf{P}_{i} ，并且去畸变的点为 \tilde{\mathbf{P}}_{i} ，我们有
\tilde{\mathbf{P}}_{i}=\left(\mathbf{T}_{k+1}^{L}\right)^{-1} \mathbf{T}_{i}^{L} \mathbf{P}_{i}
其中 \mathbf{T}_{k+1}^{L}, \mathbf{T}_{i}^{L} 是通过最近的IMU频率的VIO位姿变换得到的LiDAR帧位姿。

B. Scan to map registration

来自去畸变帧 \tilde{\chi} 的特征点被配准到地图上，用于优化 t_{k+1} 时刻的LiDAR建图位姿 \mathbf{L}_{k+1} 。表示 \mathbf{L}_{k+\mathbf{1}} 的初始估计为 \mathbf{L}_{k+1}^{*}，我们有：

\mathbf{L}_{k+1}^{*}=\mathbf{L}_{k} \mathbf{T}_{t r a n s}^{L}

其中 \mathbf{L}_{k}是之前被优化的LiDAR建图位姿， \mathbf{T}_{\text {trans }}^{L} 是基于IMU频率的VIO位姿获得的相对变换。所有去畸变的特征点然后被变换到世界坐标系中通过\mathbf{L}_{k+1}^{*} 用于配准。

当前帧中边缘特征点的残差 \mathbf{r}_{E} 是它自身到地图中由两个最近边缘点行程的线的欧式距离。当前帧表面点的残差\mathbf{r}_{U} 是它自身到地图中最近的三个表面点形成的平面块之间的距离[22]。组合\mathbf{L}_{k+1}^{*} ，我们可以重写上述两个残差为：
\begin{aligned} &f_{E}\left(E_{(c, i)}^{L}, \mathbf{L}_{k+1}^{*}\right)=\mathbf{r}_{E}\\ &f_{U}\left(U_{(c, i)}^{L}, \mathbf{L}_{k+1}^{*}\right)=\mathbf{r}_{U} \end{aligned}
其中E_{(c, i)}^{L}和U_{(c, i)}^{L} 是LiDAR坐标系中的第i个去畸变特征点的3D位置。LM优化器被用于解决上述非线性优化问题，该问题通过堆叠所有特征点的代价函数构成。

8：VII. LIDAR ENHANCED LOOP CLOSURE

闭环是关键的对于任何SLAM系统由于长期操作进入误差。闭环的目标是排除漂移 通过执行一个全局位姿图优化，其整合了回环约束 和来自LiDAR建图的相对变换信息 。为了更好地支持LiDAR建图，修正的LiDAR位姿被实时地发送回来以至于来自新帧的特征点被配准到重遍历的地图。我们提出在BoW[2]回环检测和PnP回环约束构建之外增加ICP对齐 。系统使用iSAM2[26]，一个增量的求解器用于优化全局位姿图，达到实时表现。

A. Loop detection

双目图片和LiDAR帧被关联使用它们的时间戳 。让我们分别表示这些为关键图片和关键帧。为了避免虚假的回环检测，我们限制候选在一定时间阈值 内。回环候选被检测通过使用之前图片构建的BoW[2]数据库来测试关键图片。此外，我们匹配左图片的特征描述子和回环候选来滤除误报。

B. Loop constraint

系统首先获得视觉回环约束为一个初始估计 。因为我们使用非结构化构架用于视觉路标，在回环候选中的所有双目匹配的特征中的三角化被执行来获得他们的3D位置。它们和当前关键图片的关联被给定通过描述子匹配。视觉回环约约束然后被计算使用EPNP[37]。为了提高视觉回环约束的准确性，我们在对应LiDAR关键帧的特征点上使用ICP对齐 。有了一个坏的初始化或者大的点数，ICP消耗更长时间来收敛并消耗更多的时间资源。但是，视觉回环约束提供一个好的初始化点并且ICP只使用稀疏特征点，其使得它收敛的更快。

C. Global pose graph optimization

全局位姿图的图表示被展示在下图中。它包含了所有可用的LiDAR建图位姿作为变量，由LiDAR里程计因子和回环约束因子组成，同时是相对变换的测量 \left(\mathbf{L}_{u}\right)^{-1} \mathbf{L}_{v}其中u和v分别表示帧ID，并且 \mathbf{L}_{u}, \mathbf{L}_{v} 是对应的位姿。对于LiDAR里程计因子，u 是之前帧ID。对于回环约束因子，u 是回环的关键帧ID。对于两种情形， v 都是当前帧ID。位姿在优化中被表示为6自由度形式。为了实现实时表现，我们使用iSAM2[36]来增量地优化全局位姿图。

D. Re-localization

一旦真实的闭环候选被发现，LiDAR建图缓存特征点（没有配准它们到地图）直到它接收到一个闭环修正。闭环修正包含全局优化的位姿。LiDAR建图更新它的地图，增加缓存的特征点到地图中然后恢复它的操作。我们可以保证实时更新地图因为（a）闭环具有实时表现（b）稀疏特征地图没有消耗太多内存（c）帧图配准是足够快的来追上LiDAR数据频率。

9：EXPERIMENTAL RESULTS

评估了VIL-SLAM，并将其与基于最佳实时激光雷达的系统LOAM2在自定义数据集上进行了比较。还使用EuRoC MAV数据集 评估立体VIO子模块（VIL-VIO）。

这里提供了五个典型环境的结果，包括无特色的走廊、杂乱无章的高台、隧道和室外环境。所有这些序列的数据收集在同一点开始和结束。基于最终漂移误差 （FDE）对里程计（LiDAR建图姿态）进行评估。以Faro扫描为基本真实值，用平均配准误差（MRE）对建图结果进行评估。首先将地图与模型对齐（Faro扫描），然后计算地图点与模型中最近点之间的欧几里德距离。里程表FDE和建图结果如表一所示，较好的用粗体显示。


地图比较：VIL-SLAM（右）和LOAM（左）与模型相比的地图配准误差。0.3m以上的误差（a-b）用红色表示，0.5m以上的误差（c）用红色表示。蓝色和绿色内部不连续的红色区域是由于扫描设备的遮挡导致的模型缺失。

10：CONCLUSIONS

VIL-SLAM是一种先进的里程计和地图构建系统，旨在在不同的环境中长期稳定地运行。当前的框架将VIL-VIO和激光雷达建图松耦合在一起。我们正在将其扩展到一个紧密耦合的框架中，以便从LiDAR建图得到的精确姿态估计可以用于IMU偏差校正。在循环闭合中对扫描帧之间的稀疏特征点进行ICP精细化获得更好的回环约束。