# DVL-SLAM：基于稀疏深度增强的直接视觉-LiDAR SLAM
申英植 $^{1}$ · 朴永相 $^{1}$ · 金亚永 $^{1}$
收稿日期：2018年6月11日 / 录用日期：2019年7月20日 / 在线发表日期：2019年8月6日 © Springer Science+Business Media, LLC，Springer Nature旗下公司，2019年
## 摘要
本文提出了一种直接视觉-LiDAR SLAM框架，该框架将激光雷达（LiDAR）的稀疏深度测量数据与单目相机数据相结合。文献中已有关于利用两种传感器模态之间的深度测量的研究，但大多采用基于关键帧的方法或利用密集深度图。当深度数据稀疏程度较高时，现有方法会暴露出局限性。本文的核心发现是：在视野较窄且深度数据稀疏的情况下，直接法更具鲁棒性。通过实现多个关键帧下各类测量数据的联合优化，即可直接利用稀疏深度信息。为确保实时性能，通过严格的边缘化操作保持滑动窗口内关键帧数量的恒定。通过交叉验证可知，即使在大规模建图场景下，回环闭合机制也能保证算法的鲁棒性。我们利用自主研发的便携式相机-LiDAR传感器系统以及KITTI数据集对所提方法进行了充分评估。评估过程中，通过将激光束数量从64个分别缩减为16个和8个，模拟了不同稀疏程度的LiDAR数据对算法性能的影响。实验结果表明，在稀疏深度测量条件下，所提方法在精度和鲁棒性方面均显著优于现有方法。
关键词：直接视觉SLAM · 相机-LiDAR · 稀疏深度
## 1 引言
过去二十年间，同步定位与地图构建（SLAM）技术在机器人研究领域受到了越来越多的关注。从单目相机到密集RGB-D相机，各类传感器系统的应用推动了该技术的发展。对于户外场景机器人系统而言，LiDAR与相机相结合的传感器系统作为一种实用的传感类型备受瞩目。该系统通过SLAM重建三维空间结构，从而为路径规划、避障及机器人操控等各类机器人任务提供支持。然而，如何利用LiDAR提供的相对稀疏的深度测量数据来提升视觉SLAM性能，目前尚未得到充分研究。
文献中的大多数野外机器人系统依赖多种传感器（如相机、LiDAR和雷达）来实现导航与感知功能。尤其是随着自动驾驶汽车商业化的推进，固态LiDAR等视野更窄、稀疏度更高的新型LiDAR产品相继问世，预计其需求增长将推动低成本LiDAR的大规模生产。LiDAR的深度测量数据能够消除视觉图像中的尺度歧义，从而补充图像的几何信息，进而完善SLAM中的视觉图像。然而，当LiDAR仅提供稀疏测量数据时，两种传感器视野内的共视区域会变小，这会阻碍利用LiDAR数据来增强视觉SLAM的相关关联操作。
近年来，基于单目相机的视觉SLAM领域取得了显著进展。传统的基于视觉特征的方法已经趋于成熟，从而形成了稳定的视觉SLAM算法。最近，直接法被引入，这类方法直接利用图像的亮度信息（Forster等人，2014年；Engel等人，2014年、2017年；Newcombe等人，2011b年；Kerl等人，2013b年；Steinbrücker等人，2011年）。这些方法在运动估计方面表现优异。
---
尽管直接法无需依赖视觉特征的对应关系，仍能实现出色的定位性能，但单目系统下的视觉SLAM仍面临一些重大挑战。当相机沿光轴移动时，视觉特征的像素位置几乎保持不变，因此只能估计旋转运动，而平移运动的估计则受尺度因素影响。为了克服单目系统的尺度歧义问题，这些方法不可避免地需要借助外部信息来初始化三维环境（Gauglitz等人，2012年；Civera等人，2008年；Forster等人，2014年）。最终，这种初始化的质量直接决定了与真实世界之间的尺度误差大小。
为克服单目系统的局限性，人们提出了将IMU测量数据融入视觉SLAM的方案。利用IMU可以获得度量级的尺度观测值，但由于缺乏深度测量能力，仍需进行可扩展的初始化。此外，在运动和环境特征难以观测或处于退化条件下时，深度信息的初始化也会面临困难（Yang和Shen，2017年；Qin等人，2018年）。另外，立体相机系统也可用于解决单目视觉系统的尺度歧义问题。不过，要从立体相机获得精确的深度测量值，需要进行精密校准。此外，要获取远距离的深度信息，必须采用基线长度较大的配置（Sibley等人，2007年）；而测量远距离物体的深度时，也需要基线长度较大的传感器配置。在立体相机中，视差误差会影响深度估计结果；且基线长度越大，两个相机之间的重叠区域就越小，可利用的信息量也随之减少（Pinggera等人，2014年）。激光雷达获得的深度测量误差相对较小，且不像立体相机那样受外部校准质量的影响。单独使用立体相机或激光雷达来实现度量级SLAM，已受到广泛关注。
近期，基于学习的方法也取得了进展，视觉SLAM算法中引入了基于深度学习的深度估计技术，以解决单目相机的固有难题（Wang等人，2017年；Tateno等人，2017年；Yang等人，2018年）。虽然基于学习的深度预测能生成密集的深度图，但随着深度估计范围的扩大，误差也会随之增大，在米级范围内仍会存在显著误差（Yang等人，2018年；Kuznietsov等人，2017年）。此外，要在SLAM应用中部署这些模型，需要预先训练好的深度预测模型；若要将模型直接应用于内部参数不同的相机，还需额外调整。目前这些方法仅在驾驶场景或室内环境中进行了评估，还需进一步研究它们在不同环境和平台上的通用性（Yang等人，2018年）。基于此，我们旨在将激光雷达高精度深度测量能力与直接视觉SLAM的运动估计相结合。
50米
(a)
(b)
(c)
Fig. 1 A sensor system combined with camera and LiDAR for motion estimation and 3D mapping. a A camera-LiDAR sensor system that acquires the image and sparse depth measurement. The data acquired by the sensor is projected onto the image as shown in b. Using this sensor configuration, the proposed method performs precise pose estimation and 3D mapping. The points are color-coded with respect to the scene depth. c Estimated trajectory and 3D reconstruction result
本文提出了一种视觉SLAM系统$^{1}$，该系统将激光雷达的稀疏深度测量数据与相机图像的强度信息相结合，并采用无需视觉特征的直接法。需要注意的是，如图1所示，激光雷达仅在图像的有限区域内能提供稀疏深度测量数据。尽管这些稀疏测量数据可以与视觉特征相关联，但由于模糊图像中缺乏有意义的特征，其效用会大打折扣。因此，我们得出结论：直接法在处理与图像配对的稀疏深度测量数据时十分有效。此外，已知直接SLAM在低分辨率相机中能提供更精确的运动估计结果（Engel等人，2017年）。不过，稀疏3D激光雷达的窄视场范围仍是一个难题——它仅允许在部分图像区域内实现深度数据与图像的匹配。为克服这一限制，我们将相邻帧的深度信息整合到关键帧中，并利用多个关键帧进行运动估计。我们还通过回环检测与校正技术，确保在大规模场景下获得无漂移的建图结果。
本文针对稀疏深度条件下的相机-激光雷达系统，提出了一种直接视觉SLAM方法。其主要
---
所提出方法的特点可归纳如下：
- 当激光雷达的稀疏深度数据仅在窄视场范围内可用时，我们提出了相应的直接视觉SLAM方法。由于稀疏测量数据直接在图像域中被利用，因此无需依赖视觉特征。
- 基于窗口的优化算法克服了由激光雷达数据稀疏性以及异构传感器视场范围差异带来的限制。
- 在大规模环境中，为确保全局地图的一致性，采用了回环闭合技术。我们通过对接候选回环进行交叉验证，从而提供稳健的回环约束。
- 我们利用自主研发的传感器系统以及包含多种场景的公共数据集开展了大量实际实验，对所提方法进行了全面评估。
本文其余部分的结构安排如下：第2节探讨了从单相机到具备深度感知能力的视觉SLAM相关研究；第3节概述了所提出的直接视觉-激光雷达SLAM方法；第4节详细阐述了所提方法的原理；第5节展示了实验结果，最后我们对本研究结论进行了总结。
## 2 相关工作
### 2.1 基于特征的视觉SLAM
视觉SLAM是一种利用连续拍摄的相机图像来进行运动估计与建图的技术。视觉SLAM领域已有大量研究成果，其中大多数集中于利用角点类视觉特征，如尺度不变特征变换（SIFT）、加速稳健特征（SURF）以及定向FAST和旋转BRIEF（ORB）等（Klein和Murray，2007年；Kitt等人，2010年；Strasdat等人，2011年；Mur-Artal等人，2015年）。
Davison等人（2007年）仅使用单目相机，就在房间大小的环境中成功实现了基于扩展卡尔曼滤波（EKF）的实时视觉SLAM。不过，这类基于滤波的方法计算效率较低，因为它们需要在连续帧中执行运算，且积累的线性化误差比基于束调整（BA）的方法更大。另一方面，基于关键帧的方法仅利用选定的帧来构建地图，虽然计算复杂度稍高，但能通过BA优化实现精确的结果。Klein和Murray（2007年）提出的并行跟踪与建图（PTAM）是最具代表性的基于关键帧的视觉SLAM系统。这是首个将视觉SLAM系统拆分为用于相机跟踪和建图的并行线程的研究。他们已在小型环境中验证了该系统的出色性能，旨在应用于增强现实领域。Mur-Artal等人（2015年）提出了一种更具代表性的基于特征的SLAM方法——ORB-SLAM。他们在跟踪与建图框架中加入了循环线程，以提升其在大规模环境中的鲁棒性和精确度。Gálvez-López和Tardós（2012年）以及Strasdat等人（2011年）提出的方法被用于解决循环检测和尺度漂移问题。不过，这类基于特征的方法的缺点是：当环境存在简单重复的图案或无特征时，它们很难找到对应的特征点。最终，这类环境条件会导致运动估计失败。
### 2.2 直接法视觉SLAM
近年来，出现了无需依赖具体特征对应关系仍能展现卓越性能的直接法。直接法并不采用特征对应点的重投影误差，而是将像素的亮度差异视为光度误差模型（Engel等人，2014年、2017年；Forster等人，2014年；Newcombe等人，2011b；Pizzoli等人，2014年）。不过，要使优化过程收敛，需要良好的初始化条件；若要实时估计运动并生成全深度图，还需借助GPU进行大量计算（Newcombe等人，2011b；Pizzoli等人，2014年）。为了减轻这一计算负担，Forster等人（2014年）和Engel等人（2014年）提出了半稠密方法。他们利用一定数量的图像块，或仅选取图像梯度幅度超过阈值的区域，以降低计算复杂度，从而确保直接法能在CPU上实时运行。Engel等人（2017年）提出了一种最先进的直接法视觉里程计——直接稀疏里程计（DSO）。DSO会针对所有相关参数（如相机内参、运动参数及逆深度值）进行基于窗口的优化。为实现实时性能并减少尺度漂移，他们维持固定数量的点和帧，且无需闭环检测。不过，与所有单目SLAM系统一样，尺度模糊问题仍是一大挑战，且其性能往往取决于用于初始化深度的引导过程。
### 2.3 RGB-D SLAM
RGB-D相机在视觉SLAM领域受到了广泛关注（Newcombe等人，2011a；Kerl等人，2013a、b；Whelan等人，2015年）。这类相机不仅能提供RGB图像，还能在有限范围内为每个像素提供稠密的深度测量数据。Endres等人（2014年）将给定RGB图像和深度信息下的运动估计问题转化为三维特征匹配问题。Huang等人……
---
(2011年)的研究追踪了与深度相关的视觉特征，并将其用于运动估计。Henry等人（2012年）将基于视觉特征的方法与迭代最近点（ICP）算法相结合。他们利用带有深度信息的视觉特征来获得初始对齐结果，随后联合优化二维和三维对齐误差。Newcombe等人（2011a年）提出了KinectFusion算法，该算法采用场景的隐式表面模型，即截断有向距离函数（TSDF）。这种密集的场景模型被用于结合ICP算法进行运动估计。此外，也有研究应用直接法（Steinbrücker等人，2011年；Kerl等人，2013a、b年）。这些方法通过最小化光度误差来确定最优刚体运动，从而实现两帧连续RGB-D图像之间的配准。Whelan等人（2015年）提出了一种运动估计方法，该方法结合了基于TSDF体积的ICP算法与光度误差计算。这些使用Kinect等RGB-D相机的方法需要密集的深度测量数据来支持视觉SLAM。由于此类传感器在开放环境中的近距离测量范围有限（即最多5米），因此其在开放环境中的应用受到限制；为克服这一局限，通常会采用三角测量技术（Concha和Civera，2017年）。
### 2.4 基于激光雷达的SLAM
由于测距精度高且不受感知混淆现象的影响，激光雷达传感器被广泛应用于众多计算机视觉和机器人学研究领域。激光雷达主要采用基于ICP的方法，这类经典的扫描匹配方法在最新研究中不断演进，重点针对密集点云数据以实现高效利用（Segal等人，2009年；Serafin和Grisetti，2015年）。然而，当低成本3D激光雷达提供稀疏测量数据时，由于垂直方向测量值稀少，这类方法无法完成三维建图任务。例如，16线3D激光雷达在垂直方向上有16个扫描环，每个环可提供数千个点。由于垂直方向的稀疏性，俯仰运动和平移运动的估计尤为困难，进而导致三维建图失败。Zhang和Singh（2017年）提出了一种运动估计方法，该方法从激光雷达点云中提取尖锐边缘和平面表面特征。通过将这种基于几何特征的运动估计方法拓展到地图与扫描匹配中，他们取得了较高的精度。不过，在没有几何特征的环境中，该方法的性能无法得到保证。
另一种基于激光雷达的SLAM将扫描式2D激光雷达与惯性测量单元（IMU）相结合（Bosse等人，2012年；Park等人，2018年）。这些传感器系统属于异步工作模式且运行速率较高，加之激光雷达存在运动畸变现象，因此三维建图更具挑战性。Bosse等人（2012年）通过将连续时间轨迹模型应用于SLAM，成功解决了这一难题。不过，由于全局批量轨迹优化的存在，连续时间SLAM在长时间运行过程中会面临计算复杂度高的问题。Park等人（2018年）提出了一种以地图为中心的激光雷达融合方法。基于曲面元素的融合技术和地图变形处理不仅能保持全局一致性，还能降低地图噪声。通过将连续时间轨迹优化限制在局部建图范围内，该方法还能确保实时性能。
### 2.5 视觉-激光雷达SLAM
陆（2016）提出了一种名为VELO的视觉-LiDAR SLAM方法，该方法利用多摄像头和LiDAR进行定位与建图。陆将稀疏视觉里程计数据与LiDAR扫描匹配结果紧密耦合到一个目标函数中，从而实现了全局一致的3D建图，包括闭环检测功能。该方法通过结合LiDAR提供的ICP约束，以及连续立体图像中的3D-3D、3D-2D和2D-2D视觉约束，来完成帧间运动估计。不过，要实现视觉特征匹配和ICP计算，就需要使用开销较大的基于树的数据结构。此外，由于该方法没有考虑传感器运动时LiDAR点云的运动畸变问题，因此其性能无法得到保证。
张等人（2017）提出了一种名为深度增强单目里程计（DEMO）的方法，该方法能够处理来自LiDAR的稀疏深度测量数据。DEMO所采用的传感器配置与我们类似（即单目摄像头搭配LiDAR）。该方法将视觉特征与深度测量数据相关联，并利用它们来估算摄像机的运动。格拉特等人（2018）则提出了一种结合基于特征的视觉里程计方法与高精度深度测量的方案，以实现精确的车辆运动估计。我们通过将跟踪到的视觉特征与深度测量数据进行插值，并利用基于关键帧的束调整技术来提高局部精度，从而实现度量尺度的运动估计。不过，为了实现视觉特征与深度测量数据的关联，该方法采用了成本较高的64线LiDAR；而当使用更稀疏的3D LiDAR时，其精度也无法得到保障。我们所提出的方法与他们的方法类似，都是通过将单目摄像头与LiDAR结合来克服单目摄像头的局限性；但我们的重点在于采用直接法来处理深度稀疏性问题，而无需依赖视觉特征。我们与其他使用密集深度数据的方法（Newcombe等人，2011a；Kerl等人，2013b；Steinbrücker等人，2011；Whelan等人，2015）也有所不同，因为我们直接在单个摄像头上处理LiDAR的稀疏测量数据。这类传感器配置适用于大多数野外机器人系统，但由于异构传感器之间的差异，它们尚未得到深入研究。据我们所知，我们的方法是首个将LiDAR的稀疏测量数据应用于直接法的方案。
---
Fig. 2 Block diagram of the direct visual-LiDAR SLAM. The proposed method consists of the front-end for data association and back-end solving for the global pose-graph optimization including loop constraint
## 3 算法概述
与我们研究的通用SLAM方法类似，所提出的方法也由前端和后端组成。算法的输入为序列化的图像数据和序列化的LiDAR扫描数据。对于包含稀疏深度的图像，仅利用图像来进行跟踪过程，这样我们就可以在初始跟踪完成后补偿由运动引起的LiDAR数据畸变。随后，该模块采用点采样策略来实现快速且稳健的运动估计；通过减少算法中使用的点数，从而将计算复杂度控制在恒定水平。如图2所示，前端侧重于利用窗口优化和数据关联技术来实现精确的运动估计，以完成闭环检测。LiDAR能够提供最远达100米的精确深度测量数据。由于图像中的视差误差无法保证远距离深度测量的准确性，且相关计算开销较大，因此我们只能对多个关键帧的姿态进行优化。前端传来的相关数据会被用于后端的全局位姿图优化。下面将对整个系统的各个组成部分进行详细说明：
视觉里程计（VO）模块负责在缺乏视觉特征的情况下进行快速运动估计的跟踪过程。初始运动仅通过少量采样点来估算，这些采样点直接通过图像域中的强度信息进行跟踪。与间接方法不同，这种跟踪过程无需提取和匹配类似角点的特征，且能以较快的速度进行。此外，仅使用与激光雷达相关的点便可省去用于估计深度的三角测量步骤。这种简洁性对配备摄像头和激光雷达的各种机器人系统而言十分有益。与其他直接法类似，该过程采用了图像金字塔和恒定运动模型（Forster等人，2014年；Engel等人，2017年；Kerl等人，2013b年）。
激光雷达去畸变：当通过跟踪过程获得初始运动数据并获取到激光雷达扫描数据时，便可通过运动插值来补偿这种畸变。对于VLP-16传感器而言，由于扫描数据是以数据包为单位获取的，因此我们基于相机帧的姿态，利用插值运动来补偿激光雷达点的畸变。在执行激光雷达点去畸变操作时，会将其与最近的相机帧相关联。
窗口化优化：一旦跟踪过程成功完成，当前帧就会被加入滑动窗口，随后进行优化以提高窗口内的局部精度。这种优化能使跟踪过程得到的运动结果保持较低的漂移。超出用户设定数值（$N_{w}$）的最旧帧会被边缘化处理。随后，这些被边缘化的关键帧会被送入后端位姿图中。
地点识别：前端的最终组件是地点识别模块。该模块会确认被边缘化的关键帧所处位置是否已被重新访问，随后整合相邻帧的深度信息来进行双视图对齐。这种深度信息整合能够解决两个候选帧之间出现大幅位移时的歧义问题。
位姿图优化器：后端模块会对全局位姿图进行优化，其中涵盖当前运动以及前端提供的闭环约束。我们采用了g2o（Kuemmerle等人，2011年）工具，利用其增量式稀疏线性代数功能来实现优化的在线运行。
## 4 直接视觉-激光雷达SLAM
### 4.1 符号说明
帧$\mathcal{F}_i$由来自相机-激光雷达传感器系统的图像$\mathcal{I}_i$和点云$\mathcal{P}_i$组成。每个点$\mathbf{p}_k = (x, y, z)^\top \in$
---
$\mathbb{R}^3$中的$\mathcal{P}_i$会通过投影函数$\mathbf{u}_k = (u, v) \in \mathbb{R}^2$在图像平面的坐标中表现为像素$\pi(\mathbf{p}_k)$。如果跟踪过程已经完成，那么每帧都会对应一个在世界坐标系下的变换矩阵$\mathbf{T}_{wi} \in SE(3)$形式的位姿。
本文中，李群代数元素$\xi \in \mathbb{R}^6$被用作$SE(3)$上位姿优化的最小表示形式，其中元素$\xi = [w, v]$同时代表角速度和线速度向量。李群代数元素通过指数映射映射到$SE(3)$，其逆运算则通过对数映射获得。这种关系可用于通过李群代数得到的增量来更新位姿矩阵：$se(3) \boxplus SE(3) \to SE(3)$。
$$ \mathbf{\xi}\boxplus\mathbf{T}:=\exp(\hat{\xi})\mathbf{T} $$
在帧$\mathcal{F}_i$与帧$\mathcal{F}_j$之间进行跟踪时，相对位姿变换可用$\mathbf{T}_{ji} = \mathbf{T}_{wj}^{-1} \mathbf{T}_{wi}$表示，这种关系可用于如下转换点的坐标：
$$ \mathbf{p}_{~j}^{^{\prime}}=\mathbf{T}_{ji}\mathbf{p}_{i}=\begin{bmatrix}\mathbf{R}_{ji}~\mathbf{t}_{ji}\\ \mathbf{0}~\mathbf{1}\end{bmatrix}\mathbf{p}_{i} $$
其中 $\mathbf{p}_i$ 是框架 $\mathcal{F}_i$ 上的一个点，而 $\mathbf{p}'_j$ 是框架 $\mathcal{F}_j$ 上的一个观测点。$\mathbf{R}_{ji} \in SO(3)$ 和 $\mathbf{t}_{ji} \in \mathbb{R}^3$ 分别是旋转矩阵和平移向量。
广泛采用的针孔相机模型用于将框架 $\mathbf{p}'_j$ 中变换后的点 $\mathcal{F}_i$ 投影到框架 $\mathcal{F}_j$ 上。设 $\pi(\cdot)$ 为投影到图像平面的函数：
$$ \pi(\mathbf{p}^{\prime}_{j})=\begin{bmatrix}f_{x}&0&c_{x}\\ 0&f_{y}&c_{y}\end{bmatrix}\begin{bmatrix}\frac{x^{\prime}_{j}}{z^{\prime}_{j}}\\ \frac{y^{\prime}_{j}}{z^{\prime}_{j}}\\ 1\end{bmatrix} $$
其中 $f_{x}$ 和 $f_{y}$ 是焦距，而 $c_{x}$ 和 $c_{y}$ 是主点。
### 4.2 帧管理
#### 4.2.1 帧生成
所提出的方法将激光雷达的点及其关联图像定义为一个帧。给定下一个序列中的相机图像，我们在优化6自由度运动的同时进行跟踪。为了将两个传感器的数据关联为一个帧，必须已知两个传感器之间的相对位置。相机的工作频率较高（30Hz），而3D激光雷达的工作频率较低（10Hz）。所提出的方法基于跟踪过程中获得的相机图像中的位姿，来补偿由运动引起的激光雷达点的畸变。给定最接近的两个相机位姿 $\mathbf{T}_k$ 和 $\mathbf{T}_{k+1}$，线性插值位姿 $\mathbf{T}_\tau$ 定义如下：
$$ \mathbf{T}_{\tau}=\mathbf{T}_{k}\exp(\lambda\xi_{k,k+1}) $$
其中它们的时间戳满足 $\tau_k < \tau < \tau_{k+1}$。$\xi_{k,k+1} = \log(\mathbf{T}_k^{-1}\mathbf{T}_{k+1})$ 用于以插值比例 $T_\tau$ 插值位姿 $\lambda = (\tau - \tau_k)/((\tau_{k+1} - \tau_k)$。最终，基于插值位姿，对激光雷达扫描点进行补偿，并将其关联到最近的相机帧。我们的系统会记录激光穿过相机中心的时间，并使用最接近该时间的图像来关联相机帧和激光雷达。一旦两个传感器数据被关联起来，激光雷达坐标下的3D点就会被转换到相机坐标下，从而使输入帧的图像和3D点能够置于相机坐标中。
#### 4.2.2 关键帧管理
滑动窗口会维持固定数量的关键帧，以确保视觉里程计模块的处理时间恒定。滑动窗口中最新的关键帧由输入帧的当前图像进行跟踪。在里程计模块中使用滑动窗口的策略能够提高局部精度并降低计算负载。在管理滑动窗口中的关键帧时，会遵循以下两个准则：
– 当跟踪过程成功时，可以通过检查可投影点的比例来向窗口中添加新关键帧。由于相机和激光雷达的视场角是固定的，可投影点的减少意味着信息的丢失。我们通过保持一定比例的可投影点来防止这种信息丢失。任何比例低于 $\gamma_{kf}$ 的帧会立即被登记到滑动窗口中。
视角变化可以通过可投影点的比例来解决，但光照变化的影响仍然存在。即使没有检测到视角变化，我们也会每次使用公式(5)检查亮度变化后再添加关键帧。
#### 4.2.3 关键帧边缘化
为了减轻窗口优化的计算负担，我们的视觉里程计模块通过从滑动窗口中边缘化最旧的关键帧来维持帧数据。跟踪过程结束后，每当添加新关键帧时，就会按照以下策略执行边缘化操作：
---
50m
(a)
(b)
Fig. 3 Salient point selection. a The entire LiDAR measurement projected onto the image without selection. b The selected salient points. For computational efficiency, only salient points are used for motion estimation
该窗口至少会保留两个关键帧。如果可见点的比例超过 $\gamma_{kf}$，就会向滑动窗口中添加一个新帧。
– 如果关键帧数量超过了用户定义的窗口大小（$N_{w}$），就会添加一个新的关键帧，同时把最旧的关键帧剔除。
我们利用舒尔补来剔除最旧的关键帧。当添加新关键帧时，剩余的Hessian矩阵块可以被优化器反复使用。被剔除的关键帧虽然不再属于窗口，但会作为全局位姿图的节点添加到图中，并在建图阶段被使用。基于单摄像头的直接法会在跟踪过程结束后同时优化点和位姿（Engel等人，2017年）。由于激光雷达的距离测量相当精确，为了提升计算效率，我们的方法仅在窗口内估计位姿。
#### 4.2.4 点管理
无需跟踪激光雷达采集的所有点。激光雷达的测量数据可能包含纹理较少区域的点，这类点在图像上难以被跟踪。此外，由于与密集的RGB-D传感器相比，激光雷达的重叠区域更小，因此采用类似DSO中模式4那样的稀疏图案块，比逐像素跟踪更能保证鲁棒性；使用这些稀疏图案块能在仅需少量像素的情况下保证足够的性能（Engel等人，2017年）。不过，这种基于图案块的跟踪需要妥善管理，因为计算复杂度会随着图案块尺寸的增大而升高。
尽管激光雷达的测量数据本身已经很稀疏，但为了进一步提升计算效率，我们通过显著点选择来获取更少但更具信息量的测量数据。为了采样显著点，我们重点关注两点：(i) 球坐标系下激光雷达数据的空间分布；(ii) 图像的梯度。我们将方位角和仰角各划分为2度的区间，以此作为空间分箱依据。随后，选取图像中梯度最大的点作为显著点。由于这些点是从间隔恒定的空间分箱中选取的，因此它们的空间分布十分均匀。如果某点的梯度过小，由于信息量低，该点会被排除。图3展示了所选显著点的示例。激光雷达测量数据在图像中分布均匀，因此更为稀疏。
### 4.3 跟踪
所提出的方法会在获取新帧时执行跟踪，以得到相机位姿的初始估计值。受稀疏直接法的启发（Engel等人，2017年），我们专注于快速且鲁棒的运动估计，无需处理视觉特征。
为方便表述，最新的关键帧被称为参考帧 $\mathcal{F}_i$，当前帧则被称为目标帧 $\mathcal{F}_j$。为了估计两帧之间的相对变换，需将参考帧 $\mathbf{P}_i$ 中的点 $\mathcal{F}_i$ 投影到目标帧 $\mathcal{F}_j$ 中，并将稀疏图案 $\Omega$ 对应区域内的强度差定义为光度误差。尽管这类基于图案块的方法比逐像素方法需要更多的计算量，但它们能有效抵御运动模糊和噪声等图像退化问题。此外，我们采用了参数为a和b的仿射光照模型来应对光照变化（Jin等人，2001年）。方程(9)中仿射光照模型的参数a和b可由Engel等人（2015年）在每次迭代中以封闭形式求解。光度残差的定义如下：
$$ r(\mathbf{p}_{i})=\mathcal{I}_{j}(\pi(\mathbf{T}_{j i}\mathbf{p}_{i}))-(a\mathcal{I}_{i}(\pi(\mathbf{p}_{i}))+b). $$
最后，利用帧 $\mathbf{P}_i$ 中各点 $\mathcal{F}_i$ 的残差来定义跟踪的目标函数，具体如下：
$$ \underset{\mathbf{T}_{ji}}{\mathrm{argmin}}\frac{1}{2}\sum_{\mathbf{p}_{k}\in\mathbf{P}_{i}}\sum_{i\in\Omega\left(\mathbf{p}_{k}\right)}\rho(r(\mathbf{p}_{i}))\left(r(\mathbf{p}_{i})^{2}\right), $$
---
其中，函数 $\rho(\cdot)$ 是基于t分布的鲁棒加权函数，Lange等人（1989）对此有相关论述。
$$ \rho(r)=\frac{\nu+1}{\nu+(\frac{r-\mu_{r}}{\sigma_{r}})^{2}} $$
此处，$\nu$ 是t分布的自由度，按照Kerl等人（2013b）以及Li和Lee（2016）的研究，该值被设为5。$\mu_{r}$ 和 $\sigma_{r}$ 分别代表所有强度残差的均值和标准差。为了应对因自动曝光引起的光照变化导致两帧间光度一致性被破坏的情况，我们采用中位数作为 $\mu_{r}$，采用绝对中位差作为 $\sigma_{r}$。
$$ \begin{aligned}\mu_{r}&=median\left(\{r_{i}\}_{i=1}^{N_{r}}\right)\\\sigma_{r}&=1.4826 median(\{|r_{i}-\mu_{r}|\}_{i=1}^{N_{r}})\end{aligned} $$
其中 $r_{i}$ 为残差值，i为其索引，$N_{r}$ 为残差的数量。当均值受异常值影响而产生偏差时，这种中位数计算方法能有效解决问题。
为了求解方程(6)中描述的跟踪过程的非线性目标函数，我们采用了高斯-牛顿法，并在每次迭代中重新计算权重。从初始相对位姿 $\mathbf{T}_{ji}$ 出发，求解得到的增量 $\xi \in se(3)$ 通过 $\boxplus$ 运算符对其进行逐步更新。为了提高计算效率，我们采用了逆合成方法：在参考帧中求解 $\xi$，而非在目标帧中求解，同时更新参考帧中的 $\mathbf{T}_{ji}$。此时残差可表示为如下形式：
$$ r(\mathbf{p}_{i})=\mathcal{I}_{j}(\pi(\mathbf{T}_{j i}\mathbf{p}_{i}))-(a\mathcal{I}_{i}(\pi(\mathbf{T}(\xi)\mathbf{p}_{i}))+b). $$
为了找到最优增量，我们在 $\xi = 0$ 处对导数进行求解。由于逆合成方法在迭代过程中会在同一点进行线性化，因此我们可以预先计算雅可比矩阵，从而显著提升计算速度。增量 $\xi$ 可利用方程(10)在目标帧中计算得出：
$$ \begin{array}{r}{\mathbf{\delta\xi}=-(\mathbf{J}^{T}\mathbf{W}\mathbf{J})^{-1}\mathbf{J}^{T}\mathbf{W}\mathbf{r},}\end{array} $$
其中J为雅可比矩阵，W为t分布权重矩阵，r为堆叠后的残差。一旦得到 $\xi$，即可按如下方式执行更新步骤。
$$ \mathbf{T}_{ji}\leftarrow\mathbf{T}_{ji}\exp(-\hat{\xi}) $$
此外，我们还利用由粗到精的策略以及恒定速度模型来处理局部极小值和大位移情况。
Fig. 4 Illustration of window-based optimization. The window consists of $N_{w}^{th}$ keyframes, and each keyframe has its image and pointcloud. Gray dots represent the pointcloud and rectangles represent images. A point $p_{k}$ in keyframe is projected into all keyframes with covisibility. A photometric residuals are then calculated between the patch at the existing point and the other patch at a projected point
### 4.4 基于窗口的优化
本节详细介绍了通过基于窗口的优化实现位姿精修的过程。如果当前帧满足关键帧创建条件，就会对多个关键帧执行基于滑动窗口的精修操作。与跟踪模块类似，此处也采用了光度误差模型，唯一区别在于该误差模型被应用于多帧数据。在多帧中进行精修可以弥补信息不足的问题，比如单帧测量存在偏差的情况。不过，随着帧数的增加，这种基于滑动窗口的方法需要更多的计算量。为确保计算量保持恒定，我们根据测量的稀疏程度，将帧数固定在3到10帧之间。
窗口 $(\mathcal{F}_i)$ 中的每个关键帧 $(\mathbf{W}_{\mathcal{F}})$ 都有其关联的点 $(\mathbf{P}_i)$ 和位姿 $(\mathbf{T}_i)$。如图 4 所示，点 $(\mathbf{p}_k)$ 根据窗口内的共视关系被投影到其他关键帧中。该过程构建了一个以点为潜变量的位姿图；我们希望优化与这些关键帧相关联的位姿参数 $\boldsymbol{\Xi} = \{\mathbf{T}_0, \ldots, \mathbf{T}_{N_W}\}$。由于 LiDAR 的距离测量相当精确，我们仅对关键帧的位姿进行优化。为了求解该优化问题，我们在多帧中定义光度误差，并如上一节所述使用稀疏模式 $\Omega$ 的面块。
$$ \underset{\Xi}{\mathrm{a r g m i n}}\frac{1}{2}\sum_{\mathcal{F}_{w}\in\mathbf{W}_{\mathcal{F}}}\sum_{\substack{\mathbf{p}_{k}\in\mathbf{P_{i}}}\atop i\in\Omega(\mathbf{p}_{k})}w(r(\mathbf{p}_{k}))\left(r(\mathbf{p}_{k})^{2}\right) $$
与公式(6)不同，点测量的误差会出现在多个帧中。为了增强鲁棒性，我们采用了t分布权重$w(\cdot)$。
### 4.5 回环闭合
尽管利用滑动窗口技术能够实现低漂移的里程计估计，但误差仍会逐渐累积，进而破坏大规模环境中轨迹的一致性。这一问题依然存在。
---
为缓解这一问题，有必要通过引入回环闭合来优化整个轨迹。为实现这一任务，我们使用全局位姿图。一旦通过地点识别模块发现重新访问的区域，回环闭合模块就会向位姿图添加约束并进行优化。
#### 4.5.1 地点识别
地点识别模块为重新观测到的区域提供数据关联，从而在大规模环境中获得一致的地图。在我们的框架中，地点识别模块依赖于Gálvez-López和Tardós（2012）提出的DBoW算法。每当视觉里程计模块提供新的关键帧时，该帧会被表示为词袋向量v。随后，关键帧数据库会计算如下归一化相似度得分，以找出查询向量$v_q$的匹配候选：
$$ \eta(\mathbf{v}_{c},\mathbf{v}_{q})=\frac{s(\mathbf{v}_{c},\mathbf{v}_{q})}{s(\mathbf{v}_{c},\mathbf{v}_{1})} $$
其中$\mathbf{v}_q$是先前添加的关键帧的词袋向量。为了计算相似度$s(\mathbf{v}_c, \mathbf{v}_q)$，会使用如下所示的$L_1$得分：
$$ s(\mathbf{v}_{c},\mathbf{v}_{q})=1-\frac{1}{2}\left|\frac{\mathbf{v}_{c}}{|\mathbf{v}_{c}|}-\frac{\mathbf{v}_{q}}{|\mathbf{v}_{q}|}\right| $$
在该模块中，我们使用了Mur-Artal等人（2015）预训练的词汇表。如果公式(13)中的得分低于阈值$\alpha = 0.7$，我们就会排除该候选；否则，我们将该关键帧视为回环闭合的潜在匹配候选。最后，我们通过检查候选关键帧与当前关键帧相邻关键帧之间的相似度得分，来验证其时间一致性。
#### 4.5.2 回环闭合
地点识别模块为位姿图的约束提供了匹配候选。我们并非一找到候选就执行回环闭合，而是通过双向比较相对位姿来进行交叉验证。给定一个新查询关键帧$\mathcal{F}_q$和一个候选关键帧$\mathcal{F}_c$，我们可以根据公式(6)估算它们的相对位姿。通过改变该公式中参考帧的角色，我们可以独立计算出两个相对位姿$\mathbf{T}_{qc}$和$\mathbf{T}_{cq}$。如果这两个相对位姿之间的差异很小，这种交叉验证就能达成高度一致，此时该相对位姿就会作为回环约束被加入位姿图中。
$$ \|\log\left(\mathbf{T}_{q c}\mathbf{T}_{c q}\right\|<\epsilon $$
Table 1 Parameters for keyframe management
| Description | KITTI | Indoor | Outdoor |
| $\gamma_{kf}$ | Ratio for keyframe creation | 0.8 | 0.8 | 0.8 |
| $t_{kf}$ | Time for keyframe creation (s) | 1.0 | 1.0 | 1.0 |
| $N_{w}$ | Window size (number of frames) | 5 | 7 | 5 |
该流程在基于特征的间接方法中，用随机样本一致性（RANSAC）算法取代了几何验证步骤。最后，对闭环边应用动态协方差缩放（DCS）技术，以防止因约束不可靠而导致的故障（Agarwal等人，2013年）。
## 5 实验评估
所提出的方法在摄像头和激光雷达可同时使用的多种环境中得到了验证。该方法既使用了公开可用的开源数据集，也借助了我们自行开发的便携式摄像-激光雷达传感器系统进行评估。所有实验均在一台配备英特尔酷睿i7-7700HQ处理器和8 GB内存的消费级笔记本电脑上完成。算法前端的参数详见表1。
### 5.1 在KITTI数据集上的视觉里程计评估
为了验证视觉里程计的性能，我们在排除闭环检测的情况下，将所提出的算法应用于KITTI里程计基准数据集（Geiger等人，2012年）。在KITTI数据集中，我们仅使用左侧摄像头采集的黑白图像以及激光雷达生成的点云来对所提方法加以验证。该数据集提供分辨率为$1230 \times 370$的图像，激光雷达拥有64束激光，其垂直视场角为$30^\circ$，水平视场角为$360^\circ$。第一组
Fig. 5 From a rich LiDAR data (64 laser beam from HDL64-E) in the original data, we simulate a sparse LiDAR by sampling laser. The points are color-coded by their depth value
---
(a)
(b)
(c)
(d)
(e)
(f)
(g)
(h)
Fig. 6 The sample result of the propose method in the KITTI dataset. From the left column, the result represents sequence 00, 01 and 05. The results represent a comparison 64 ray LiDAR as well as with simulated
(i)
16 and 8 ray LiDAR. Top row shows the estimated trajectory in a–c. The middle shows the translational error according to the path length in d–f, and g–i of the bottom shows the rotational error
实验重点在于评估所提视觉里程计的精度，并验证其在激光束稀疏情况下的鲁棒性。为此，如图5所示，将64束激光等分为16束和8束进行采样。本次评估不包含闭环检测，以便仅对比视觉里程计的运动估计性能。
KITTI里程计基准测试提供了11组带有真实值的训练集。图6展示了使用所提方法对训练集中多个序列的样本处理结果。图6的上行显示了估计出的轨迹，中间和下行则分别展示了相对于行驶距离的平移误差和旋转误差。结果涵盖了序列00、01和05的数据。需注意，序列01是在动态环境中高速行驶的车辆采集的高速公路数据集。与其他结果相比，序列01中8束激光对应的估计轨迹退化最为严重。由于序列01属于高速公路数据集，高度动态环境中的重复图案导致使用8束激光时误差较大。不过，所提方法在其他测试案例中成功应对了稀疏的距离测量问题。
所有训练集的定量对比结果见表2。表中列出了不同激光束稀疏程度下的结果。DEMO的结果直接引自Zhang等人（2017年）的研究。平移误差是将整个轨迹划分为100米、200米、$\cdots$、800米等段落后测得误差的平均值。在64束激光条件下，所提方法的平均平移误差为0.98%，性能优于DEMO。16束激光下的结果也为0.98%，与64束激光的结果相近。而在8束激光的情况下，除高速公路场景（序列01）外，估计精度仍较为理想。
图7展示了根据路径长度和车辆速度进行的误差分析结果。此外，我们还纳入了64束、16束和8束激光这三种情况的结果，以考察激光雷达的稀疏程度。图7上方为平移误差，下方为旋转误差。尽管累积路径长度增加，平移误差却降低了约10%。
---
Table 2 Comparison result of KITTI dataset
| Seq no. | Path len. (m) | Environment | DEMO (64 ray) | Ours (64 ray) | Ours (16 ray) | Ours (8 ray) |
| 0 | 3714 | Urban | 1.05 | 0.93 | 0.93 | 0.95 |
| 1 | 4268 | Highway | 1.87 | 1.47 | 1.09 | 7.25 |
| 2 | 5075 | Urban+country | 0.93 | 1.11 | 1.26 | 0.95 |
| 3 | 563 | Country | 0.99 | 0.92 | 0.98 | 0.94 |
| 4 | 397 | Country | 1.23 | 0.67 | 1.08 | 1.08 |
| 5 | 2223 | Urban | 1.04 | 0.82 | 0.77 | 0.77 |
| 6 | 1239 | Urban | 0.96 | 0.92 | 0.75 | 0.75 |
| 7 | 695 | Urban | 1.16 | 1.26 | 1.30 | 1.42 |
| 8 | 3225 | Urban+country | 1.24 | 1.32 | 1.30 | 1.30 |
| 9 | 1717 | Urban+country | 1.17 | 0.66 | 0.72 | 0.72 |
| 10 | 919 | Urban+country | 1.14 | 0.70 | 0.57 | 0.57 |
| avg | | | 1.16 | 0.98 | 0.98 | 1.52 |
该结果代表了不同分段长度下的平均平移误差。粗体部分表示最佳精度。
(a)
(b)
(c)
(d)
Fig. 7 Error comparison with respect to the sparsity of LiDAR in the whole training sequences. The first row shows the translational error and the second row shows the rotational error. The left column and the right column show the errors according to path length and vehicle speed.
如图7a所示，误差被稳定地限制在一定范围内，而图7b中旋转误差有所减小。由于车辆数据集中的大部分运动都是朝向前方的，因此整个路径上的平移误差相对较小。右侧列展示了根据车辆速度产生的误差情况。如图7d所示，车辆行驶速度越快，旋转误差反而越小。这种现象似乎源于非完整约束平台的特性。不过，如果测量数据过于稀疏（例如采用8线激光雷达），则如图7b、d所示，误差会随车辆速度增加而增大。
### 5.2 基于自研传感器系统的评估
我们采用图1a中所示的自研相机-激光雷达传感器系统对所提出的方法进行了评估。该传感器
(a)
(b)
Fig. 8 Sensor configuration for outdoor experiment. a A camera-LiDAR sensor on vehicle and b a VRS-GPS that is only used for validation
该系统由相机和3D激光雷达组成。相机可输出分辨率为$800 \times 600$、帧率为40 Hz且水平视场角为$90^{\circ}$的RGB图像。传感器系统中使用的3D激光雷达为Velodyne公司的VLP-16型号。VLP-16通过16束激光产生相对稀疏的点云，考虑到相机与激光雷达的视场角，两者产生的点云重叠区域非常小。需要注意的是，我们的实验仅使用了重叠区域内的激光雷达测量数据。
为了使所提出的方法在该传感器系统中发挥出最优性能，必须确保异构传感器之间完成精确校准。我们首先进行校准以获取相机的内部参数，从而实现对三维点的投影。随后按照Kassir和Peynot（2010）提出的传统方法，进行外部校准以获得相机与激光雷达之间的相对变换关系。我们在室外和室内环境中利用该传感器系统对所提出的方法进行了验证，其中包括闭环场景测试。在室外实验中，传感器系统被安装在类汽车
---
图9 所提出方法、GPS以及ORB-SLAM的估计轨迹对比。a为停车场实验，b为大规模校园实验。红色和品红色代表GPS位置，蓝色为使用原始16线激光雷达的所提出方法；绿色为基于采样8线激光雷达的所提出方法；橙色代表ORB-SLAM的结果（彩色图在线查看）。
(a)
平台上，在校园区域内行驶。另一方面，室内实验在校园建筑内的走廊中进行，传感器系统由手持方式移动。与KITTI评估类似，视觉前端的相关参数列于表1中。在每项实验中，我们比较了两种方法的轨迹结果：一种是所提出的方法，另一种是基于单摄像头的ORB-SLAM。由于ORB-SLAM使用的是单目摄像头，我们通过真实位置与估计位置的对齐误差来进行尺度补偿，再进行比较（Zhang和Scaramuzza，2018）。
$$ \operatorname*{a r g m i n}_{s,\mathbf{R},\mathbf{t}}\sum\parallel\mathbf{p}_{{g t}}-s\mathbf{R}\hat{\mathbf{p}}-\mathbf{t}\parallel $$
其中，s、$\mathbf{R}$和$\mathbf{t}$为相似变换参数，$\mathbf{p}_{gt}$为真实位置，$\hat{\mathbf{p}}$为估计位置。由于所提出方法中尺度已知，因此将其固定为$s = 1$。不过，在室内实验中我们无法获取真实轨迹，因此仅报告所提出方法的结果。
#### 5.2.1 室外实验
户外实验采用我们自行研发的搭载于类汽车平台上的相机-激光雷达传感器系统开展。如图8所示，该车辆配备了我们的便携式传感器系统，以及额外的虚拟参考站（VRS）-全球定位系统（GPS）。VRS-GPS在固定状态下平均误差为20毫米，浮动状态下误差可达1米；而在正常状态下，其精度仅能保证在几十米范围内。我们将VRS-GPS的测量结果视为真实值，以此计算所提方法的位置误差。为了公平对比所提方法与其他单传感器方法的性能，实验在由建筑物环绕的停车场以及校园等大规模环境中进行。
图9展示了卫星图像以及来自GPS的车辆行驶路线。在停车场实验中，总行驶距离——
(b)
Table 3 Absolute position error between the VRS-GPS and the estimated pose
| 16-Ray (m) | 8-Ray (m) | ORB-SLAM (m) |
| Parking lot | 1.09 | 1.17 | 1.79 |
| Campus | 8.77 | 11.58 | 17.42 |
与ORB-SLAM相比，所提方法在8射线和16射线测试结果中均表现出更优的性能。
该路段长度约为0.15公里，包含一个环路。第二次实验在校园内进行，这是一个大规模环境，总行驶距离为3.97公里，包含两个环路。我们将两种方法的结果与VRS-GPS测量的基准位置进行了比较。
在停车场实验中，我们以正常状态下的GPS数据作为基准来对比轨迹。由于周边建筑物会干扰GPS信号，我们只能获取到正常状态下的信号。图9a展示了停车场中的轨迹估计结果及其与基准数据的对比；图9b则呈现了校园实验中的对比结果。尽管处于大规模环境中，所提出的方法依然表现出一致性和准确性。
表3显示了两种方法估计的位置与VRS-GPS测量值之间的绝对位置误差。为了模拟激光雷达测量密度较低的情况，我们进一步将VLP-16的16束激光缩减为8束进行比对。校园实验中GPS测量存在间断，因此我们只利用可用的GPS数据进行误差计算。所提出的方法在这两次实验中的绝对位置误差分别为1.09米和8.77米。尤其是在大规模校园实验中，其误差仅为ORB-SLAM方法的一半左右。此外，当使用8束激光运行时，误差分别为1.17米和17.42米。
我们的方法包含闭环检测功能，能够在大规模环境中生成一致的轨迹结果。此外，该方法在所有帧中都能获取点云数据，从而可直接利用估计的轨迹生成彩色点云。图10a展示了从户外场景获得的点云地图。
---
(a)
(b)
(c)
(d)
(e)
Fig. 10 3D reconstruction result on the campus experiment. a The pointcloud obtained by performing loop-closing in the entire area of the experiment. b–e Depict the close-up of the generated pointcloud
实验表明，最终生成的点云地图具有足够的精度，尽管并未进行额外的几何点匹配操作。由于仅依靠视觉里程计和闭环检测结果就取得了这样的成果，因此如果采用张和辛格（2015年）所使用的方法，即应用点匹配或地图合并算法，还有进一步提升的可能。
(a)
(b)
Fig. 11 Indoor experimental environment. a The floor plan with the location where the ArUCo marker is installed. The location where the marker was installed is shown as blue dots. b The corridor environment with the marker in which the experiment was performed (Color figure online)
#### 5.2.2 室内实验
下一次实验是在室内环境中使用我们的相机-LiDAR传感器系统进行的。室内环境属于典型的GPS拒止环境，因此无法提供真实值。为了评估所提出方法在本次实验中的性能，我们按照图11a所示，在四个位置安装了Garrido-Jurado等人（2014年）设计的ArUCo标记点。图11b展示了走廊中安装标记点的场景。我们通过对比从标记点测量得到的相对位姿与对应的估计位姿，来评估所提出方法的性能。我们沿着该环路行驶了三次，总路径长度达到180米。由于室内实验中标记点的绝对位置无法被精确获知，因此每当在重新访问时再次观测到这些标记点，我们就对比标记点测量的相对位置与所提出方法估计的位置之间的均方根误差（RMSE）。由于轨迹估计失败，基于单一传感器的方法仅进行了定性比较。
如图11a所示，标记点被放置在走廊的四个角落（M1至M4）。它们会被相机识别，用以测量标记点与相机之间的相对位姿。
Table 4 Relative position error between the measured relative pose by the marker and the relative pose in the estimated trajectory
| M1 (m) | M2 (m) | M3 (m) | M4 (m) |
| Loop 1–2 | 0.1263 | 0.0712 | 0.1074 | 0.0535 |
| Loop 2–3 | 0.0655 | 0.0633 | 0.2540 | 0.0668 |
| Loop 3–1 | 0.1323 | 0.0770 | 0.1837 | 0.0636 |
---
(a)
(c)
(d)
(b)
(e)
(f)
Fig. 12 Comparison of estimated trajectories and 3D reconstruction result on the indoor experiment. Magenta points depict marker position. b The pointcloud obtained by performing loop closing in the entire area of the experiment. c–f The close-up of the generated pointcloud (Color figure online)
表4总结了ArUCo标记点测量的位姿与所提出方法估计的位姿之间的相对位置误差。当标记点距离相机过远时，精度会有所下降（Malbezin等人，2002年）。因此，我们仅对比在近距离内识别出标记点时测量的位姿结果。尽管规划的路径包含三次重新访问，但平均相对误差仍保持在较小水平（0.105米）。这是因为所提出的方法通过位姿图优化实现了闭环，成功保持了地图的一致性并减少了整个轨迹的漂移。在M3的情况中，第二次和第三次访问时的误差略大。这是由于M3标记点的位置所致：该标记点位于拐角处，实验中是在拐角之后才突然被识别出来，从而导致基于标记点的位姿测量出现误差。
图12展示了各方法估计的轨迹与三维重建结果的对比。图12a描绘了带有标记点估计位置的轨迹结果。每当图像中观测到标记点时，就会根据估计的位姿来确定标记点的位置。三次重新访问产生了多个场景下每个标记点的位置估计值，我们绘制了每个标记点的平均测量值。由于三次循环中引入了闭环约束，所提出方法推断出的轨迹具有一致性。图12b展示了三维重建结果。与密集的RGB-D相机相比，重建的三维点云地图可能较为稀疏，但如图12c–f所示，其在全局一致性方面表现出色。即便没有通过点匹配进行进一步细化，所提出方法仍能生成环境的一致点云地图。
## 6 结论
在这项工作中，我们针对具备稀疏深度数据的相机-LiDAR传感器系统提出了一种直接视觉SLAM方法。即便在大规模环境中仅能获取极其稀疏的深度测量数据（8射线）时，该方法仍能给出稳健的SLAM结果。基于滑动窗口的姿态估计能够防止局部层面的漂移，而基于外观的位置识别模块与姿态图优化器则能维持全局层面的一致性。该方法通过我们自研的传感器系统以及KITTI基准数据集进行了评估。我们的方法无需进行点匹配，便能生成符合现实世界尺度的精确轨迹与点云地图。
尽管直接利用LiDAR数据在传感器层面具有优势，但两种传感器之间的位移可能会导致多帧之间出现遮挡点。正如Huang和Stachniss（2019）所指出的，通过显式处理遮挡问题，可进一步提升系统在存在大量遮挡时的鲁棒性与精度。在所提出的方法中，点采样策略
---
以及异常值剔除方案有效改善了这种遮挡效应。针对更为杂乱的环境，我们未来的工作将是通过融入语义信息，确保在各类移动物体和遮挡情况下仍能保持精度与鲁棒性。
在未来的研究中，我们计划利用LiDAR提供的几何信息，将我们的方法拓展为能够应对光照变化的鲁棒型相机-LiDAR SLAM系统。若将两种传感器的外部参数纳入优化器，精度还能进一步提升。
致谢：本研究得到了韩国国土交通部（MOLIT，项目编号19TSRD-B151228-01）和韩国产业通商资源部（MOTIE，项目编号10067202）的支持。Shin Y.的研究经费来自韩国国土交通部设立的“智慧城市创新人才培养计划”。
## 参考文献
Agarwal, P., Tipaldi, G. D., Spinello, L., Stachniss, C., & Burgard, W. (2013). 利用动态协方差缩放实现稳健的地图优化。载于IEEE机器人及自动化国际会议（ICRA）（第62–69页）。
Bosse, M., Zlot, R., & Flick, P. (2012). Zebedee：一种弹簧安装的3D测距传感器的设计及其在移动测绘中的应用。《IEEE机器人学汇刊》，28(5)，1104–1119。
Civera, J., Davison, A. J., & Montiel, J. M. M. (2008). 单目SLAM的逆深度参数化。《IEEE机器人学汇刊》，24(5)，932–945。
Concha, A., & Civera, J. (2017). RGBDTAM：一种低成本且精确的RGB-D跟踪与建图系统。载于2017年IEEE/RSJ智能机器人与系统国际会议（IROS）（第6756–6763页）。
Davison, A. J., Reid, I. D., Molton, N. D., & Stasse, O. (2007). MonoSLAM：实时单摄像头SLAM。《IEEE模式分析与机器智能汇刊》，29(6)，1052–1067。
Endres, F., Hess, J., Sturm, J., Cremers, D., & Burgard, W. (2014). 基于RGB-D相机的3D建图。《IEEE机器人学汇刊》，30(1)，177–187。
Engel, J., Koltun, V., & Cremers, D. (2017). 直接稀疏里程计。《IEEE模式分析与机器智能汇刊》，40(99)，611–625。
Engel, J., Schöps, T., & Cremers, D. (2014). LSD-SLAM：大规模直接单目SLAM。载于欧洲计算机视觉会议（ECCV）。
Engel, J., Stückler, J., & Cremers, D. (2015). 基于立体摄像头的大规模直接SLAM。载于2015年IEEE/RSJ智能机器人与系统国际会议（IROS）（第1935–1942页）。
Forster, C.、Pizzoli, M.和Scaramuzza, D.（2014）。SVO：快速半直接单目视觉里程计。载于2014年IEEE国际机器人与自动化会议（ICRA）（第15–22页）。
Gálvez-López, D.和Tardós, J. D.（2012）。用于图像序列中快速位置识别的二进制词袋。IEEE机器人学报，28(5)，1188–1197。
Garrido-Jurado, S.、Salinas, R. M.、Madrid-Cuevas, F.和Marín-Jiménez, M.（2014）。遮挡条件下高可靠性基准标记的自动生成与检测。模式识别，47(6)，2280–2292。
Gauglitz, S.、Sweeney, C.、Ventura, J.、Turk, M.和Höllerer, T.（2012）。基于普通运动及仅旋转运动的相机实时跟踪与建图。载于第11届IEEE国际研讨会论文集。
混合与增强现实研讨会（ISMAR），佐治亚州亚特兰大，（第13–22页）。
Geiger, A.、Lenz, P.和Urtasun, R.（2012）。我们准备好迎接自动驾驶了吗？KITTI视觉基准测试套件。载于计算机视觉与模式识别会议（CVPR）。
Gräter, J.、Wilczynski, A.和Lauer, M.（2018）。LIMO：激光雷达-单目视觉里程计。载于2018年IEEE/RSJ智能机器人与系统国际会议（IROS）（第7872–7879页）。
Henry, P.、Krainin, M.、Herbst, E.、Ren, X.和Fox, D.（2012）。RGB-D建图：利用Kinect式深度相机对室内环境进行密集3D建模。国际机器人研究杂志，31(5)，647–663。
Huang, A. S.、Bachrach, A.、Henry, P.、Krainin, M.、Maturana, D.、Fox, D.等（2011）。基于RGB-D相机的自主飞行视觉里程计与建图。载于国际机器人研究研讨会（ISRR）（第1–16页）。
Huang, K.和Stachniss, C.（2019）。具有显式遮挡处理与平面检测功能的高精度直接视觉-激光里程计。载于IEEE国际机器人与自动化会议（ICRA）论文集，已录用（即将发表）。
Jin, H.、Favaro, P.和Soatto, S.（2001）。光照变化下的实时特征跟踪与异常值剔除。载于第8届IEEE国际计算机视觉会议论文集。ICCV 2001（第1卷，第684–689页）。
Kassir, A.和Peynot, T.（2010）。可靠的自动相机-激光校准。载于G. Wyeth和B. Upcroft主编的澳大利亚机器人技术与自动化会议（ACRA）论文集，ARAA。昆士兰州布里斯班。
Kerl, C.、Sturm, J.和Cremers, D.（2013a）。面向RGB-D相机的密集视觉SLAM。载于IEEE/RSJ智能机器人与系统国际会议论文集（第2100–2106页）。
Kerl, C.、Sturm, J.和Cremers, D.（2013b）。面向RGB-D相机的鲁棒里程计估计。载于IEEE国际机器人与自动化会议论文集（第3748–3754页）。
Kitt, B.、Geiger, A.和Lategahn, H.（2010）。基于立体图像序列且采用RANSAC异常值剔除方案的视觉里程计。载于IEEE智能车辆研讨会（IV）论文集（第486–492页）。
Klein, G.和Murray, D.（2007）。小型AR工作空间的并行跟踪与建图。载于2007年第6届IEEE与ACM混合与增强现实国际研讨会论文集（第225–234页）。
Kuemmerle, R.、Grisetti, G.、Strasdat, H.、Konolige, K.和Burgard, W.（2011）。G2o：一种通用的图优化框架。载于IEEE国际机器人与自动化会议论文集，中国上海（第3607–3613页）。
库兹涅佐夫, Y., 斯图克勒, J., 与莱贝, B. (2017)。基于半监督深度学习的单目深度图预测。载于IEEE计算机视觉与模式识别会议（CVPR）。
兰格, K. L., 利特尔, R. J., 与泰勒, J. M. (1989)。基于t分布的稳健统计建模。《美国统计协会杂志》，84(408)，881–896。
李, S., 与李, D. (2016)。利用强度辅助的迭代最近点算法实现快速视觉里程计。IEEE机器人与自动化快报，1(2)，992–999。
卢, D. (2016)。视觉增强型激光雷达里程计与建图。卡内基梅隆大学硕士论文，宾夕法尼亚州匹兹堡。
马尔贝津, P., 皮卡斯基, W., 与托马斯, B. H. (2002)。长距离跟踪实验中ARTootKit精度的测量。载于第一届IEEE增强现实工具包国际研讨会（第2页）。
穆尔-阿塔尔, R., 蒙蒂尔, J. M. M., 与塔尔多斯, J. D. (2015)。ORB-SLAM：一种多功能且精准的单目SLAM系统。IEEE机器人学报，31(5)，1147–1163。
纽科姆, R. A., 伊扎迪, S., 希利格斯, O., 莫利纽克斯, D., 金, D., 戴维森, A. J.等 (2011a)。Kinectfusion：实时密集表面映射与跟踪。载于2011年第10届IEEE混合与增强现实国际研讨会（第127–136页）。
---
纽科姆, R. A., 洛夫格罗夫, S. J., 与戴维森, A. J. (2011b)。DTAM：实时密集跟踪与映射。载于2011年国际计算机视觉会议（第2320–2327页）。
朴, C., 莫加达姆, P., 金, S., 埃尔费斯, A., 福克斯, C., 与斯里达拉南, S. (2018)。弹性激光雷达融合：以密集地图为中心的连续时间SLAM。载于IEEE国际机器人与自动化会议论文集，布里斯班（第1206–1213页）。
平格拉, P., 普费弗, D., 弗兰克, U., 与梅斯特, R. (2014)。了解你的极限：长距离立体物体测量的实际精度。载于欧洲计算机视觉会议（第96–111页）。
皮佐利, M., 福斯特, C., 与斯卡拉穆扎, D. (2014)。Remode：实时概率性单目密集重建。载于2014年IEEE国际机器人与自动化会议（ICRA）（第2609–2616页）。
秦, T., 李, P., 与沈, S. (2018)。Vins-mono：一种鲁棒且多功能的单目视觉-惯性状态估计器。IEEE机器人学报，34(4)，1004–1020。
西格尔, A., 海内尔, D., 与特龙, S. (2009)。广义ICP。载于机器人学：科学与系统会议论文集，美国西雅图。
塞拉芬, J., 与格里塞蒂, G. (2015)。NICP：基于法线的密集点云配准。载于IEEE/RSJ智能机器人与系统国际会议（IROS）（第742–749页）。
申, Y. S., 朴, Y. S., 与金, A. (2018)。针对摄像头-激光雷达系统的直接视觉SLAM，利用稀疏深度信息。载于IEEE国际机器人与自动化会议论文集，布里斯班（第5144–5151页）。
西布利, G., 马西斯, L., 与苏卡特梅, G. (2007)。长距离立体视觉中的偏差消除与滤波器收敛。载于S. 特龙、R. 布鲁克斯、H. 杜兰特-怀特编著的《机器人学研究》（第285–294页）。柏林：施普林格出版社。
斯坦布鲁克, F., 斯图姆, J., 与克雷默斯, D. (2011)。基于密集RGB-D图像的实时视觉里程计。载于2011年IEEE国际计算机视觉研讨会（ICCV研讨会）（第719–722页）。
Strasdat, H.、Davison, A. J.、Montiel, J. M. M.和Konolige, K.（2011）。用于恒定时间视觉SLAM的双窗口优化。载于2011年国际计算机视觉会议论文集（第2352–2359页）。
Tateno, K.、Tombari, F.、Laina, I.和Navab, N.（2017）。CNN-SLAM：基于深度学习深度预测的实时密集单目SLAM。载于IEEE计算机视觉与模式识别会议（CVPR）论文集。
Whelan, T.、Kaess, M.、Johannsson, H.、Fallon, M.、Leonard, J. J.和McDonald, J.（2015）。基于体积融合的实时大规模密集RGB-D SLAM。《国际机器人研究杂志》，34卷（4–5期），第598–626页。
Yang, N.、Wang, R.、Stuckler, J.和Cremers, D.（2018）。深度虚拟立体里程计：利用深度学习深度预测实现单目直接稀疏里程计。载于欧洲计算机视觉会议（ECCV）论文集。
Yang, Z.和Shen, S.（2017）。具有在线初始化和相机-IMU外部校准功能的单目视觉惯性状态估计。《IEEE自动化科学与工程学报》，14卷（1期），第39–51页。
Yin, X.、Wang, X.、Du, X.和Chen, Q.（2017）。利用深度卷积神经场估计的深度信息恢复单目视觉里程计。载于IEEE国际计算机视觉会议（ICCV）论文集。
Zhang, J.、Kaess, M.和Singh, S.（2017）。一种实时深度增强型视觉里程计方法。《自主机器人》，41卷（1期），第31–43页。
Zhang, J.和Singh, S.（2015）。视觉-激光雷达里程计与建图：低漂移、鲁棒且快速。载于IEEE机器人与自动化会议（ICRA）论文集（第2174–2181页）。
Zhang, J.和Singh, S.（2017）。低漂移且实时的激光雷达里程计与建图。《自主机器人》，41卷（2期），第401–416页。
Zhang, Z.和Scaramuzza, D.（2018）。视觉（惯性）里程计量化轨迹评估教程。载于IEEE/RSJ智能机器人与系统会议论文集（第7244–7251页）。
出版商声明：Springer Nature对已发表地图中的管辖权声明及机构归属保持中立。
Young-Sik Shin于2013年在韩国仁川仁荷大学获得电气工程学士学位。他随后在韩国大田的韩国科学技术院（KAIST）机器人专业获得硕士学位。目前，他是KAIST土木与环境工程系的博士研究生。他的研究方向主要集中在机器人导航、视觉定位及SLAM领域。
Yeong Sang Park是KAIST土木与环境工程系的博士研究生。他分别于2015年和2017年在韩国仁川仁荷大学获得电子工程学士学位和电气工程硕士学位。他的研究兴趣包括视觉同时定位与建图（SLAM）以及导航。
Kim Ayoung是土木与环境工程系的助理教授，同时隶属于KAIST的KI Robotics。她于2005年和2007年分别在美国密歇根大学安娜堡分校获得机械工程专业的学士和硕士学位，并于2011年和2012年分别在该校获得电气工程硕士学位和机械工程博士学位。20013年，她曾在南海大学建筑与海洋工程学院担任博士后研究员，之后在ETRI担任高级研究员。她的研究方向包括视觉同步定位与建图（SLAM）、导航技术，尤其侧重于长期且大规模环境下的机器人自主运行。