ORB-SLAM论文详解

Abstract

本文介绍了ORB-SLAM，这是一种基于特征的单目SLAM系统，可在大小型室内外环境中实时运行。该系统对严重的运动模糊具有鲁棒性，可实现较宽的基线回环和重定位，并包括全自动的初始化。我们基于近年来的出色算法，从头开始设计了一个新颖的系统，该系统对所有SLAM任务使用相同的特征 ：跟踪，映射，重定位和回环。利用优胜劣汰的策略选择关键帧和点，从而有出色的鲁棒性，并生成了紧凑的跟踪地图，该地图仅在场景内容发生变化时才会增长，从而实现持续运行。我们提供了来自最受欢迎数据集的27个序列的详尽评估。与其他最新的单目SLAM方法相比，ORBSLAM取得了空前的性能。

I INTRODUCTION

BA可以提供准确的摄像机定位估计值以及稀疏的几何重构，只要提供强大的匹配网络和良好的初始值估计即可。长期以来，这种方法被认为无法应用于实时应用程序（如Visual SLAM）。 Visual SLAM的目标是在重建环境的同时估算摄像机的轨迹。因此为了以不增加计算成本的方式获得准确的结果，实时SLAM算法必须为BA提供：
•在所选帧的子集（即关键帧）匹配的场景特征的观测（地图点），即对所选帧中点云的观测信息；
•由于关键帧数量的增长，需要做筛选避免冗余，即选择合适的关键帧；
•关键帧的特征点需要有显著的视差和回环匹配；
•提供非线性优化算法的初值；
•构建局部地图时优化的可扩展性；
•快速全局优化（位姿图）。

BA的第一个实时应用是Mouragon等人的视觉里程计工作随后是Klein和Murray的突破性SLAM工作，称为Parallel Tracking and Mapping（PTAM）。 该算法虽然限于小规模操作，但为关键帧选择，特征匹配，点三角剖分，每帧的摄像机定位以及跟踪失败后的重定位提供了简单而有效的方法。
然而PTAM缺乏回环检测和适当的对遮挡场景的处理(handling of occlusions)，视角(viewpoint)不变性差，地图初始化需要人工干预，使PTAM算法的应用受到严重限制。

在这项工作中，我们以PTAM的主要思想，采用了Galvez-L´opez和Tard´os的位置识别工作，采用了Strasdat等人的尺度感知闭环检测（scale-aware loop closing）以及共视信息（covisibility information）大尺度运行，从头开始设计ORB-SLAM，这是一种新颖的单目SLAM系统，其主要贡献是：

•对所有任务使用同一种特征：跟踪，建图，重定位和回环。这使我们的系统更加高效，简单和可靠。我们使用ORB特征，无需GPU即可实现实时性能，而且具有视角不变性和照明不变性。
•大型环境中的实时运行。由于使用了共视图（covisible graph），特征点的跟踪和建图都集中在局部共视区域，而与全局地图的大小无关。
•基于pose graph优化的实时闭环，称之为本质图（Essential graph）。它是由系统维护的生成树（spanning tree），回环链（loop closure links）和共视图的强边构成。
•实时摄像机重定位，具有明显的视角不变性和光照不变性。这使得系统跟踪丢失后恢复，还可以增强地图重用性。
•提出了一种基于模型选择的自动的、鲁棒的初始化方法，可以创建平面（planar）和非平面（non-planar）场景的初始地图。
•采用了优胜劣汰(survival of the fittest)的方法选择关键帧和地图点，即宽选取、严剔除。该策略提高了跟踪的鲁棒性，并由于丢弃了多余的关键帧因此提高了系统长时间运行的能力。

我们对来自室内和室外环境的流行公共数据集进行了广泛的评估，包括手持设备、汽车和机器人的数据集。值得一提的是，我们模型的结果优于基于直接法的sota算法LSD-SLAM。直接法优化像素强度而特征点法优化的是重投影误差。我们在第IX-B节中讨论了特征点法比直接法更精确的原因。

本文在我们前面的工作[11]的基础上，提出了闭环和重定位方法。系统的初步版本见[12]。本文加入了初始化方法、本质图，并完善了其它模块。我们还详细描述了各个模块，并进行了详尽的实验验证。据我们所知，我们的工作是目前最完整和可靠的单目SLAM系统。为了社区的利益，我们公开了源代码。演示视频和代码可以在项目网页[http://webdiis.unizar.es/∼raulmur/orbslam ]中找到。

II. RELATED WORK

A. Place Recognition

Williams等人比较了几种用于位置识别的方法，并得出结论基于外观的方法（图像到图像）优于地图到地图和图像到地图的方法。在基于外观的位置识别方法中，词袋技术是比较高效的方法，例如概率方法FAB-MAP 。 DBoW2 首次使用了二进制词袋（bags of words），二进制单词是BRIEF描述子，该描述子基于FAST关键点。目前为止，与词袋方法中使用的SURF 和SIFT特征相比，使用FAST减少了一个以上的数量级特征提取时间。尽管系统被证明是非常有效和强大的，但BRIEF不支持旋转不变性和尺度不变性，系统也只能运行在同一平面内（否则会造成尺度变化），回环检测也只能从相似的视角中进行。在我们以前的工作中[11]，我们提出了一个使用ORB[9]构建DBoW2的位置识别器。 ORB是具有旋转不变性和尺度不变性（在一定范围内）的二值特征，检测速度很快，对视角变化比较鲁棒。我们在四个不同的数据集中展示了位置识别器的高召回率和鲁棒性，只需不到39毫秒（包括特征提取）即可从10K图像数据库中检索候选回环。在这项工作中，我们使用该位置识别器的改进版本，在检索数据库时使用了共视（covisibility）信息并返回几个假设（hypotheses），而不仅仅是最佳匹配。

B. Map Initialization

单目SLAM需要一个策略来创建初始地图，因为无法从单个图像中恢复深度。其中一种方法是一开始跟踪一个已知的结构[20]。 后端使用滤波器，可以使用逆深度参数化[21]来初始化点的深度，然后收敛到它们的真值。 Engel等人[10]最近的半稠密工作遵循了类似的方法，将像素深度初始化为具有高方差的随机值。

来自两个视图的初始化方法要么假设局部场景平面性（locally scene planarity），然后使用Faugeras等人的方法从单应性矩阵（homegraphy）中恢复相对相机姿态，要么将场景建模为通用情况并使用Nister 的五点法（five-point）计算本质矩阵（essential matrix），但是该算法存在多个解的情况(从本质矩阵恢复运动会存在四个解)。 两种重建方法在低视差下都不会受到很好的约束，并且如果平面场景的所有点都靠近相机中心之一，并且会受到双重歧义（twofold ambiguity）的问题。 另一方面，非平面场景使用八点法[2]计算唯一的基本矩阵（fundamental matrix），可以在不产生歧义的情况下恢复相对相机位姿。

我们在第四节中介绍了一种基于模型选择的自动初始化方法，平面场景选择单应性矩阵和非平面场景选择基本矩阵。 Torr等人的提出了模型选择的统计方法， 在类似的理论基础下，我们开发了一种启发式初始化算法，该算法考虑了在接近退化（degenerate）的情况下（即平面，近平面和低视差）选择基本矩阵的风险，倾向于选择单应矩阵。 在平面情况下，安全起见，如果解有双重歧义，我们将不进行初始化，因为这可能选择损坏的解。 因此我们延迟初始化，直到该方法产生具有明显视差的唯一解。

C. Monocular SLAM

最初的单目SLAM通过滤波器法来求解。 在这种方法中，滤波器对每个帧进行处理，联合估计地图特征位置和相机位姿。 该方法的问题是在处理具有少量信息的连续帧时浪费计算资源，而且线性化误差也不断累积。另一种方法是基于关键帧的方法，仅使用选定帧（关键帧）来估计地图，允许通过计算量更大但更精确的BA优化，因此建图与帧率不再相关。 Strasdat等人证明了在相同的计算成本下，基于关键帧的技术比过滤更为准确。

最有代表性的基于关键帧的SLAM系统是Klein和Murray提出的PTAM。 这是第一个将跟踪和建图分开平行处理的工作，并在小型环境中的AR应用中被证明是成功的。后来引入边缘特征对原始PTAM进行了改进，在跟踪过程中的添加旋转估计步骤，使用更好的重定位方法。由于PTAM中的地图云点通过图像区块与FAST角点匹配，因此仅适合于特征跟踪并不适合用于后期的位置识别。实际上，PTAM算法并没有进行大闭环检测，其重定位也仅是基于关键帧低分辨率缩略图的相关性进行的，因此视角不变性较差。

Strasdat等人[6]提出了一种大尺度单目SLAM系统，其前端基于GPU上实现的光流，然后进行FAST特征匹配并进行运动BA（motion-only BA），后端基于滑动窗口BA。在回环检测方面，通过具有相似性约束（7DoF）的pose graph优化，该方法能修正单目SLAM中出现的尺度漂移问题。这项工作我们采用了7DoFpose graph优化进行回环的想法，并将其应用于定义的本质图（essential graph）。

Strasdat等人[7]使用PTAM的前端，但其跟踪部分仅在一个从共视图提取的局部图中进行。他们提出了一种双窗口优化后端的方法，在内窗口中连续执行BA，在有限大小的外窗口中构建位姿图。但是，仅当外部窗口的大小足够大以包括整个回环时，回环检测才有效。在我们的系统中，我们利用了基于可共视性的局部地图的思想，通过共视图构建位姿图，但是完全重新设计了前端和后端。另一个区别是，我们没有使用特定的特征（SURF）进行回环检测，而是使用与跟踪和建图所用特征一致的特征进行位置识别，从而获得鲁棒的重定位和回环检测。

Pirker等人[33]提出了CD-SLAM，这是一个非常完整的SLAM系统，包含回环检测、重定位、大尺度运行和尝试在动态环境中工作，然而地图初始化未被提及。由于缺乏开源代码，我们无法对准确性、鲁棒性或大规模能力进行比较。

Song等人[34]的视觉里程计使用ORB特征进行跟踪，并使用时间滑动窗口BA作为后端。相比之下，我们的系统更通用，因为Song的系统没有全局重定位、回环检测，地图也不能重用。他们还利用相机到地面的已知距离来限制单目尺度漂移。

Lim等人在我们提交这项工作的初步版本之后发表，他们也使用相同的特征进行跟踪、建图和回环检测。但是他们选择了BRIEF描述子，不具有尺度不变性，限制了系统在平面轨迹上。他们的系统仅跟踪来自最后一个关键帧的点，因此如果重新访问，该地图就不会重用（类似于视觉里程表），并且存在无限增长的问题。我们在第VIII-E节中使用这种方法定性比较了我们的结果。

Engel等人的最新工作为LSD-SLAM，能够使用直接法（即直接根据图像像素强度进行优化）而不是根据特征点法进行BA来构建大规模半稠密地图。他们的结果非常令人印象深刻，因为该系统能够实时运行而无需GPU加速，构建一个半稠密地图，比基于特征的SLAM产生的稀疏地图更有潜力。尽管如此，它们的回环检测仍然需要特征点，并且它们的相机定位精度明显低于我们的系统和PTAM，如我们在第VIII-B中的实验所示。

介于直接法和特征点法的是Forster等人[22]的半直接视觉里程计SVO。无需在每个帧中提取特征，它们就可以以高帧速率运行，从而在无人机中获得令人印象深刻的结果。然而，他们不执行闭环检测，并且当前的实现主要考虑朝向下的相机。

最后，我们要讨论关键帧选择。文几乎所有的VSLAM系统都认为基于所有点和所有帧运行BA是不可行的。 Strasdat等人的工作表明，最具成本效益的方法是保留尽可能多的点，同时仅保留非冗余关键帧。 PTAM方法是非常谨慎地插入关键帧，以避免计算复杂性的过度增长。这种严格的的关键帧插入策略使跟踪算法在硬探索条件（hard exploration conditions）下失败，我们的优胜劣汰策略（survival of the fittest）得以生存，通过尽快插入关键帧并在以后删除多余的关键帧来避免额外的成本，从而在恶劣的场景中获得了前所未有的鲁棒性。

III. SYSTEM OVERVIEW

A. Feature Choice

我们系统的中心思想是建图、跟踪、位置识别、重定位、回环检测均使用相同的特征。这使我们的系统高效，并且避免了像以前的工作那样需要额外插入一些识别性强的特征以用于后期的闭环检测。我们要求每个图像的提取时间要少于33ms，这排除了流行的SIFT（∼300ms）、SURF（∼300ms）或最近的A-KAZE（∼100ms）。为了获得通用的位置识别能力，我们需要旋转不变性，这排除了BRIEF和LDB。

我们选择了ORB 特征，它是定向的多尺度FAST角，具有256位的描述子。它们的计算和匹配速度非常快，同时对视角有良好的旋转不变性，因此可以在更宽的基准线进行匹配，提高BA的精度。我们已经在[11]中展示了ORB在位置识别方面的良好性能。虽然我们目前的实现使用ORB特征，但并不局限于此。

B. Three Threads: Tracking, Local Mapping and Loop Closing

我们的系统，如图1所示，包含三个并行运行的线程：跟踪(tracking)、局部建图(local mapping)和回环检测(loop closing)。Tracking线程负责在每帧中定位相机，并决定何时插入新的关键帧。我们首先将当前帧与上一帧执行特征匹配，然后使用motion-only BA优化位姿。如果跟踪丢失（比如由于特征点被遮挡或突然移动），位置识别模块用于执行全局重定位。一旦特征匹配和对相机姿势进行了初始估计，则使用由系统维护的关键帧的共视图检索局部可见地图，参见图2（a）和图2（b）。

然后通过重投影搜索当前帧与局部地图匹配的地图点，利用所有匹配点优化相机位姿。最后，跟踪线程决定是否插入新的关键帧。第V节详细说明了所有跟踪步骤。第IV节介绍了创建初始地图的新方法。

局部地图模块负责处理新的关键帧并执行局部BA，对周围的相机位姿进行局部BA以优化重构。在共视图已连接的关键帧中搜索新关键帧中ORB特征的匹配点，以对新点进行三角测量。有时尽管已经创建了新的地图点，但基于跟踪线程过程中新收集的信息，为了保证地图点的高质量，可能会根据筛选策略临时删除一些点。局部地图构建模块也负责剔除冗余关键帧。我们将在第六节中详细说明所有本地映射步骤。

回环检测将每个新关键帧进行回环搜索。如果检测到回路，我们计算一个相似性转换，该转换查看在回环中累积的漂移。然后将循环的两边对齐，并将重复的点融合。这样回环的两侧可以对齐，对重复的点可以融合。最后在相似性约束上进行位姿图优化以获得全局一致性。主要的创新之处在于，我们对本质图进行了优化，本质图是共视图的一个稀疏子图，这将在第III-D节进行解释。第VII节详细说明了回环检测和矫正的步骤。

我们使用g2o中实现的Levenberg-Marquardt算法来执行所有优化。在附录中，我们描述了误差项，代价函数和每个优化涉及到的变量。

C. Map Points, KeyFrames and their Selection

对每个关键帧Ki保存以下信息：
•它在世界坐标系中的3D位置Xw,i。
•视角方向ni，它是所有视角方向的平均单位向量（将点与观察该点的关键帧的光学中心相连的光线）。
•ORB特征描述符子Di，在匹配特征点可以通过计算描述子的汉明距离得到，其汉明距离相对于观察点的关键帧中的所有其他关联的描述符最小。
•根据ORB特征的尺度不变性，可观测的地图点的最大距离dmax和最小距离dmin

对每个关键帧Ki保存以下信息：
•相机位姿Tiw，这是一种刚体变换，可以将点从世界坐标系转换到相机坐标系下。
•相机固有特性，包括焦距和主点。
•从图像帧提取的所有ORB特征，不管其是否已经关联了地图云点， 这些ORB特征点都经过畸变模型矫正过。

地图点和关键帧的创建条件较为宽松，但是删除条件则是严苛的，主要删除检测出的冗余关键帧和错误匹配或不可跟踪的地图点。 这允许在探索过程中灵活地扩展地图，从而提高了在恶劣条件下（例如旋转，快速移动）的跟踪鲁棒性，而与此同时，当相机对同一个环境重访问时，地图的尺度大小是可控的，这就利于该系统的长期工作。另外，与PTAM相比，我们的地图包含外点(outliers )的很少。 地图点和关键帧的剔除过程分别在第VI-B节和第VI-E节中进行了说明。

D. Covisibility Graph and Essential Graph

关键帧之间的共视信息在我们系统的几个模块上很有用，使用[7]中的无向加权图表示共视图。每个节点是一个关键帧，如果两个关键帧之间共同观测到的地图点超过15个，则两个关键帧节点之间存在边，边的权重θ为共视地图点的数量。

为了修正回环，我们执行pose graph优化[6]，优化方法延着位姿图将闭环回路的误差进行分散。为了不包括共视图中所有的边，因为可能会很密集，我们构建了本质图(Essential Graph)。本质图保留所有节点（关键帧），但是边更少，仍然保留了可以产生准确结果的强大网络。系统从初始关键帧开始逐步构建生成树，该树提供具有最小数量的边的共视图的连接子图（其实就是共视图的最小生成树）。插入新的关键帧后，它将被包含在树中，与共视最多地图点的关键帧节点连接，并且当关键帧被淘汰策略删除时，系统会重新更新与其相关的连接。本质图包含一个生成树，一个高共视性(θmin>100)的共视图的边的子集，以及回环的边，组合成强大的相机网络。图2展示了共视图、生成树和对应的本质图的实例：

如第VIII-E节的实验所示，执行位姿图优化时，结果是如此精确，以至于额外全局BA几乎没有带来提高。第VIII-E节的结尾显示了本质图的效率和θmin的影响。

E. Bags of Words Place Recognition

该系统嵌入了一个基于词袋的位置识别模块，使用DBoW2[5]，用于执行回环检测和重定位。视觉单词（visual words）是特征描述子空间的离散化，称为视觉词典（visual vocabulary）。词典是从大量图像中提取的ORB描述子离线创建的。如果图像足够通用，同一词典可用于不同环境而且获得良好性能，如我们先前的工作[11]所示。该系统以增量方式构建一个数据库，该数据库包含一个反向指针，用于存储每个视觉词典里的视觉单词。关键帧可以通过这个数据库查询视觉词典，从而实现高效检索。删除关键帧时，也会更新该数据库。
由于关键帧之间存在视觉重叠，因此在查询数据库时，将不存在唯一得分高的关键帧。原始的DBoW2考虑到了这种重叠，将时间上接近的图像的得分相加。这有一个局限性，即不包括观测同一位置但在不同时间插入的关键帧。此外，我们的数据库还返回所有得分高于最佳得分75％的关键帧。

[5]描述了用词袋模型进行图像匹配的另一个好处，当我们要计算两组ORB特征之间的对应关系时，我们可以仅在词汇树某一层（我们从6层里选第2层）进行暴力匹配，以加快速度搜索。我们在搜索匹配的特征点、三角化新的地图点、回环检测、重定位时都使用这个技巧。我们还使用方向一致性测试完善了对应关系，有关详细信息参见[11]，该测试舍弃了异常值，从而确保了所有对应关系方向的一致性。

IV. AUTOMATIC MAP INITIALIZATION

地图初始化的目标是计算两帧之间的相对位姿，以三角化一组初始地图点。此方法应独立于所处场景（平面场景或一般场景），且不需要人为干预来选择良好的（即具有明显视差的）两个关键帧。我们的方法是，同时计算两个几何模型，一个是假设为平面场景的单应矩阵，另一个是假设为非平面场景的基础矩阵。然后使用一种启发式方法来选择其中一个模型，并通过该模型恢复出相对位姿。我们的方法只有当这两个模型是确定的，即两个帧是可用的才会进行初始化。而当检测到两帧之间低视差时，会碰到著名的twofold planar ambiguity[27]问题，此时需要避免初始化，否则会得到一个损坏的地图。我们算法的步骤如下：

1）进行初始的特征匹配：

已知参考帧Fr的ORB特征为x_r，对当前帧Fc抽取ORB特征x_c，然后对x_c和x_r进行匹配。如果没有匹配到足够的特征对，则重置参考帧为当前帧。

2）并行计算两个几何模型（单应矩阵和基础矩阵）：

通过两个并行线程计算单应矩阵Hcr和基础矩阵Fcr，计算公式如下：
x_c = Hcr * x_r
x_cT * Fcr * x_r=0
基于RANSAC方案，分别使用normalized DLT和8点法计算Hcr和Fcr。 为了使得这两个模型一致，它们的迭代次数是预定义的而且是一致的，每次迭代时，基础矩阵使用8个点，单应矩阵使用4个点。每次迭代时我们对每个模型M（单应矩阵H或基础矩阵F）计算一个分数SM：
其中dcr2和drc2是从某帧到另一帧的对称的转换误差(transfer errors)。 TM是outlier删除阈值，该阈值基于χ2测试值为95%（T_H=5.99，T_F=3.84，假设测量误差的标准差为1像素）。Γ被定义为等于T_H，这样两个模型对于inlier，d的得分相等，也是为了两个模型一致。
我们保留了得分最高的单应矩阵和基本矩阵。如果没有模型合格（没有足够的内点），返回步骤1。

3）模型选择：

如果所处的场景是平面场景、接近平面场景或低视差场景，那么可以使用单应矩阵对场景进行建模。然而，如果场景也可以建模为基础矩阵但是没有很好的约束[2]，那么从基础矩阵中恢复运动会得到错误的结果。 我们应该选择单应矩阵，因为重构方法可以从平面场景或低视差场景正确的初始化。另一方面，一个非平面场景而且有足够的视差是可以通过基础矩阵进行建模，虽然也可以通过单应矩阵建模但是最好选择基础矩阵。我们发现一个鲁棒的启发式方法，计算：
R_H=S_H/(S_H+S_F)
如果R_H>0.45则选择单应建模，这足以捕获平面和低视差情况，否则选择基础矩阵。

4)恢复运动和结构

选择好一个模型后，就可以从中恢复出运动。在单应矩阵的情况下，我们使用Faugeras等人的方法[23]得到8个解，该方法提出了用cheriality tests选择一个有效解。而，如果在低视差的情况下，这些测试就会失效，因为点很容易在相机的前面或后面移动，会导致选择一个错误的解。我们提出的方法是直接三角化这8个解对应的二维点，然后检查是否有一种解可以使得所有的地图点都在两个相机的前面，而且重投影误差较小。如果还是没有得到一个可用的解，那么放弃本次初始化且回到步骤1。这种解的消歧技术使得我们的初始化在低视差和解存在双歧义的情况下具有鲁棒性，可以认为是我们方法鲁棒的关键。

在使用基础矩阵的情况下，我们使用校准矩阵K (calibration matrix)将其转换为本质矩阵：

然后用[2]所述的奇异值分解(singular value decomposition)获得4个解。我们对得到的四个解进行三角化，然后像上面单应矩阵的做法那样获得一个可用解。

5)光束法平差(BA)

最后我们执行全局BA（细节参见附录）

图3展示了NewCollege机器人的户外序列[39]中具有挑战性的初始化的示例：
上图：PTAM，中间：LSD-SLAM，下图：ORB-SLAM，在NewCollege中初始化后的一段时间， PTAM和LSD-SLAM初始化损坏的平面解，而当我们的方法检测到足够的视差时，它会根据基本矩阵自动进行初始化。 根据手动选择的关键帧，PTAM也可以很好地进行初始化。

V. TRACKING

本节我们描述了跟踪线程在每帧上的执行步骤。几个步骤都提到的相机位姿优化仅包括motion-only BA，如附录中的描述。

A. ORB Extraction

我们在8层图像金字塔上提取FAST角点，金字塔尺度因子为1.2。 如果图像分辨率从512x384到752x480，我们发现提取1000个角点比较合适。如果分辨率提高，如KITTI数据集[40]的1241x376，则提取2000个角点。为了确保特征点均匀分布，我们将每层图像划分为网格，每格提取至少5个角点。 然后检测每格角点数量，如果数量不够则调整提取阈值。如果某些单元格内检测不出角点（无纹理或低视差），则其对应提取的角点数量也相应减少。最后，根据保留的FAST的角点计算方向和描述子。ORB特征描述子将用于算法后续所有的特征匹配，而不是像PTAM算法只用于跟踪。

B. Initial Pose Estimation from Previous Frame

如果上一帧图像跟踪成功，我们就用运动速率恒定模型来预测当前相机的位置（即认为摄像头处于匀速运动），然后搜索上一帧图像中的特征点在地图点与当前帧图像的匹配点，最后利用搜索到的匹配点对当前相机的位姿进行优化。但是如果没有找到足够的匹配点（比如，运动模型失效，非匀速运动），我们就加大搜索范围，搜索地图点附近的点，然后通过寻找到的对应匹配点对来优化当前时刻的相机位姿。

C. Initial Pose Estimation via Global Relocalization

如果丢失了跟踪，则将当前帧转换为词袋向量，然后在识别数据库（recognition database）中查询关键帧候选者。 如第III-E节所述，我们对每个候选帧计算和地图点对应的ORB特征。 然后，接着，对每个备选关键帧轮流执行PnP算法[41]计算当前帧的位姿（RANSAC迭代求解）。如果我们找到一个姿态能涵盖足够多的有效点，则搜索该关键帧对应的更多匹配地图点。最后，基于找到的所有匹配点对相机位置进一步优化，如果有效数据足够多，则跟踪程序将持续执行。

D. Track Local Map

一旦我们有了初始相机位姿和一组初始的特征匹配之后，我们可以将更多的地图点投影到该帧上并寻找更多的匹配点。为了限制大地图的复杂度，我们仅仅投影局部地图。该局部地图包含一组关键帧K1，他们与当前帧有共同的地图点，还包含在共视图中关键帧K1周围的一组关键帧K2。该局部地图还拥有一个参考帧Kref ∈K1，该参考帧与当前帧共视最多的地图点。在K1和K2中看到的每个地图点都在当前帧中进行如下搜索：
1）计算地图点在当前帧图像中的投影点x。如果投影位置超出图像边缘，就将对应的地图云点删除。
2）计算当前视图射线v和地图点平均视线方向n之间的角度，如果v·n <cos（60◦），就删除对应地图点。
3）计算从地图点到相机中心的距离d。如果它不在地图点的尺度不变区间内，即d∉[dmin,dmax]，就删除该地图点。
4）计算该帧图像的尺度比 d/dmin
5）对比地图点的特征描述子D和当前帧中还未匹配的ORB特征，在预测的尺度层和靠近x的地图点作最优匹配。

相机位姿通过在该帧的所有的地图点得到优化。

E. New Keyframe Decision

最后一步是确定是否将当前帧作为新的关键帧局部地图有一个机制删除多余的关键帧，我们尝试尽可能快的插入关键帧，这使得系统对相机运动更加鲁棒，比如旋转运动。插入关键帧的条件如下：

1）距离上一次全局重定位后需要超过20帧图像。

2）局部地图构建处于空闲状态，或距上一个关键帧插入后，已经有超过20帧图像。

3）当前帧跟踪少于50个地图云点。

4）当前帧跟踪少于参考关键帧K_ref云点的90%。

与PTAM中用关键帧之间的距离作为判断标准不同，我们加入一个最小的视图变化(minimum visual change)，如条件4。条件1 确保一个好的重定位，条件3保证好的跟踪。如果局部地图构建处于忙状态（条件2的后半部分）的时候插入关键帧，就会发信号去暂停局部BA，这样就可以尽可能快地去处理新的关键帧。

VI. LOCAL MAPPING

在本节我们描述每个新关键帧Ki的局部建图步骤。

A. KeyFrame Insertion

首先，我们更新共视图，增加一个新的节点Ki，检查与Ki由共视地图点的其它关键帧节点，并添加相应的边。然后更新生成树：将Ki连接到与它有最多共视点的节点上。然后计算该关键帧的词袋向量，并用三角测量法生成新的地图点。

B. Recent Map Points Culling

为了保留在地图中，地图点必须在创建后的前三个关键帧期间通过限制性测试，以确保它们是可跟踪的并且不会错误地进行三角剖分（即由于虚假数据关联）。 一个点必须满足以下两个条件：

跟踪线程必须在超过25%的图像中找到该特征点

如果创建地图点后经过了多个关键帧，那么它至少被3个关键帧观测到。

一旦一个地图云点通过测试，它只能在被少于3个关键帧观测到的情况下移除。这样的情况在关键帧被删除以及局部BA排除异值点的情况下发生。这个策略使得我们的地图包含很少的无效数据。

C. New Map Point Creation

新的地图点是通过对共视图中已连接关键帧Kc中的ORB特征点三角化而来。对于Ki中每个未匹配的ORB特征，我们在其它关键帧的为匹配的地图点中进行查找，这个匹配过程可以看第III-E节，然后将那些不满足对极约束的匹配点删除。ORB特征点对 三角化后，需要满足两个相机坐标均正深度、视差、重投影误差、尺度一致性等条件，通过后则该点被插入地图。起初，一个地图云点通过2个关键帧观测，但它在其他关键帧中也有对应匹配点，所以它可以映射到其他相连的关键帧中，搜索算法的细则在本文第5部分D节中有讲述。

D. Local Bundle Adjustment

局部BA优化当前关键帧Ki和在共视图中与Ki连接的其它关键帧Kc，以及被这些关键帧观测到的地图点。看到这些地图点但是没有连接到当前关键帧的其它关键帧保持固定。outliers的观测值将被丢弃。更多关于局部BA细节请查阅附录。

E. Local Keyframe Culling

为了保持一个精简的结构，局部地图尝试删除多余的关键帧。这有利于BA，因为BA的复杂度随着关键帧的数量增长，也有利于系统在同一个地方的长期运行，因为关键帧的数量不会无线增长，除非场景改变。如果关键帧Kc中90%的点都可以被其他至少三个关键帧同时观测到，那认为Kc的存在是冗余的，我们则将其删除。尺度条件确保地图点以最准确的方式保留他们的关键帧。这个策略受到Tan等人[24]工作的启发，他们的工作中经过一系列变化检测后将关键帧删除。

VII. LOOP CLOSING

回环检测线程接收Ki，Ki是局部建图处理的上一个关键帧，回环线程尝试检测和闭合回环。步骤如下。

A. Loop Candidates Detection

首先计算关键帧Ki与Ki在共视图上相邻关键帧(θmin = 30)的相似性，通过词袋向量计算，保留最低分值Smin。然后我们检索图像识别数据库，丢弃所有与Ki相似性分值低于Smin的关键帧。这和DBoW2中归一化分值的操作类似，可以获得好的鲁棒性，DBoW2中计算的是前一帧图像，而我们是使用的共视信息。另外，所有连接到Ki的关键帧都会从结果中删除。为了获得候选回环，我们必须检测到3个连续的候选回环（共视图中相连的关键帧）。如果对Ki来说环境样子都差不多，就可能有几个候选回环。

B. Compute the Similarity Transformation

单目SLAM系统有7个自由度，3个平移自由度，3个旋转自由度和一个尺度自由度[6]，这可能会导致地图漂移。因此，闭合回环，我们需要计算从当前关键帧Ki到回环关键帧Kl的相似变换，以获得回环的累积误差。计算相似变换也可以作为回环的几何验证。

我们首先将当前帧和回环候选帧进行特征匹配，这个过程请参阅第III-E节。此时我们可以得到3D到3D对匹配。我们对每个候选回环帧执行RANSAC迭代，尝试使用Horn[42]的方法找到相似性变换。如果有足够的内点，我们就能找到相似性变换Sil，然后我们优化它(具体看附录)，然后搜索更多的对应关系。如果Sil有更多的内点，我们将再次优化它，Kl被回环接受。

C. Loop Fusion

回环修正的第一步是融合重复的地图点，在共视图中插入新的边。首先通过相似变换Sil矫正当前关键帧的位姿Tiw，这种矫正方法应用于所有与Ki相邻的关键帧，这样回环两端就可以对齐。然后，回环关键帧及其近邻能观测到的所有地图点都映射到Ki及其近邻中，并在映射的区域附近小范围内搜索它的对应匹配点，如第5部分D节所述。所有匹配的地图点和计算Sil过程中的有效数据进行融合。融合过程中所有的关键帧将会更新它们在共视图中的边，创建的新边将用于回环检测。

D. Essential Graph Optimization

为了高效的闭合回环，我们在本质图(Essential Graph)上执行一个位姿图优化，如第III-D节描述的那样。这样可以将回环误差分散到各个关键帧上。优化程序通过相似变换校正尺度漂移[6]。误差项和代价计算如附录所示。优化过后，每一个地图点都根据关键帧的校正进行变换。

VIII. EXPERIMENTS

我们采用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. System Performance in the NewCollege Dataset

NewCollege数据集[39]包含一个2.2km的图像序列，它来自机器人横穿校园和附近的公园。该数据集由双目相机拍摄，帧率为20fps，分辨率为512x382，包含几个回环和快速旋转，这使得该序列对单目视觉十分有挑战性。据我们所知，没有什么其它的单目系统能够处理这个序列。比如Strasdat等人[7]，尽管能够闭合回环和工作在大尺度环境，但也只能在小部分序列上运行成功。

我们在图4中展示了使用支持相似性转换的内点的回环检测：
在这里插入图片描述
图5则对比了回环闭合前后的环境地图重构状况。其中，红色标注的是局部地图，回环检测后可以看到其两端扩展到连接整个运行轨迹。

图6是以实时帧率速度运行整个图像序列后的全局地图，从图中可以看出，后边的大回环并没有完全闭合，它从另外一个方向穿过，位置识别程序没能发现闭合回路。

我们统计了ORB_SLAM算法每个线程所用的时间。表1显示了算法跟踪和局部构图的时间：

可以看到，跟踪的帧率可以达到25-30。如果需要的话，这个时间还可以更快，只要减少局部地图中所包含的关键帧数量即可。在局部建图线程中最费时间的是局部BA。局部BA所需的时间根据机器人所处环境的变化而变化，在陌生环境所需时间比熟悉的环境所需时间更多。因为在未知环境中如果跟踪线程插入一个新的关键帧，BA优化会被中断，如第V-E节所示。如果不需要插入新的关键帧，局部BA优化则会执行大量已经设置的迭代程序。

表2展示了6个闭合回环的结果：
可以看到回环检测的时间随着关键帧的数量的增长而增长。这是因为数据库的高效查询只比较图像的子集和共同的单词，这显示了词袋模型在位置识别中的潜力。我们的本质图包含的边的数量约是关键帧数量的5倍。

B. Localization Accuracy in the TUM RGB-D Benchmark

TUM RGB-D benchmark[38]是一个用于评估相机定位精度的优秀数据集，它提供了几个图像序列和对应的由外部运动捕捉系统提供的轨迹真值。我们放弃了所有那些我们认为不适合纯单目SLAM系统的序列，因为它们包含强旋转、无纹理或无运动。

作为对比，我们选择了最近提出的直接法半稠密LSD-SLAM[10]和经典算法PTAM[4]。我们还比较了由RGBD-SLAM[43]生成的轨迹，使用基准网站上提供的图像序列。为了比较ORB-SLAM，LSD-SLAM，PTAM和ground truth，我们用相似变换对齐关键帧轨迹，在尺度未知的情况下，测量轨迹的绝对误差(ATE)[38]。对RGBD-SLAM算法，我们通过相机坐标变换来对齐轨迹，也采用同样的方法检测尺度是否重构良好。LSD-SLAM从随机深度值开始初始化，然后随机值逐渐收敛，因此与真值对比的时候，我们丢掉前10个关键帧。对于PTAM算法，我们从一个好的初始化中，手动选择两个关键帧。表3展示对我们选择的16个图像序列运行5次的中间结果。

从表中可以看出，ORB-SLAM可以处理所有的图像序列，除了fr3_nostructure_texture_far(fr3_nstr_tex_far)以外。这是一个平面的场景，相机的轨迹在这种情况下有两种可能，正如[27]中的描述的。我们的初始化方法检测到这种歧义，为了保证算法的安全运行选择不进行初始化。PTAM初始化有时会选择对的方案，有些可能会选择错的方案，且导致的错误可能不能接受。我们没有注意到LSD-SLAM的2种不同的重构方案，但在这个图像序列出现的错误非常多。针对其他的图像序列，PTAM和LSD-SLAM算法的鲁棒性都比我们的方法差，且分别有八组序列和三组序列中地图点容易跟踪丢失。

关于精度问题，没有回环检测期间，ORB-SLAM和PTAM算法的定位精度相当，但回环检测成功后，ORB-SLAM算法将达到更高的定位精度，正如在图像序列fr3_nostructure_texture_near_withloop (fr3_nstr_tex_near)中表现的。非常意外的结果是PTAM和ORB-SLAM的精度都非常明显高于LSD-SLAM和RGBD-SLAM。一个可能的原因是它们将地图的优化过程简化为一个单纯的位姿图优化过程，这样就造成了传感器测量信息的丢失，但在我们的算法中，采用BA优化，同时通过传感器测量优化相机位姿和地图点位置，这是解决SfM的经典标准算法。我们将在第IX-B节进一步讨论了这个结果。另一个有趣的结果是在图像序列fr2_desk_with_person 和 fr3_walking_xyz中，LSD-SLAM对动态物体的鲁棒性相比ORB-SLAM差一些。

我们注意到RGBD-SLAM在图像序列fr2上的尺度有一个偏差，用7自由度对齐轨迹则误差明显减少。最后我们注意到Engle等人在[10]中提出在f2_xyz上PTAM的精度比LSD-SLAM算法低，RMSE是24.28cm。然而，论文没有给出足够的细节说明如何获得这些结果的，因此我们没有办法复现它。

C. Relocalization in the TUM RGB-D Benchmark

我们在TUM RGB-D数据集上进行了两组重定位实验。在第一个实验中，我们通过fr2_xyz图像序列的前30秒构建了一个地图，然后对后来的每帧图像都进行全局重定位，并评估重构出来的相机位姿精度。我们对PTAM算法进行了相同的实验以进行对比。图7展示了是创建初始地图的关键帧，重定位的图像帧位姿和这些帧对应的真值。

在fr2 xyz中进行重定位实验。地图最初是在序列（KF）的前30秒内创建的。 目标是重定位后续帧。重定位（R），ground truth（GT）。

从图中可以看出PTAM算法只能够对重定位关键帧附近的图像帧，这是因为其算法中重定位方法并不具备不变性导致的。表4显示了PTAM算法和ORB_SLAM算法相对于真值的误差。

从表中数据可以看出，ORB-SLAM比PTAM精确两倍。在第2个实验中，我们通过fr3_sitting_xyz图像序列来初始化地图，然后用fr3_walking_xyz图像序列对所有帧进行重定位。这是一个很有挑战性的实验，由于场景中有人移动会造成图像局部区域的遮挡。在该试验中，PTAM并没有实现重定位，而ORB-SLAM重定位了78%的图像帧。

图8显示了ORB-SLAM重定位的一些实验图例：

具有挑战性的重新定位示例（严重的尺度变化，动态对象）是我们的系统在重定位实验中成功的

D. Lifelong Experiment in the TUM RGB-D Benchmark

之前的重定位实验表明我们的系统可以从非常不同的视角定位地图，在中等动态环境中的鲁棒性也较好。这个特性和关键帧筛选程序使得算法在不同的视角和局部动态环境中能够一直运行。

在全静态场景情况下，即使相机从不同视角观测场景，ORB-SLAM也可以使关键帧数量保持在一个有限的水平内。我们在一个自定义的图像序列中验证了这一点，手持相机在93秒以内都拍摄同一张桌子，但视角一直变换，形成一个轨迹。我们对比了我们地图的关键帧数量和PTAM生成的关键帧，如图9所示。

PTAM一直都在插入关键帧，而ORB-SLAM会删除冗余的关键帧，将其总数保持在一个稳定的范围内。

当然，在整个程序运行过程中，静态环境下的正常操作是任何SLAM系统的一个基本要求，更引人关注的是动态环境下的状况。我们在fr3: sitting_xyz, sitting_halfsphere, sitting_rpywalking_xyz, walking_halfspehere, walking_rpy这些动态序列上分析了系统的表现。所有的序列中，相机都对着桌子，但运动轨迹不同，拍摄场景中有人在移动，椅子也被移动了。

图10(a)展示了地图中总的关键帧数量的变化情况：
图10(b)展示每个关键帧的创建和丢弃，可以知道这么关键帧在地图中存在的时间
从创建帧到销毁每条水平线都对应一个关键帧

可以看到前2个图像序列中新看到（增加）场景时地图的大小一直在增加。图10(b)是前2个序列中创建的关键帧。在序列sitting_rpy和walking_xyz中，地图没有变大，地图是通过已有场景创建。相反，在最后两个视频中，更多的关键帧被插入，这表示在该场景对于系统来说是新的。

最后，图10©展示出了根据关键帧存活时间的直方图：
可以看到大部分关键帧在创建不久后就被删除了，只有一小部分存活到实验结束。一方面，这展示了我们的系统拥有宽松的关键帧插入策略，这在机器人剧烈运动的时候非常有用。另一方面，系统最终能够选择一小部分具有代表性的关键帧。

在这些长期运行实验中展示了我们的地图随着场景内容的增长而增长，而不是随着时间增长。它能够存储场景的动态变化，通过在环境中积累经验，有助于进行场景理解。

E. Large Scale and Large Loop Closing in the KITTI Dataset

KITTI数据集中里程计的数据包括11个序列，这些序列在住宅区驾驶汽车得来的，由于使用了GPS和Velodyne激光雷达，因此其ground truth的精度非常高。这个数据集对单目SLAM非常具有挑战性，因为存在快速旋转、很多树叶的区域，这使得数据关联更加困难。相较于高速运动的汽车，序列的帧率为10fps。我们以实际帧率播放序列，ORB-SLAM可以处理除01外的所有序列，因为01序列实在高速路上的序列，可追踪的物体很少。序列00,02,05,06,07,09包含回环，可以被我们的系统检测并修正过来。序列09包含一个回环，仅仅在最后几帧才能被检测到，因为我们的系统并不是每次都能检测到（结果显示的是其被检测到的情况）。

我们的轨迹与ground truth的定性比较如图11和图12：

在TUM RGB-D数据集中，我们可以通过相似变换对齐轨迹的关键帧和ground truth。图11是定性比较的结果，图12是[25]中的最新单目SLAM在视频00,05,06,07和08上执行的结果。除了08有一些偏移以外，ORB-SLAM在这些视频上的轨迹都很精准。

表5显示了每个序列的关键帧轨迹的中间RMSE误差：

我们基于地图尺寸提供了轨迹的误差。结果表明我们的轨迹误差是地图尺寸的1%左右。大致范围低的是视频03的0.3%高的是视频08的5%。视频08中没有闭环，漂移也没办法纠正，因为闭环控制需要获得更精确的重构。

在本次实验中，我们还确认了到底全局BA的20层迭代最终能提高地图重构，相关细节如附录所示。我们还注意到全局BA优化可以稍微增加闭环轨迹的精度，但这对开环轨迹有负面影响，这意味着我们的系统已经非常精确了。在有些应用中，如果需要非常精确的结果我们的算法会提供一组匹配，需要定义一个比较强的相机网络，一个初始估计，这样全局BA优化迭代次数就会变少。

最后我们想展示一下我们的回环检测方法的效率，以及在本质图中用于包含边的最小权重θmin的影响。我们选择视频09（一段非常长的图像序列，在最后有一个闭环），然后评估不同的闭环检测算法。表6是关键帧轨迹RMSE和不同情况下没有闭环检测优化所用的时间，表中的相关内容包括：如果直接采用全局BA优化（20层或100层迭代）的情况，如果只用位姿图优化（10层迭代不同数量的边缘）的情况，如果先用位姿图优化再执行全局BA优化的情况。

第一行显示不回环的结果。 BA括号之间的数字表示LM迭代的次数，而EG（本质图）的θmin是构建基本图的θmin。 所有的EG优化都执行10次LM迭代。

结果表明，在闭环检测之前，算法的RMSE误差较大，以至于BA优化没办法收敛，即便是迭代100次之后后误差仍旧非常大。另一方面，本质图优化收敛速度很快，而且结果也更精确。θmin对精度影响并不大，减少边的数量会明显降低运行速度。位姿图优化后再执行一个BA优化则可以稍微增加精度，但时间也增加了。

IX. CONCLUSIONS AND DISCUSSION

A. Conclusions

本文中，我们提出了一个新的单目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]提出的的闭环检测程序和共视图，Kuemmerle等人[37]中提出的g2o优化框架以及Rubble等人提出的ORB特征[9]。到目前为止就我们所知，本文提出的ORB_SLAM方法的定位精度最高，也是最可靠最完整的单目SLAM系统。我们提出的新的生成和删除关键帧策略，允许每个几帧就创建一个关键帧，然后当关键帧冗余时则删除。这样的构图方式很灵活，在外界条件很差的情况下可以保证系统正常运行，比如相机作纯旋转运动或快速移动时。当算法在相同场景下运行时，地图在只有拍摄到新内容的情况下才会增长，可以从我们的长期构图结果中看到这个特性。

最后，我们还展示了ORB特征具有很好的识别能力，可识别剧烈视角变换情况下的场景信息。此外，它们能够被非常快速的提取和匹配（不需要多线程或GPU加速），这就使得跟踪和地图构建更加实时精确。

B. Sparse/Feature-based vs. Dense/Direct Methods

最近，实时单目SLAM算法，比如DTAM[44]和LSD-SLAM[10]，能够执行稠密和半稠密的重建，通过直接优化像素亮度来定位。这类方法即为直接法，直接方法不需要特征提取和匹配。他们对图像模糊，弱纹理环境和像[45]这样的高频纹理环境的鲁棒性更好。与由稀疏点构建的地图相比，比如ORB-SLAM或PTAM算法，稠密/直接法SLAM对相机定位之外的其他应用任务可能更有用途。

然而，直接方法有他们自己的局限。首先，这些方法假设真实场景中的物体的像是由该物体本身的表面反射模型产生的，因此，算法采用的光度一致性寻找匹配点的思路就限制了匹配点之间的基线距离，通常都比特征匹配点的基线要窄。这对重构的精度影响很大，因为重构需要较宽的基线来减少深度的不确定性。如果直接建模不准确，则可能会受到快门，自动增益和自动曝光的影响（如TUM RGB-D 的对比测试）。最后，由于直接方法计算要求较高，因此为了满足计算速度，DTAM算法采用地图增量式扩张的方法，而LSD-SLAM则丢掉传感器测量信息，将地图优化降低为对位姿图的优化。

相反，基于特征的方法可以在更宽的基线上匹配特征，主要得益于特征匹配算法较好地视角不变特性。BA优化联合优化相机位姿和传感器测量的点。在运动和结构估计中，Torr和Zisserman[46]已经指出了基于特征的方法相比直接方法的优势。在我们的实验第VIII-B节中也直接提供了证据，表明基于特征的定位精度更高。未来单目SLAM应该会整合两种最好的方法。

C. Future Work

我们系统的精度可以通过结合无限远点跟踪来进一步增强。这些在视图中看不到的平行线交点，并没有包含在本文算法构建的地图中，但对相机的旋转非常有用[21]。

另外一种方法是将稀疏地图更新到一个更加稠密的地图。由于我们关键帧的选择机制，关键帧组成了一个紧凑的地图，地图具有非常高精度的位姿信息和丰富的共视信息。所以，ORB-SLAM稀疏地图是一个非常优秀的初始估计框架，在此基础上可以构建一个稠密、精确的地图。这个方向的首次尝试如[47]。

APPENDIX

NON-LINEAR OPTIMIZATIONS

BA：
地图点3D位置Xw,j∈R3，关键帧位姿Tiw∈SE（3），其中w表示世界参考坐标系，最小化相对于匹配的关键点xi，j∈R2的重投影误差。 在关键帧i中观察地图点j的误差项为：

其中πi是投影函数：

其中Riw∈SO（3）和tiw∈R3分别是Tiw的旋转和平移部分，而（fi，u，fi，v）和（ci，u，ci，v）是与之相关的焦距和主点要最小化的成本函数是：

其中ρh是Huber代价函数，Ωi，j =σ2i，j I2×2是与检测到关键点的尺度相关的协方差矩阵。

对于完整的BA（用于第IV节中说明的地图初始化以及第VIII-E节中的实验），我们将优化所有点和关键帧，但第一个关键帧除外，该第一个关键帧保持固定为原点。

在局部BA中（请参见VI-D节），局部区域中包含的所有点均得到优化，而关键帧的子集是固定的。

在位姿优化或仅运动的BA中（请参阅第V节），所有点都是固定的，只有相机姿势会得到优化。

Pose Graph Optimization over Sim(3) Constraints

给定二进制边的位姿图（请参阅第VII-D节），我们将边的误差定义为：

其中Sij是从SE（3）位姿计算出的两个关键帧之间的相对Sim（3）转换，在位姿图优化并将尺度因子设置为1之前。在闭环边缘的情况下，该相对变换是使用 Horn方法[42]。 logSim3 [48]转换为切线空间，因此误差是R7中的向量。 目标是优化Sim（3）关键帧姿势以最小化成本函数：

其中，Λi，j是边缘的信息矩阵，如[48]所示，我们将其设置为单位阵。 尽管此方法是完整BA的粗略近似，但我们在VIII-E节中通过实验证明了它比BA具有更快，更好的收敛性。

Relative Sim(3) Optimization:

给定关键帧1和关键帧2之间的n个匹配项i⇒j（关键点及其关联的3D映射点），我们要优化相对Sim（3）（请参见VII-B部分），以最大程度地减小投影中的重投影误差。 两个图像：

最小化代价函数：

其中Ω1,i和Ω2,i是与尺度相关的协方差矩阵。 在此优化中，点是固定的