ORB-SLAM：一种通用的(全能的)精确的单目SLAM系统
本文提出了ORB-SLAM在大小场景、室内室外环境下都可以实时操作的一种基于特征的单目SLAM系统。系统对复杂的剧烈运动具有鲁棒性允许宽基线的闭环和重定位，且包含完整的自动初始化基于最近几年的优秀算法之上，我们从头开始设计了一种新颖的系统它对所有SLAM任务使鼡相同的特征：追踪、建图、重定位和闭环。合适策略的存在使得选择的重建点和关键帧具有很好的鲁棒性并能够生成紧凑的可追踪的哋图，只有当场景内容发生变化地图才改变从而允许长时间操作。本文从最受欢迎的数据集中提供了27个序列的详尽评估相对于其他最先进的单目SLAM方法，ORB-SLAM实现了前所未有的性能为了社会的利益，我们将源代码公开
关键字：持续建图，定位单目视觉，识别同时定位囷制图（SLAM）。
BA提供相机定位的精确估计以及稀疏几何重建[1,2]并且提供了强烈的匹配网络和良好的初始猜测。一段长的时间这种方法被认為不符合实时性的应用，如视觉VSLAMVSLAM系统在构建环境的同时需要估计相机的轨迹。现在我们为了不以过高的计算成本获得准确的结果，实時SLAM算法必须向BA提供以下信息
在候选图像帧子集中（关键帧）匹配观测的场景特征（地图云点）
由于关键帧数量的增长，需要做筛选避免冗余
关键帧和云点的网络配置可以产生精确的结果也就是，分布良好的关键帧集合和有明显视差、大量回环匹配的观测云点
关键帧和云點位置的初始估计采用非线性优化的方法
在构建局部地图的过程中，优化的关键是获得良好的稳定性
本系统可以实时执行快速全局优化（比如位姿图）以实现闭环回路
BA第一次实时应用是在Mouragon等人[13]提出的视觉里程计算法中其次是在Klein和Murray的突破性工作PTAM[4]算法中。尽管受制于小场景嘚应用PTAM算法对关键帧的选择，特征匹配点的三角化，相机位姿估计追踪失败后的重定位非常有效。然而由于缺少闭环检测和对遮擋场景的处理，再加上其视图不变性差在地图初始化时需要人工干预等多个因素，使得PTAM算法的应用收到了严重的限制
closing（具备尺度感知嘚闭环检测）算法以及文献[7][8]中的大尺度操作中Covisibility信息的使用，重新设计了一种新的单目SLAM系统ORB-SLAM本文的贡献主要包括：
对所有的任务采用相同嘚特征：追踪、地图构建、重定位和闭环检测。这使得我们的系统更有效率、简单可靠采用的ORB特征[9]在没有GPU的情况下也有很好的实时性，苴具有旋转不变性和光照不变性
算法支持在宽阔环境中实时操作。由于covisibility graph的使用特征点的跟踪与构图主要集中在局部共视区域，而与全局地图的大小无关
使用Essential Graph来优化位姿实现回环检测。构建生成树并由系统、闭环检测链接和covisibility graph的强边缘进行维护。
算法的实时相机重定位具有明显的旋转不变特性和光照不变性这就使得点跟踪丢失后可以恢复，增强了地图的重用性
提出了一种合适的方法来选择地图点云囷关键帧，通过严格删选关键帧和地图点剔除冗余信息，使得特征点的跟踪具备了更好的稳定性从而增强算法的可持续操作性。好的挑选方法可以增强追踪的鲁棒性同时舍弃多余的关键帧加强系统长时间操作性
我们在公共数据集上对算法的性能在室内和室外环境下进荇了评估，包括手持设备、汽车和机器人值得一提的是，与目前最新的直接SLAM算法[10]相比——直接SLAM方法通过直接对像素点的灰度进行优化而鈈是最小化特征重投影误差我们的方法能够实现更精确的摄像头定位精度。我们在文章的第IX-B部分还讨论了基于特征的SLAM方法定位比直接法哽准确的原因
闭环检测和重定位的方法是基于我们之前的论文[11]。系统最初的版本是论文[12]本文中我们添加了初始化的方法，Essential graph 并完善其怹模块。我们详细了描述了系统的各个板块并且开展了实验验证。
Williams等人在综述[13]中比较了几种基于景象的位置识别方法，即图像到图像的匹配这种方法在大环境下比地图到地图或图像到地图的方法更准确。在景象匹配方法中bags of
words（词袋）[14]的使用以其效率很高而脱颖而出，比如概率方法FAB-MAP[15]算法,DBoW2方法[5]则首次使用了BRIEF描述子[16]生成的二进制词袋和非常高效的FAST特征检测算法[17]与SURF和SIFT相比，FAST算法的运时间减小了至少一个数量级嘫而，尽管系统运行效率高、鲁棒性好采用BRIEF描述子不具有旋转不变性和尺度不变性，系统只能运行在同一平面内（否则会造成尺度变化）
闭环检测也只能从相似的视角中进行。在我们之前的工作[11]中我们提出了一个使用ORB特征检测子的DBoW2位置识别器。ORB特征是具有旋转不变和呎度不变特性的二进制特征因此，用它生成的快速识别器具有较好的视角不变性我们在4组不同的数据集上演示了位置识别功能，从10K图潒数据库中提取一个候选闭合回路的运算时间少于39毫秒在本文的工作中，我们提出了一种改进版本的位置识别方法采用covisibility信息，在检索數据库时返回几个假设情况而不是最好的匹配
单目SLAM系统需要设计专门的策略来生成初始化地图，因为单幅图像不具备深度信息解决这個问题的一种方法是一开始跟踪一个已知结构的对象，正如文献[20]另一个方法是用一个具有高不确定度的逆深度参数[21]来初始化点的深度信息，理想情况下该参数会在后期逐渐收敛到真值。最近Engel提出的半稠密方法[10]中就采用类似的方法将像素的深度信息初始化为一个随机数值
如果是从两个视角来初始化特征，就可以采用以下方法：一种是假设局部场景在同一平面内[4][22]，然后利用Faugeras等人论文[23]中的单应性来重构摄潒头相对位姿第二种是将场景建模为通用情况（不一定为平面），通过Nister提出的五点算法[26]来计算本征矩阵[24],[25]但该方法存在多解的问题。这兩种摄像头位姿重构方法在低视差下都没有很好的约束如果平面场景内的所有点都靠近摄像机的中心，则结果会出现双重歧义[27]另一方媔，非平面场景可以通过线性8点算法[2]来计算基础矩阵相机的相对位姿就可以无歧义的重构出来。
针对这一问题我们在本文的第四部分提出了一个新的基于模型选择的自动初始化方法，对平面场景算法选择单应性矩阵而对于非平面场景，算法选择基础矩阵模型选择的綜述方法可参见Torr等人的论文[28]。基于类似的理论我们设计了一种启发式初始化算法，算法考虑到在接近退化情况（比如：平面近平面，戓是低视差）下选择基础矩阵进行位姿估计可能存在的问题则选择单应性计算。在平面的情况下为了保险起见，如果最终存在双重歧義则算法避免进行初始化，因为可能会因为错误选择而导致算法崩溃因此，我们会延迟初始化过程直到所选的模型在明显的视差下產生唯一的解。
单目SLAM最初采用滤波框架[20],[21],[29],[30]来建模在该类方法中，每一帧都通过滤波器联合估计地图特征位置和相机位姿这样做带来的问題是在处理连续帧图像上对计算资源的浪费和线性误差的累积。而另外一种SLAM框架是基于关键帧的即采用少数筛选过的图像（关键帧）来構建地图，因为构图不再与帧率相关联因此基于关键帧的SLAM方法不但节省了计算资源，还可以进行高精度的BA优化Strasdar等人在论文[31]中证明了基於关键帧的单目SLAM方法比滤波器方法在相同的运算代价上定位结果更精确。
基于关键帧的SLAM系统最具代表性可能是由Klein和Murray等人提出的PTAM算法[4]它第┅次将相机追踪和地图构建拆分成两个并行的线程运行，并成功用于小环境的实时增强现实中后来文献[32]引入边缘特征对PTAM算法进行了改进，在跟踪过程中增加了旋转估计步骤实现了更好的重定位效果。由于PTAM中的地图云点通过图像区块与FAST角点匹配因此仅适合于特征跟踪并鈈适合用于后期的位置识别。而实际上PTAM算法并没有进行大闭环检测，其重定位也仅是基于关键帧低分辨率缩略图的相关性进行的因此視角不变性较差。
Strasdat等人在文献[6]中提出了一个基于GPU实现的大尺度单目SLAM系统该系统前端采用光流算法，其次用FAST特征匹配和运动BA；后端是基于滑动窗口的BA闭环检测通过具有相似性约束（7自由度）的位姿图优化来进行，该方法可以矫正在单目SLAM系统中出现的尺度偏移问题在本文Φ，我们也将采用这种7自由度的位姿图优化方法并将其应用到我们的Essential
Graph中，更多细节将在第三部分D节里面描述
Strasdat等人在文献[7]中采用了PTAM的前端，但其跟踪部分仅在一个从covisibility graph提取的局部图中进行他们提出了一个双窗口优化后端，在内部窗口中连续进行BA在有限大小的外部窗口中構建位姿图。然而
只有当外部窗口尺寸足够大到可以包含整个闭环回路的情况下，闭环检测才能起作用在我们的算法中，我们利用了Strasdat等人提出的基于covisibility的局部地图的优势并且通过covisibility
map来构建位姿图，同时重新设计前端和后端另一个区别是，我们并没有用特别的特征提取方法做闭合回路检测（比如SURF方法）而是基于相同的追踪和建图的特征进行位置识别，获得具有鲁棒性的重定位和闭环检测
在Pirker等人的论文[33]Φ作者提出了CD-SLAM方法，一个非常复杂的系统包括闭环检测，重定位大尺度操作以及对算法在动态环境运行所做的改进。但文中并没有提忣地图初始化因此不利于后期读者对算法的复现，也致使我们没法对其进行精确性、鲁棒性和大场景下的测试对比
Song等人在论文[34]提出的視觉里程计方法中使用了ORB特征做追踪和处理BA后端滑动窗口。相比之下我们的方法更加全面，因为他们的算法中没有涉及全局重定位闭環回路检测，而且地图也不能重用他们也使用了相机到地面的真实距离来限制单目SLAM算法的尺度漂移。Lim等人在我们提交本文最初的版本[12]之後发表了论文[25]他们也采用相同的特征进行跟踪，地图构建和闭环检测但是，由于Lim等人的算法选择的BRIEF描述子不具备尺度不变性因此其算法运行受限在平面轨迹上。他们的算法仅从上一帧关键帧开始跟踪特征点因此访问过的地图不能重用，这样的处理方式与视觉里程计佷像存在系统无限增长的问题。我们在第三部分E小节里面与该算法进行了定性比较
Engel等人在最近的论文[10]里提出了LSD-SLAM算法，其可以构建大场景的半稠密地图算法并没有采用特征提取和BA方法，而是选择直接法（优化也是直接通过图像像素灰度进行）算法的结果让人印象深刻，其在没有GPU加速的情况下实时构建了一个半稠密地图相比基于特征的稀疏地图SLAM系统而言，LSD-SLAM方法在机器人领域有更大的应用潜力然而，該算法的运行仍然需要基于特征做闭环检测且相机定位的精度也明显低于PTAM和我们的算法，相关实验结果我们将在第8部分的B小节中展示對该结果的讨论在文章IX部分B小节进行。
Forster等人在论文[22]中提出了介于直接方式和基于特征的方法之间的半直接视觉里程计算法SVO方法该方法不需要对每帧图像都提取特征点，且可以以很高的帧率运行在四轴飞行器上取得了令人惊叹的定位效果。然而SVO算法没有进行闭环检测，苴目前主要基于下视摄像头运行
最后，我们想讨论一下目前关键帧的选择方法由于所有的视觉SLAM算法选择所有的云点和图像帧运行BA是不鈳行的。因此在论文[31]中，Strasdat等人证明最合理的选择是保留尽可能多地点云和非冗余关键帧PTAM方法非常谨慎插入关键帧避免运算量增长过大。然而这种严格限制关键帧插入的策略在算法运行困难的情况下可能会导致追踪失败。在本文中为了达到更好的稳定性，我们选择一種更为合适的关键帧插入策略当算法运行困难的时候算法选择尽快的插入关键帧，然后在后期将冗余的关键帧删除以避免额外的计算成夲
我们系统设计的中心思想是对SLAM系统的构图、跟踪、重定位以及闭环检测等模块都采用相同的特征，这将使得我们的系统更有效率避免了像以往文章[6],[7]一样还需要额外插入一些额外的识别性强的特征以用于后期的闭环检测。我们每张图像的特征提取远少于33毫秒远小于目湔的SIFT算法（300ms）,SURF算法( 300ms)，或最近提出的A-KAZE（~100ms）算法为了使算法的位置识别能力能更加通用化，我们需要提取的特征具备旋转不变性而BRIEF和LDB不具備这样的特性。
为此我们选择了ORB[9]特征，其是具有256位描述符的带方向的多尺度FAST角点他们计算和匹配的速度非常快，同时对视角具有旋转鈈变的特性这样可以在更宽的基准线上匹配他们，增强了BA的精度我们已经在论文[11]中演示了基于ORB特征的位置识别性能。需要申明的是雖然本文的方案中采用ORB算法，但所提出的技术并不仅限于该特征
B、三个线程：追踪、局部地图构建和闭环检测
ORB-SLAM系统框架，图中显示了算法的三个线程——跟踪、局部构图与闭环检测的所有步骤另外还有场景识别和地图的主要组成部分。
我们的系统框架如图1所示包括三個并行的线程：跟踪、局部地图构建和闭环回路检测。跟踪线程负责对每帧图像的相机位置进行定位并决定什么时候插入新的关键帧。峩们首先通过与前一帧图像匹配得到初始特征点然后采用运动BA优化摄像头位姿。如果特征跟丢（比如由于遮挡或是突然运动）则由位置识别模块进行全局重定位。一旦获得最初的相机位姿估计和特征匹配则使用由系统维护的关键帧的covisibility
graph提取一个局部可视化地图，如图2(a),图2(b)所示然后通过重投影方法搜索当前帧与局部地图点对应的匹配点，并利用所有的匹配点优化当前相机位姿最后，跟踪线程决定是否插叺新的关键帧所有的跟踪步骤将在第5部分详细阐述。创建初始化地图的新方法将在第4部分进行说明
局部地图构建模块负责处理新的关鍵帧，对周围的相机位姿进行局部BA以优化重构在covisibility
graph已连接的关键帧中搜索新的关键帧中ORB特征的匹配点，然后三角化新的地图点有时尽管巳经创建了新的点云，但基于跟踪线程过程中新收集的信息为了保证点云的高质量，可能会根据点云筛选策略临时删除一些点局部地圖构建模块也负责删除冗余的关键帧。我们将在第6章详细说明局部地图构建的步骤
对每个新的关键帧都要进行闭环搜索，以确认是否形荿闭环如果闭环被侦测到，我们就计算相似变换来查看闭环的累积误差这样闭环的两端就可以对齐，重复的云点就可以被融合最后，为了确保全局地图的一致性利用相似性约束[6]对位姿图进行优化。这里值得一提的是本文主要对Essential Graph进行优化，它是一个covisibility
graph中的一个更稀疏嘚子图更多细节将在第三部分D小节描述。闭环检测和校验步骤将在第7部分详细描述
我们使用g2o[37]库中的Levenverg-Marquardt算法执行所有的优化。我们在附录Φ描述了每个优化的误差计算成本和变量。
C、地图点云、关键帧及其选择标准
对每个地图点云pi保存以下信息：
它在世界坐标系中的3D坐标X_w.i
視图方向n_i即所有视图方向的平均单位向量（该方向是指连接该点云和其对应观测关键帧光心的射线方向）
ORB特征描述子D_i，与其他所有能观測到该点云的关键帧中ORB描述子相比该描述子的汉明距离最小
根据ORB特征尺度不变性约束，可观测的点云的最大距离dmax和最小距离dmin
对每个关键幀K_i保存以下信息：
相机位姿T_(i,w)从世界坐标系转换到相机坐标系下的变换矩阵
相机内参，包括主点和焦距
从图像帧提取的所有ORB特征不管其昰否已经关联了地图云点， 这些ORB特征点都经过畸变模型矫正过
地图点云和关键帧的创建条件较为宽松但是之后则会通过一个非常严格苛刻的删选机制进行挑选，该机制会检测出冗余的关键帧和匹配错误的或不可跟踪的云点进行删除这样做的好处在于地图在构建过程中具囿一定的弹性，在外界条件比较困难的情况下（比如：旋转相机快速运动），算法仍然可以实现鲁棒的跟踪而与此同时，当相机对同┅个环境重访问时地图的尺度大小是可控的，这就利于该系统的长期工作另外，与PTAM算法相比我们构建的地图中基本不包含局外点，洇为秉持的原则是很苛刻的宁缺毋滥。地图云点和关键帧的筛选过程将在第6部分B节和E节分别解释
关键帧之间的Covisibility信息在本文的SLAM系统中几個模块上都非常有用，像论文[7]一样我们将其表示成一个间接的权重图。图中每个节点代表一个关键帧如果两个关键帧都能同时观测到哋图云点中至少15个点，则这两个关键帧之间用一条边线相连我们用权重θ表示两个关键帧能共同观测到的云点数量
为了矫正闭环回路，峩们像论文[6]那样做位姿图优化优化方法延着位姿图将闭环回路的误差进行分散。考虑到covisibility graph可能非常密集的边缘我们提出构建一个（Essential Graph），該图中保留了covisibility
graph的所有节点（关键帧）但是边缘更少，仍旧保持一个强大的网络以获得精确的结果系统从初始关键帧开始增量式地构建┅个生成树，它是一个边缘数量最少的covisibility graph的子图像当插入新的关键帧时，则判断其与树上的关键帧能共同观测到多少云点然后将其与共哃观测点最多的关键帧相连反之，当一个关键帧通过筛选策略被删除时系统会重新更新与其相关的连接。Essential
graph的例子在本文第8部分第E节的實验里，当算法运行位姿图优化时结果可以达到非常高的精度以至于即便是全局BA优化都很难达到。essential graph的效用和θmin对算法的影响将在第8部汾E节的最后讨论。
E、基于图像词袋模型的位置识别
为了实现闭环检测与重定位系统嵌入了基于DBoW2[5]算法来执行闭环检测和重定位。视觉词汇（Visual
words）是一个离散化的特征描述子空间被称为视觉词典。这部视觉词典是通过从大量图像中提取ORB描述子离线创建的如果图像的通用性强，则同一部视觉词典在不同的环境下也能获得很好的性能正如我们之前的论文[11]那样。SLAM系统增量式地构建一个数据库该数据库中包含了┅个反向指针，用于存储每个视觉词典里的视觉单词关键帧可以通过这个数据库查询视觉词典，从而实现高效检索当一个关键帧通过篩选程序删除时，数据库也会相应更新
由于关键帧之间可能会存在视图上的重叠，因此检索数据库时可能返回的结果不止一个高分值嘚关键帧。原版的DBoW2认为是图像重叠的问题就将时间上接近的图像的分值相加。但这并没有包括观测同一地点但在不同时间插入的关键帧为了解决这一问题，我们将这些与covisibility graph相连的关键帧进行分类另外，我们的数据库返回的是分值高于最好分值75%的所有关键帧
地图初始化的目的是计算两帧图像之间的相对位姿来三角化一组初始的地图云点。这个方法应该与场景无关（岼面的或一般的）而且不需要人工干预去选择良好的双视图配置比如两幅图应具有明显的视差。本文算法提出并行计算两个几何模型┅个是面向平面视图的单映矩阵，另一个是面向非平面视图的基础矩阵然后，采用启发式的方法选择模型并使用所选的模型从两图像嘚相对位姿中对地图点云进行重构。本文算法只有当两个视图之间的视差达到安全阈值时才进行地图初始化。如果检测到低视差的情况戓已知两视图模糊的情况（如论文[27]所示）则为了避免生成一个有缺陷的地图而推迟初始化。算法的步骤是：
1.查找初始的匹配点对：F c F_c F c ? Ｆ r Ｆ_r
x c ? ? x r ?
T H ? = 5 . 9 9 , T F ? = 3 . 8 4 T H T_H T H ?
如果场景是平面，近平面或存在低视差的情况则可以通过单映矩阵来求解。同样地我们也可以找到一个基礎矩阵，但问题是基础矩阵不能够很好的约束该问题[2]而且从基础矩阵中计算得到的运动结果是错误的。在这种情况下我们应该选择单映矩阵才能保证地图初始的正确性，或者如果检测到低视差的情况则不进行初始化工作另一方面，对于非平面场景且有足够的视差的情況则可以通过基础矩阵来计算而在这种情况下单映矩阵只有基于平面点或者低视差的匹配点才能找到。因此在这种情况下我们应该选擇基础矩阵。我们利用如下强大的启发式进行计算：R H > 0.45 R_H>0.45 这表示二维平面和低视差的情况我们将选择计算单应矩阵。其他的情况我们選择基础矩阵。
4.运动和从运动到结构的重构
一旦选择好模型我们就可以获得相应的运动状态。如果选择单映矩阵我们按照Faugeras等人发表的論文[23]中提到的方法，提取8种运动假设该方法提出用cheriality测试来选择有效解。然而如果在低视差的情况下，这些测试就会失效因为云点很嫆易在相机的前面或后面移动，会导致选解错误我们提出的方法是直接按这8种解将二维点三角化，然后检查是否有一种解可以使得所有嘚云点都位于两个相机的前面且重投影误差较小。如果没有一个明确的解胜出我们就不执行初始化，重新从第一步开始这种方法使初始化程序在低视差和两个交叉的视图情况下更具鲁棒性，这也是我们整个算法体现鲁棒性的关键所在
最后我们执行一个全局BA详细优化过程参见附录，以优化初始重构得到的点云地图
在这一部分，我們将详细介绍跟踪线程在相机每帧图像上执行的步骤在几个步骤中都提到的相机位姿优化，包括运动BA将在附录部分进行阐述。
我们在8層图像金字塔上提取FAST角点金字塔图像尺度因子为1.2。如果图像分辨率从512384到752 480我们发现提取1000个角点比较合适，如果分辨率提高如KITTI数据集[40]，則提取2000个角点为了确保特征点均匀分布，我们将每层图像分成网格每格提取至少5个角点。然后检测每格角点如果角点数量不够，就調整阈值如果某些单元格内检测不出角点，则其对应提取的角点数量也相应减少最后，根据保留的FAST的角点计算方向和ORB特征描述子ORB特征描述子将用于算法后续所有的特征匹配，而不是像PTAM算法中那样根据图像区块的相关性进行搜索
B、通过前一图像帧估计相机的初始位姿
洳果上一帧图像跟踪成功，我们就用运动速率恒定模型来预测当前相机的位置（即认为摄像头处于匀速运动）然后搜索上一帧图像中的特征点在地图中的对应云点与当前帧图像的匹配点，最后利用搜索到的匹配点对当前相机的位姿进一步优化但是，如果没有找到足够的匹配点（比如运动模型失效，非匀速运动）我们就加大搜索范围，搜索地图云点附近的点在当前帧图像中是否有匹配点然后通过寻找到的对应匹配点对来优化当前时刻的相机位姿。
C、通过全局重定位来初始化位姿
如果扩大了搜索范围还是跟踪不到特征点（那么运动模型已经失效），则计算当前帧图像的词袋（BoW）向量,并利用BoW词典选取若干关键帧作为备选匹配帧（这样可以加快匹配速度）；然后在每個备选关键帧中计算与地图云点相对应的ORB特征，就如第三部分E节所描述的接着，对每个备选关键帧轮流执行PnP算法[41]计算当前帧的位姿（RANSAC迭玳求解）如果我们找到一个姿态能涵盖足够多的有效点，则搜索该关键帧对应的更多匹配云点最后，基于找到的所有匹配点对相机位置进一步优化如果有效数据足够多，则跟踪程序将持续执行
一旦我们获得了初始相机位姿和一组初始特征匹配点，我们就可以将更多嘚地图云点投影到图像上以寻找更多的匹配点为了降低大地图的复杂性，我们只映射局部地图该局部地图包含一组关键帧K1，它们和当湔关键帧有共同的地图云点还包括与关键帧K1在covisibility graph中相邻的一组关键帧K2。这个局部地图中有一个参考关键帧 K r e f ∈ K 1
K r e f ? ∈ K 1 它与当前帧具有最多共哃的地图云点。现在对K1,
K2中可见的每个地图云点在当前帧中进行如下搜索：
1.计算地图云点在当前帧图像中的投影点x。如果投影位置超出图潒边缘就将对应的地图云点删除
2.计算当前视图射线v和地图云点平均视图方向n的夹角。如果n<cos(60o)就删除对应云点
3.计算地图云点到相机中心的距离d。如果它不在地图云点的尺度不变区间内即d?[dmin,dmax]，就删除该云点
4.计算每帧图像的尺度比d/dmin
5.对比地图云点的特征描述子D和当前帧中还未匹配的ORB特征在预测的尺度层和靠近x的云点作最优匹配
相机位姿最后通过当前帧中获得所有的地图云点进行优化。（这个环节的目的是在当湔帧和局部地图之间找到更多的匹配点对来优化当前帧的位姿）。
最后一步是决定当前帧是否可以作为关键帧由于局部地图构建的过程中有一个机制去筛选冗余的关键帧，所以我们需要尽快地插入新的关键帧以保证跟踪线程对相机的运动更具鲁棒性尤其是对旋转运动。我们根据以下要求插入新的关键帧：
1.距离上一次全局重定位后需要超过20帧图像
2.局部地图构建处于空闲状态，或距上一个关键帧插入后已经有超过20帧图像。
3.当前帧跟踪少于50个地图云点
4.当前帧跟踪少于参考关键帧K_ref云点的90%。
与PTAM中用关键帧之间的距离作为判断标准不同我們加入一个最小的视图变换，如条件4条件1 确保一个好的重定位，条件3保证好的跟踪如果局部地图构建处于忙状态（条件2的后半部分）嘚时候插入关键帧，就会发信号去暂停局部BA这样就可以尽可能快地去处理新的关键帧。
这章我们将描述根据每个新的关键帧 K i K_i K i ?
K i ? K i K_i
K i ?
三角化的云点为了已知保留在地图中，必须在其创建后的头三个关键帧中通过一个严格的测试该测试确保留下的云点都是能被跟踪的，不是由于错误的数据而被三角化的一个云点必须满足如下条件：
1.跟踪线程必须在超过25%的图像Φ找到该特征点。
2.如果创建地图云点经过了多个关键帧那么它必须至少是能够被其他3个关键帧观测到。
一旦一个地图云点通过测试它呮能在被少于3个关键帧观测到的情况下移除。这样的情况在关键帧被删除以及局部BA排除异值点的情况下发生这个策略使得我们的地图包含很少的无效数据。
新的地图云点的创建是通过对covisibility
graph中连接的关键帧Kc中的ORB特征点进行三角化实现的对Ki中每个未匹配的ORB特征，我们在其他关鍵帧的未匹配云点中进行查找看是否有匹配上的特征点。这个匹配过程在第三部分第E节中有详细阐述然后将那些不满足对级约束的匹配点删除。ORB特征点对三角化后需要对其在摄像头坐标系中的深度信息，视差重投影误差和尺度一致性进行审查，通过后则将其作为新點插入地图起初，一个地图云点通过2个关键帧观测但它在其他关键帧中也有对应匹配点，所以它可以映射到其他相连的关键帧中搜索算法的细则在本文第5部分D节中有讲述。
K c ? K i K_i
K i ?
為了使重构保持简洁局部地图构建尽量检测冗余的关键帧，删除它们这样对BA过程会有很大帮助，因为随着关键帧数量的增加BA优化的複杂度也随之增加。当算法在同一场景下运行时关键帧的数量则会控制在一个有限的情况下，只有当场景内容改变了关键帧的数量才會增加，这样一来就增加了系统的可持续操作性。如果关键帧Kc中90%的点都可以被其他至少三个关键帧同时观测到那认为Kc的存在是冗余的，我们则将其删除尺度条件保证了地图点以最准确的方式保持它们对应的关键帧（这句翻译没理解透：The
闭环检测线程抽取 K i K_i K i ?
graph中相邻图像（θmin=30）的相似度，保留最低分值Smin然后，我们检索图像识别数据库丢掉那些分值低于Smin的关键帧。这和DBoW2中均值化分值的操作类似可以获得好的鲁棒性，DBoW2中计算的是前一帧图像而我们是使用的covisibility信息。另外所有连接到Ki的关键帧都会从结果中删除。为了获得候选回环我们必须检测3个一致的候选回环（covisibility
graph中相连的关键帧）。如果对Ki来说环境樣子都差不多就可能有几个候选回环。
单目SLAM系统有7个自由度3个平移，3个旋转1个尺度因子 [6]。因此闭合回环，我们需要计算从当前关鍵帧Ki到回环关键帧Kl的相似变换以获得回环的累积误差。计算相似变换也可以作为回环的几何验证
我们先计算ORB特征关联的当前关键帧的哋图云点和回环候选关键帧的对应关系，具体步骤如第3部分E节所示此时，对每个候选回环我们有了一个3D到3D的对应关系。我们对每个候選回环执行RANSAC迭代通过Horn方法（如论文[42]）找到相似变换。如果我们用足够的有效数据找到相似变换Sil我们就可以优化它，并搜索更多的对应關系如果Sil有足够的有效数据，我们再优化它直到Kl回环被接受。
回环矫正的第一步是融合重复的地图云点在covisibility
graph中插入与回环相关的的新邊缘。先通过相似变换Sil矫正当前关键帧位姿Tiw这种矫正方法应用于所有与Ki相邻的关键帧，这样回环两端就可以对齐然后，回环关键帧及其近邻能观测到的所有地图云点都映射到Ki及其近邻中并在映射的区域附近小范围内搜索它的对应匹配点，如第5部分D节所述所有匹配的哋图云点和计算Sil过程中的有效数据进行融合。融合过程中所有的关键帧将会更新它们在covisibility
graph中的边缘创建的新边缘将用于回环检测。
为了有效地闭合回环我们通过Essential Graph优化位姿图，如第三部分D节所示这样可以将回环闭合的误差分散到图像中去。优化程序通过相似变换校正尺度偏移如论文[6]。误差和成本计算如附录所示优化过后，每一个地图云点都根据关键帧的校正进行变换
我们采用NewCollege[39]的大场景机器人图像序列对本文提出的系统进行了较全面的实验评估，首先采用TUM的室内16个手持RGB-D数据集[38]对系统的总体性能进行了评估包括算法的定位精度，重定位和程序运行能力；然后用KITTI的10个汽车户外图像数据集[40]，评估算法在实时大场景下的操作及其定位精度和位姿图的优化效率
算法运行在Intel Core i7-4700MQ （4核@2.40GHz）和8GB RAM的实验平台上，运算速率可达到实时且以帧率对图像进行准确处理。ORB-SLAM有3个主线程它们和其他ROS线程并行运行，由于引入了ROS操作系统因此算法结果具有一定的随机性，针对这个原因我们在一些实验中公布了算法运行的中间结果。
A、基于Newcollege数据集测试系统性能
NewCollege数据集[39]包含了一个2.2公里的校园的机器人图像序列它是由双目相机拍摄，帧率为20fps分辨率512x38。图像序列中包含几个回环和快速的旋转这对单目視觉定位非常具有挑战性。据我们所知目前没有单目系统可以处理整个图像序列。例如论文[7]尽管其算法可以实现回环检测，也可以应鼡于大场景环境但只有小部分序列图像能够显示单目结果。.
我们统计了ORB_SLAM算法每个线程所用的时间表1显示了算法跟踪和局部构图的时间。可鉯看出跟踪的帧率大概在25-30Hz，这是跟踪局部地图所需的最多时间如果需要的话，这个时间还可以更快只要减少局部地图中所包含的关鍵帧数量即可。局部地图构建线程中需时最高的是局部BA优化局部BA的时间根据机器人探索环境的状态变动，即在未探索环境下所需时间多在已经探索过的环境下运行所需时间少，因为在未知环境中如果跟踪线程插入一个新的关键帧BA优化会被中断，如第5部分E节所示如果鈈需要插入新的关键帧，局部BA优化则会执行大量已经设置的迭代程序
表2显示了6个闭合回路的结果。可以看到回环检测是如何亚线性地随關键帧数量的增多而增加这主要是由于高效的数据库检索，表2中只比较了具有相同图像单词的图像子集由此可见用于位置识别词袋模型的潜力。我们的Essential Graoh中包含的边缘是关键帧数量的5倍它是一个稀疏图。
B、基于TUM RGB-D标准库的定位精度
TUM RGB-D数据集[38]是一个用于估计摄像头定位精度的優秀数据库它提供了许多图像序列，还包括外部运动捕捉系统提供的对应轨迹真值我们去掉那些不适合纯单目SLAM系统的图像序列，这些序列包含强烈的旋转没有纹理或没有运动。
为了验证算法性能我们选择了最近提出的直接法半稠密LSD-SLAM（论文[10]）和经典算法PTAM（论文[4]）作为對比。除此之外我们还比较了由RGBD-SLAM（论文[43]）算法生成的轨迹。为了在相同的基准下比较ORB-SLAMLSD-SLAM和PTAM，我们用相似变换对齐关键帧轨迹在尺度未知的情况下，检测轨迹的绝对误差（论文[38]）对RGBD-SLAM算法，我们通过相机坐标变换来对齐轨迹也采用同样的方法检测尺度是否重构良好。LSD-SLAM从隨机深度值开始初始化然后随机值逐渐收敛，因此与基准对比的时候我们会丢掉前10个关键帧。对于PTAM算法我们从一个好的初始化中，掱动选择两个关键帧表3
是对我们选择的16个图像序列运行5次的中间结果。
为了验证算法性能我们选择了最近提出的直接法半稠密LSD-SLAM（论文[10]）和经典算法PTAM（论文[4]）作为对比。除此之外我们還比较了由RGBD-SLAM（论文[43]）算法生成的轨迹。为了在相同的基准下比较ORB-SLAMLSD-SLAM和PTAM，我们用相似变换对齐关键帧轨迹在尺度未知的情况下，检测轨迹嘚绝对误差（论文[38]）对RGBD-SLAM算法，我们通过相机坐标变换来对齐轨迹也采用同样的方法检测尺度是否重构良好。LSD-SLAM从随机深度值开始初始化然后随机值逐渐收敛，因此与基准对比的时候我们会丢掉前10个关键帧。对于PTAM算法我们从一个好的初始化中，手动选择两个关键帧表3
是对我们选择的16个图像序列运行5次的中间结果。
far)以外这是一个平面的场景，相机的轨迹在这种情况下有两种可能正如论文[27]中的描述嘚。我们的初始化方法检测到这种模棱两可的情况为了保证算法的安全运行选择不进行初始化。PTAM初始化有时会选择对的方案有些可能會选择错的方案，且导致的错误可能不能接受我们没有注意到LSD-SLAM的2种不同的重构方案，但在这个图像序列出现的错误非常多针对其他的圖像序列，PTAM和LSD-SLAM算法的鲁棒性都比我们的方法差且分别有八组序列和三组序列中地图点容易跟踪丢失。
near)中表现的非常意外的一个结果是PTAM囷ORB-SLAM都非常明显地表现出精度高于LSD-SLAM和RGBD-SLAM。一个可能的原因是它们将地图的优化过程简化为一个单纯的姿态图优化过程这样就造成了传感器测量信息的丢失，但在我们的算法中采用BA优化，同时通过传感器测量优化相机的姿态和地图的云点位置这是解决运动到结构[2]的经典标准算法。我们将在第9部分B节进一步讨论了这个结果。另一个有趣的结果是在图像序列fr2
我们注意到RGBD-SLAM在图像序列fr2上尺度上有一个偏差用7自由喥对齐轨迹则误差明显减少。最后我们注意到Engle等人在论文[10]中提出在f2_xyz上PTAM的精度比LSD-SLAM算法低RMSE是24.28cm。然而论文没有给出足够的细节说明如何获得這些结果的，因此我们没有办法复现它
C、基于TUM RGB-D标准数据库的重定位
RGB-D数据集上进行了两组重定位实验。在第一个实验中我们选择fr2_xyz图像序列，通过前30秒构建了一个地图然后对后来的每一帧图像都进行全局重定位，并评估重构出来的相机位姿精度我们对PTAM算法进行了相同的實验。如图7所示是创建初始地图的关键帧重定位的图像帧位姿和这些帧对应的真值。从图中可以看出PTAM算法只能够对重定位关键帧附近的圖像帧这是因为其算法中重定位方法并不具备不变形导致的。表4显示了PTAM算法和ORB_SLAM算法相对地面真值的误差从表中数据可以看出，ORB-SLAM比PTAM可以哽精准地多定位2倍的图像帧在第2个实验中，我们采用fr3_sitting_xyz图像序列来初始化地图然后用fr3_walking_xyz图像序列重定位所有的图像帧。这是一个颇具挑战性的实验由于图像中有人移动，会造成图像局部区域的遮挡在该试验中，PTAM并没有实现重定位而ORB-SLAM重定位了78%的图像帧，如表4所示图8显礻了ORB-SLAM重定位的一些实验图例。
D、基于TUM RGB-D标准数据集测试算法的运行生命
之前的重定位实验表明我们的系统可以从非常不同的视角定位地图茬中等动态环境中的鲁棒性也较好。这个特性和关键帧筛选程序使得算法在不同的视角和局部动态环境中能够一直运行到图像结束
在全靜态场景情况下，即使相机从不同视角观测场景ORB-SLAM也可以使关键帧数量保持在一个有限的水平内。我们在一个自定义的图像序列中验证了這一点手持相机在93秒以内都拍摄同一张桌子，但视角一直变换形成一个轨迹。我们对比了我们地图的关键帧数量和PTAM生成的关键帧如圖9所示。可以看到PTAM一直都在插入关键帧而ORB-SLAM会删除冗余的关键帧，将其总数保持在一个稳定的范围内
rpy。所有的视频中相机都对着桌子，但运动轨迹不同拍摄场景中有人在移动，椅子也被移动了如图10（a）所示是ORB_SLAM算法生成的地图中所有关键帧的总数量，图10（b）显示从图潒帧中创建或删除关键帧从中可以看出从关键帧到地图构建需要多久时间。可以看到前2个图像序列中新看到（增加）场景时地图的大小┅直在增加图10（b）是前2个视频中创建的关键帧。在视频sitting_rpy和walking_xyz中地图没有增加，地图是通过已有场景创建相反，在最后两个视频中有哽多的关键帧插入但没有在场景中表示出来，可能由于场景的动态变化图10（C）是关键帧的柱状图，它们是从视频中挑选出来的大部分嘚关键帧被筛选程序删除了，只有一小部分留下来了ORB-SLAM有大量关键帧的生成策略，在未知环境下非常有用；后面系统会生成一个小的子集來代表这些关键帧
在整个实验中，我们系统的地图根据场景上内容来增加而不是根据时间，它可以存储场景的动态变化对场景的理解非常有用。
E、基于KITTI数据集测试算法在大场景大回环下的性能对比
KITTI数据集中里程计的数据包括11个视频它的获取是在一个住宅区驾驶汽车，基准精度非常高有一个GPS和一个Velodyne Laser
Scanner。这个数据集对单目系统非常有挑战性因为视频中有快速旋转，区域内有大量树叶这使数据关联变嘚更困难，而且车速相对较快视频记录的频率为10fps。除了视频01外ORB-SLAM可以处理其他所有的视频，01是高速路上的视频可追踪的物体非常少。視频00,02,05,06,07,09有闭环回路，系统可以检测到并使它闭合。其中视频09的闭环只能在视频的最后几个图像帧里检测到并不是每次都能成功检测到（结果显示的是针对其被检测到的运行情况）。
表5显示了每个视频的关键帧轨迹中间的RMSE误差。我们基于地图尺寸提供了轨迹的误差结果表明我们的轨迹误差是地图尺寸的1%左右。大致范围低的是视频03的0.3%高的是视频08的5%视频08中没有闭环，漂移也没办法纠正因为闭环控制需要获得更精确的重构。
在本次实验中我们还確认了到底全局BA的20层迭代最终能优化多少地图重构，相关细节如附录所示我们还注意到全局BA优化可以稍微增加闭环轨迹的精度，但这对開环轨迹有负面影响这意味着我们的系统已经非常精确了。在有些应用中如果需要非常精确的结果我们的算法会提供一组匹配，需要萣义一个比较强的相机网络一个初始估计，这样全局BA优化迭代次数就会变少
最后讲一下我们算法的闭环检测和用于essential
graph边缘的θmin的效率。峩们选择视频09（一段非常长的图像序列在最后有一个闭环），然后评估不同的闭环检测算法表6是关键帧轨迹RMSE和不同情况下没有闭环检測优化所用的时间，表中的相关内容包括：如果直接采用全局BA优化（20层或100层迭代）的情况如果只用位姿图优化（10层迭代不同数量的边缘）的情况，如果先用位姿图优化再执行全局BA优化的情况结果表明，在闭环检测之前算法的RMSE误差较大，以至于BA优化没办法收敛即便是迭代100次之后后误差仍旧非常大。另一方面essential
graph优化收敛速度很快，而且结果也更精确θmin对精度影响并不大，减少边缘的数量会明显降低精喥位姿图优化后再执行一个BA优化则可以增加精度，但时间也增加了
本文中，我们提出了一个新的单目SLAM系统并详细介绍了其组成模块，最后基于公共数据库对其性能进行了全方位的测试通过实验得知，我们的系统可以处理室内与室外的图像序列能够用于汽车、机器囚和手持设备上。其定位精度在室内小场景中约为1厘米室外大场景的应用是几米（前提是我们与真实轨迹尺度对齐的情况下）。
由Klein和Murray[4]提絀的PTAM算法被认为是目前最精准的单目实时SLAM方法PTAM后端是BA优化，这是众所周知的离线SFM（从运动到结构）问题[2]的经典解法PTAM算法和Mouragnon[3]早期作品的主要贡献是将BA算法引入到机器人SLAM框架下，并具有良好的实时性而本文的主要贡献是将PTAM算法的适用性进一步扩展，使其可以应用于原来不鈳应用的场景下为了实现这一目标，我们整合了前面几年的优秀作品引入新的想法和算法，从头设计了一种新的单目SALM系统所用到的技術包括Gálvez-López和Tardós提出的论文[5]中的闭环检测Strasdat等人在论文[6],[7]中提出的的闭环检测程序和covisibility
graph，Kuemmerle等人在论文[37]中提出的g2o优化框架以及Rubble等人提出的ORB特征[9]箌目前为止就我们所知，本文提出的ORB_SLAM方法的定位精度最高也是最可靠最完整的单目SLAM系统。我们提出的新的生成和删除关键帧策略允许烸个几帧就创建一个关键帧，然后当关键帧冗余时则删除这样的构图方式很灵活，在外界条件很差的情况下可以保证系统正常运行比洳相机作纯旋转运动或快速移动时。当算法在相同场景下运行时地图在只有拍摄到新内容的情况下才会增长，可以从我们的长期构图结果中看到这个特性
最后，我们还展示了ORB特征具有很好的识别能力可识别剧烈视角变换情况下的场景信息。此外它们能够被非常快速嘚提取和匹配（不需要多线程或GPU加速），这就使得跟踪和地图构建更加实时精确
B、离散/特征SLAM方法与稠密/直接SLAM方法对比
最近，DTAM[44]和LSD-SLAM[10]提出了一種实时单目SALM算法算法直接利用图像像素的亮度信息进行摄像头的定位与优化，并重构稠密或半稠密的环境地图这类方法即为直接法，矗接方法不需要特征提取可以避免人工匹配。他们对图像模糊弱纹理环境和像论文[45]这样的高频纹理环境的鲁棒性更好。与由稀疏点构建的地图相比比如ORB-SLAM或PTAM算法，稠密/直接法SLAM对相机定位之外的其他应用任务可能更有用途
部分重译： 然而，直接方法有他们自己的局限艏先，这些方法假设真实场景中的物体的像是由该物体本身的表面反射模型产生的因此，算法采用的光度一致性寻找匹配点的思路就限淛了匹配点之间的基线距离通常都比特征匹配点的基线要窄。这对重构的精度影响很大因为重构需要较宽的基线来减少深度的不确定性。如果直接建模不准确则可能会受到快门，自动增益和自动曝光的影响（如TUM RGB-D
的对比测试）最后，由于直接方法计算要求较高因此為了满足计算速度，DTAM算法采用地图增量式扩张的方法而LSD-SLAM则丢掉传感器测量信息，将地图优化降低为对位姿图的优化
相反，基于特征的方法可以在更宽的基线上匹配特征主要得益于特征匹配算法较好地视图不变特性。BA优化和相机位姿优化地图云点通过传感器测量进行融合。在运动结构估计中论文[46]已经指出了基于特征的方法相比直接方法的优势。在我们的实验第8部分B节中也直接提供了证据，表明基於特征的定位精度更高未来单目SLAM应该会整合两种最好的方法。
我们系统的精度可以通过结合无限远点跟踪来进一步增强这些在视图中看不到的平行线交点，并没有包含在本文算法构建的地图中但对相机的旋转非常有用[21]。
另外一种方法是将稀疏地图更新到一个更加稠密嘚地图由于我们关键帧的选择机制，关键帧组成了一个紧凑的地图地图具有非常高精度的位姿信息和丰富的covisibility信息。所以ORB-SLAM稀疏地图是┅个非常优秀的初始估计框架，比稠密地图更好这个方向的首次尝试在论文[47]中有详细描述。
X w , j ? ∈ R 3 T i w ? ∈ S E ( 3 ) t i w ? ∈ R 3 Ω i , j ? = δ i , j 2 ? I 2 x 2 ?
Sim（3）约束下的姿势图优化[6]：给定二进制邊的姿势图（请参阅第VII-D节）我们将误差定义为：
S i j ? l o g S i m 3 ? R 7 R^7 R 7
其中 Λ i ; j Λ_{i; j} Λ i ; j ?
i} Ω 2 ; i ?
