orbslam2

orb_slam2

abstract

ORB-SLAM2是一个完整的slam系统，包括单眼、双眼和RGBD相机，包括地图重用、回环检测、重定位功能。
orbslam2基于单眼和双眼的bundle adjustment能够进行精确的轨迹估计。
ORB-SLAM2在后端上采用的是基于单目和双目的光束法平差优化（BA）的方式，这个方法允许米制比例尺的轨迹精确度评估。
ORB-SLAM2包含一个轻量级的定位模式，该模式能够在允许零点漂移（zero-drift）的条件下，利用视觉里程计（ visual odometry）来追踪未建图的区域（unmapped regions）并且匹配特征点。

introduction

最后，单目SLAM可能会造成尺度漂移,以及在探索的过程中执行纯旋转的时候可能会失败。通过使用一个双目或者RGB-D相机将会解决这些问题，并且能够成为一种更加有效的视觉SLAM的解决方案。

该论文的主要贡献

1. 这是首个基于单眼、双眼和RGBD相机的开源方案。(包括了回环检测、地图重用和重定位)。

2. RGBD结果说明，光速平差优化（ Bundle Adjustment ,BA)比ICP或者光度和深度误差最小（photometric and depth errorminimization.）方法的更加精确。

3. 通过匹配远处和近处的双目匹配的点和单目观测，双目的结果比直接使用双目系统更加精确。

4. 针对无法建图的情况，提出了一个轻量级的定位模式 ，能够更加有效的重用地图。

RGB-D通过评估关键帧的位姿，映射深度图，最终形成一个稠密的点云图。

第二部分讨论相关工作；第三部分谈论ORBSLAM2系统框架；第四部分评价ORBSLAM2；第五部分得不结论。

相关工作

双目SLAM

最早研究双目SLAM方案的Paz等人，基于条件分割和扩展SLAM，显著特点是能够在大型场景中运行。
这是第一个使用近特征点和远特征点的双目SLAM系统，使用一个逆深度参数进行估计。
根据经验，如果深度小于40倍双目的基线，那么这个点就被三角测量化。
ORB-SLAM2中也是采用了该思想处理远近不同的特征点。将在第三部分解释。

目前双目系统大多基于特征匹配和局部BA优化的方式，来获得尺度。
Strasdat等人使用在一个输出窗口的关键帧和位姿的BA优化的联合优化算法。
通过限制窗口大小，实现在全局不一致的情况下，约束时间复杂度。

Mei等人在限定时间复杂度的条件下，使用路标和位姿相关性的方法实现了RSLAM方案，并提出和实现了在活动区域的BA优化算法。即使在全局不一致的条件下，RSLAM也能进回环检测，同时扩大回环两侧的活动区域。

Pire等人把局部的BA运用到了临近的S-PTAM上，但是这种方法缺少回环检测。
在ORB-SLAM2中对局部关键帧采用BA优化，所以，地图的大小和复杂度相互独立。因此，可以在大场景中使用。
然而ORB-SLAM2的目标是全局不变地图。因此，系统首先在回环两端执行。
这与RSLAM相似，以便能使用旧地图定位，然后进行位姿估计，将回环产生的累计漂移最小化。

Engel等人提出的临近双目LSD-SLAM方案，采用的是一种半稠密方法，最小化高梯度的图像区域中的光度误差。
这种方法希望能够在不依赖特征提取的条件下，能够在纹理不清或者模糊运动的过程中获得更高的鲁棒性。然而，直接法的性能会由于滚动（卷帘）快门，或者非朗伯反射的未建模的因素影响而下降。

RGBD SLAM

最早的RGBD-SLAM是Newcombe等人提出的KinectFusion。由于体积的表现形式和缺乏回环检测，这种算法只能在小空间工作。

Whelan 等人提出的Kintinuous能够在大环境中运行。它通过使用一个滚动循环缓冲器和包括使用位置定位和位姿优化来达到回环检测的目的。

第一个开源的RGB-DSLAM方案是由Endres[13]提出的，这是一种基于特征点提取的系统，他的前端采用提取和匹配特征点和ICP来计算帧与帧之间的运动。

后端采用位姿图优化的方式，回环检测约束条件来源于一个启发式搜索。相似的是，Kerl 等人[14]提出的DVO-SLAM，是在关键帧与关键帧之间的优化位姿图，视觉里程计通过计算最小化光度和深度误差来计算约束条件。DVO-SLAM同时在以前的所有帧当中，搜索回环的候选者，而不依赖于位置识别。

Whelan等人[15]提出的邻近ElasticFusion算法，是建立在基于确定环境的地图。这是一种以地图为中心的方法。这种方法忽略了非刚性形变地图的位姿和回环检测的性能，也是不是一个标准的位姿图优化。这种方法在重建和定位的精度都是十分优秀的，但是目前的应用十分有限对于一个房间大小的地图，由于在地图当中面元的数量影响计算的复杂程度。

ORB-SLAM2使用深度信息去合成一个三维坐标，能够精确的提取到一副图像的信息。
ORB-SLAM2能够处理来自双目和RGBD的图像，后端使用BA优化，建立一个全局稀疏的地图重建，因此该方法更加轻量级能在普通的CUP上运行。目标是长时间且精确地定位，而不是一个有很多细节的稠密地图。
然而，高精度的关键帧的位姿，能够融合深度图像以及在计算中得到精准的重建，或者能够处理所有的关键帧和深度图，以及所有的BA并且得到一个精准的3D模型。

ORBSLAM2

针对双目相机和RGB-D相机的ORB-SLAM2建立在单目ORB-SLAM的基础上，它的核心组件，如图2所示。

ORB-SLAM2有三个平行的线程组成：跟踪、局部建图和回环检测。

在一次回环检测之后，会执行第4个线程，去执行BA优化。
跟踪线程在双目或者RGBD输入之前进行，因此剩下的系统模块能够跟传感器模块独立运行。

系统中主要有三个并行的线程：

1. 寻找局部地图的特征，并进行匹配，以及运用BA算法来最小化重投影误差，进行跟踪和定位每帧的相机位姿。

2. 运用局部BA算法设置局部地图并优化。

3. 回环检测能够通过位姿图优化来更正累计漂移误差。在位姿优化之后，会启动第四个线程来执行全局BA算法，来计算整个系统最优结构和运动结果。

系统中使用DBoW2的嵌入式识别模型来重定位、防止跟踪失败、防止已知地图场景重新初始化和回环检测。
这个系统产生关联可见的图，连接两个关键帧的共同点，连接所有关键帧的最小生成树。
这些关键帧的图结构能够得到一个关键帧的局部窗口，以便跟踪和局部建图，并且在大型的环境当中的回环检测部分，作为一种图优化的结构。

这个系统使用相同的ORB特征进行：跟踪、建图和位姿识别。
这些特征在旋转不变和尺度不变性上有良好的鲁棒性，同时对相机的自动增益，曝光和光线的变化表现出良好的稳定性。
并且能够迅速的提取特征和进行匹配，能够满足实时操作的需求，能够在基于词袋的位置识别过程中，显示出良好的精度。

接下来介绍双目或者深度信息是如何利用的。

单目、近处双目和远处双目特征点

系统处理单目或者双目的特征点，分成远处特征点和近处特征点两类。

双目特征点通过三个坐标定义：$X_s=(u_L,v_L,u_R)$,$(u_L,v_L)$在左边图像上，$u_R在右边图像的水平坐标上。
对于双目相机而言，提取两幅图像中的ORB特征，对于每个左边的ORB特征将其匹配到右边的图像中。
之后在左边图像上产生双目ORB特征点，和一条水平的线向右边的图像进行匹配，通过修补相关性来重新定义亚像素（subpixel）。
对于RGBD相机，在图像通道上提取特ORB征点。
对于每个 $(u_L,v_L)$ 坐标，将它的深度值$d$转变成虚拟的右图片坐标：

$$u_R=u_L-\frac{f_{x}b}{d}$$

其中$f_x$是水平焦距（horizontal focal length），并且$b$是结构光投影 horizontal focal length 和 红外摄像机the infrared camera的基线（baseline）。
kinect和华硕 Xtion 精度大约是8cm。

深度传感器的不确定性由虚拟右坐标的不确定性表示。
通过这种方法，双目和RBGD图像的特征点接下来的处理在系统中是相同的。

如果一个双目特征点的深度小于双目/RGBD基线的40倍，那么它会被归类为close，否则归类为far。
因为深度信息可以被准确估计，close关键点可以安全的三角化（triangulate）并且提供尺度（scale)、平移（translation）和旋转（rotation）信息。
另一方面，far关键点可以提供准确的旋转信息，不能提供尺度和平移信息。
当far point被多视图（multiple views） 提供，将其三角化。

单目关键点（Monocular keypoints）: 它被左边图像上的$X_m=(u_L,v_L)$定义。并且对应于在RGBD情况下找不到双目匹配 或者 深度值无效 的所有ORB。
这些点仅仅从多个视图进行了三角测量，没有提供尺度信息，但有助于旋转和平移评估计。

系统引导

使用双目或者RGBD相机的一个主要优点是：可以直接获得深度信息，不需要单目情况中那样做一个特定的SFM初始化。
在系统初始化的时候就创建一个关键帧，即第一帧，将它的位姿初始化，从所有的立体点中创建一个初始化地图。

使用单目或者双目的BA优化

该ORB-SLAM2系统提供BA优化来在追踪线程中进行相机位姿优化（Motion-only BA），优化局部窗口的关键帧和局部建图线程中的点（local BA）,并且在回环检测之后优化所有的关键帧和特征点（full BA)。系统中使用g2o中的Levenberg–Marquardt方法来实现。

纯运动BA优化(Motion-only BA):优化相机的旋转矩阵（orientation R）：$R\in SO(3)$ 和 位移：$t\in {\mathbb{R}}^3$,最小化世界坐标系下的匹配的3D点云$X^i\in {\mathbb{R}}^3$和特征点$X_{(.)}^i$（单目的$X_m^i\in {\mathbb{R}}^2$ 或者双目的$X_s^i\in {\mathbb{R}}^3$）的重投影误差（reprojection error）。其中$i\in \chi$，表示所有匹配的集合。

$$R,t=\underset{R,t}{\mathrm{argmin}}\sum _{i\in \chi }\rho \left({\Vert X_{(.)}^i-{\pi }_{(.)}(R{X}^i+t)\Vert }_{\sum }^2\right)\text{\hspace{1em}(2)}$$

其中，$\rho$是鲁棒核函数Huber(cost function)和 关联于特征点尺寸的协方差矩阵$\sum$。
其中重投影函数${\pi }_{(.)}$,包括单目的${\pi }_m$和纠正双目${\pi }_s$。定义如下：

$$\begin{gathered} {\pi }_m\left(\left[\begin{array}{c}X\\ Y\\ Z\end{array}\right]\right)=\left[\begin{array}{c}{f}_x\frac{X}{Z}+c_x\\ f_y\frac{Y}{Z}+c_y\end{array}\right], \\ {\pi }_s\left(\left[\begin{array}{c}X\\ Y\\ Z\end{array}\right]\right)=\left[\begin{array}{c}{f}_x\frac{X}{Z}+c_x\\ f_y\frac{Y}{Z}+c_y\\ f_x\frac{X-b}{Z}+c_x\end{array}\right]\text{\hspace{1em}(3)} \end{gathered}$$

其中$(f_x,f_y)$是焦距（focal length），$(c_x,c_y)$是主点（principal point），$b$是基线，这些参数都是通过标定获得。

局部BA优化（local BA）: 局部BA优化一系列可见关键帧${\mathcal{K}}_L$和所有在关键帧上的可见点${\mathcal{P}}_L$。所有别的关键帧${\mathcal{K}}_F$（不在${\mathcal{K}}_L$中），观察中的点${\mathcal{P}}_L$有助于代价函数（cost function,但是在优化中是不变的。
定义${\mathcal{X}}_k$作为 ${\mathcal{P}}_L$中的点和一个关键帧$k$中的关键点匹配的集合。这个优化问题如下：

{ X i , R l , t l ∣ i ∈ P L , l ∈ K L } = arg min ⁡ X i , R l , t l ∑ k ∈ K L ∪ K F ∑ j ∈ X k ρ ( E k j ) E k j = ∥ x ( . ) j − π ( . ) ( R k X j + t k ) ∥ ∑ 2 (4) \{ X^i,R_l, t_l |i \in \mathcal{P}_L,l \in \mathcal{K}_L \}= \argmin_{X^i,R_l,t_l}\sum_{k \in \mathcal{K}_L\cup \mathcal{K}_F} \sum_{j \in \mathcal{X}_k} \rho(E_{kj}) \newline E{kj} = \| x^j_{(.)} -\pi_{(.)} (R_kX^j + t_k) \|^2_{\sum} \tag{4} {Xi,Rl​,tl​∣i∈PL​,l∈KL​}=Xi,Rl​,tl​argmin​k∈KL​∪KF​∑​j∈Xk​∑​ρ(Ekj​)Ekj=∥x(.)j​−π(.)​(Rk​Xj+tk​)∥∑2​(4)

全局BA优化（Full BA）:是一个局部BA的特殊情况，在地图中的所有关键帧和点都被优化，除了初始帧，初始帧是固定的，用来消除随机化。

回环检测和全局BA

回环检测分为两步：

1. 首先一个回环信息被确定检测到
2. 然后利用这个回环纠正和优化位姿图

相比于单目的ORB-SLAM中可能出现尺度漂移的地方，这个双目或者深度的信息将会使得尺度信息可观测。
并且，几何校验（geometric vali-dation）和位姿图优化（pose-graph optimization）将不再需要处理尺度漂移，而且是基于刚体变换的，而不是基于相似性。

在ORB-SLAM2的位姿优化后，进行全局的BA优化。这个优化过程非常耗时，所以将其放在单独的线程中，使得系统能够继续执行建图和回环。
然而，这个方法带来一个问题：如何融合BA优化的输出结果和当前的地图。
如果全局BA优化正在执行时，检测到一个新的回环，那么系统将终止优化并进行进行回环检测，之后将再次启动全局优化。
当全局BA完成后，系统将更新的关键帧子集和由完整BA优化的点与未更新的关键帧和在优化运行时插入的点合并。
它通过生成树传播修正的更新关键帧（例如，从未优化的位姿到优化位姿的转变）到没更新的关键帧。
根据校正参考帧来改造呢些未更新的特征点。

关键帧的插入

ORB-SLAM2遵循在单目ORB-SLAM中提的法则，即经常插入关键帧并且剔除上一帧的冗余。
远近特征点的差异为我们插入一个新的关键帧提供了条件，这在大场景的条件下是至关重要的，如图3所示。

图三中，绿色的特征点深度小于40倍双目的基线，蓝色特征点大于40倍双目的基线，在这种数据集当中，需要插入大量的关键帧，以便于能够让近处的特征点更加精确的估计平移和尺度，远处的特征点来估计方向，但是不能够计算平移和尺度。

定位模式

ORB-SLAM2包括一个定位模式（Localization Mode），该模式适用于轻量级以及在地图已知情况下长期运行，只要那个环境没有发生剧烈变化。
在该模式中，局部建图和回环检测的线程中是停用的，并且这个相机始终都是在通过追踪进行重定位。
在这个模式下，追踪模块使用视觉里程计匹配图像的点云。
视觉里程计匹配当前帧的ORB算子和由双目或者深度相机收集的3D点云。
这些匹配使得在没有地图的区域也能够精确重新定位，但是漂移将会被累加。地图点云匹配要确保在一个已经存在的地图当中零漂移定位，这个模型在附带的video当中会显示。

评估

我们使用三个著名的数据集来评估ORB-SLAM2的算法的性能。我们在一台16G的RAM，Intel Core i7-4790的台式机运行，以低于传感器的帧率，对处理跟踪时间求平均。我们运行数据集5次，取中间值，来消除多线程系统的不确定性。我们开源了在运行这几个系统的数据集的方法包括标定具体操作实现。

KITTI数据集

KITTI数据集包含双目数据，这些数据从一个正在高速公路上行驶的车上采集到的。这个双目传感器有个小于54厘米的基线并且在在1392*512像素上，以10Hz的采样速率进行采样，其中序列00,02,05,06，和09包含回环。
我们的ORB-SLAM2能够检测出回环并且能够地图重用，除了09序列以外，09序列的回环只发生在尾端少数的几帧当中。
表1显示的这个结果11个训练数据，这是一个公开的真实数据，对比于原先的LSD-SLAM算法，我们展示了的双目SLAM系统测试数据结果。
我们使用两个不同的指标：绝对平移 SLAM（均方差）$t_{abs}$，平均相相对平移误差$t_{rel}$和平均旋转误差$r_{rel}$。
我们的系统在大多数序列当中比双目的LSD-SLAM要优秀很多，并且能够获得的相对误差低于1%。
序列01如图3所示，是数据集中唯一一个高速公路数据集，并且平移误差稍微差一些。
平移误差是在这个数据集当中难以评估的，因为只有几个很近的点能够被追踪，由于很高的速度和较低的帧率。
然而旋转能够被精确的评估，达到每100米0.21度误差，很多较远的点能够被追踪。
如图4所示，显示了一些评估的轨迹。

与单目的结果比较，该系统提供的双目版本能够处理单目系统处理失败的01数据集，见图3，仅仅在几帧上能够看见近点。
双目版能够从一个双目关键帧创建ponits，而不是单眼的延迟初始化（包括查找两个关键帧之间的匹配），这在此数据集中至关重要，使得系统不会丢失跟踪。
此外，双目系统会使用米制比例尺估算地图和轨迹， 不会出现轨迹漂移，如图5所示。

图5：黑线表示估计轨迹，红线表示真实轨迹，在数据集KITTI08上。
左图是单目ORB-SLAM，右边是双目ORB-SLAM2。
单目的ORB-SLAM出现一些尺度漂移，在这个数据集上，尤其在转弯的时候。
相反，所提出的双目版本能够估计轨迹和地图的真实比例，而不会发生比例漂移。

EuRoC 数据集

EuRoC数据集包含了11个双目数据集，从一个微型飞行器（from a micro aerial vehicle ，MAV），在不同的房间和工业环境中采集的。
双目传感器有一个大约11cm的基线和提供一个20HZ的WVGA图片。
这个数据集根据MAV的速度、照明和场景分为简单、中等和困难等级。
在所有数据集中，MAV都会重新访问环境，并且ORB-SLAM2能够重用map，并在必要时回环检测。
表2展示了ORB-SLAM2在所有数据序列上的绝对平移的REMS（均方误差），并与双目的LSD-SLAM比较。
ORB-SLAM2的定位精度达到了几厘米，比双目LSD-SLAM更精确。
在V2_03_difficult的数据序列上由于一些严重的运动模糊，ORB-SLAM2在这些部分追踪丢失。
但是，这些数据序列可以使用IMU信息处理。
图6示出了与地面真相比较的计算轨迹的示例。

TUM RGB-D数据集

TUM RGB-D数据集包含来自RGB-D传感器的室内数据序列，这些传感器分为几类，以评估在不同纹理，光照和结构条件下的对象重建和SLAM/里程计。
和大多数RGB-DSLAM方法一样，我们将实验结果展示在一个序列子集当中，在表格3当中，我们比较我们方法和其他方法的精准性，例如ElasticFusion，Kintinuous，DVO-SLAM以及RGB-DSLAM，ORB-SLAM2是唯一一种基于BA方法，并且比其他的方法都更加优秀。
正如我们在[1]中已经注意到的RGB-D SLAM结果一样，freiburg2序列的depthmap具有4％的比例偏差，可能是由于校准不当所致，我们在运行中对此进行了补偿，并且可以部分解释我们取得更好结果的原因。
图7显示的点云的结果来源于反重投影传感器深度图（backprojecting thesensor depth maps）。
实验的结果显示，ORB-SLAM2很好的展示了桌子和海报的轮廓线，以及高精度的重定位。

计时结果

为了完成系统的评价，我们提供了表IV，展示了在3个有不同图片分辨率和传感器的数据集数列上的计时结果。
显示了每个线程任务的平均值和两个标准偏差范围。
由于这些数据序列包含一个回环，因此full BA和回环检测线程的某些任务仅执行一次，并且仅报告一次测量。
每帧的平均跟踪时间低于每个序列的相机帧速率的倒数，这意味着我们的系统能够实时工作。
由于双目图像中的ORB提取是并行的，因此可以看出，在V202的双目WVGA图像中提取1000个ORB特征类似于在fr3office的单个VGA图像通道中提取相同数量的特征。

回环中的关键帧数作为与时间相关的回环检测的参考。
尽管KITTI 07中的包含更多关键帧，但为室内fr3office建立的可共性图更密集，因此回环融合、位姿图优化和全局BA任务更加费时。
更高的密集图片使得局部建图包含更多的关键帧和points，因此局部追踪和局部BA也会更加费时。

结论

本文呈现了一个基于于单目，双目和RGB-D传感器的完整SLAM框架，在实时和标准的CPU的前提下能够进行重新定位和回环检测，以及地图的重用。
在实验当中，我们关心的是在大场景中建立可用的地图和长期的定位。
所提出的具有系统重新定位功能的定位模式（localization mode）可为已知环境提供非常精确、零漂移和轻量级的定位方法。
此模式对于某些应用程序可能很有用，例如在建图良好的空间中跟踪虚拟现实中的用户视点（viewpoint）。

与最新技术的比较表明，ORB-SLAM2在大多数情况下都可以达到最高的精度。
在KITTI视觉里程计基准ORB-SLAM2 是目前最好的双目视觉SLAM方案。
关键的，与其他双目视觉里程计方法相比，ORB-SLAM2在已经建立的地图中达到零漂移的定位。

令人吃惊的，我们的RGB-D结果说明了如果需要精确的相机定位，BA方法比直接法和ICP方法表现更好，此外计算成本更低，在事实运行中不需要GPU。

我们已经开源了代码，包括例子和说明，因此可以容易的使用。据我所知，ORB-SLAM2是最好的开源slam系统，它可以工作在单目、双目和RGB-D输入源上。
而且，我们的开源代码中包含了一个使用单目相机增强现实的应用，以展示我们的方案的潜力。

未来的扩展可能包括：增加一些例子；非重叠多摄像机；鱼眼镜头和全向摄像机支持；大规模密集的融合；协作建图；增强运动模糊。

代码：https://youtu.be/kPwy8yA4CKM