# 基于稀疏深度的相机-激光雷达系统直接视觉SLAM
申英植、朴永相、金亚永
摘要—本文提出了一种结合单目相机与激光雷达（LiDAR）提供的稀疏深度信息的直接视觉同步定位与建图（SLAM）框架。为在保持高精度运动估计的同时确保实时性能，我们提出了：(i) 基于滑动窗口的跟踪方法；(ii) 用于精确位姿图SLAM的严格位姿边缘化技术；(iii) 用于大规模建图的深度融合帧匹配算法。与传统的基于特征的视觉和激光雷达建图方法不同，所提方法是直接的，无需在目标函数中引入视觉特征。我们使用自研的便携式相机-激光雷达系统以及KITTI里程计基准数据集对结果进行了评估。实验结果表明，两种互补传感器的特性在提升实时性能和精度方面非常有效。经验证，在包含高速公路和居民区等多种环境的KITTI基准测试中，我们的算法漂移误差仅为0.98%。
## I. 引言
过去二十年间，同步定位与建图（SLAM）一直是机器人领域的研究热点。从单目相机到密集的RGB-D传感器，已有大量视觉SLAM相关研究发表。然而，利用激光雷达提供的相对稀疏深度信息的视觉SLAM系统却鲜受关注。对于室外机器人和自动驾驶车辆的三维感知而言，3D激光雷达与相机的组合被视为最实用的传感器类型。该传感器系统通过SLAM维持三维空间结构，从而能够支持路径规划、避障和机器人操控等高级任务。虽然传统激光雷达传感器能由高成本设备生成密集且丰富的点云，但包括固态传感器在内的新一代激光雷达有望面向更广泛的消费群体提供稀疏点数据。
在机器人三维建图领域，激光雷达是运动估计和建图中常用的传感器。从传统的迭代最近点（ICP）方法发展而来，近期研究人员开始利用密集点数据 [^1]、[^2]。正如[^3]中所报道的，传统的基于ICP的方法由于垂直方向测量稀疏性而容易失效。这种有限的点云密度会导致运动估计不准确，进而造成三维环境重建失败。在数据稀疏的情况下实现一致的地图尤为关键，尤其是在大规模环境中的闭环检测环节。
近年来，基于单目相机的视觉SLAM在基于视觉特征的间接方法和直接方法两方面都取得了显著进展。直接方法
(a) Sensor system
(b) LiDAR points overlaid on an image
(c) The reconstructed 3D pointcloud map
Fig. 1. A combined system of camera and sparse 3D LiDAR for motion estimation and mapping. The proposed method aims to estimate sensor pose and to reconstruct 3D environments. (a) A prototype of our portable camera-LiDAR sensor system that acquires a single image and sparse depth measurements as in (b). (c) From these sensor configuration, the proposed methods perform the precise motion estimation and the 3D mapping.
尤其受到了广泛研究，其在不使用视觉特征的情况下便展现出出色的运动估计性能 [^4] [^5] [^6] [^7] [^8] [^9]。然而，当相机沿光轴移动时，显著视觉特征的像素位置变化十分细微，导致相机运动估计问题难以求解。为解决单目相机的尺度模糊问题，单目系统不可避免地需要借助外部信息来初始化三维环境的几何结构 [^4]、[^10]、[^11]。这种初始化的质量直接决定了真实世界尺度与估计尺度之间的误差。
在这项工作中，我们提出了一种视觉SLAM系统，该系统结合了从摄像头获取的图像以及可从3D激光雷达获得的稀疏深度信息。需要注意的是，如图1所示，传感器提供的深度信息十分有限。由于将这些稀疏深度信息与视觉特征关联起来会导致有意义的特征数量不足，因此我们得出结论：直接法在处理稀疏深度数据时更为有效。对于低分辨率摄像头与稀疏激光雷达的组合而言，直接法具有明显优势，因为降低图像分辨率对视觉里程计性能的负面影响较小。
---
Fig. 2. Block diagram of the direct sparse odometry and mapping system. The proposed method consists of the front-end visual odometry and back-end solver for the graph optimization considering loop-closures.
若如[^5]中所述，对应的深度信息能够恰当关联，采用直接法即可实现精准定位。
不过，稀疏3D激光雷达狭窄的垂直视场角（FOV）仅能保证在部分图像区域内实现有限的深度关联。为克服这一缺陷并确保系统的鲁棒性，我们对固定数量的帧进行里程计优化，并将相邻帧的稀疏深度信息整合到关键帧中，用以构建六自由度闭环约束。
本文针对摄像头与3D激光雷达相结合的传感器系统，提出了一种增强深度的视觉SLAM方法。所提方法的主要特点可概括如下：
- 我们仅在存在稀疏深度测量数据时才采用直接视觉SLAM方法。为应对激光雷达数据的稀疏性问题，运动估计过程中会排除视觉特征。
- 我们采用基于滑动窗口的优化方法来克服视场角不同的异构传感器的局限性。在进行位姿图优化时，我们会严格对滑动窗口内的位姿进行边缘化处理。
- 为确保大规模环境下的地图一致性，我们采用了基于关键帧的闭环约束。
- 我们已在包含多种环境的大规模公开数据集以及直接采集的数据上对所提方法进行了全面评估。
### II. 相关工作
已有大量研究致力于利用摄像头进行运动估计[^12] [^13] [^14] [^15]。长期以来，单目和双目摄像头最主流的方法都是基于特征的方法（间接法）。然而，有研究指出，当环境存在简单且重复的模式或无特征时，这种基于特征的方法会出现严重缺陷，进而导致运动估计失败。
近期出现的直接法无需依赖特征对应关系，从而缓解了特征相关问题。这类方法不采用基于特征的重投影误差模型，而是主要使用考虑像素强度差异的光度误差模型[^4] [^5] [^6]，[^9]，[^16]。不过，对于单摄像头而言，系统性能取决于初始化过程的质量。此外，在同时估计完整深度图和运动时，通常需要图形处理器（GPU）的支持[^6]，[^16]。
作为上述两种方法的折中方案，我们针对基于中央处理器（CPU）的实时性能需求提出了一种半密集处理方法，涉及[^4]、[^5]以及[^9]。直接稀疏里程计（DSO）是一种先进的直接视觉里程计方法，它仅使用单个摄像头即可实现定位功能，对应[^5]。该方法会维持固定数量的特征点，并通过基于窗口的优化来调整所有相关参数（如相机内参、运动信息、逆深度值）。这种基于窗口的优化能够确保较高的里程计精度。不过，与所有单摄像头方法一样，它也存在尺度模糊问题。受此启发，我们采用类似的基于窗口的优化方式来提升相机-激光雷达系统的里程计精度。与他们的方法不同，利用激光雷达数据可以解决单摄像头的尺度模糊问题。此外，与前述方法相比，所提出的方法还构建了一个完整的SLAM框架。
在使用RGB-D传感器时，直接方法的应用优势明显。由于有密集的深度信息辅助，几何误差的计算变得十分简单，涉及[^7]、[^8]、[^17]以及[^18]。其中，深度增强单目里程计（DEMO）与我们提出的方法最为相似，对应[^18]。DEMO针对的传感器配置与我们的类似，即单目摄像头搭配激光雷达。DEMO将摄像头和激光雷达数据紧密融合以实现视觉里程计功能，当存在深度信息时，它会同时追踪视觉特征以及对应的特征和深度测量值。我们的方法与他们的类似，都是通过关联激光雷达数据来克服单目摄像头的局限性，从而展现出优异的性能。不过，他们的方法是间接式的，在里程计计算中依赖密集的深度信息。
### 三、算法概述
如图2所示，所提出的方法包含基于追踪的视觉里程计（VO）模块、闭环检测模块以及位姿图优化器。输入帧由……
---
同步获取的摄像头图像和激光雷达点云组成。所提出的方法分为用于构建位姿图的前端和用于优化位姿图的后端。前端包括视觉里程计模块和基于外观的地标识别模块。
追踪环节：视觉里程计模块通过滑动窗口优化来实现鲁棒且漂移小的运动估计。该过程并非依赖视觉特征，而是直接在强度域中分析原始图像。新传入的图像帧和稀疏深度测量值会被送入追踪模块。与一般的直接方法类似，追踪过程中会应用多尺度图像金字塔以及基于恒定运动模型的初始化机制，涉及[^4]、[^5]和[^7]。
窗口化优化：成功完成追踪后，该帧会被加入滑动窗口，同时剔除最旧的帧。随后通过窗口化优化来微调窗口内的各帧。这种基于滑动窗口的优化能够保证追踪过程的输出结果具有较低的漂移。
闭环检测：前端的最终组成部分是地标识别模块。我们并非使用所有帧进行地标识别，而仅选取选定的关键帧。一旦确认属于有效的重访场景，就会整合相邻帧中的深度信息来进行双视图匹配。
后端优化器：后端模块负责优化位姿图，包括由位置识别模块提供的回环约束。我们采用了 $g_{2o}$ [^19]，结合增量式稀疏线性代数方法来实现优化的在线运行。
### 四、深度增强的直接视觉里程计
我们首先介绍采用直接方法的视觉里程计模块。第一阶段包括帧管理、帧边缘化、跟踪以及窗口化优化。
### A. 符号说明
某一帧 $\mathcal{F}_i$ 会提供RGB图像 $\mathcal{I}_i$ 和稀疏点云 $\mathcal{P}_i$。每个点 $\mathbf{p}_k = (x, y, z)^T \in \mathbb{R}^3$ 会被投影为图像坐标，通过相机投影函数 $\mathbf{u}_k = (u, v) \in \mathbb{R}^2$ 可表示为像素 $\pi(\mathbf{p}_k)$。这些点相对于该帧的相机坐标系进行锚定。每一帧的位姿由世界坐标系下的变换矩阵 $\mathbf{T}_n \in SE(3)$ 表示。以跟踪过程为例，$\mathcal{F}_n$ 与 $\mathcal{F}_m$ 之间的相对位姿变换可用 $\mathbf{T}_m^n = \mathbf{T}_n^{-1} \mathbf{T}_m$ 表示，可用于两点在不同坐标系间的坐标转换，如下所示：
$$ \mathbf{p}_{n}=\mathbf{T}_{m}^{n}\mathbf{p}_{m}=\begin{bmatrix}\mathbf{R}_{m}^{n}&\mathbf{t}_{m}^{n}\\ \mathbf{0}&1\end{bmatrix}\mathbf{p}_{m} $$
其中 $\mathbf{p}_m$ 和 $\mathbf{p}_n$ 是各帧上的点，$\mathbf{R}_m^n \in SO(3)$ 和 $\mathbf{t}_m^n \in \mathbb{R}^3$ 分别为旋转矩阵和平移向量。本文中均采用李代数元素 $\xi \in \mathbb{R}^6$ 作为线性化的位姿增量。$\xi = [\mathbf{w}, \mathbf{v}]$ 由角速度和线速度向量组成。利用该增量计算得到的更新后变换矩阵，可通过李代数到李群的指数映射得出：$se(3) \times SE(3) \to SE(3)$。
$$ \bar{\mathbf{T}}_{m}^{n}=\exp(\hat{\xi})\mathbf{T}_{m}^{n} $$
### B. 帧管理
1) 帧生成：为了将激光雷达的深度测量数据直接应用于图像帧，需要对传感器进行时间同步。两种传感器在物理上是分离的，且3D激光雷达的工作频率（10 Hz）通常低于相机。为缩小这一差异，我们会选取最接近的时间戳，将同步后的激光雷达与相机状态定义为带有稀疏深度的RGB帧。一旦找到同步数据，就会将激光雷达获取的三维点转换到相机坐标系中。
2) 关键帧管理：所提出的方法始终维持固定数量的关键帧，最新帧由当前帧进行跟踪。设置关键帧的目的是减轻里程计结果的漂移现象，并降低滑动窗口优化器的计算负载。创建这些关键帧时，我们采用以下两个准则：
- 当跟踪成功时，根据可投影点的比例来创建新关键帧。由于激光雷达和相机的视场角是固定且已知的，两个视点间的点投影情况很容易决定关键帧的创建时机。只有当帧间比例小于 $\gamma_{kf}$ 时，才会将其注册为关键帧。
- 使用重叠比例时，若运动较为细微且随时间发生光照变化，就会出现问题。为解决这一问题，我们会在一定时间 $(t_{kf})$ 后创建关键帧，以增强对光照变化的鲁棒性。
本文使用的参数总结在表I中。
3) 关键帧边缘化：为维持固定数量的关键帧并构建位姿图，我们需要对滑动窗口中最旧的帧进行边缘化。这一过程可在每次创建新关键帧时执行，需遵循以下额外策略：
- 我们始终至少保留两个关键帧。如果创建了新关键帧，例如当最近一帧中可见点的比例小于 $\gamma_{kf}$ 时，该关键帧就会被插入到关键帧窗口中。
- 当关键帧数量超过窗口大小 $(N_{w})$ 时，会按最旧的顺序进行边缘化处理。
当新增关键帧时，窗口中最旧的关键帧应被边缘化。与将地标视为状态的单目相机不同（[^5]），我们的框架中的点来自激光雷达，状态中并未定义地标。利用舒尔补码，我们显式地将最旧关键帧的因子从位姿图中边缘化。随后，当新增关键帧时，我们会利用剩余的赫斯矩阵块进行迭代优化。
---
4) 点管理：使用全部激光雷达点会增加每帧的计算负担。为了在保持运动估计精度的同时降低计算复杂度，所提出的方法在帧中采用了分布均匀的点采样策略。为提取采样点，我们按 $2^{\circ}$ × $2^{\circ}$ 的空间网格对激光雷达数据进行均匀采样，然后选取每组中梯度最大的点作为活跃点。由于点是按间隔分离的，梯度较低的区域中的点也能被激活。
### C. 跟踪
受稀疏直接法 [^4]、[^5] 的启发，我们的跟踪方法在最新关键帧 $\mathbf{p}_m$ 的图像 $\Omega$ 上，定义了一个位于特定稀疏模式图像块 $\mathcal{I}_m$ 中的点 $\mathcal{F}_m$ 的光度误差。这种基于图像块的方法在其规模上需要更多的计算量，但能确保对运动模糊和噪声等图像退化情况的鲁棒性。在跟踪阶段，关键帧中的点 $\mathbf{p}_m$ 通过初始变换被投影到当前帧 $\mathcal{I}_n$ 的图像 $\mathcal{F}_n$ 上，表示灰度亮度差异的残差定义为
$$ r(\mathbf{p}_{m})=\mathcal{I}_{n}(\pi(\mathbf{T}_{m}^{n}\mathbf{p}_{m}))-\mathcal{I}_{m}(\pi(\mathbf{p}_{m})). $$
最终，我们的跟踪目标函数定义如下：
$$ E_{t r a c k}=\sum_{\mathbf{p}\in\Omega}w(r(\mathbf{p}))(r(\mathbf{p}))^{2} $$
其中 $\Omega$ 是稀疏模式的区域，函数 $w(\cdot)$ 是基于 t 分布的权重函数，如 [^7] 所示。
$$ w(r)=\frac{\nu+1}{\nu+(\frac{r}{\sigma_{r}})^{2}} $$
此处，$\nu$ 为自由度，其值设为5；残差方差 $\sigma_{r}$ 在执行高斯-牛顿优化算法时会被迭代估算。权重的效果详见 [^7]。现在，我们采用由粗到精的策略来避免优化陷入局部最小值，并应对较大的位移情况。
### D. 基于窗口的优化
在前一节中，跟踪过程会估算新帧与参考帧之间的相对运动。如果新帧满足创建新关键帧的条件，它就会被加入窗口，而窗口中最旧的帧会被边缘化。当新增关键帧时，我们会利用高斯-牛顿算法进行基于窗口的批量优化。这种局部优化能够弥补仅部分图像区域存在深度测量信息的问题。不过，这种基于窗口的方法在计算速度与精度之间存在权衡，具体取决于窗口大小。因此，我们将窗口中的帧数固定在5到10帧之间。
Fig. 3. Example 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 the rectangle represent images. A point $\mathbf{p}_{k}$ in keyframe is projected into all keyframes with covisibility. A photometric residuals are calculated between the patch at the point and the other patch at projected point.
如图3所示，窗口$(\mathcal{F}_i)$中的每个关键帧$(\mathbf{W}_{\mathcal{F}})$都与若干点$(\mathbf{P}_i)$及其自身的位姿$(\mathbf{T}_i)$相关联。一个点$(\mathbf{p}_k)$会被投影到窗口内所有与其共视的关键帧中。这一过程会生成一个位姿图，我们仅对该图中关键帧的位姿进行优化。滑动窗口内所有关键帧之间的最终光度误差是在图像块$\Omega$上定义的。
$$ E_{w i n}=\sum_{\mathcal{F}_{i}\in\mathbf{W}_{\mathcal{F}}}\sum_{\mathbf{p}_{k}\in\mathbf{P_{i}}}\sum_{\mathbf{p}_{k}\in\Omega}w(r(\mathbf{p}_{k}))(r(\mathbf{p}_{k}))^{2}, $$
与跟踪过程中的代价函数相比，主要区别在于它反映了所有关键帧之间的误差。在此过程中，我们同样采用了t分布权重函数$w(\cdot)$。
### E. 回环闭合
视觉里程计模块为被严格边缘化的关键帧提供位姿。我们将所有关键帧的位姿添加到位姿图的节点中，从而生成来自激光雷达的一致性点云图。一旦关键帧被边缘化，我们便会提取定向FAST和旋转BRIEF（ORB）特征及描述符。随后，我们使用基于ORB-SLAM训练的词袋模型来应用DBoW算法[^15]，[^20]。为防止出现误匹配，我们以当前帧与其相邻帧之间的相似度得分作为筛选回环候选帧的标准。
我们进一步通过对比两种相对位姿估计结果来进行交叉验证。给定一个回环候选帧$\mathcal{F}_c$和当前关键帧$\mathcal{F}_n$，我们分别计算将候选帧投影到当前帧时的相对位姿（$\mathbf{T}_c^n$），以及将当前帧投影到候选帧时的相对位姿（$\mathbf{T}_n^c$）。由于候选帧和当前关键帧都包含来自激光雷达的深度信息，我们可以独立计算出$\mathcal{T}_c^n$和$\mathcal{T}_n^c$。只有当这种交叉对比结果高度一致（即差异很小）时，我们才会将其作为回环约束添加到位姿图中。最后，我们对回环边应用动态协方差缩放（DCS）技术，以应对不可靠的约束条件[^21]。
---
### V. 实验结果
我们提出的方法通过多种相机-激光雷达系统进行了验证。该方法既应用于公开的KITTI里程计数据集[^22]，也应用于我们的便携式相机-激光雷达传感器系统。所有实验均在配备英特尔酷睿i7-7700HQ处理器（4核，最高频率3.8 GHz）和8 GB内存的笔记本电脑上进行。具体而言，为了验证里程计性能，我们使用了不含回环闭合功能的KITTI数据集。尽管该数据集提供了立体图像，但我们仅使用单目相机和激光雷达传感器来验证所提出的方法。
接下来，我们使用自行搭建的传感器平台对所提出的方法进行了验证。我们的传感器系统由摄像头和3D激光雷达组成。该摄像头可输出分辨率为$800 \times 600$、帧率为30 Hz且水平视场角为$90^\circ$的RGB图像。在我们的摄像头-激光雷达系统中，采用了成本相对较低的3D激光雷达（VLP-16）。VLP-16通过16线测距方式生成稀疏点云，仅能为小范围图像区域提供深度信息。为了获得最佳性能，所提出的方法需要对异构传感器进行精确校准。针对我们的传感器系统，我们按照常规方法完成了摄像头的内部校准以及摄像头与激光雷达之间的外部校准[^23]。所提出的方法在室内和室外环境中均进行了评估：室内实验如图7所示，是在手持设备的情况下于大厅中开展的；室外实验则如图8所示，是将设备安装在移动测绘系统上进行的[^24]。表I中列出了用于关键帧管理的若干参数，整体处理流程及生成的点云地图也展示在camlidar.mp4文件中。
TABLE I
PARAMETERS FOR KEYFRAME MANAGEMENT
| Deer TTI | Indoor | Outdoor |
| $\gamma_{kf}$ | $R^{**}$ | 0.8 | 0.8 |
| $t_{kf}$ | | | 1.0 |
### A. 基于KITTI数据集的视觉里程计评估
我们从KITTI数据集中选取左侧单色图像和激光雷达数据进行验证。我们在原始图像分辨率$1230 \times 370$下开展了运动估计实验。本次验证的重点在于评估该方法在激光雷达数据稀疏情况下的鲁棒性。为了模拟并验证所提出的方法，
Fig. 4. Given a rich LiDAR data (64 laser beam from HDL64-E), we simulate a sparse LiDAR by sampling laser. The color is coded by the depth value.
(a) sequence 00
(b) sequence 01
(c) sequence 03
(d) sequence 05
Fig. 5. Sample comparison results with ground truth. The results are chosen from each environmental type. The red line represents the ground truth and the blue line represents the result of the proposed method.
(a)
(b)
(c)
(d)
Fig. 6. The detailed results in the overall training sequences. The top and bottom rows show rotational errors and translational. The errors left and the right columns show the results according to path length and speed.
我们将64束激光束分别采样为16束和8束，以模拟稀疏数据（见图4）。通过这项测试，我们仅评估了不使用回环闭合时的视觉里程计性能。
评估工作基于11个数据序列提供的真实值开展。图5展示了样本序列中该方法估计路径与真实路径的对比结果。需注意，序列01属于高速行驶且存在移动物体的高速公路数据集。
表II中显示了该方法与DEMO在全部训练序列上的对比结果。由于针对16线稀疏激光雷达数据，DEMO的最优参数尚不清楚，因此我们直接参考[^18]中报告的结果。所测量的平移误差是将轨迹分段后，分别以100米、200米、$\cdots$、800米为区间计算出的误差平均值。所提出的方法在64线条件下平均平移误差仅为0.98%，这一数值低于使用DEMO时产生的误差。
---
TABLE II
COMPARISON RESULT OF KITTI DATASET. THE RESULT REPRESENT AN AVERAGING TRANSLATIONAL ERROR AT EACH SEGMENT LENGTH. THE BOLD TYPE INDICATES THE BEST ACCURACY.
| Seq No. | Path len. [m] | Environment | DEMO (64 ray) [^18] | 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 |
我们进一步给出了激光雷达密度降低至16线和8线时该方法的鲁棒性测试结果。16线条件下的平均平移误差为0.98%，与64线时几乎一致。但在8线条件下，序列01中的误差较为明显。该数据集具有相似的纹理重复特征，且存在快速移动的车辆。尤其是由于特定区域内移动车辆的影响，产生了7.25%的巨大误差。
我们在图6中展示了根据轨迹段长度划分的详细结果。上下两行分别显示了旋转误差和平移误差。图6(a)展示了基于路径长度的旋转误差，图6(b)则展示了基于速度的旋转误差。平移误差则根据相同标准分别在图6(c)和图6(d)中显示。如图6(a)所示，随着路径长度的增加，旋转误差会减小。另一方面，无论路径长度如何，图6(c)中的平移误差都呈现出稳定的趋势。这意味着所提出的方法在无加速度的平移运动上可能存在轻微偏差，不过整体一致性仍相当好。右侧列中基于速度的误差数据存在异常值。图6(b)中的旋转误差会随着速度的增加而减小。这是由于受非完整约束的车辆特性所致，这一特点仅体现在数据集本身上。
### B. 室内相机-激光雷达数据集
为了在室内环境中提供真实值，我们按照图7(b)所示，在四个位置设置了ArUCo标记[^25]并开展实验。图7(a)展示了实验中安装的标记。由于室内环境中标记的绝对位置无法被精确获知，我们采用了通过标记计算出的相对位姿与返回同一区域时估计轨迹中的相对位姿之间的均方根误差（RMSE）来衡量精度。
评估是在四个点（M1至M4）上进行的。表III总结了通过标记测量的相对位姿与所提方法估计的相对位姿之间的误差。得益于位姿图优化带来的漂移抑制效果，所提方法表现优异——
(a) Indoor environment
(b) Floor plan with markers
Fig. 7. Indoor experimental environment. (a) shows an image of the corridor environment in which the experiment was performed. (b) shows the floor plan of the indoor environment, and red dot indicates the point where the ArUCo marker is installed.
即便每次循环的运动距离达到约60米，其相对误差也仅为0.105米左右。
图9展示了整体轨迹及生成的点云图的定性结果。图9(a)显示了在施加合适的闭环约束后，三个循环内的稳定轨迹结果。根据结果生成的彩色点云图如图9(b)所示。与密集的RGB-D相机相比，该点云图较为稀疏。随着运动过程中点的不断累积，整个区域的点云数据便被采集完毕。即便不进行任何点匹配操作，闭环检测也能生成质量极佳的点云图。
### C. 室外相机-激光雷达数据集
为了验证所提方法在大规模环境中的性能，我们将便携式相机-激光雷达设备安装在类汽车平台上开展实验。如图8(a)所示，该车辆配备了虚拟参考站。
TABLE III
RELATIVE POSITION ERROR BETWEEN THE MEASURED RELATIVE POSE BY THE MARKER AND THE RELATIVE POSE IN THE ESTIMATED TRAJECTORY
| M1 | M2 | M3 | M4 |
| loop 1-2 | 0.1263m | 0.0712m | 0.1074m | 0.0535m |
| loop 2-3 | 0.0655m | 0.0633m | 0.2540m | 0.0668m |
| loop 3-1 | 0.1323m | 0.0770m | 0.1837m | 0.0636m |
---
(a) Sensors
(b) Experimental route
Fig. 8. Outdoor experimental environment. (a) shows an sensor configuration in the experiment. The top-left is a camera-LiDAR sensor on vehicle and the top-right is a VRS-GPS that is only used for validation. (b) shows a satellite map with the experimental route.
我们将自己估计的轨迹与VRS-GPS提供的基准轨迹进行了对比。GPS在固定状态和浮动状态下都能提供相当精确的信息，因此仅对比这两种状态下的测量值。不过，由于在建筑物间的狭窄道路上存在多路径效应，固定状态和浮动状态无法同时出现，因此VRS-GPS被视为非连续信号源。
(VRS)-全球定位系统(GPS)以及相机-激光雷达设备。VRS-GPS在固定状态下的平均误差为20毫米，浮动状态下的误差可达1米，这些数值被用作车辆真实位置的基准。图8(b)展示了实验开展的卫星图像及路线。实验的总行驶距离为3.97公里，共包含两个循环。
表四展示了所提出的方法与VRS-GPS在固定状态下相对位置误差的对比情况。为了能在深度信息更稀疏的条件下比较性能，我们将16条光线的实际传感器数据与8条光线的数据进行了对比。由于测量的GPS数据并不连续，我们选取数据连续采集的11个时间段来计算并取误差的平均值。所提出方法的相对位置误差为1.11%；当该方法应用于8条光线数据时，误差为1.4%。由此可得出结论：即便使用垂直视场角仅为30°的稀疏激光雷达数据，SLAM算法的表现也相当出色。
TABLE IV
RELATIVE POSITION ERROR BETWEEN THE VRS-GPS AND THE ESTIMATED POSE
| 16-ray | 8-ray |
| Outdoor experiment | 1.11% | 1.42% |
我们的方法包含回环检测机制，即便在大规模环境中也能生成连贯的轨迹。此外，该传感器系统利用了投影到相机上的激光雷达深度信息，因此我们可以直接获取带有轨迹的彩色点云地图。图9(e)展示了实验所得的点云地图。即便完全没有进行点匹配操作，也能得到相当精确的点云地图。需要注意的是，这一结果仅通过视觉里程计和回环检测即可得到；若像[^26]中描述的那样进一步细化建图过程，精度还会大幅提升。
### 六、结论
在这项研究中，我们针对具备稀疏深度信息的相机-激光雷达传感器系统，提出了一种直接的视觉SLAM方法。实验表明，即便在大规模环境中仅有极其稀疏的深度测量数据（8条光线），该方法仍能给出稳健的SLAM结果。基于滑动窗口的姿态估计机制能够防止局部层面的漂移，而基于外观的位置识别模块与位姿图优化器则能保障全局层面的一致性。我们通过自身实验及KITTI基准数据集对该方法进行了评估，结果显示无需进行点匹配操作，该方法就能生成符合真实世界尺度的精确轨迹与点云地图。
在未来的工作中，我们计划利用激光雷达提供的几何信息，将该方法拓展为能应对光照变化的鲁棒型相机-激光雷达SLAM系统。此外，我们还打算通过处理运动物体来确保系统的精度与鲁棒性。
## 致谢
本研究得到了MSIP（编号2015R1C1A2A01052138）以及韩国国土交通部“U-City硕士及博士课程资助计划”的支持。
## 参考文献
[^1]: A. Segal, D. Haehnel, and S. Thrun, “Generalized-ICP,” in Proc. Robot.: Science & Sys. Conf., Seattle, USA, June 2009.
[^2]: J. Serafin and G. Grisetti, "NICP: Dense normal based point cloud registration," in Proc. IEEE/RSJ Intl. Conf. on Intell. Robots and Sys., Sept 2015, pp. 742–749.
[^3]: M. Velas, M. Spanel, and A. Herout, “Collar line segments for fast odometry estimation from velodyne point clouds,” in Proc. IEEE Intl. Conf. on Robot. and Automat., May 2016, pp. 4486–4495.
[^4]: C. Forster, M. Pizzoli, and D. Scaramuzza, “SVO: Fast semi-direct monocular visual odometry,” in Proc. IEEE Intl. Conf. on Robot. and Automat., May 2014, pp. 15–22.
[^5]: J. Engel, V. Koltun, and D. Cremers, “Direct sparse odometry,” IEEE Trans. Pattern Analysis and Machine Intelligence, vol. PP, no. 99, pp. 1–1, 2017.
[^6]: R. A. Newcombe, S. J. Lovegrove, and A. J. Davison, “DTAM: Dense tracking and mapping in real-time,” in Proc. IEEE Intl. Conf. on Comput. Vision, Nov 2011, pp. 2320–2327.
[^7]: C. Kerl, J. Sturm, and D. Cremers, “Robust odometry estimation for RGB-D cameras,” in Proc. IEEE Intl. Conf. on Robot. and Automat., May 2013, pp. 3748–3754.
[^8]: F. Steinbrcker, J. Sturm, and D. Cremers, “Real-time visual odometry from dense RGB-D images,” in Proc. IEEE Intl. Conf. on Comput. Vision, Nov 2011, pp. 719–722.
[^9]: J. Engel, T. Schöps, and D. Cremers, "LSD-SLAM: Large-scale direct monocular SLAM," in Proc. European Conf. on Comput. Vision, September 2014.
[^10]: S. Gauglitz, C. Sweeney, J. Ventura, M. Turk, and T. Höllerer, “Live tracking and mapping from both general and rotation-only camera motion,” in Proc. Intl. Symp. on Mixed and Aug. Reality, Atlanta, Georgia, Nov. 5-8 2012, pp. 13–22.
## [^11]: J. Civera, A. J. Davison, and J. M. M. Montiel, “Inverse depth parametrization for monocular slam,” IEEE Trans. Robot., vol. 24, no. 5, pp. 932–945, Oct 2008
---
(a) Estimated trajectory
(b) Resulting colored point cloud map
(c) Zoomed sample view
(d) Estimated trajectory
(e) Resulting colored point cloud map
(f) Zoomed sample view
Fig. 9. Results of the indoor and the outdoor experiment. The experiment was performed by turning around the same hall three times. (a) indicate the estimated trajectory result and marker position. (b) shows resulting acquired point cloud map without rendering. (d) Both ground truth trajectory and the estimated trajectory are overlaid for comparison. Blue dot represents the estimated trajectory, and red and magenta represent the position measured by the VRS-GPS in the fixed and floating states. (e) Resulting point cloud map from the outdoor test.
[^12]: B. Kitt, A. Geiger, and H. Lategahn, “Visual odometry based on stereo image sequences with ransac-based outlier rejection scheme,” in Proc. IEEE Intell. Vehicle Symposium, 2010.
[^13]: G. Klein and D. Murray, “Parallel tracking and mapping for small ar workspaces,” in Proc. Intl. Symp. on Mixed and Aug. Reality, Nov 2007, pp. 225–234.
[^14]: H. Strasdat, A. J. Davison, J. M. M. Montiel, and K. Konolige, "Double window optimisation for constant time visual slam," in Proc. IEEE Intl. Conf. on Comput. Vision, Nov 2011, pp. 2352–2359.
[^15]: R. Mur-Artal, J. M. M. Montiel, and J. D. Tards, “ORB-SLAM: A versatile and accurate monocular slam system,” IEEE Trans. Robot., vol. 31, no. 5, pp. 1147–1163, Oct 2015.
[^16]: M. Pizzoli, C. Forster, and D. Scaramuzza, “REMODE: Probabilistic, monocular dense reconstruction in real time,” in Proc. IEEE Intl. Conf. on Robot. and Automat., May 2014, pp. 2609–2616.
[^17]: T. Whelan, M. Kaess, H. Johannsson, M. Fallon, J. J. Leonard, and J. McDonald, “Real-time large-scale dense RGB-D slam with volumetric fusion,” Intl. J. of Robot. Research, vol. 34, no. 4-5, pp. 598–626, 2015. [Online]. Available:
[^18]: J. Zhang, M. Kaess, and S. Singh, “A real-time method for depth enhanced visual odometry,” Autonomous Robots, vol. 41, no. 1, pp. 31–43, 2017. [Online]. Available:
[^19]: R. Kuemmerle, G. Grisetti, H. Strasdat, K. Konolige, and W. Burgard, "g2o: A general framework for graph optimization," in Proc. IEEE
国际机器人与自动化会议，中国上海，2011年5月，第3607–3613页。
[^20]: D. Gálvez-López and J. D. Tardós, “Bags of binary words for fast place recognition in image sequences,” IEEE Trans. Robot., vol. 28, no. 5, pp. 1188–1197, October 2012.
[^21]: P. Agarwal, G. D. Tipaldi, L. Spinello, C. Stachniss, and W. Burgard, "Robust map optimization using dynamic covariance scaling," in Proc. IEEE Intl. Conf. on Robot. and Automat., May 2013, pp. 62–69.
[^22]: A. Geiger, P. Lenz, and R. Urtasun, "Are we ready for autonomous driving? the KITTI vision benchmark suite," in Proc. IEEE Conf. on Comput. Vision and Pattern Recog., 2012.
[^23]: A. Kassir and T. Peynot, “Reliable automatic camera-laser calibration,” in Australasian Conference on Robotics and Automation (ACRA), G. Wyeth and B. Upcroft, Eds. Brisbane, Queensland: ARAA, December 2010.
[^24]: J. Kim, J. Jeong, Y. S. Shin, Y. Cho, H. Roh, and A. Kim, "LiDAR configuration comparison for urban mapping system," in Proc. Intl. Conf. on Ubiquitous Robots and Ambient Intell., Jeju, S. Korea, Aug. 2017, in print.
[^25]: S. Garrido-Jurado, R. M. noz Salinas, F. Madrid-Cuevas, and M. Marín-Jiménez, “Automatic generation and detection of highly reliable fiducial markers under occlusion,” Pattern Recognition, vol. 47, no. 6, pp. 2280 – 2292, 2014. [Online]. Available:
[^26]: J. Zhang and S. Singh, “Visual-lidar odometry and mapping: low-drift, robust, and fast,” in Proc. IEEE Intl. Conf. on Robot. and Automat., May 2015, pp. 2174–2181.