ORB-SLAM2: an Open-Source SLAM System for Monocular, Stereo and RGB-D Cameras

gaunthan Posted on Oct 17 2018

> Mur-Artal, Raul, and Juan D. Tardós. "Orb-slam2: An open-source slam system for monocular, stereo, and rgb-d cameras." IEEE Transactions on Robotics 33.5 (2017): 1255-1262.

## Introduction
ORB-SLAM2是ORB-SLAM的第二代版本，在一代的基础上，添加了对双目相机、RGB-D相机的支持。第一代的ORB-SLAM基于单目相机。

视觉SLAM可使用的传感器有许多种：单目相机、双目相机、RGB-D相机等。单目相机是最低成本和最小型的传感器配置。**然而，由于单个相机无法获取深度信息，通过单目构建的地图的尺度是未知的。此外，单目SLAM系统在启动时需要多视角信息或滤波技术以执行地图初始化操作，固定地图的尺度。最后，单目SLAM会遇到尺度漂移(scale drift)，而且在相机只有旋转运动时可能会失败。**

由于单目SLAM固有的这些缺陷，ORB-SLAM2将重心放在了双目相机和RGB-D相机上。特别地，它通过将RGB-D采集的深度信息转换为虚拟的右目相机的坐标信息，使得RGB-D数据可以转换为双目数据，进而使用双目SLAM的方法进行问题求解。

## Related work
ORB-SLAM2并不是第一个基于视觉的SLAM系统，在此之前已经有许多相关的研究，并取得了一定的成果。

### Stereo SLAM
论文[^1]提出了一个早期表现比较突出的双目SLAM系统，它是第一个探索**近点和远点**(close and far points)的双目系统。**如果点的深度小于双目基准线$b$的40倍，则被认为是近点，否则为远点。相比于远点，近点具有更加可靠的深度估计值。**

![](https://leanote.com/api/file/getImage?fileId=5bc7df2cab64416802001413)

大多数现代双目SLAM系统都基于关键帧(keyframe-based)，它们在局部区域执行BA(Bundle Adjustment)优化以提供可拓展性。为了消除建图中的累积误差以保证地图的全局一致性，系统需要能够识别闭环并正确进行地图配准。RSLAM[^2]也执行局部BA操作，并且能够闭合回环，但没有强制全局一致性。S-PTAM[^3]也具有局部BA操作，但缺乏大型闭环功能。

与这些方法类似，ORB-SLAM2在关键帧的局部集合进行BA操作(Local BA)，这样该操作的复杂性就与地图的规模无关，因此能够在大规模环境中执行。为了构建全局一致性的地图，ORB-SLAM2在闭合回环时，先对地图进行配准，然后执行因子图优化以最小化回环处的累积漂移，最后进行全局BA(full BA)优化。

视觉SLAM系统除了可以基于关键帧之外，也可以基于直接法(direct method)——LSD-SLAM[^4]便是基于视觉直接法的双目SLAM系统。直接法不依赖特征，因此在有遮挡和纹理较少的环境中也能稳定工作。**然而，直接法的表现可能会因为卷帘快门或非朗伯体反射中尚未建模的影响而严重下降。**

### RGB-D SLAM
RGB-D相机可以直接获取RGB图像的深度信息，许多使用RGB-D作为传感器的SLAM系统使用ICP(Iterative Closest Point, 迭代最近点)进行姿态求解。不同于这些方法，ORB-SLAM2基于BA进行求解。由于BA的计算量远远低于ICP，ORB-SLAM2可以在标准的CPU上运行。此外，虽然它构建的是稀疏的基于关键帧的地图，但是由于关键帧的姿态非常准确，用户可以将姿态与深度信息相融合，构建场景的三维模型。

## Framework
ORB-SLAM2整体由三个线程组成：Tracking（追踪）、Local mapping（局部建图）和Loop closing（闭环）。在执行闭环后会新建一个线程执行全局BA。系统的框架如下所示：

![](https://leanote.com/api/file/getImage?fileId=5bc6a126ab64410cd0001ddf)

图片左侧展示了系统的组成与工作流程，右侧展示了预处理过程的细节。

## Monocular and Stereo Keypoints
ORB-SLAM2的预处理基于特征进行：输入的图片使用ORB特征进行特征提取，得到一组关键点(keypoints)的位置坐标。关键点被定义为三元组$(u_L, v_L, u_R)$。$(u_L, v_L)$是关键点在左目图像的横纵坐标，$u_R$是关键点在右目图像的横坐标。如果假设双目已经校准，那么关键点在左右目图像上的坐标在同一条水平线上。因此，只需要关键点在左目图像的坐标与在右目图像的横坐标即可定义它。这样定义的关键点被称为双目关键点(Stereo keypoints)。

如果关键点没有配对（单目或者双目情况下没有特征匹配上的点）或者深度值非法（RGB-D的情况下），那么这样的点被称为单目关键点(Monocular keypoints)，它定义为二元组$(u_L, v_L)$。如果在多种视角下观测到同一个关键点，那么可以通过三角测量估计它的深度值。单目关键点可以用于优化姿态估计。

## RGB-D points to Stereo points
ORB-SLAM2将深度信息转换为虚拟的右目图像坐标信息，转换公式如下：

$$
u_R = u_L - \frac{f_x b}{d} \tag{1}
$$

其中，$f_x$为相机的焦距，$b$是结构光投影器和红外相机之间的基准线，$d$是特征点的深度信息，$u_L$是特征点在左目图像的横坐标，$u_R$是特征点在（虚拟的）右目图像的横坐标。特别的，对于Kinect和Asus Xtion设备，ORB-SLAM2使用8 cm作为$b$的近似值。

经过这样的处理，一组携带深度信息的单目关键点可以转换为一组双目关键点。

## Bundle Adjustment
ORB-SLAM2中有三个地方用到了BA操作。

### Motion-only BA
ORB-SLAM2在Tracking线程中执行BA优化相机的姿态，此处的BA仅优化相机的姿态（即运动模型），因此被称为Motion-only BA。

相机的姿态可用旋转$R \in SO(3)$和平移$t \in R^3$表示，Motion-only BA通过最小化所有匹配的世界坐标下的三维点$X^i \in R^3$和关键点${\bf x}_{(\cdot)}^i$的重投影误差求出最优的姿态估计（单目：$X_m^i \in R^2$，双目：$X_s^i \in R^3$，$i \in X$）：

$$
\{R, t\} = \underset{R,t}{\operatorname{argmin}} \sum_{i \in X} \rho \left( \lVert {\bf x}_{(\cdot)}^i - \pi_{(\cdot)} (R X^i + t) \rVert_\Sigma^2 \right)    \tag{2} 
$$

从上式可知Motion-only BA利用两帧间的匹配点对帧间姿态进行估计。对单目点，求解的是3D-2D问题；对双目点，求解的是3D-3D问题。式中，$\rho$是鲁棒Huber代价函数，$\Sigma$是关键点的协方差矩阵。$\pi_{(\cdot)}$是投影函数，将三维点从世界坐标系变换到相机坐标系。根据传感器是单目或双目，$\pi_{(\cdot)}$有不同定义$\pi_m$和$\pi_s$：

$$
\pi_m = \left( \begin{bmatrix} X \\ Y \\ Z \end{bmatrix} \right) = \begin{bmatrix} f_x \frac{X}{Z} + c_x \\ f_y \frac{Y}{Z} + c_y \end{bmatrix}, \pi_s = \left( \begin{bmatrix} X \\ Y \\ Z \end{bmatrix} \right) = \begin{bmatrix} f_x \frac{X}{Z} + c_x \\ f_y \frac{Y}{Z} + c_y \\ f_x \frac{X - b}{Z} + c_x  \end{bmatrix}    \tag{3}
$$

单目情况下投影函数等价于关键点左乘相机内参矩阵K后提取前二维。上式中的$(f_x, f_y)$是焦距，$(c_x, c_y)$是主点，$b$是基线。

### Local BA
在Local mapping中也有BA操作，称为Local BA，它优化一组共视的关键帧$K_L$和其中的所有的关键点$P_L$。所谓关键帧共视指的是这些关键帧都观测到了相同的关键点，这代表进行特征匹配时我们几乎总是可以得到配对。集合$K_F$包含所有其他不在$K_L$中的关键帧，这些关键帧里面可能包含$P_L$中的关键点（即$K_L$中的帧没有包含$P_L$中所有的点），因此虽然这些关键帧本身不会被优化，但它们可以用来优化相机的姿态。若定义$X_k$为$P_L中的关键点$和关键帧$k$的匹配集合，则Local BA优化的问题可形式化为：

$$
\{X^i, R_l, t_l | i \in P_L, l \in K_L \} = \underset{X^i, R_l, t_l}{\operatorname{argmin}} \sum_{k \in K_L \cup K_F} \sum_{j \in X_k} \rho (E(k, j)) \\
E(k,j) = {\lVert X_{(\cdot)}^j - \pi_{(\cdot)} (R_k X^j + t_k) \rVert}_\Sigma^2    \tag{4}
$$

### Full BA
ORB-SLAM2在闭环修正后会新建线程执行Full BA对所有的关键帧和关键点进行优化。它在Local BA的基础上，对不共视的关键帧也进行了优化（为了消除规范自由度，初始关键帧是固定的，不进行优化）：

$$
\{X^i, R_l, t_l | i \in P_L, l \in K_L \cup K_F - K_o \} = \underset{X^i, R_l, t_l}{\operatorname{argmin}} \sum_{k \in K_L \cup K_F} \sum_{j \in X_k} \rho (E(k, j)) \\
E(k,j) = {\lVert X_{(\cdot)}^j - \pi_{(\cdot)} (R_k X^j + t_k) \rVert}_\Sigma^2    \tag{5}
$$

式中的$K_o$是由初始关键帧构成的集合。

## Loop Closing
[闭环(Loop Closing)](http://blog.leanote.com/post/gaunthan/vSLAM%E4%B8%AD%E7%9A%84%E5%9B%9E%E7%8E%AF%E6%A3%80%E6%B5%8B%E6%96%B9%E6%B3%95%E7%A0%94%E7%A9%B6)由两步骤组成：

1. 找到闭环：检测和验证闭环的有效性。
2. 使用闭环：利用闭环修正位姿图。

在执行完第二步后，LoopClosing模块会新建线程，执行Full BA优化以求得最优的估计解。由于Full BA作用在所有的关键帧上，操作很费时，因此它将这一过程放到了一个独立的线程中，使操作在后台执行，不影响系统其他部分的工作。若在运行BA优化的过程中，系统又检测到了新的闭环，那么它会直接中断优化，使用这个新的闭环再一次运行BA。

由于执行BA的过程中系统仍然会继续添加新的关键帧，将BA的优化结果和当前的地图的状态相合并是一项具备挑战性的任务。ORB-SLAM2通过生成树(Spanning tree)将修正值从已更新的关键帧传播到未更新的关键帧上，这样即便是运行BA后添加的关键帧，也能根据它与之前关键帧的关系得到恰当的修正。

## Keyframe Insertion
ORB-SLAM2是基于关键帧的，关键帧的质量和数量与系统的表现具有重大的联系。ORB-SLAM2依循了第一代版本的策略：频繁地插入关键帧，后续再删除冗余帧。由于近点比远点对状态估计提供的帮助大得多，我们可以根据关键帧中近点的数量来决定该关键帧是否是好的关键帧。同样，我们也可以根据近点的数量决定是否需要插入关键帧。

如果追踪中的近点的数量低于$\tau_t$，而当前的关键帧至少能够提供$\tau_c$个新的近点，那么就插入这个关键帧。ORB-SLAM2依据经验将这两个超参数设为$\tau_t = 100, \tau_c = 70$。

## Localization Mode
除了SLAM模式，ORB-SLAM2还提供了定位模式。SLAM模式会同时建立地图和定位，而定位模式则是利用已有的地图进行定位。如果我们的环境是静态的，而且我们已经获得了环境的高精度地图，那么切换到轻量级的定位模式则是明智的决定。

在定位模式下，Local mapping和Loop closing线程都不会运行，Tracking线程的工作表现就像一个视觉里程计，它利用相邻帧的匹配点进行位姿估计。当然，地图匹配信息也是必须的，因为视觉里程计只进行单纯的帧间估计，而这个操作会带来累积误差。利用地图点可以确保定位没有漂移，因此定位模式下会使用帧间匹配点和地图匹配点求解姿态。

定位模式可以用在虚拟现实应用中，这类应用中地图通常是已经构建好的，最重要的任务是快速、准确地估计虚拟显示设备的姿态。

## Conclusion
ORB-SLAM2的一个宗旨是让其他领域的研究者可以快速地构建他们的目标应用，不用受制于逐个实现SLAM中的模块。这一宗旨已经得到了很好地践行——许多流行的机器人、虚拟现实应用都使用ORB-SLAM2作为它们的基础框架。

ORB-SLAM2还有许多的扩展工作可以进行，如使用其他的相机配置（无遮挡的多相机组、鱼眼相机、全向相机等）、大规模稠密融合、协作建图、增加运动模糊鲁棒性等。

[^1]: L. M. Paz, P. Pinies, 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, Oct. 2008.

[^2]: 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.

[^3]: T. Pire, T. Fischer, J. Civera, P. De Cristoforis, and J. J. Berlles, “Stereo ´ parallel tracking and mapping for robot localization,” in Proc. IEEE/RSJ Int. Conf. Intell. Robots Syst., 2015, pp. 1373–1378.

[^4]: J. Engel, J. Stueckler, and D. Cremers, “Large-scale direct SLAM with stereo cameras,” in Proc. IEEE/RSJ Int. Conf. Intell. Robots Syst., 2015, pp. 1935–1942