# 立体视觉惯性激光雷达同步定位与建图
邵伟照、Srinivasan Vijayarangan $^{*}$、李聪 $^{*}$ 以及 George Kantor
摘要—同步定位与建图（SLAM）是移动机器人与无人机领域的一项基础任务。由于激光雷达系统具有更高的精度和鲁棒性，其性能已被证明优于基于视觉的系统。尽管激光雷达系统优势明显，但在穿越隧道等特定退化场景下仍会失效。我们提出了立体视觉惯性激光雷达（VIL）SLAM算法，该算法在这些退化场景下的表现更优，且在其他场景下性能与之相当。VIL-SLAM通过将紧耦合的立体视觉惯性里程计（VIO）与激光雷达建图、激光雷达增强型视觉回环检测相结合来实现这一目标。该系统能够实时生成经回环校正后的六自由度激光雷达位姿，并近乎实时地生成1厘米精度的体素稠密地图。相较于现有的先进激光雷达方法，VIL-SLAM在精度和鲁棒性方面均有显著提升。
## I. 引言
SLAM旨在解决在未知环境中建图的同时估计机器人状态的问题。尽管过去几十年来SLAM一直是研究热点，但Cadena等人 [^1] 指出，在处理多样化环境与长期连续运行方面仍存在挑战。SLAM系统依托多种传感器模态运行，每种模态都试图发挥其优势。近年来，由于对环境变化的鲁棒性更强，基于激光雷达的SLAM系统比基于视觉的系统更受欢迎。然而，纯激光雷达系统也存在缺陷：在隧道或走廊等具有重复结构的环境中，它们无法正常工作。这类环境的建图与定位难度较大，因此必须部署能充分利用各类传感器优势的系统才能取得成功。我们提出的VIL-SLAM系统结合了惯性测量单元、立体相机和激光雷达，充分发挥它们的协同优势。实验表明，在大多数情况下VIL-SLAM的性能与纯激光雷达系统相当，而在纯激光雷达系统无法胜任的场景下表现更佳。VIL-SLAM通过整合立体VIO、激光雷达建图以及回环检测功能来实现这一效果。据我们所知，这是同类研究中的首例。此外，我们还提出了一种利用飞行时间激光扫描仪（Faro）评估建图结果的方法，同时提供了在EuRoC MAV数据集上的VIO验证结果。
VIL-SLAM采用了一种紧耦合的立体VIO模块，该模块可执行固定延迟的位姿图优化；激光雷达建图模块利用稀疏三维特征进行地图配准；回环检测模块则将稀疏点云对齐与视觉回环检测相结合。回环检测模块会对全局位姿进行优化。
(a)
(b)
Fig. 1: (a) Experimental platform built. (b) Mapping result from an outdoor test. Streetlight is reconstructed clearly.
利用增量求解器来构建图。VIL-SLAM旨在能够长期、稳定地在不同环境中运行。高频的IMU测量所产生的估计值在短时间间隔内还算合理，但很快就会产生漂移。当结合立体视觉测量数据时，我们就能修正这些偏差并估算出精确的相对运动（即VIO）。这种相对运动估计值可用于辅助激光雷达扫描匹配，进而累积高保真度的3D点云以生成精确的地图。不过，机器人在长时间移动过程中，其状态估计仍会积累误差。回环闭合机制通过视觉或激光雷达方法识别机器人曾到访过的地点，从而解决这一问题。视觉方法采用词袋模型[^2]来识别地点，再利用透视-点（PnP）算法来估算姿态修正量。在激光雷达方法中，则利用基于片段的算法（如SegMatch[^3]）来识别地点，再通过迭代最近点（ICP）[^4]算法来估算姿态修正量。虽然词袋模型方法速度快且适用性广，但其精度不如耗时较长但更为稳健的基于ICP的激光雷达方法。VIL-SLAM采用了一种混合策略：首先利用词袋模型技术找出可能的回环闭合候选点，接着用透视-点（PnP）算法得出姿态修正量的粗略估计，最后再用ICP对这一粗略估计进行精细化处理。
### 二、相关工作
当前的VIO文献提出了多种融合视觉与惯性数据的方法。这些研究将不同方法分为紧耦合系统[^5] [^6] [^7]——即视觉信息与惯性测量数据被共同优化；以及松耦合系统[^8] [^9] [^10] [^11]——此时IMU作为独立模块，与仅依赖视觉的状态估计器进行数据融合。这些方法还可进一步划分为基于滤波的[^11] [^12] [^13] [^14] [^15] [^16]和基于图优化的[^5] [^6] [^7] [^17] [^18]。基于紧耦合优化的方法能通过迭代最小化残差，通常能在付出更高计算成本的前提下获得更好的精度和鲁棒性。在我们的研究中，我们限制了
---
Fig. 2: The system diagram of VIL-SLAM. Sensors are in gray and modules are in green. Arrows indicate how messages flow within the system. The dark thick arrows indicate the system real-time output and the light thick arrow indicates the output generated in post-processing near real-time.
计算成本，具体做法是以无结构的方式生成路标，且仅针对固定大小的位姿图进行优化，从而实现实时性能。
目前最先进的仅使用激光扫描仪的SLAM系统有[^19] [^20] [^21] [^22] [^23]，这些系统都需要运动模型，可以是恒定速度模型或高斯过程模型。[^24]中的方法结合了立体相机和激光扫描仪，其运动估计由视觉里程计（VO）生成，再通过匹配激光扫描数据来优化。与我们系统不同的是，他们采用多分辨率网格地图表示法，而我们则利用稀疏点云进行定位并输出密集点云。此外，与VO相比，VIO通常更加稳健且精度更高[^25]。VLOAM[^26]使用了惯性测量单元（IMU）、单目相机和激光扫描仪，是与我们系统最为相似的现有系统。两者的区别在于：我们使用紧耦合的VIO作为运动模型来初始化激光雷达建图算法，而VLOAM采用的是松耦合的IMU和相机。尽管我们的VIO更为稳健，但VLOAM的系统交互性更强，能够利用相机和激光雷达模块的信息来校正IMU偏差。VIL-SLAM新增的功能是激光雷达增强的回环闭合机制。
### III. 系统概述
如图2所示，该系统包含四个模块。视觉前端从立体相机获取立体图像对，执行帧间跟踪和立体匹配，并将立体匹配结果作为视觉测量值输出。立体VIO模块接收立体匹配结果和IMU测量数据，进行IMU预积分以及基于位姿图的紧耦合固定滞后平滑处理，该模块会以IMU频率和相机频率输出VIO位姿。激光雷达建图模块利用VIO提供的运动估计信息，执行激光雷达点云去畸变处理和扫描数据到地图的配准。回环闭合模块负责视觉回环检测及初始回环约束估计，再通过稀疏点云ICP对齐进一步优化。系统会对约束所有激光雷达位姿的全局位姿图进行增量式优化，从而实时获得全局校正的轨迹和激光雷达位姿修正值，这些值会被传回激光雷达建图模块用于地图更新和重新定位。在后处理阶段，我们将去畸变后的激光雷达扫描数据与最佳估计的激光雷达位姿进行拼接，得到密集的建图结果（见图5）。
### IV. 视觉前端
视觉前端接收一对立体图像，通过帧间跟踪和立体匹配来生成一组立体匹配的特征点，即立体匹配点对。一个立体匹配点对既可以是从之前的立体图像对中追踪到的，也可以是在当前图像对中新提取的。帧间跟踪性能直接影响时间约束的质量，而立体匹配则有助于约束尺度。这两项任务对于任何立体视觉里程计而言都至关重要。近年来，直接法在时序跟踪方面展现出了鲁棒且高效的结果[^8]，[^27]。因此，我们采用Kanade-Lucas-Tomasi（KLT）特征跟踪器[^28]来追踪之前立体匹配点对中的所有特征点，无论它们位于左图还是右图中。只有当这些特征点在两幅图像中均被成功追踪到时，才构成有效的立体匹配点对并输出。较大的立体基线有利于尺度估计，并能减少由远距离特征引起的退化问题。我们采用基于特征的方法来处理大基线情况，因为这类方法比KLT更适用。如果追踪到的立体匹配点对数量低于阈值，我们就使用Shi-Tomashi角点检测器[^29]进行特征提取，随后执行特征剔除操作：将那些与已有特征的像素坐标距离小于阈值的特征点删除。接着对剩余的特征计算ORB（定向FAST与旋转BRIEF）描述符[^30]，再通过暴力搜索法进行立体匹配以获取新的立体匹配点对。系统初始化时会对第一对立体图像执行立体匹配操作。
### V. 立体视觉惯性里程计
立体视觉惯性里程计的目标是提供实时且精确的状态估计，其频率相对较高，以便为激光雷达建图算法提供运动模型。在位姿图上运行的紧耦合固定滞后平滑器在精度与效率之间取得了良好的平衡。基于优化的方法通常允许多次线性化以逼近全局最小值。固定滞后的位姿图优化器进一步限制了变量的数量，从而控制了计算成本。由于不良的视觉测量会导致收敛问题，我们对视觉测量采用了严格的异常值剔除机制。系统通过检查立体投影误差与时间投影误差的平均值来剔除异常值。
---
Fig. 3: Fixed-lag pose graph formulation in the VIO. State variables being optimized are circled, where i stands for the current state and N is the window size. (a) The state to be marginalized is crossed. (b) After marginalization, prior factors are added back on related variables.
所提出的视觉惯性里程计以IMU预积分因子和无结构视觉因子作为约束条件。其图表示如图3所示。待优化的变量是窗口内的状态量。用$\mathbf{S}_t$表示立体帧时刻t的状态变量。$\mathbf{S}_t$包含6自由度（DoF）的系统位姿$\xi_t$（IMU坐标系）、对应的线速度$\mathbf{v}_t$、加速度计偏差$\mathbf{b}_t^a$以及陀螺仪偏差$\mathbf{b}_t^g$。待估计的状态变量窗口涵盖了最近的N帧立体图像。过去的状态变量会被边缘化，从而为相关变量产生先验因子。
### A. IMU预积分因子
我们遵循IMU预积分方法[^31] [^32]来生成$\mathbf{S}_i$与$\mathbf{S}_j$之间的相对IMU测量值。借助预积分技术，优化过程中可高效地进行线性化。由IMU预积分因子表示的残差为$\mathbf{r}_{ij}^l$，它由三项组成：位姿残差$(\mathbf{r}_{\Delta\xi_{ij}})$、速度残差$(\mathbf{r}_{\Delta vij})$以及偏差残差$(\mathbf{r}_{\Delta bij})$。
### B. 无结构视觉因子
视觉测量是以无结构的方式建模的，类似于[^31]、[^33]、[^34]。考虑一个地标p，其在全局坐标系中的位置为$\mathbf{x}_p \in \mathbb{R}^3$，该地标被多个状态观测到，将观测到p的状态集合记为$\{\mathbf{S}\}_p$。对于$\mathbf{S}_k$中的任意状态$\{\mathbf{S}\}_p$，用左摄像头图像中p的测量值所形成的残差来表示，即$\mathbf{r}_{\xi,l,c,p}^V$（其中$\xi_{k,l,c,p}$为左摄像头位姿，是通过对$\xi_k$应用IMU-摄像头变换得到的）：
$$ \mathbf{r}_{\xi_{k,l c},p}^{V}=\mathbf{z}_{\xi_{k,l c},p}-h(\xi_{k,l c},\mathbf{x}_{p}) $$
其中$\mathbf{z}_{\xi_{k,lc},p}$是p在图像中的像素测量值，$h(\xi_{k,lc},\mathbf{x}_{p})$则用于描述透视投影关系。针对右摄像头图像也有相同的公式推导。由于采用迭代方法优化位姿图，因此需要对上述残差进行线性化。公式(2)给出了地标p的线性化残差。
$$ \sum_{S_{p}}||\mathbf{F}_{k p}\delta\xi_{k}+\mathbf{E}_{k p}\delta\mathbf{x}_{p}+\mathbf{b}_{k p}||^{2} $$
Fig. 4: The global pose graph consists of the LiDAR Odometry Factor and the Loop Constraint Factor. i stands for the current scan.
其中雅可比矩阵$\mathbf{F}_{kp}$、$\mathbf{E}_{kp}$以及残差$\mathbf{b}_{kp}$均为线性化的结果，它们会由视觉测量协方差$\Sigma_{c}^{1/2}$进行归一化处理。将求和式中的每个分量堆叠成矩阵后可得：
$$ ||\mathbf{r}_{p}^{V}||_{\Sigma_{C}}^{2}=||\mathbf{F}_{p}\delta\boldsymbol{\xi}_{k}+\mathbf{E}_{p}\delta\boldsymbol{x}_{p}+\mathbf{b}_{p}||^{2} $$
为了避免对 $\mathbf{x}_p$ 进行优化，我们将残差投影到 $\mathbf{E}_p$ 的零空间中：将每一项都乘以 $\mathbf{Q}_p \doteq \mathbf{I} - \mathbf{E}_p(\mathbf{E}_p^\top \mathbf{E}_p)^{-1} \mathbf{E}_p^\top$，它是 $\mathbf{E}_p$ 的一个正交投影算子 [^31]。由此我们得到了地标 p 对应的无结构视觉因子。
$$ ||\mathbf{r}_{p}^{V}||_{\Sigma_{C}}^{2}=||\mathbf{Q}_{p}\mathbf{F}_{p}\mathbf{\delta\xi}_{k}+\mathbf{Q}_{p}\mathbf{b}_{p}||^{2} $$
### C. 优化与边缘化
给定残差后，位姿图优化就转化为一个最大后验概率（MAP）问题，其最优解为：
$$ \mathbf{S}_{w}^{*}=\arg\min_{S_{w}^{*}}(||\mathbf{r}_{0}||_{\Sigma_{0}}^{2}+\sum_{i\in w}||\mathbf{r}_{i(i+1)}^{I}||_{\Sigma_{I}}^{2}+\sum_{p}||\mathbf{r}_{p}^{V}||_{\Sigma_{C}}^{2}) $$
其中$\mathbf{S}_w^*$是窗口内的状态变量集合。$\mathbf{r}_0$和$\Sigma_0$为先验因子及其对应的协方差，$\Sigma_I$则为IMU测量的协方差。我们采用Levenberg-Marquardt优化器来求解这一非线性优化问题。优化器中会保留最新的N个状态变量，而对于超出窗口的状态变量则会执行Schur补边缘化操作[^35]。随后如图3(b)所示，将先验因子添加到窗口内的相关变量中。
### VI. 激光雷达建图
激光雷达建图利用高频IMU速率下的VIO位姿作为运动先验，来进行激光雷达点云去畸变以及扫描数据与地图的配准。将一次完整的激光雷达旋转所获取的点云记为$\chi$。在去畸变处理后$\chi$、[^22]，从[^26]中提取出包括锐边点和平面上的点在内的几何特征。随后配准过程基于当前扫描数据中的特征点与地图（所有以往的特征点）之间的匹配，通过最小化特征点形成的欧几里得距离残差来求解，这一过程如[^22]所述。
### A. 激光雷达扫描去畸变
由于激光雷达扫描中的各点时间戳并不一致，因此需要进行去畸变处理。将扫描过程中的任意时刻记为$t_{i}$。我们基于IMU速率下的VIO位姿，将所有点去畸变到扫描结束时刻$t_{k+1}$：
---
设$t_i$处的激光雷达点为$\mathbf{P}_i$，去畸变后的点为$\tilde{\mathbf{P}}_i$，则有：
$$ \tilde{\mathbf{P}}_{i}=(\mathbf{T}_{k+1}^{L})^{-1}\mathbf{T}_{i}^{L}\mathbf{P}_{i} $$
其中$\mathbf{T}_{k+1}^L$、$\mathbf{T}_i^L$是从最接近的IMU速率VIO位姿转换得到的激光雷达坐标系下的位姿。
### B. 扫描数据与地图的配准
来自去畸变扫描结果 $\tilde{\chi}$ 的特征点会被注册到地图中，以优化位于 $t_{k+1}$ 处的激光雷达测绘位姿，该位姿记为 $\mathbf{L}_{k+1}$。将 $\mathbf{L}_{k+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}_{trans}^L$为基于IMU速率的VIO位姿得到的相对变换。随后通过$\mathbf{L}_{k+1}^*$将所有去畸变特征点转换到世界坐标系下进行注册。
当前扫描中边缘特征点的残差$\mathbf{r}_E$，是指该点到地图中由两个最近边缘点所连直线之间的欧几里得距离；当前扫描中表面点的残差$\mathbf{r}_U$，是指该点到地图中由三个最近表面点所形成的平面区域之间的距离。[^22] 结合$\mathbf{L}_{k+1}^*$，我们可以将这两个残差重新表示为：
$$ f_{E}(E_{(c,i)}^{L},\mathbf{L}_{k+1}^{*})=\mathbf{r}_{E} $$
$$ f_{U}(U_{(c,i)}^{L},\mathbf{L}_{k+1}^{*})=\mathbf{r}_{U} $$
其中，$E_{(c,i)}^{L}$和$U_{(c,i)}^{L}$分别为第i个去畸变特征点在LiDAR坐标系下的三维位置。采用Levenberg-Marquardt优化器来求解这个由所有特征点的代价函数堆叠而成的非线性优化问题。
### 七、基于LiDAR的闭环检测
闭环检测对任何SLAM系统而言都至关重要，因为长时间运行会产生漂移现象。闭环检测的目标是通过执行全局位姿图优化来消除漂移，该优化过程会纳入闭环约束以及来自LiDAR建图的相对变换信息。为了更好地辅助LiDAR建图，修正后的LiDAR位姿会被实时反馈回去，以便将新扫描中的特征点注册到已访问过的地图中。我们提出在视觉词袋[^2]闭环检测和PnP闭环约束建模的基础上，增加ICP对齐步骤。该系统采用增量求解器iSAM2 [^36]来对全局位姿图进行优化，从而实现实时性能。
### A. 闭环检测
利用时间戳将立体图像与LiDAR扫描关联起来，分别将其记为关键图像和关键扫描。为防止出现错误的闭环检测结果，我们会将候选者限制在特定的时间阈值内。通过将关键图像与以往关键图像的词袋[^2]数据库进行比对来检测出闭环候选者。此外，我们还会将左侧关键图像的特征描述符与闭环候选者进行匹配，以过滤掉假阳性结果。
(a) Highbay
(b) Hallway
(c) Tunnel
(d) Huge Loop
(e) Outdoor
Fig. 5: Trajectories from VIL-SLAM and LOAM are shown on the left and maps generated by VIL-SLAM are shown on the right. Start(end) position is labeled with red triangle in the map and is the origin in the plot.
### B. 闭环约束
系统首先获取视觉闭环约束作为初始估计值。由于我们对视觉地标采用了无结构的表述方式，因此会对闭环候选集中的所有立体匹配特征执行三角测量，以确定它们的三维位置。这些特征与当前关键图像的关联由描述符匹配给出——
---
TABLE I: FDE (%) and MRE (m) TEST RESULTS
| Test | Total Length | FDE | MRE |
| VIL-SLAM | LOAM | VIL-SLAM | LOAM |
| Highbay | 118 | 0.08 | 0.56 | 0.08 | 0.22 |
| Hallway | 103 | 0.61 | 0.91 | 0.10 | 0.27 |
| Tunnel | 85 | 1.86 | $-^{1}$ | ✗ | ✗ |
| Huge Loop | 318 | 0.01 | - | 0.22 | 0.36 |
| Outdoor | 528 | 0.02 | 0.02 | ✗ | ✗ |
随后利用 EPNP 算法对视觉闭环约束进行评估 [^37]。为提高视觉闭环约束的精度，我们会对相应激光雷达关键扫描中的特征点执行 ICP 对齐操作。如果初始化不佳或点云数量较多，ICP 收敛所需的时间会更长，且会消耗更多计算资源。不过，视觉闭环约束能提供良好的初始化点，且 ICP 仅使用稀疏特征点（见第 VI 节），因此能更快收敛。
### C. 全局位姿图优化
全局位姿图的图形表示如图4所示。图中将所有可用的激光雷达建图位姿作为变量，这些变量受到激光雷达里程计因子和回环约束因子的约束，二者均为相对变换的测量值：$(\mathbf{L}_{u})^{-1}\mathbf{L}_{v}$，其中u和v分别代表扫描ID以及相关的位姿$\mathbf{L}_{u}$、$\mathbf{L}_{v}$。对于激光雷达里程计因子而言，u是前一个扫描ID；对于回环约束因子而言，u是作为回环找到的关键扫描ID。在这两种情况下，v都是当前扫描ID。在优化过程中，位姿至少以六自由度形式表示。为了实现实时性能，我们采用iSAM2 [^36]来逐步优化全局位姿图。
### D. 重定位
一旦找到真正的回环闭合候选点，激光雷达建图模块会缓存特征点（不将其注册到地图中），直到收到回环校正信息为止。该回环校正信息包含全局优化的轨迹。激光雷达建图模块会更新其地图，将缓存的特征点添加到地图中，然后恢复正常运行。我们能够实时更新地图是因为：(a)回环闭合具有实时性能；(b)稀疏特征地图占用的内存不多；(c)扫描与地图的配准速度足以跟上激光雷达的数据速率。
### VIII. 实验结果
我们评估了VIL-SLAM，并将其在自定义数据集上与目前最优秀的实时激光雷达基系统LOAM $^{2}$ [^22]进行了比较。我们没有使用KITTI里程计数据集 [^38]，因为其评估序列中没有VIO所需的惯性测量数据。此外，大多数KITTI序列并不具备挑战性，因此无法评估这些系统的鲁棒性，而这正是我们实验的主要关注点。我们还使用EuRoC MAV数据集 [^39]来评估立体视觉VIO子模块（VIL-VIO）。
$^{2}$这是我们能在网上找到的LOAM的最佳实现版本
(a) Highbay
(b) Hallway
(c) Huge Loop
Fig. 6: Map registration error of VIL-SLAM (right) and LOAM (left) comparing to the model. Errors above 0.3m are colored red for (a-b) and 0.5m for (c). Discontinuous red regions inside the blue and green are due to lack of the model caused by occlusions of the Faro scans.
### A. 平台与软件
我们搭建了一个平台（图1(a)），配备两台百万像素摄像头、一台16线激光雷达、一个400Hz的IMU以及一台4GHz、拥有4个物理核心的计算机。我们基于时间同步电路定制了一款微控制器，通过模拟GPS时间信号来同步摄像头、激光雷达、IMU和计算机。软件流程用C++实现，并带有ROS通信接口。我们使用GTSAM库 [^40]来构建VIO中的固定滞后平滑器。对于回环闭合，我们使用LibPointMatcher中的ICP模块 [^41]来对齐点云，用DBoW3 [^42]来构建视觉词典，并利用GTSAM中的iSAM2实现 [^36] [^40]来进行全局优化。
### B. 测试与结果
我们展示了五个典型环境下的测试结果，包括无特征走廊、杂乱的高棚环境、隧道以及室外环境。所有序列的数据采集起点和终点均相同。基于最终漂移误差（FDE）来评估里程计（激光雷达建图位姿）的性能。建图结果则利用Faro扫描仪作为基准，通过平均配准误差（MRE）来评估。我们首先将地图与模型（Faro扫描仪数据）对齐，然后
---
(b)
(c)
(d)
Fig. 7: (a) Map of the tunnel stitched using LIDAR mapping poses. (b) Map of the tunnel stitched using globally refined poses. Double image in (a) is mostly eliminated but not fully, because only one loop constraint is generated, not enough for a full correction. (c) Map of the hallway stitched using LIDAR mapping poses. (d) Map of the hallway stitched using globally refined poses. Double image in (c) is mostly eliminated. Walls are aligned with two loop constraints.
计算地图中的点与模型中最近点之间的欧几里得距离 [^43]。里程计的FDE和建图结果如表I所示，表现更好的结果以粗体显示。轨迹和剖面图如图5所示，地图对比结果如图6所示。
高顶仓库是一种开放式、结构复杂且功能丰富的室内仓储环境。然而，频繁出现的结构遮挡可能会对视觉前端和激光雷达特征提取模块构成挑战。VIL-SLAM与LOAM在这类环境中表现均较为出色。对于VIL-SLAM而言，激光雷达建图模块能将大部分扫描数据注册到地图中，从而大幅降低里程计误差。回环检测能够识别起始位置并闭合回环，最终生成的地图是基于全局优化后的位姿数据，其中绝大多数地图误差都低于0.15米。
走廊和隧道属于极具挑战性的测试环境，因为这类场景缺乏视觉特征，且激光雷达在行进方向上存在数据退化问题。LOAM在走廊环境中会累积较大误差，而在隧道测试中则主要因数据退化问题导致失败。在立体视觉惯性测量单元（VIL-VIO）的辅助下，VIL-SLAM成功通过了这两项测试。在走廊测试中，由于墙壁缺乏特征，视觉前端输出的可靠测量数据较少，导致惯性导航系统约束不足；这会造成地图出现偏差，表现为墙壁对齐错误，随后通过回环检测得以纠正，如图7(c-d)所示。当接近终点时，回环检测会两次识别出回环，从而将FDE降至0.05%，并生成更精确的地图。在隧道测试中，受数据退化问题影响，VIL-SLAM同样面临困难，行进方向上的误差也会逐渐累积；不过回环检测会在距终点约3米处识别出回环，将FDE降至0.08%，进而修正地图，如图7(a-b)所示。
大型回环测试综合了走廊和高顶仓库环境的挑战；此外，测试轨迹还需在穿过狭长走廊后重新进入高顶仓库。LOAM在重新进入高顶仓库后无法完成该测试，具体位置如图5(d)中的红色叉号标记处所示。我们认为这是因为走廊中积累的z方向误差过大，导致新扫描数据无法注册到原有的高顶仓库地图中。而VIL-SLAM成功通过了这项测试。
Fig. 8: Root mean square error of ATE for EoRoC Dataset.
在未触发回环检测的情况下，其里程计FDE仅为0.01%。VIL-SLAM具有很强的鲁棒性，能够在重新进入高顶仓库时成功将新扫描数据注册到原有地图中，从而实现这一结果。我们将VIL-SLAM基于里程计估计生成的地图，与LOAM失效前生成的地图进行对比，方框区域正是LOAM因累积误差而导致失效的部分。
户外测试的轨迹全长546米，包含一段缓坡；测试过程中观察到了行人和车辆，它们可能成为异常值。VIL-SLAM与LOAM在xy平面上的结果相近，但LOAM无法捕捉z方向的变化。此前的测试中也已发现LOAM在z方向上的不准确性。
总体而言，在两者均能完成测试的情况下，VIL-SLAM生成的地图结果更精确，FDE也更低；此外，LOAM无法通过的更具挑战性的环境，VIL-SLAM也能成功应对，且地图与里程计结果的质量都较好。
### C. EuRoC MAV数据集测试
VIL-VIO 对 VIL-SLAM 的鲁棒性和精确度有显著贡献。我们使用 EuRoC MAV 数据集 [^39] 来评估 VIO，评估指标为绝对轨迹误差（ATE），具体方法如 [^44] 所述。图 8³ 展示了 VIL-VIO 与三种最先进方法之间的对比结果。VIL-VIO 的结果是确定性的，是在配备 3.60GHz i7-4790 CPU 的台式机上实时获得的。其他方法的结果则取自实验中的最佳结果，相关实验见 [^7] 和 [^12]。VIL-VIO 在所有序列上的表现都十分出色，其精度与其他方法相当，这证明了它处理剧烈运动、光照变化、运动模糊以及无纹理区域的能力。
### 九、结论
VIL-SLAM 是一种先进的里程计与建图系统，旨在不同环境中长期稳定运行。当前的框架将 VIL-VIO 与激光雷达建图模块松散耦合。我们正将其扩展为紧密耦合的框架，以便利用激光雷达建图得到的精确位姿估计来校正惯性测量单元（IMU）的偏差。在回环闭合环节，ICP 细化算法会在扫描点云间的稀疏特征点上操作。我们认为，通过将扫描数据与地图进行匹配，能够获得更优的回环约束。
$^{3}$ 序列名称由前四个字母构成，难度级别则由最后一个字母表示（E：简单，M：中等，D：困难）。
---
## 参考文献
[^1]: C. Cadena, L. Carlone, H. Carrillo, Y. Latif, D. Scaramuzza, J. Neira, I. Reid, and J. J. Leonard, “Past, present, and future of simultaneous localization and mapping: Toward the robust-perception age,” IEEE Transactions on Robotics, vol. 32, pp. 1309–1332, Dec 2016.
[^2]: D. G’alvez-L’opez and J. D. Tard’os, “Bags of binary words for fast place recognition in image sequences,” IEEE Transactions on Robotics, vol. 28, pp. 1188–1197, October 2012.
[^3]: R. Dub, D. Dugas, E. Stumm, J. Nieto, R. Siegwart, and C. Cadena, “Segmatch: Segment based place recognition in 3d point clouds,” in 2017 IEEE International Conference on Robotics and Automation (ICRA), pp. 5266–5272, May 2017.
[^4]: P. J. Besl and N. D. McKay, “A method for registration of 3-d shapes,” IEEE Transactions on Pattern Analysis and Machine Intelligence, vol. 14, pp. 239–256, Feb 1992.
[^5]: T. Qin, P. Li, and S. Shen, “Vins-mono: A robust and versatile monocular visual-inertial state estimator,” IEEE Transactions on Robotics, vol. 34, pp. 1004–1020, Aug 2018.
[^6]: S. Leutenegger, S. Lynen, M. Bosse, R. Siegwart, and P. Furgale, "Keyframe-based visual-inertial odometry using nonlinear optimization," vol. 34, 02 2014.
[^7]: J. Hsiung, M. Hsiao, E. Westman, R. Valencia, and M. Kaess, "Information sparsification in visual-inertial odometry," in IEEE/RSJ Intl. Conf. on Intelligent Robots and Systems (IROS), (Madrid, Spain), Oct. 2018. To appear.
[^8]: J. Engel, V. Koltun, and D. Cremers, “Direct sparse odometry,” IEEE Transactions on Pattern Analysis and Machine Intelligence, vol. 40, pp. 611–625, March 2018.
[^9]: G. Klein and D. Murray, “Parallel tracking and mapping for small or workspaces,” in 2007 6th IEEE and ACM International Symposium on Mixed and Augmented Reality, pp. 225–234, Nov 2007.
[^10]: S. Lynen, M. W. Achtelik, S. Weiss, M. Chli, and R. Siegwart, “A robust and modular multi-sensor fusion approach applied to mavenavigation,” in 2013 IEEE/RSJ International Conference on Intelligent Robots and Systems, pp. 3923–3929, Nov 2013.
[^11]: S. Weiss, M. W. Achtelik, S. Lynen, M. Chli, and R. Siegwart, "Real-time onboard visual-inertial state estimation and self-calibration
“未知环境中的微型飞行器”，载于 2012 年 IEEE 国际机器人与自动化会议论文集，第 957–964 页，2012 年 5 月。
[^12]: K. Sun, K. Mohta, B. Pfrommer, M. Watterson, S. Liu, Y. Mulgaonkar, C. J. Taylor, and V. Kumar, “Robust stereo visual inertial odometry for fast autonomous flight,” IEEE Robotics and Automation Letters, vol. 3, pp. 965–972, April 2018.
[^13]: M. Quan, S. Piao, M. Tan, and S.-S. Huang, “Map-based visual-inertial monocular slam using inertial assisted kalman filter,” 09 2017.
[^14]: M. Bloesch, S. Omari, M. Hutter, and R. Siegwart, “Robust visual inertial odometry using a direct ekf-based approach,” pp. 298–304, Sept 2015.
[^15]: K. Wu, A. Ahmed, G. A. Georgiou, and S. I. Roumeliotis, “A square root inverse filter for efficient vision-aided inertial navigation on mobile devices,” 2015.
[^16]: J. Engel, J. Sturm, and D. Cremers, “Camera-based navigation of a low-cost quadrocopter,” in 2012 IEEE/RSJ International Conference on Intelligent Robots and Systems, pp. 2815–2821, Oct 2012.
[^17]: V. Indelman, S. Williams, M. Kaess, and F. Dellaert, “Information fusion in navigation systems via factor graph based incremental smoothing,” Robotics and Autonomous Systems, vol. 61, pp. 721–738, 2013.
[^18]: V. Usenko, J. Engel, J. Stckler, and D. Cremers, “Direct visual-inertial odometry with stereo cameras,” in 2016 IEEE International Conference on Robotics and Automation (ICRA), pp. 1885–1892, May 2016.
[^19]: S. Ceriani, C. Snchez, P. Taddei, E. Wolfart, and V. Sequeira, “Pose interpolation slam for large maps using moving 3d sensors,” in 2015 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), pp. 750–757, Sept 2015.
[^20]: M. Velas, M. Spanel, and A. Herout, “Collar line segments for fast odometry estimation from velodyne point clouds,” in 2016 IEEE International Conference on Robotics and Automation (ICRA), pp. 4486–4495, May 2016.
## [^21]: C. Wei, T. Wu, and H. Fu, "Plain-to-plain scan registration based on geometric distributions of points," in 2015 IEEE International Conference on Information and Automation, pp. 1194–1199, Aug 2015
---
[^22]: J. Zhang and S. Singh, “Loam: Lidar odometry and mapping in real-time,” 07 2014.
[^23]: J.-E. Deschaud, “Imls-slam: scan-to-model matching based on 3d data,” CoRR, vol. abs/1802.08633, 2018.
[^24]: D. Droeschel, J. Stckler, and S. Behnke, “Local multi-resolution representation for 6d motion estimation and mapping with a continuously rotating 3d laser scanner,” in 2014 IEEE International Conference on Robotics and Automation (ICRA), pp. 5221–5226, May 2014.
[^25]: J. A. Delmerico and D. Scaramuzza, “A benchmark comparison of monocular visual-inertial odometry algorithms for flying robots,” 2018.
[^26]: J. Zhang and S. Singh, “Laser-visual-inertial odometry and mapping with high robustness and low drift,” 08 2018.
[^27]: C. Forster, M. Pizzoli, and D. Scaramuzza, “Svo: Fast semi-direct monocular visual odometry,” in 2014 IEEE International Conference on Robotics and Automation (ICRA), pp. 15–22, May 2014.
[^28]: J.-Y. Bouguet, “Pyramidal implementation of the lucas kanade feature tracker description of the algorithm,” vol. 1, 01 2000.
[^29]: J. Shi and Tomasi, “Good features to track,” in 1994 Proceedings of IEEE Conference on Computer Vision and Pattern Recognition, pp. 593–600, June 1994.
[^30]: E. Rublee, V. Rabaud, K. Konolige, and G. Bradski, “Orb: An efficient alternative to sift or surf,” in 2011 International Conference on Computer Vision, pp. 2564–2571, Nov 2011.
[^31]: C. Forster, L. Carlone, F. Dellaert, and D. Scaramuzza, “Imu preintegration on manifold for efficient visual-inertial maximum-a-posterior estimation,” in Robotics: Science and Systems, 2015.
[^32]: L. Carlone, Z. Kira, C. Beall, V. Indelman, and F. Dellaert, “Eliminating conditionally independent sets in factor graphs: A unifying perspective based on smart factors,” in 2014 IEEE International Conference on Robotics and Automation (ICRA), pp. 4290–4297, May 2014.
[^33]: A. I. Mourikis and S. I. Roumeliotis, “A multi-state constraint kalman filter for vision-aided inertial navigation,” in Proceedings 2007 IEEE International Conference on Robotics and Automation, pp. 3565–3572, April 2007.
[^34]: S. L. Bowman, N. Atanasov, K. Daniilidis, and G. J. Pappas, “Proba-
“语义 SLAM 中的概率数据关联”，载于 2017 年 IEEE 国际机器人与自动化会议（ICRA）论文集，第 1722–1729 页，2017 年 5 月。
[^35]: G. Sibley, L. Matthies, and G. Sukhatme, “Sliding window filter with application to planetary landing,” Journal of Field Robotics, vol. 27, no. 5, pp. 587–608.
[^36]: M. Kaess, H. Johannsson, R. Roberts, V. Ila, J. Leonard, and F. Dellaert, “isam2: Incremental smoothing and mapping with fluid relinearization and incremental variable reordering,” in 2011 IEEE International Conference on Robotics and Automation, pp. 3281–3288, May 2011.
[^37]: V. Lepetit, F. Moreno-Noguer, and P. Fua, “Epnp: An accurate o(n) solution to the pnp problem,” Int. J. Comput. Vision, vol. 81, pp. 155–166, Feb. 2009.
[^38]: A. Geiger, P. Lenz, and R. Urtasun, “Are we ready for autonomous driving? the kitti vision benchmark suite,” in Conference on Computer Vision and Pattern Recognition (CVPR), 2012.
[^39]: M. Burri, J. Nikolic, P. Gohl, T. Schneider, J. Rehder, S. Omari, M. W. Achtelik, and R. Siegwart, “The euroc micro aerial vehicle datasets,” The International Journal of Robotics Research, vol. 35, no. 10, pp. 1157–1163, 2016.
[^40]: F. Dellaert, “Factor graphs and GTSAM: A hands-on introduction,” tech. rep., Georgia Tech, Sept. 2012.
[^41]: F. Pomerleau, F. Colas, R. Siegwart, and S. Magnenat, “Comparing ICP Variants on Real-World Data Sets,” Autonomous Robots, vol. 34, pp. 133–148, Feb. 2013.
[^42]: "DBoW3 dbow3." , 2017.
[^43]: "Cloud-to-Cloud Distance cloudcompare." .
[^44]: J. Sturm, N. Engelhard, F. Endres, W. Burgard, and D. Cremers, “A benchmark for the evaluation of rgb-d slam systems,” in 2012 IEEE/RSJ International Conference on Intelligent Robots and Systems, pp. 573–580, Oct 2012.