论文：ORB-SLAM：a Versatile and accurate Monocular SLAM System

前言

一、介绍

PTAM（parallel tracking and Mapping）系统局限于小尺度场景中，提供简单但是有效的关键帧选择、特征匹配、三角化、重定位技术，但是具有以下缺点：（1）缺少闭环优化；（2）重定位的不稳定性；（3）需要人工初始化地图。ORB-SLAM在PTAM系统基础上构建， ORB-SLAM系统具有以下优点：

（1）在追踪、建图、重定位和回环过程中采用同一种特征点-ORB，使得该系统在没有GPU加速下实时运行，并且对光照和视角变化具有良好的鲁棒性。

（2）在追踪和建图过程中使用局部共视地图，而不是全局地图，使得该系统能够在大尺度场景下实时运行。

（3）在共视图上通过最小生成树生成本质图，简化地图结构，使得系统能够实时的闭环优化。

（4）在特征跟踪失败情况下，通过词典模型匹配，使得系统能够实时的重定位。

（5）采用适者生存的方法来筛选地图点和关键帧，从而提高系统追踪的鲁棒性和系统的可持续运行。

二、相关工作

A、地点识别（place recognition）

B、地图初始化（map initialization）

C、单目SLAM（monocular SLAM）

三、系统概观

A、特征点选择（feature choice）

　　ORB-SLAM系统的一个重要特点就是在追踪和建图线程的地点识别中使用同一种特征，从而保证了实时的重定位和回环检测。首先为了满足系统实时性要求，每帧图像提取特征的时间应该少于33ms，从而排除主流的SIFT特征点（约300ms）、SURF特征点（约300ms）和最近的A-KAZE特征点（约100ms）。然后为了实现地点识别的性能，该特征点需要旋转不变性，从而排除BRIEF描述子和LDB描述子。我们选择ORB（Oriented FAST and Rotated BRIEF）特征点，它是由改进后的FAST角点（具有方向和尺度）和256位二进制的BRIEF描述子组成，使得ORB算法不仅保持旋转和尺度不变性，而且能满足实时性要求。

B、三个线程：追踪、局部地图和闭环优化（tracking、local mapping and loop closing）

　　如上图所示，ORB-SLAM系统主要运行三个并行线程：特征追踪线程（主线程）、局部建图线程和回环优化线程。

　　特征追踪线程主要负责估计每一帧相机的位姿和选择新的关键帧。首先初始化地图，然后基于前一帧和匀速运动模型来初始化当前帧的位姿，如果当前帧丢失，则通过全局重定位来初始化当前帧的位姿。一旦获取当前帧的初始化位姿和一组匹配点对，构建当前帧的局部地图，通过将局部地图中的地图点投影到当前帧中寻找更多的3D-2D匹配约束，使用PnP算法估计当前帧的位姿。最后根据准则判断当前帧是否可作为新的关键帧插入。

　　局部建图线程处理新的关键帧，主要包括管理局部地图和局部BA优化。首先根据当前关键帧更新共视图和本质图信息。然后在相邻帧中搜索当前帧未匹配的ORB特征点，三角化创建新的地图点。根据一些准则剔除冗余的地图点和关键帧。最后对当前帧局部地图中的关键帧和地图点进行局部BA优化。

　　回环优化线程处理局部建图线程后的关键帧，主要包括回环检测和全局优化。首先查找当前关键帧是否存在回环候选帧，如果存在，则计算当前帧与回环候选帧的相似变换sim3，从而确定是否为回环帧和估计回环处的累积漂移。然后根据相似变换信息，融合地图点和更新共视图中的边。最后在本质图上进行位姿图优化，从而实现全局位姿的一致性。

C、地图点、关键帧和选择标准（MapPoint and KeyFrame）

　　每个地图点Pi都包含以下信息：

　　（1）该地图点在世界坐标系下的3D坐标X_w,i；

　　（2）该地图点的视角方向ni, 为所有视角方向（该地图点与所有观测到它的关键帧的相机中心的射线）的平均单位向量；

　　（3）该地图点的ORB描述子，选取该地图点对应的ORB特征点与其他关键帧的对应ORB特征点的汉明距离最小的ORB描述子。

　　（4）该地图点被观测到的最大距离d_max和最小距离d_min，根据ORB特征点的尺度不变性确定。

　　每个关键帧Ki都包含以下信息：

　　（1）该关键帧的相机位姿T_iw，是一种刚体运动变换。

　　（2）相机的内参K，包括相机在x，y轴上的焦距f_x、f_y，相机的光圈中心c_x、c_y和深度图的缩放因子s。

　　（3）该关键帧上提取的ORB特征点，它的坐标是经过畸变矫正后的坐标。

　　地图点和关键帧的创建是在一个相对宽松的条件下，为了保证特征追踪线程在恶劣条件下（旋转、快速移动等）下具有鲁棒性；但是想要保留下来需要满足严格的条件，为了剔除冗余的关键帧和错误匹配的地图点，这样有利于系统的可持续运行。

D、共视图和本质图（Covisibility graph and Essential graph）

　　在共视图中将每个关键帧作为一个节点，若两个关键帧存在一定数量（至少15个）共视地图点则这两个这两个关键帧之间存在一条边，边的权重为共视地图点的数量。　

　　我们使用位姿图来校正闭环处的累积误差，由于共视图中边的数量较多，我们创建共视图的子图-本质图，保留全部的节点，但是选择更少的边。ORB-SLAM系统在第一个关键帧开始就维护一个生成树，每个关键帧只与共视点最多的相邻帧产生边。本质图包含生成树、共视地图点数量超过100的边和回环的边。

E、基于词袋的地点识别（bags of words）

　　ORB-SLAM系统中嵌入了基于DBoW2的地点识别模块，用于回环检测和重定位。视觉词典是一种描述子空间的离散化表示，可由大量图像中提取ORB描述子来离线创建。如果训练的图像足够充足，那么训练的视觉词典可用于不同的环境下工作。本ORB-SLAM系统增量式地维护一个关键帧数据库，该数据库存储了每个关键帧中的视觉单词，同时在关键帧删除后该数据库也会进行更新。

　　由于关键帧之间存在视觉重叠，所以在查询关键帧数据库时不存在独一无二的高分关键帧。原始的DBoW2考虑到重叠问题，提高相近时间内关键帧的最小相似度分数。本系统中在查询关键帧数据库时会首先剔除共视图中相邻的关键帧，然后选取相似度分数高于最高分数75%的关键帧。

四、地图自动初始化（Automatic Map Initialization）

　　地图初始化的目的就是计算两帧的相对位姿和初始化一些地图点。初始化的方法应该适用于平面和非平面场景，并且不需要人工选择好的图像帧来干预。我们提出了一种鲁棒性的方法-首先并行计算两个几何模型：适用于平面场景的单应矩阵H和适用于非平面场景的基础矩阵F，然后根据一些具体的方法来选择一个模型，最后恢复运动与结构。该算法步骤如下：

1）找到对应点

　　在当前帧Fc中提取ORB特征点Xc，然后在参考帧Fr中寻找匹配Xr，如果匹配点数量不够，则重置参考帧。

2）并行计算两个模型

　　根据下列公式并行计算单应矩阵Hcr和基础矩阵Fcr：

　　在RANSAC迭代中分别使用DLT和八点法来计算单应矩阵H和基础矩阵F。迭代的次数都是固定且相同的，每次迭代中计算基础矩阵需要8个点，计算单应矩阵需要4个点，而且每次迭代都要计算每个模型的分数S，公式如下：

　　　其中dcr2和drc2为当前帧到参考帧的对称转换误差；TM为基于X2测试为95%的外点阈值（TH为5.99，TF为3.84）；τ的值等于TH。

3）模型选择

　　在平面场景或者低视差场景中，选择单应矩阵；在高视差场景中，选择基础矩阵。ORB-SLAM系统根据以下方法来选择模型：

　　如果R_H>0.45,选择单应矩阵模型，否则选择基础矩阵模型。

4）运动与结构恢复

　　对于单应矩阵模型，Faugeras et .al提出恢复8种运动假设的方法，该方法容易在低视差的情况下失效。ORB-SLAM系统提出了直接三角化出8种运动假设，然后根据是否满足低视差、低重投影误差和景深是否为正等条件选择其中的一种运动假设。

　　对于基础矩阵，首先根据公式计算出本质矩阵E，然后根据SVD的方法恢复出4种运动假设，最后根据景深是否为正条件选择其中的一种运动假设。

5）BA优化

　　最后进行一个全局BA优化。

　　由上图对比实验可知：PTAM系统和LSD-SLAM系统初始化平面中的所有地图点，但是ORB-SLAM系统仅当拥有足够的视差下才会初始化地图点。

五、特征点追踪（Tracking）

　　特征点追踪线程处理相机的每一帧，主要做两件事：（1）估计每一帧相机的位姿，（2）选择新的关键帧。

A、ORB特征提取（ORB Extraction）

　　我们在尺度因子为1.2的8个尺度图像下提取FAST角点，对于分辨率为512X384至752X480的图像提取1000个角点，对于图像分辨率为1241X376的KITTI数据集中提取2000个角点。为了确保提取的角点分布均匀，将图像划分若干个网格（cell），并保证在每个网格上至少提取5个角点。如果提取的角点不够则自适应调整角点的检测阈值，从而保证总角点数量的一致。最后计算每个FAST角点的方向及相应的ORB描述子，用于特征匹配。

B、基于前一帧的位姿初始化（Initial Pose Estimation from Previous Frame）

　　如果上一帧被成功追踪到，使用匀速运动模型来估计当前帧的位姿，并且将上一帧观测到的地图点投影到当前帧确定匹配搜索范围。如果没有搜索到足够的匹配点，则扩大搜索的范围。通过计算匹配到的3D-2D点来初始化当前帧相机的位姿。

C、基于全局重定位的位姿初始化（Initial Pose Estimation via Global Relocalization）

　　如果当前帧追踪失败，则将当前帧转化为词袋向量并通过查询关键帧数据库来实现全局重定位。对每一个关键帧，通过使用RANSAC算法和PnP算法来估计当前帧相机的位姿，如果足够多的内点，则确定候选关键帧，通过确定搜索范围搜索更多的3D-2D匹配点，进一步优化当前帧相机的位姿。

D、基于局部地图的追踪（Track Local Map）

　　基于前一帧的位姿初始化，是建立当前帧与上一帧之间的联系；基于全局重定位的位姿初始化，是建立当前帧与某一关键帧的联系；基于局部地图的位姿估计，是建立当前帧与局部关键帧地图的联系，也就是通过搜索更多的3D-2D的匹配点来对更精确的估计当前帧的位姿。

　　一旦我们获取了相机的初始化位姿和一组3D-2D的匹配点，我们可以将地图和当前帧联系起来从而得到更多的3D-2D匹配对应关系。为了减少大地图的复杂性，我们只映射局部地图。局部地图包括一个帧集合K1（与当前帧有超过一定数量相同地图点的关键帧），和一个帧集合K2（与K1有超过一定数量相同的地图点但是与当前帧无相同的地图点的关键帧）组成。局部地图中定义一个参考关键帧Kf，它是与当前帧共有最多地图点的帧。

　　针对局部地图中的每一个地图点在当前帧中搜索：

（1）计算地图点X在当前帧中的投影，如果该投影超出图像边界则删除该地图点；

（2）计算当前帧的视角射线v与地图点平均视角方向n的夹角，如果vn < cos(60°)则删除该地图点；

（3）计算地图点到相机中心的距离d，如果该距离不在地图点的尺度不变区域内（d∉ [dmin, dmax]）则剔除该地图点。

（4）计算当前帧的尺度因子，比值为d/dmin。

（5）比较地图点的描述子D和当前帧中还未匹配的ORB特征点，选择尺度因子和距离最近的作为最优匹配对。

　　相机的位姿最终通过所有的3D-2D匹配点进行优化。

E、选择新关键帧（New Keyframe Decision）

　　跟踪线程除了计算每帧的位姿，还有一个很重要的目的是确定当前帧是否可作为关键帧。由于在局部建图线程中会剔除冗余的关键帧，所以在关键帧插入时应该尽可能的迅速，从而使得追踪线程对相机的运动（尤其是旋转运动）更具有鲁棒性。我们根据下列的要求插入新的关键帧：

（1）距离上一次全局重定位超过20帧；

（2）局部建图线程处于空闲，或者距离上一次关键帧插入超过20帧；

（3）当前帧至少能追踪50个地图点；

（4）当前帧能追踪到的地图点要少于参考关键帧Kf中地图点的90%。

　　其中条件1确保一个良好的重定位，条件3保证一个良好的追踪，条件4保证了一个最小的视角变化。

六、局部建图（Local Mapping）

　　局部建图线程处理每个关键帧，主要做以下两件事：（1）管理地图点和关键帧，（2）局部BA优化

A、插入新的关键帧（KeyFrame Insertion）

　　　每插入一个关键帧，首先更新共视图，包括增加一个新的节点Ki和更新关键帧中拥有共视点的边信息。然后更新本质图，即更新关键帧中拥有最多共视点的边信息。最后计算当前关键帧的词袋模型表示。

B、创建新的地图点（New MapPoint Creation）

　　通过三角化共视图中相连关键帧的ORB特征点来创建新的地图点。对于关键帧Ki中的未匹配ORB特征点，在其他关键帧中查找匹配点。删除不满足对极约束的匹配点对，然后三角化匹配点对生成新的地图点，检查是否满足景深为正，视差和重投影误差、尺度一致性。最后将该地图点投影到其它关键帧中寻找匹配。

C、地图点剔除（Recent MapPoint Culling）

　　　一个地图点想要保留在地图中必须通过严格的检验，即它能够在被创建后的连续三个关键帧被追踪到，这样是为了剔除错误三角化的地图点。一个良好的地图点需要满足以下两个条件：

（1）超过25%的帧数在理论上可以观察到该地图点；

（2）该地图点创建后能够被至少三个连续关键帧追踪到。

　　当然即使地图点满足以上条件，还是有可能会被删除，比如某个关键帧被删除或者当做局部BA的外点被剔除等。

D、局部地图的BA优化（Local Bundle Adjustment）

　　局部BA优化的对象有当前关键帧Ki、共视图中的相邻关键帧Kc和这些关键帧观测到的地图点，另外可以观测到这些地图点但是不与当前帧相邻的关键帧仅作为约束条件而不作为优化的对象。

E、关键帧剔除（Local Keyframe Culling）

　　随着新关键帧的插入，为了不增加BA优化的压力，和维持系统的可持续运行，需要删除冗余的关键帧。如果某一关键帧观测到的90%地图点的能够被至少三个其他关键帧观测到，那么剔除该关键帧。

七、闭环优化（Loop Closing）

　　闭环优化线程处理局部建图后的关键帧，主要做以下两件事：（1）闭环检测；（2）全局优化。

A、检测闭环候选帧（Loop Candidates Detection）

　　首先我们计算当前关键帧Ki与共视图中相邻关键帧Kc词袋向量的相似度，并保留最小相似度分数Smin。然后查询关键帧数据库，剔除相似度分数低于最小分数的关键帧和直接相邻的关键帧，将其他关键帧作为闭环候选帧。最后通过检测连续三个闭环候选帧的一致性来选择闭环帧。

B、计算Sim3相似变换（Computer the Similarity Transformation）

　　在单目SLAM系统中，地图（关键帧和地图点）有七个自由度可以漂移，包括三个平移自由度、三个旋转自由度和一个尺度自由度。通过计算当前帧Ki与关键帧Kl的相似变换，不仅可以计算出闭环处的累计误差，而且可以判断该闭环的有效性。

　　首先计算当前帧和闭环候选帧中ORB特征点对应的地图点，得到当前帧与每个闭环候选帧的3D-3D对应关系。然后通过对每个闭环候选帧采用RANSAC算法迭代，试图计算出相似变换。如果某闭环候选帧的相似变换具有足够多的内点，通过寻找更多匹配点优化，如果仍具有足够多的内点，则该闭环候选帧为闭环帧。

C、闭环融合（Loop Fusion）

　　从上一步可以得到当前帧的闭环帧和相似变换，回环优化的第一步就是在当前帧融合地图点和在共视图中插入新的边信息。首先当前关键帧Ki的位姿Tiw通过相似变换校正，并校正当前关键帧的相邻帧Kc的位姿Tcw。回环帧Kl和它相邻帧Klc的地图点投影到当前帧Ki和它的相邻帧Kc中寻找匹配点对，所有匹配的3D-3D地图点和相似变换的内点进行融合。所有参与融合的关键帧都更新他们在共视图中的边信息，并创建与回环帧相邻的边。

D、基于本质图的优化（Essential Graph Optimization）

　　为了有效的闭合回环，我们在本质图上进行位姿图优化。

八、试验

　　我们在大场景机器人序列-NewCollege数据集上评估ORB-SLAM系统的整体性能，在16组手持室内RGBD序列-TUM数据集上评估系统的定位精度、重定位和可持续性能。在10组汽车室外序列-KITTI数据集中评估系统在大场景下的实时性能，定位精度和位姿图优化的效率。实验电脑配置为：Intel Core i7-4700MQ（4 [email protected]）和8Gb内存。

A、基于NewCollege数据集的系统性能

　　NewCollege数据集是通过移动机器人采集校园而形成的2.2km图像序列，该序列由双目相机以20fps的速率采集，图像的分辨率为512X382。该数据集包含了几个大回环和快速旋转的情况，使得此前无单目SLAM可以跑完整个数据集。

　　从图4可知，ORB-SLAM系统检测到出NewCollege数据集中的闭环，并且具有局够多的内点来计算出相似变换矩阵。

　　图5显示了回环优化前后的全局地图，蓝色表示回环检测，红色表示局部地图。

　　图6表示ORB-SLAM处理完整个数据集序列后的全局地图，由于右边大的循环是反方向测得数据，不能够检测到闭环，所以不能进行闭环优化。

　　特征追踪和局部建图所消耗的时间如表1所示，我们可以看出，特征追踪线程能够能够以帧率为25-30Hz的速度处理图像，其中追踪局部地图最为消耗时间。在局部建图线程中，局部BA优化最为消耗时间。

　　表2表示ORB-SLAM系统处理数据集中6个回环时时间。我们可知回环检测的时间与关键帧数量呈现亚线性关系，这是因为查询关键帧数据库只需要比较图像中公共的视觉单词，从而视觉词典可用于地点识别。另外本质图中边的数量仅为节点数量的5倍左右，因此是一个相当稀疏的地图。

B、基于TUM RGBD数据集的定位精度

　　由于TUM RGBD数据集提供相机的真实运动轨迹，所以能够测量ORB-SLAM系统的定位精度。将ORB-SLAM系统与PTAM系统、LSD-SLAM系统、RGBD-SLAM系统进行比较，运行TUM数据集中的的16组序列，比较他们的均方根误差（RMSE）。

　　从表3可知，ORB-SLAM系统能够处理所有的序列除了fr3_nostructure_texture_far(fr3_nstr_tex_far)序列，该序列为平面场景，相机位姿在平面场景中由两种可能的解。ORB-SLAM系统初始化时检测到了歧义后拒绝初始化地图，然而PTAM系统初始化后有时效果好有时差，LSD-SLAM系统初始化后误差非常大。

　　在开环序列中，ORB-SLAM系统和PTAM系统的定位精度相差不大，但是在大场景闭环序列-fr3_nostructure_texture_near _withloop (fr3 nstr tex near)中，ORB-SLAM的定位精度明显优于PTAM。令人惊讶的是，PTAM和ORB-SLAM系统的定位精度都要优于LSD-SLAM和RGBD-SLAM系统。一个可能的原因是，前者使用了位姿图优化，而且没有使用传感器测量的深度数据。另一个有趣的结果是，从fr2_desk_with_person and fr3_walking_xyz序列运行结果可以看出，LSD-SLAM系统在动态环境下具有较差的鲁棒性。　