论文阅读：ORB-SALM2

2021-10-26

科研

Word count: 5.1k | Reading time≈ 20 min

0. 摘要

ORB-SLAM2：一个适用于单目、双目、RGB-D相机的完整SLAM系统。

系统包括地图重利用、回环检测和重定位功能
该系统在标准CPUs上实时工作，适用于多种环境：室内小型手持设备、工业环境中的无人机、城市中行驶的车辆。
该系统后端采用基于单目和双目观测的BA，允许metric尺度下对轨迹的精确估计。
该系统包括一个轻量级的定位模式，该模式可以在零点漂移(zero-drift)的情况下，利用视觉里程计来追踪未建图的区域并匹配地图点。

实验方面：
在29个公开序列上对ORB-SLAM2进行评估，该系统实现了SOTA的准确性，并且在多数情况下是最准确的SLAM方案。

代码已公开。

1. 介绍

在过去20年间，CV和机器人领域中，SLAM算是热门方向，近年更是吸引了不少高科技公司的关注。
SLAM技术旨在建立一个未知环境的地图，并且在地图中地位传感器，其重点在于实时操作。
在众多传感器中，相机相对廉价且能提供丰富的环境信息。相机传感器允许鲁邦且准确的位置识别(place recoginition)。因此，在VSLAM中，主流方案还是使用相机传感器。
位置识别是SLAM系统中回环检测部分的关键模块，此外，位置识别对于跟踪失败（由于遮挡occlusion或者剧烈运动或者系统重新初始化）后相机的重定位也很关键。

VSLAM可以只用单目相机来完成，这是一种最廉价且最小型的传感器设置。然而，单目存在一些问题：

由于仅用一台相机不能观测深度，地图和估计轨迹的尺度是未知的。
由于不能从第一帧中进行三角化测量，单目SLAM系统的启动(bootstrapping)需要多视角或滤波技术【？】才能生成初始地图。
单目SLAM可能出现尺度漂移
如果探索过程中出现纯旋转可能会失败。

使用双目/RGBD相机可以解决上述问题。

在这篇论文中，我们基于单目ORB-SLAM[1]提出ORB-SLAM2，主要贡献如下：

首个关于单目、双目和RGB-D相机的开源SLAM系统，系统包括回环检测、重定位和地图重用。
RGB-D结果显示，BA比ICP或者光度(photometric)和深度误差最小化更准确。
ORB-SLAM2使用远近双目点和单目观测，双目结果比SOTA中直接使用双目的SLAM系统更准确。【？】
在无法建图的情况下，ORB-SLAM2提供一个轻量级的定位模式，实现地图的高效重用。

下图分别是输入为双目和RGB-D时，ORB-SLAM2的输出结果：

双目：

这是KITTI数据集[2]序列00的最终轨迹和稀疏重建。该序列是一个有多个回环的城市数据集，从结果看，ORB-SLAM2成功检测到了回环。
RGB-D：

这是TUM RGB-D数据集[3]序列fr1_room的关键帧位姿估计和一个稠密点云图，该稠密点云图是通过从估计的关键帧位姿反映射(backproject)到传感器深度地图来构建的。【？】

ORB-SLAM2没有做KinectFusion[4]一类的数据融合处理【？】，但是关键帧位姿的估计还是很准确的。
其他范例可以参考附录视频

2. 相关工作

2.1 Stereo SLAM

2.2 RGB-D SLAM

3. ORB-SLAM2

系统包括三个主线程和一个小线程：
- 跟踪：通过与局部地图进行特征匹配和用motion-only BA来最小化重投影误差，来跟踪每一帧对应的相机位置
- 局部建图：通过执行局部BA，来建立和优化局部地图
- 回环检测：通过执行位姿图优化，来检测大回环并且矫正累积漂移
- Full BA（小线程）:在位姿图优化后进入第四个线程，该线程执行full BA，来计算出最优的结构和运动。
系统还嵌入了一个Place Recognition模块
- Place Recognition模块基于DBoW2[16]
- 用途：
  - 在跟踪失败(例如遮挡occlusion)情况下重定位
  - 在已建图场景中重新初始化
  - 回环检测
该系统维护一个共视图(covisibility graph)[8]和一个最小生成树(minimum spanning tree)
- 共视图：连接任意两个共同观测点的关键帧
- 最小生成树：连接所有关键帧
- 这些图结构能够得到一个关键帧的局部窗口，便于在局部范围内跟踪和建立局部地图。图结构使系统可以工作在大规模环境中，并在回环检测部分作为图优化的一种结构。
该系统在跟踪、建图和place recognition中使用ORB特征[17]
- ORB特征优点：
  - 对于旋转和尺度变换具有鲁棒性
  - 对于相机自增益auto-gain、自曝光auto-exposure、光度变化具有良好的稳定性
  - 能快速进行特征提取和特征匹配
    - 有助于实时性操作
    - 在词袋place recognition中表现出良好的精确率和召回率[18]

3.1 单目、近双目、远双目关键点

ORB-SLAM2是一种基于特征的方法，对输入进行预处理后，再提取显著关键点位置特征。

然后，丢弃输入图像，之后所有的系统操作都是基于这些提取的特征，因此，ORB-SLAM2系统独立于传感器（stereo、RGB-D）。

系统对单目和双目的关键点进行处理，这些关键点分为近点和远点两类。

SLAM相机成像原理

3.1.1 双目关键点

双目关键点定义为$X_s=(u_L,v_L,u_R)$，其中$(u_L,v_L)$是左图的坐标，$u_R$是右图的水平坐标。

对于双目相机：我们对每对图像提取ORB特征，在右图中寻找左图的ORB匹配。这对于双目图像矫正(stereo rectified images)【？】很有效，因此极线(epipolar lines)是水平的。

然后，使用左ORB的坐标和右匹配的水平坐标生成双目关键点，通过修补相关性(patch correlation)来细化亚像素subpixel。【？】
对于RGB-D相机：我们在RGB图像上提取ORB特征，如[8]所述，对于每个坐标为$(u_L,v_L)$的特征，我们将深度d转换成虚右坐标(virtual right coordinate)（对标双目里的$u_R$）：

其中$f_x$是水平焦距，b是结构光投影仪(structured light projector)和红外摄像机之间的基线。Kinect和Asus Xtion的基线大约8cm。

深度传感器的不确定性可以用虚右坐标的不确定性来表示。这样一来，双目的输入特征和RGB-D的输入特征在系统其余部分都可以等效处理。

双目关键点分为近点和远点两类：

近点：深度小于40倍的双目/RGB-D基线[5]
- 这是因为近点的深度估计是准确的，且提供了尺度、平移和旋转信息。
- 因此，可以从单独一帧中安全的三角化。
远点：其他
- 远点提供了准确的旋转信息，但是尺度信息和平移信息比较弱。
- 只有提供多视角的情况下，才能对远点进行三角化。

3.1.2 单目关键点

单目关键点定义为$x_m=(u_L,v_L)$，对应找不到双目匹配、以及RGB-D深度无效的情况。

这些关键点没有尺度信息
只能从多视角中三角化
有助于旋转估计和平移估计【？】

3.2 系统启动

使用双目/RGBD相机的一个最主要的好处是可以从单独一帧获取深度信息，不需要像单目那样从运动初始化中获得特定的结构。

在系统启动时，我们将第一帧创建为关键帧，将其位姿设置为原点，从所有双目关键点中创建初始化地图。

3.3 单目和双目约束的BA

在跟踪线程中，系统执行motion-only BA来优化相机位姿。
在局部建图线程中，系统执行local BA来优化关键帧和点的局部窗口。
在回环检测后的第四线程中，系统执行full BA来优化所有关键帧和点。

以上所有BA都使用Levenberg-Marquardt梯度法，该方法基于g2o库[19]实现。

3.3.1 Motion-only BA

优化相机位姿，即优化相机方向$R（SO(3)）$和位置$t（R^3）$。

方法：最小化匹配的3D点$X^i（R^3）$的世界坐标和关键点$x_(.)^i$之间的重投影误差：

（单目：$x_m^i（R^2）$；双目：$x_s^i（R^3）$；i属于匹配集）

3.3.2 Local BA

优化一组共视关键帧$K_L$以及这些关键帧观测到的所有点$P_L$。

其他观测到这些点但是不在待优化关键帧组$K_L$中的关键帧$K_F$，在优化过程中固定不变（即这些关键帧不被优化），但是参与优化方程。

$X_k$表示关键帧k中的关键点与点集$P_L$中的点相匹配的点的集合。

目标函数：

3.3.3 Full BA

Full BA优化地图中所有的关键帧和点。

Full BA实际上只是local BA的一种特殊情况。
原始关键帧固定（不被优化），这是为了消除gauge freedom【？】

3.4 回环检测和全BA

回环检测包括两个步骤：

检测并验证回环
通过优化位姿图来矫正回环

ORB-SLAM2不用管单目ORB-SLAM中的尺度漂移[20]：

这是因为双目/深度信息可以直接获得，这使得尺度是可以被观测的
ORB-SLAM2中，几何验证和位姿图优化时不再需要处理尺度漂移【？】
- ORB-SLAM中，这两个部分是基于相似性的
- ORB-SLAM2中，这两个部分是基于刚体变换的

在ORB-SLAM2中，我们在位姿图优化后加了一个full BA优化来实现方案最优化。这个优化过程开销比较大，我们为它开辟了一个单独的线程，因此系统可以继续创建地图、检测回环。

但这也带来了一个挑战：如何将BA的输出与当前地图相融合。

如果检测到新的回环，而full BA优化正在进行中，就中断优化先执行闭环，然后再启动优化。
当full BA优化结束后，需要将BA优化后的更新版关键帧和点与优化允许时插入的未更新版关键帧和点相融合。
- 通过生成树，我们将更新版关键帧的修正（即未优化位姿到优化位姿的转换）传播给未更新版关键帧。
- 根据对参考关键帧的修正来转换未更新版点。【？】

3.5 关键帧插入

ORB-SLAM2延续了ORB-SLAM中的策略：频繁插入关键帧，严格剔除冗余关键帧。

双目远点和近点的区别允许我们在关键帧插入环节引入新的条件。在大部分场景距离双目传感器较远（远点较多）的情况下，该条件起到关键作用。

在该场景下，我们需要足够数量的近点来准确估计位姿转移，因此，如果跟踪到的近点数量在$\tau_t$以下，且该帧能够创建至少$\tau_c$个新的近点，系统就会插入新的关键帧。

实验值：$\tau_t=100$，$\tau_c=70$

3.6 定位模式

系统包括一个定位模式，该模块有助于在已有的良好建图区域中进行长期的轻量级定位，前提是环境没有发生太大变化。

在该模式下：

局部建图线程和回环检测线程停用，而相机始终在通过跟踪进行重定位（如果需要）
跟踪线程利用visual odometry matches（VO匹配）来匹配地图点。
- VO匹配将当前帧的ORB特征与从历史帧中根据双目/深度信息创建的3D点（地图点？）相匹配
- 这些匹配使得定位在未建图区域中具有鲁棒性，但是依然存在累积漂移【？】
- 地图点匹配使得系统可以在已有地图上进行零漂移定位

随附该模式的演示视频。

4. 评估

环境：Intel Core i7-4790 + 16G RAM

鉴于多线程系统存在不确定性，每个数据序列允许5次，取中值作为估计轨迹的精度。

4.1 KITTI数据集

KITTI数据集[2]：

城市和公路环境中汽车采集的双目序列。
00, 02, 05, 06, 07, 09 序列中存在回环
ORB-SLAM2未成功检测到序列09的回环，原因是回环位于序列末尾很少的帧中

双目传感器：

基线：~54cm
工作频率：10Hz
修正后的分辨率：1240x376

表1显示11个训练序列的结果，这些序列都有公开的ground-truth；与SOTA的双目LSD-SLAM[11]进行比较：
- 度量(metrics)：
  - 绝对 translation RMSE $t_abs$[3]
  - 平均相对 translation误差 $t_rel$[2]
  - 平均相对 rotation误差 $r_rel$[2]
我们的系统在大多数序列中的表现都超过了双目LSD-SLAM，总体上相对误差低于1%。
序列01：训练集中唯一的公路序列，translation误差略差。

由于高速度和低帧率，该序列中能跟踪到的近点比较少，因此translation很难估计。

不过，许多远点可以长时间被跟踪到，因此orientation能够被准确估计，每100m误差0.21度。
轨迹估计范例：

与ORB-SALM的结果相比，双目系统能够处理单目系统处理不了的序列01。【？KITTI不是双目序列吗】

在该序列中，近点仅在少数帧中可见，双目系统可以从单独一个双目关键帧中创建地图点，而单目需要在两个关键帧中寻找匹配，故存在初始化延迟。这点是不跟丢的关键。

此外，双目系统以metric scale估计地图和轨迹，能够估计出地图和轨迹的真实尺度，没有尺度漂移现象。如下图所示：

4.2 EuRoC数据集

EuRoC数据集[21]：

包含11个双目序列，数据来源是在两个不同房间和一个大型工业环境中飞行的微型飞行器(MAV)。
序列分类：easy, meduim, difficult（取决于MAV的速度、光照、场景纹理）

双目传感器：

基线：~11cm
频率：20Hz
分辨率：WVGA（854×480或800×480）

跑序列等于MAV重新访问环境，ORB-SLAM2能够实现地图重用、并在必要时闭环。

表2：
- ORB-SLAM2的定位精度为几厘米，比双目LSD-SLAM要高。
- 由于存在严重的motion blur，系统在V2_03_difficult序列中的部分区域跟丢了。根据[22]所述，该序列可以使用IMU信息进行处理。
图6：轨迹估计范例

4.3 TUM RGB-D数据集

TUM RGB-D数据集[3]：

包含RGB-D传感器采集的室内序列。
序列分为几种类别，用于评估不同纹理、光照、结构条件下，目标重建和SLAM/视觉里程计方法。

选择了部分RGB-D方法常用的序列来进行评估。

表3：对比了ElasticFusion [15], Kintinuous [12], DVO-SLAM [14] and RGB-D SLAM [13]。
- 我们的ORB-SLAM2是唯一一个基于BA的方法（其他都是基于直接法/ICP），且在大多数序列中表现最佳。
我们在ORB-SLAM[1]已经注意到：freiburg2序列存在4%的尺度偏差(scale bias)，该误差可能来源于校准错误(miscalibration)。我们在系统运行中对该偏差进行了补偿。【how？】
图7：从4个序列估计的关键帧位姿中反投影出传感器深度图得到的点云

桌子和海报轮廓清晰笔直，证明了ORB-SLAM2的高精度定位。

4.4 Timing Results

表4：不同图像分辨率和不同传感器的三个序列的计时结果，显示了每个线程的平均值和两个标准差范围。

由于这些序列都是单环的，Full BA线程以及回环检测线程中的一些任务只执行一次，因此只计算一次运行时间。
- 跟踪
  
  平均每帧的跟踪时间小于每个序列的相机帧率的倒数，这意味着我们的系统能够实时工作。
  
  由于双目图像中ORB特征提取是并行进行的，可以看到从V2_02序列的双目WVGA图像中提取1000个ORB特征约等于从fr3_office序列的single VGA image channel中提取1000个特征。
- 回环检测
  
  回环中关键帧的数量可以作为回环检测用时的参考：
  - 虽然KITTI 07序列中包含更多的关键帧，但是室内fr3_office序列的共视图更稠密，因此该序列的回环融合、姿态图优化、full BA任务都更耗时。
  - 共视图越稠密，局部地图包含的关键帧和点的数量就越多，因此局部图跟踪和局部BA也会更加耗时。

5. 总结

6. 参考文献

[1] R. Mur-Artal, J. M. M. Montiel, and J. D. Tard´os, “ORB-SLAM: a versatile and accurate monocular SLAM system,” IEEE Trans. Robot., vol. 31, no. 5, pp. 1147–1163, 2015.

[2] A. Geiger, P. Lenz, C. Stiller, and R. Urtasun, “Vision meets robotics: The KITTI dataset,” Int. J. Robot. Res., vol. 32, no. 11, pp. 1231–1237, 2013.

[3] J. Sturm, N. Engelhard, F. Endres, W. Burgard, and D. Cremers, “A benchmark for the evaluation of RGB-D SLAM systems,” in IEEE/RSJ Int. Conf. Intell. Robots and Syst. (IROS), 2012, pp. 573–580.

[4] R. A. Newcombe, A. J. Davison, S. Izadi, P. Kohli, O. Hilliges, J. Shotton, D. Molyneaux, S. Hodges, D. Kim, and A. Fitzgibbon, “KinectFusion: Real-time dense surface mapping and tracking,” in IEEE Int. Symp. on Mixed and Augmented Reality (ISMAR), 2011.

[5] L. M. Paz, P. Pini´es, J. D. Tard´os, and J. Neira, “Large-scale 6-DOF SLAM with stereo-in-hand,” IEEE Trans. Robot., vol. 24, no. 5, pp. 946–957, 2008.

[6] 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, 2008.

[7] H. Strasdat, J. M. M. Montiel, and A. J. Davison, “Visual SLAM: Why filter?” Image and Vision Computing, vol. 30, no. 2, pp. 65–77, 2012.

[8] H. Strasdat, A. J. Davison, J. M. M. Montiel, and K. Konolige, “Double window optimisation for constant time visual SLAM,” in IEEE Int. Conf. Comput. Vision (ICCV), 2011, pp. 2352–2359.

[9] C. Mei, G. Sibley, M. Cummins, P. Newman, and I. Reid, “RSLAM: A system for large-scale mapping in constant-time using stereo,” Int. J. Comput. Vision, vol. 94, no. 2, pp. 198–214, 2011.

[10] T. Pire, T. Fischer, J. Civera, P. De Crist´oforis, and J. J. Berlles, “Stereo parallel tracking and mapping for robot localization,” in IEEE/RSJ Int. Conf. Intell. Robots and Syst. (IROS), 2015, pp. 1373–1378.

[11] J. Engel, J. Stueckler, and D. Cremers, “Large-scale direct SLAM with stereo cameras,” in IEEE/RSJ Int. Conf. Intell. Robots and Syst. (IROS), 2015.

[12] 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,” Int. J. Robot. Res., vol. 34, no. 4-5, pp. 598–626, 2015.

[13] F. Endres, J. Hess, J. Sturm, D. Cremers, and W. Burgard, “3-D mapping with an RGB-D camera,” IEEE Trans. Robot., vol. 30, no. 1, pp. 177– 187, 2014.

[14] C. Kerl, J. Sturm, and D. Cremers, “Dense visual SLAM for RGB-D cameras,” in IEEE/RSJ Int. Conf. Intell. Robots and Syst. (IROS), 2013.

[15] T. Whelan, R. F. Salas-Moreno, B. Glocker, A. J. Davison, and S. Leutenegger, “ElasticFusion: Real-time dense SLAM and light source estimation,” Int. J. Robot. Res., vol. 35, no. 14, pp. 1697–1716, 2016.

[16] D. G´alvez-L´opez and J. D. Tard´os, “Bags of binary words for fast place recognition in image sequences,” IEEE Trans. Robot., vol. 28, no. 5, pp. 1188–1197, 2012.

[17] E. Rublee, V. Rabaud, K. Konolige, and G. Bradski, “ORB: an efficient alternative to SIFT or SURF,” in IEEE Int. Conf. Comput. Vision (ICCV), 2011, pp. 2564–2571.

[18] R. Mur-Artal and J. D. Tard´os, “Fast relocalisation and loop closing in keyframe-based SLAM,” in IEEE Int. Conf. on Robot. and Autom. (ICRA), 2014, pp. 846–853.

[19] R. Kuemmerle, G. Grisetti, H. Strasdat, K. Konolige, and W. Burgard, “g2o: A general framework for graph optimization,” in IEEE Int. Conf. on Robot. and Autom. (ICRA), 2011, pp. 3607–3613.

[20] H. Strasdat, J. M. M. Montiel, and A. J. Davison, “Scale drift-aware large scale monocular SLAM.” in Robot.: Sci. and Syst. (RSS), 2010.

[21] M. Burri, J. Nikolic, P. Gohl, T. Schneider, J. Rehder, S. Omari, M. W. Achtelik, and R. Siegwart, “The EuRoC micro aerial vehicle datasets,” Int. J. Robot. Res., vol. 35, no. 10, pp. 1157–1163, 2016.

[22] R. Mur-Artal and J. D. Tard´os, “Visual-inertial monocular SLAM with map reuse,” IEEE Robot. and Autom. Lett., vol. 2, no. 2, pp. 796 – 803, 2017.