个人博客：http://www.chenjianqu.com/
原文链接：http://www.chenjianqu.com/show-105.html
论文：Raul Mur-Artal, Juan D. Tard ´ os´.ORB-SLAM2: an Open-Source SLAM System for Monocular, Stereo and RGB-D Cameras.2017
ORB-SLAM2：一个开源的支持单目、双目和RGB-D相机的SLAM系统
摘要——我们提出了一个完整的SLAM系统，支持单目相机、双目相机、RGB-D相机，拥有地图重用、回环检测、重定位等特性。本系统可以在标准的CPU上实时运行，应用范围从室内小型手持设备的序列，工业环境无人机的序列，再到城市里汽车的序列。我们的后端基於单目视觉和立体视觉的BA，可以使用度量尺度进行精确的轨迹估计。我们的系统包含一个轻量级的定位模式，这个定位模式利用未建图区域的视觉里程计轨迹，匹配到地图点，可以实现无漂移定位。我们在29个流行的公共序列上进行评估，结果显示我们的方法达到了SOTA精度，是最精确的SLAM解决方案。我们公开了源代码，不仅为了SLAM领域的发展，也是为了能提供给其他领域的研究者一个开箱即用的SLAM解决方案。
I.引言
同步定位与地图构建（SLAM）是CV和机器人领域这20年来一个研究热点，也吸引了高科技公司的注意力。SLAM技术构建一个未知环境的地图并在地图中定位。SLAM的一个基本要求是实时运行。在各种各样的传感器中，相机是比较便宜的而且可以提供环境丰富的信息，这可以提高位置识别的精度和鲁棒性。因此基于相机的视觉SLAM，是当前的主流研究。位置识别是SLAM系统的一个关键模块，它用于回环检测（检测什么时候传感器返回到已经建图的区域，然后修正回环的误差）以及在SLAM跟踪丢失或重新初始化时后进行重定位。跟踪丢失的原因可能是遮挡、剧烈运动。
VSLAM可以仅仅使用单目相机实现，这是最便宜、最轻量的传感器配置形式。然而单个相机无法观测到深度信息，因此地图和估计轨迹的尺度未知。此外，系统初始化(system bootstrapping)需要多视图或滤波技术来生成初始地图，因为它无法在第一帧进行三角化。最重要的一点是，单目SLAM会遇到尺度漂移问题，而且在纯旋转环境中可能无法工作。使用立体相机或RGB-D相机可以解决上述的问题，可以构建一个更加可靠的VSLAM系统。
本文我们在单目的ORB-SLAM[1]的基础上提出了ORB-SLAM2，主要由以下贡献：
*这是第一个同时支持单目、双目、RGB-D相机的开源的SLAM系统，包含回环检测、重定位和地图重用等功能。
*我们的RGB-D结果表明，与基于ICP或光度和深度误差最小化的SOTA方法相比，使用BA可以获得更高的精度。
*通过使用近、远立体点(stereo points)和单目观测，我们的立体测量结果比SOTA的直接立体SLAM更精确。
*一种轻量级的定位方式，可以在无法建图的情况下高效的重用地图。
下图1展示了基于立体视觉和RGB-D的ORB-SLAM2的输出实例：
立体视觉的例子展示了KITTI数据集[2]00序列的最终的轨迹和离散重建。这是一个拥有多个回环的城市序列，但是ORB-SLAM2能成功的检测。RGB-D的例子展示了TUM RGB-D[3]数据集中fr1_room序列的关键帧位姿估计,以及一个稠密点云，通过估计的位姿反向投影(backprojecting)传感器深度图进行渲染。注意我们的SLAM不执行任何类似KinectFusion[4]那样的融合(fusion)，但是这个良好的结果表明我们系统估计的关键帧位姿的精度很高。更多实例我们将在相关视频进行展示。
在这篇论文的剩余部分，我们讨论Section II中的相关工作，在Section III中描述我们的系统，在Section IV展示评估结果，最后在Section V进行总结。
II.相关工作
本节我们讨论双目SLAM和RGBD-SLAM的相关工作。
A.双目SLAM
早期双目SLAM中一个出色的工作来自Paz et al.[5]。基于条件独立的分治(Conditionally Independent Divide and Conquer) EKF-SLAM算法，它比当时其它的方法能在更大的环境中运行。最重要的是，这是第一次利用近点和远点（即由于立体相机的视差很小而无法可靠估计其深度的点）的双目SLAM，对后者使用了深度逆参数化[6]。他们经验性表明，如果点的深度小于双目基线的40倍，可以可靠地进行三角化。这里我们遵从这个策略，以不同的方式处理近点和远点，这将在Section III-A进行解释。
大多数现代双目SLAM系统是基于关键帧的，并在局部区域执行BA，实现可伸缩性。Strasdat et al.[8]的工作是对关键帧的内窗口中的BA(点-位姿约束)和外窗口中的位姿图(位姿-位姿约束)进行联合优化。通过限制这些窗口的大小，实现恒定的时间复杂度，但是不能保证全局一致性。Mei et al.[9]的RSLAM使用路标和位姿的相对表示，在活动区域执行相对BA(relative BA)，这样可以限制为恒定时间。RSLAM能够检测回环，这允许拓展回环两边的活动区域，但不强制实现全局一致性。最近Pire et al.[10]的S-PTAM执行局部BA，然而它缺乏大型的回环检测。跟这些方法类似，我们在一组局部关键帧中执行BA，这样复杂性就与地图大小无关，我们可以在大环境中运行。然而我们的目标是构建一个全局一致的地图。当闭合一个回环时，我们的系统就像RSLAM一样，先将两边进行对齐，这样跟踪就能继续使用旧地图进行定位，然后执行一个位姿图优化，将回路中的累积漂移减至最小，接下来执行全局BA。
最近Engel et al.[11]的LSD-SLAM是一个基于半稠密、直接法的SLAM，最小化图片中高梯度区域的光度误差。不依赖于特征点，这个方法预计对运动模糊或纹理较差的环境更加鲁棒。然而作为一种直接法，它的性能会因为未建模的影响比如rolling shutter或non-lambertian reflectance而严重降低。
B.RGB-D SLAM
最早也是最著名的RGB-D SLAM系统是Newcombe et al.[4]的KinectFusion。该方法将来自RGB-D相机的所有深度数据融合到一个体积密度模型中，使用ICP计算相机位姿。该系统的被限制在一个很小的工作空间中，由于它的体积表示和缺乏回环检测。Whelan et al.[12]的Kintinuous能够运行在大型环境中，因为它使用了rolling cyclical buffer，以及包含了基于位置识别和位姿图优化的回环检测。
Endres et al.[13]的RGB-D SLAM可能是第一个流行的开源系统。这是一个基于特征的系统，整个前端通过特征匹配和ICP算法计算帧到帧运动。后端使用启发式搜索的回环约束执行位姿图优化。类似的，Kerl et al.[14]的DVO-SLAM的后端优化位姿图，其中关键帧到关键帧的约束是从视觉里程计计算出来的，该视觉里程计最小化光度误差和深度误差。DVO-SLAM还以启发式搜索之前所有的帧，以获得闭环候选，而不是依赖于位置识别。
Whelan et al.[15]最近的工作ElasticFusion构建了一个基于面片(surfel)的环境地图。这是一个以建图为中心的方法，舍弃位姿，执行回环检测，将non-rigid deformation应用到地图，而不是标准的位姿图优化。该系统的详细的重建和定位精度令人印象深刻，但是目前的实现仅限于房子大小的地图，因为它的复杂程度与地图中surfel的数量相关。
正如Strasdat et al.[8]所说的那样，我们的ORB-SLAM2使用深度信息合成立体座标，用于提取图片上的特征。这样我们的系统就不需要知道输入是双目还是RGB-D了。不同于上面所有的方法，我们的后端基于BA，而且构建一个全局一致的离散重建。因此我们的方法是轻量级的，可以工作在标准CPUs上。我们的目标是系统能长期运行和全局一致的定位，而不是构建细节最丰富的稠密重建。However from the highly accurate keyframe poses one could fuse depth maps and get accurate reconstruction on-the-fly in a local area or post-process the depth maps from all keyframes after a full BA and get an accurate 3D model of the whole scene.
III.ORB-SLAM2
用于双目和RGB-D相机的ORB-SLAM2基於单目的、基于特征点的ORB-SLAM[1]。Fig.2展示了系统的框架：
系统有三个线程：1)跟踪线程：通过与局部地图进行特征匹配，应用motion-only BA最小化重投影误差，从而实现每一帧的定位。2)局部建图线程：管理局部地图并优化它，执行局部BA。3)回环线程：检测大回环并通过执行位姿图优化修正累积漂移。在位姿图优化后，该线程会启动第四个线程用于执行全局BA，计算最佳结构和运动解。
本系统嵌入了一个基于DBoW2[16]的位置识别模块，用于重定位。在跟踪丢失(比如被遮挡)、在已经建图的场景重新初始化、回环检测的时候都会用到位置识别模块。本系统维持一个共视图[8]和关键帧的最小生成树，共视图中节点是关键帧，当两个关键帧存在共视点时，就会存在一条边将两个节点连接。这些图结构可以检索某个关键帧的局部窗口，因此跟踪和局部建图可以在局部运行，这样使得系统可以工作在大型环境中，并且当闭合回环时执行位姿图优化执行的结构。
本系统的跟踪、建图和位置识别任务中都使用相同的ORB特征[17]。这些特征对旋转和尺度都很鲁棒，而且对相机的自动增益、自动曝光、光照变化都展示了很好的不变性。而且，ORB特征的抽取和匹配速度很快，这允许系统实时运行。此外，ORB特征在词袋位置识别[18]中展示了很好的精度和回召率。
在后面的章节我们会展示立体/深度信息如何被利用以及哪些部件会受到影响。关键每个系统每个部分更详细的信息，读者可以参考单目版本[1]。
A.单目、近立体和远立体点
ORB-SLAM2作为一种基于特征的方法，预处理输入，在显著关键点位置抽取特征，如Fig.2b所示：
然后就可以舍弃输入图像了，整个系统的操作基于这些抽取的特征，因此系统独立于所用的传感器，无论是双目还是RGB-D。我们的系统处理单目关键点(monocular)和立体关键点(stereo)，立体关键点被进一步分为近关键点和远关键点。
立体关键点(stereo keypoints)定义为x_s=(uL,vL,uR)，其中(uL,vL)是双目相机中左图像座标系的座标，uR是右图像座标的水平轴的值。对于双目相机，我们在左右两个图像抽取ORB特征，然后对左图每个ORB特征都在右图寻找一个匹配的点。可以高效的设计立体矫正图像， 使得极线是水平的。然后用左图ORB特征点的座标和右图匹配的水平座标来生成立体关键点，并通过patch correlation进行亚像素细化（subpixel refined）。对于RGB-D相机，我们从RGB图像中抽取ORB特征，然后如Strasdat et al.[8]提出的方法，对每个座标为(uL,vL)的特征点，我们将它的深度值d转换到一个虚拟的右相机座标：uR=uL - fx*b/d，fx是水平焦距，b是structured light projector和红外相机的基线长度，对于Kinect和Asus Xtion大概为8cm。深度传感器的不确定程度由虚拟的右相机座标表示。通过这种方式，我们双目相机和RGB-D相机就可以等价的被系统处理。
如果立体点的深度小于双目相机/RGB-D相机的40倍，则将该点分类为近点，如[5]所示，否则将其分类为远点。近点可以被安全的三角化，因为此时深度估计是准确的，并提供尺度、平移和旋转信息。而远点可以提供精确的旋转信息，但是尺度和平移信息比较弱。当远点被其它视图支持时，我们可以对它进行三角化。
单目关键点(Monocular keypoints)定义为左相机座标x_m=(uL,vL)，对应到该ORB特征点双目匹配失败或RGB-D相机深度值无效的座标。这些点只能被多个视图三角化，不提供尺度信息，但是可以提供旋转和平移估计。
B.系统引导
使用双目相机或RGB-D相机的主要优点之一是可以仅从一次图像采集中获得深度信息，而不需要像单目相机那样运动初始化中获得特定结构。在系统初始化时，我们使用第一帧作为关键帧，设置它的位姿为原点，使用所有的立体点创建初始地图。
C.Monocular和Stereo约束的BA
我们的系统在跟踪线程中执行motion-only BA去优化相机位姿，在局部建图线程中执行local BA优化关键帧和地图点的局部窗口，在回环线程中执行full BA优化所有的关键帧和关键点。我们使用在g2o中实现的Levenberg-Marquardt方法[19]。
Motion-only BA：通过最小化匹配点之间的重投影误差优化该帧相机位姿的旋转R∈SO(3)和平移t∈R^3，这里的匹配点是世界座标3D点Xi∈R^3和立体关键点/单目关键点xi，i表示该帧所有的匹配对索引。公式如下：
其中ρ是robust Huber代价函数，Σ是与关键点的尺度有关的协方差矩阵。Π(.)是投影函数，Πm是单目投影函数，Πs是矫正后的立体函数，定义如下：
其中(fx,fy)是焦距，(cx,cy)是相机主点，b是基线长度。
Local BA优化一组共视的关键帧K_L以及被这些关键帧看到的所有地图点PL2。其它的不在K_L里的关键帧K_F，它们观察到的点保持不变。定义Xk是一组在P_L中的点与关键帧k中的点的匹配，其优化问题可以表示如下：
Full BA是local BA的特殊情况，所有的关键帧和地图点都被优化，除了用来消除gauge freedom的初始关键帧之外。
D.回环检测和全局BA
回环检测的执行有两步，首先是检测并验证回环，第二是通过位姿图优化对回环进行修正。与可能会出现尺度漂移[20]的单目ORB-SLAM相比，立体/深度信息使得尺度可观测，几何验证和位姿图优化不再需要处理尺度漂移，因此不是通过相似性而是通过刚体变换实现。
在ORB-SLAM2中，我们在位姿图优化后加入了一个全局BA优化以获得最优解。这个优化可能很费计算资源，因此我们另外开一个线程执行它。允许系统继续创建地图和检测回环。然而，这可能会存在BA输出和当前地图状态如何合并的问题。当全局BA正在运行而新的回环右检测到的时候，我们将中止优化过程，而是继续闭合回环，然后重新启动全局BA。当全局BA完成的时候，我们需要将由全局BA更新的关键帧子集、地图点，与后来插入的关键帧、地图点进行合并。这是通过生成树将更新的关键帧的校正(比如从非最佳位姿到最佳位姿的转换)传播到未更新的关键帧来完成的。未更新的点通过应用于参考关键帧的矫正进行变换。
E.关键帧的插入
ORB-SLAM2根据单目ORB-SLAM引入的策略插入关键帧，这个策略插入和删除关键帧很频繁。近立体点和远立体点之间的差别，使得我们可以引入新的关键帧插入的条件，这在恶劣环境中非常关键，因为场景大部分都是远立体点，如Fig.3所示：
在这钟环境中，我们需要足够的近点来精确的估计相机的平移，因此如果系统此时跟踪的近立体点的数量小于τ_t，并且该帧可以创建至少 τ_c个新的近立体点，那么系统将插入新的关键帧。我们经验性的发现τ_t=100，τ_c=70时，系统工作的很好。
F.定位模式
我们包含了一个定位模式(Localization Mode)，在场景没有发生大的变换的情况下，可以在已经建图的区域进行长期的、轻量的定位。在这个模式中，局部建图线程和回环检测线程并不工作，相机通过跟踪线程或重定位连续的定位。在此模式中，跟踪线程利用视觉里程计匹配和地图点匹配。视觉里程计匹配是当前帧中的ORB特征和之前的帧创建的3D点之间的匹配。这些匹配使得在未建图区域的定位更加鲁棒，但是漂移会累积。地图点匹配确保对现有地图的无漂移定位。可以在相关视频上看到这个模式的演示。
IV.评估
我们在三个流行的数据集上评估ORB-SLAM2，并将结果与其它SOTA的SLAM相比较，使用原始作者发布的结果，使用标准评估指标。我们在16Gb RAM的Intel Core i7-4790桌面电脑上运行。考虑到多线程系统的不确定性，我们将每个数据序列运行5次，展示中位数结果，以估计轨迹的准确性。我们的开源实现包括校准和在这些数据集上运行的说明。
A.KITTI数据集
KITTI数据集[2]包含从一个汽车在市区和高速路上记录的双目序列。这个双目传感器的基线长大约54cm，录制频率为10Hz，矫正后的分辨率为1240x376。序列00,02,05,06,07,09包含回环。我们的ORB-SLAM2检测到所有的回环，而且能够在之后重用地图，除了序列09，因为它只在最后的几帧出现回环。Table I展示了11个训练序列的结果，还对比了ground truth，并与SOTA的SLAM：双目LSD-SLAM[11]进行对比:
我们使用了两个不同的指标，[3]提出的绝对平移均方根误差(absolute translation RMSE) t_abs，以及[2]提出的平均相对平移(average relative translation) t_rel和旋转 r_rel误差。我们的系统在大多数序列都胜过Stereo LSD-SLAM，多数序列的相对误差低于1%。序列01,如Fig3，是训练集中唯一的高速路序列，其平移误差稍差。在这个序列中，平移误差更难去估计，因为车辆高速行驶而低帧率采集导致能跟踪的点很少。然而方向可以被精确的估计，达到每100m 0.21度的误差，因为有很多点可以被跟踪。Fig.4展示一些轨迹估计的实例：
与[1]提出的单目ORB-SLAM相比，这里的立体版本可以处理序列01，该序列使得单目ORB-SLAM失败了。在这个高速公路序列中，看Fig.3，近立体点仅在很少的帧可以被看到。双目相机/RGB-D相机可以仅从一个关键帧创建点，而不需要像单目相机那样延迟初始化(需要寻找两个关键帧之间的匹配)，这是在该序列中不跟踪丢失的关键。此外，立体版本的系统用度量的尺度估计地图和轨迹，不受尺度漂移的影响，如Fig.5：
B.EuRoC数据集
最近的EuRoC数据集[21]包含了11个立体序列，这些序列通过微型飞行器(MAV)在两个不同的房间和大型工业环境飞行录制得到。这个双目传感器的基线大约是11cm，提供20Hz录制的WVGA图像。这些序列根据MAAV的速度、光照、场景纹理被分类为easy、medium、difficult。所有的序列中MAV都会重访问环境，ORB-SLAM2能够重用地图和回环检测。Table II展示了ORB-SLAM2在所有序列的绝对平移均方根误差，同时与Stereo LSD-SLAM[11]进行比较：
ORB-SLAM2实现定位精度在几厘米，比Stereo LSD-SLAM更加精确。我们的跟踪线程在V2_03_difficult序列丢失了，由于严重的运动模糊。正如[22]展示的那样，这个序列可以通过IMU信息处理。Fig.6展示了计算的轨迹与ground truth轨迹比较的实例：
C.TUM RGB-D数据集
TUM RGB-D数据集[3]包含来自RGB-D传感器的室内序列，这些序列被分为几个类别，用于评估目标重建和SLAM/里程计算法在不同纹理、光照和结构条件下的表现。我们展示了在最常用的序列上的结果。Table III我们比较了我们的精度与当前SOTA方法：ElasticFusion[15]，Kintinuous[12]，DVO-SLAM[14]和RGB-D SLAM[13]进行的比较。
我们的方法是唯一一个基于BA的系统，而且在大多数序列中超过了其它方法。正如[1]中RGB-D SLAM的结果，freiburg2序列的深度图大约由4%的尺度误差，这可能是由于误矫正，我们已经在运行时进行了补偿，并且可以部分解释我们的明显有更好结果。Fig.7展示了4个序列中从计算的关键帧位姿反向投影传感器深度图产生的点云：
良好的清晰度和书桌、海报的直线轮廓证明了我们这个方法的高精度定位。
D.时间消耗结果
为了全面的评估系统，我们在Table IV展示了时间消耗的结果，在3个不同图像分辨率和传感器的序列上进行测试：
显示了每个线程时间消耗的平均值和标准差。这些序列包含一个单一回环，因此全局BA和回环闭合的一些任务只执行一次，仅报告一次时间测量。每帧的平均跟踪时间低于每个相机帧率的倒数，意味着我们的系统可以实时工作。因为ORB特征抽取在双目相机中是并行化的，在V2_02的立体WVGA图像中抽取1000个ORB特征类似于在fr3_office的单个VGA图像中抽取同样数量的特征。
回环中关键帧的数量与回环闭合的时候存在关系。当KITTI 07序列中的回环包含更多的关键帧时，室内fr3_office构建的共视图更加稠密，因此回环融合、位姿图优化、全局BA任务更加昂贵。共视图越稠密，使得局部地图包含更多的关键帧和地图点，因此局部建图线程和局部BA也更加昂贵。
V.总结
我们提出了一个基於单目、双目、RGB-D相机的完整SLAM系统，能够在标准CPU上实时执行重定位、回环闭合、地图重用。如实验所示，我们专注于构建全局一致的地图，以在各种环境中进行长期定位。所提出的具有定位模式可以为已知环境提供一种非常健壮，零漂移和轻量级的定位方法。此模式对某些应用可能很有用，比如在良好建图的空间的AR：跟踪用户的视角。
与其它SOTA系统的比较展示了ORB-SLAM2达到了最高的精度。在KITTI视觉里程计benchmark中ORB-SLAM2是当前最好的立体SLAM解决方案。关键的是，与其它立体视觉里程计方法相比，ORB-SLAM2在已建图区域实现了零漂移定位。
出乎意料的是，我们的RGB-D结果表明，如果想要最精确的相机定位，这BA方法优于直接法或ICP，并且BA的计算成本更低，不需要GPU也可以实时运行。
我们已经开源了，并给出了例程和文档，以便于被其它开发者使用。ORB-SLAM2据我们所知，是第一个可以通过支持单目、双目、RGB-D的VSLAM系统。此外，我们的源代码包含了一个使用单目相机实现AR的示例，以证明我们这个系统的潜力。
举例来说，将来的扩展可能包括不重叠的多相机，鱼眼镜头或全向相机支持，大规模密集融合，协作映射或增强的运动模糊鲁棒性。