# ORB-SLAM3：一个用于视觉、视觉惯性及多地图SLAM的精准开源库 Carlos Campos $^{ID}$、Richard Elvira $^{ID}$、Juan J. Gómez Rodríguez $^{ID}$（IEEE研究生会员）、José M. M. Montiel $^{ID}$（IEEE会员）以及Juan D. Tardós $^{ID}$（IEEE高级会员） 摘要—本文介绍了ORB-SLAM3，这是首个能够利用单目、双目及RGB-D相机，结合针孔和鱼眼镜头模型来执行视觉、视觉惯性及多地图SLAM的系统。其首要创新点在于一个紧密集成的视觉惯性SLAM系统，该系统完全基于最大后验估计（MAP）技术，甚至在惯性测量单元初始化阶段也是如此，从而能够在室内外小型及大型环境中实现实时且稳健的运行，其精度比以往的方法高出2到10倍。第二个主要创新点是一种多地图系统，该系统采用了一种召回率更高的新型地点识别方法，使得ORB-SLAM3能够应对视觉信息匮乏的长时间场景：一旦系统迷失方向，便会创建新地图，当重新访问旧区域时，新地图会与之前的地图无缝合并。与仅利用最近几秒信息的视觉里程计系统不同，ORB-SLAM3是首个能在所有算法阶段复用所有高视差共视关键帧中的历史信息的系统，即便这些关键帧在时间上相隔甚远或来自之前的建图会话，这一特性也显著提升了定位精度。我们的实验表明，在所有传感器配置下，ORB-SLAM3的稳健性与现有文献中最好的系统相当，且精度明显更高。值得注意的是，我们的双目惯性SLAM在EuRoC无人机数据集上的平均精度达到3.5厘米，在代表AR/VR场景的TUM-VI数据集的快速手持运动测试中，精度更是达到9毫米。为了回馈社区，我们公开了该系统的源代码。 索引词—计算机视觉、惯性导航、同时定位与地图构建。 ## 一、引言 过去二十年间，关于视觉同时定位与地图构建（SLAM）系统及视觉里程计（VO）的研究十分深入，无论是单独使用摄像头还是结合惯性传感器，都取得了精度与鲁棒性不断提升的优秀成果。现代系统依赖于最大后验估计（MAP）技术，对于视觉传感器而言，该技术对应着束调整（BA）：在基于特征的方法中，就是最小化特征重投影误差的几何束调整；而在直接法中，则是最小化选定像素集的光度误差的光度束调整。 随着近期融合了回环检测技术的视觉里程计系统的出现，视觉里程计与SLAM之间的界限愈发模糊。视觉SLAM的目标是利用移动载体上的传感器构建环境地图，并实时计算该载体在地图中的位姿。相比之下，视觉里程计系统则侧重于计算载体的自身运动，而非构建地图。SLAM地图的巨大优势在于它能够匹配并在束调整中使用以往的观测数据，从而实现三种类型的数据关联（扩展了[^1]中使用的术语）。 1) 短期数据关联：匹配最近几秒内获取的地图元素。这是大多数视觉里程计系统唯一采用的数据关联类型，一旦环境元素超出视野范围，系统就会将其遗忘，即便在同一区域移动，也会导致持续的估计漂移。 2) 中期数据关联：匹配那些距离摄像头较近且累积漂移量仍较小的地图元素。这些元素可以与短期观测数据一样被用于光束法平差（BA）中，从而当系统在已测绘区域内移动时实现零漂移。与具备回环检测功能的视觉里程计系统相比，这正是我们的系统能获得更高精度的关键。 3) 长期数据关联：利用位置识别技术将观测数据与先前访问过的区域中的元素进行匹配，无论累积漂移量如何（即回环检测）、当前区域是否属于已断开连接的地图中的一部分（即地图合并），还是跟踪丢失（即重定位）。长期匹配能够重置漂移量，并通过位姿图优化或更准确地通过光束法平差来修正地图。这是中等及大型有回环环境中SLAM精度的关键。 在本研究中，我们基于ORB-SLAM [^2]、[^3]以及ORB-SLAM视觉-惯性系统 [^4] 展开研究——这些是首批能够充分利用短期、中期和长期数据关联、从而在已测绘区域内实现零漂移的视觉及视觉-惯性系统。在此基础上，我们进一步实现了多地图数据关联技术，该技术允许我们将来自先前测绘会话的地图元素匹配并用于光束法平差，最终实现SLAM系统的真正目标：构建可供后续用于提供精准定位服务的地图。 ---
TABLEI
SUMMARY OF THE MOST REPRESENTATIVE VISUAL (TOP) AND VISUAL-INERTIAL (BOTTOM) SYSTEMS, IN CHRONOLOGICAL ORDER
SLAM or VOPixels usedData associationEstimationRelocalizationLoop closingMulti MapsMonoStereoMono IMUStereo IMUFisheyeAccuracyRobustnessOpen source
Mono-SLAM [^13], [^14]SLAMShi TomasiCorrelationEKF---✓----FairFair[^15] $^{1}$
PTAM [^16] [^17] [^18]SLAMFASTPyramid SSDBAThumbnail--✓----Very GoodFair[^19]
LSD-SLAM [^20], [^21]SLAMEdgeletsDirectPG-FABMAP PG-✓✓---GoodFair[^22]
SVO [^23], [^24]VOFAST+ Hi.grad.DirectLocal BA---✓✓--✓Very GoodVery Good[^25] $^{2}$
ORB-SLAM2 [^2], [^3]SLAMORBDescriptorLocal BADBoW2DBoW2 PG+BA-✓✓---Exc.Very Good[^26]
DSO [^27] [^28] [^29]VOHigh grad.DirectLocal BA---✓✓--✓FairVery Good[^30]
DSM [^31]SLAMHigh grad.DirectLocal BA---✓----Very GoodVery Good[^32]
MSCKF [^33] [^34] [^35] [^36]VOShi TomasiCross correlationEKF---✓-✓✓-FairVery Good[^37] $^{3}$
OKVIS [^38], [^39]VOBRISKDescriptorLocal BA-----✓✓✓GoodVery Good[^40]
ROVIO [^41], [^42]VOShi TomasiDirectEKF-----✓✓✓GoodVery Good[^43]
ORBSLAM-VI [^4]SLAMORBDescriptorLocal BADBoW2DBoW2 PG+BA-✓-✓--Very GoodVery Good-
VINS-Fusion [^7], [^44]VOShi TomasiKLTLocal BADBoW2DBoW2 PG✓-✓✓✓✓GoodExc.[^45]
VI-DSO [^46]VOHigh grad.DirectLocal BA-----✓--Very GoodExc.-
BASALT [^47]VOFASTKLT (LSSD)Local BA-ORB BA----✓✓Very GoodExc.[^48]
Kimera [^8]VOShi TomasiKLTLocal BA-DBoW2 PG----✓-GoodExc.[^49]
ORB-SLAM3 (ours)SLAMORBDescriptorLocal BADBoW2DBoW2 PG+BA✓✓✓✓✓✓Exc.Exc.[^5]
$^{1}$最新源代码由其他作者提供。原始软件可在[^50]获取。 $^{2}$源代码仅适用于首个版本，SVO 2.0并非开源软件。 $^{3}$MSCKF已申请专利 [^51]，目前仅有其他作者重新实现的版本作为开源代码提供。 本质上这是一篇系统论文，其最重要的贡献便是ORB-SLAM3库本身 [^5]——它是迄今为止最完整、精度最高的视觉、视觉-惯性和多地图SLAM系统（详见表I）。ORB-SLAM3的主要创新点如下。 1) 一种单目及双目视觉-惯性SLAM系统，该系统甚至惯性测量单元初始化阶段就完全依赖地图估计。所提出的初始化方法此前已在[^6]中介绍过。在此，我们将其与ORB-SLAM视觉-惯性系统 [^4] 进行整合，扩展为双目惯性SLAM，并在公开数据集上进行了全面评估。结果显示，单目及双目视觉-惯性系统极其稳健，且比其他视觉-惯性方法精度显著更高，即便在没有回环的序列中也是如此。 2) 召回率更高的位置识别技术。许多最新的视觉SLAM和视觉里程计系统 [^2]、[^7]、[^8] 均采用DBoW2词袋库 [^9] 来解决位置识别问题。DBoW2要求时间一致性，即需先匹配三个连续的关键帧到同一区域，之后才会检查几何一致性，这种方式虽然提升了精度，却牺牲了召回率。结果就是系统在闭合回环和复用先前测绘区域时速度过慢。我们提出了一种新颖的位置识别算法：首先检查候选关键帧的几何一致性，再验证其与三个共视关键帧的局部一致性，而这些关键帧在大多数情况下早已存在于地图中。这种策略提高了召回率并强化了数据关联，进而提升了地图精度，只是计算成本会略有增加。 3) ORB-SLAM Atlas。这是首个能够同时处理单目和双目配置下的视觉及视觉惯性系统的完整多地图SLAM系统。Atlas可以表示一组相互独立的地图，并能对其平滑地执行所有建图操作：位置识别、相机重定位、闭环检测以及精确的无缝地图合并。这使得系统能够自动利用并整合在不同时间构建的地图，从而实现增量式多会话SLAM。用于视觉传感器的ORB-SLAM Atlas初步版本已在[^10]中介绍。在此我们新增了全新的位置识别系统、视觉惯性多地图系统，并展示了其在公开数据集上的评估结果。 --- 4) 一种抽象的相机表示方法，使SLAM代码与所使用的相机模型无关，只需提供投影、反投影及雅可比函数即可添加新模型。我们提供了针孔[^11]和鱼眼[^12]模型的实现。 所有这些创新点，再加上一些代码优化，使得ORB-SLAM3成为新的视觉及视觉惯性开源SLAM库标杆。正如第VII节中的实验结果所示，它既具备文献中最佳系统的鲁棒性，精度也显著更高。我们还提供了单目、双目、单目惯性及双目惯性SLAM结果的对比，供从业者参考。 ### II. 相关工作 表I总结了最具代表性的视觉及视觉惯性系统，列出了用于估计和数据关联的主要技术。表中包含的定性精度和鲁棒性评级基于第VII节中的结果，以及[^2]中报道的并行跟踪与建图（PTAM）、大规模直接单目SLAM（LSD-SLAM）和ORB-SLAM之间的对比。 ### A. 视觉SLAM 单目SLAM最早由MonoSLAM [^13]、[^14]、[^52]解决，该系统采用扩展卡尔曼滤波器（EKF）以及Shi-Tomasi特征点，通过相关性引导搜索在后续图像中跟踪这些点。采用能保证所用特征匹配一致性的技术后，中期数据关联能力得到显著提升，从而实现了手持式视觉SLAM [^53]、[^54]。 相比之下，基于关键帧的方法仅利用少数选定的帧来估计地图，丢弃中间帧的信息。这样一来，就能以关键帧速率执行计算成本更高但精度更高的BA优化。最具代表性的系统是PTAM [^16]，它将相机跟踪和建图拆分为两个并行线程。在相同的计算成本下，基于关键帧的技术比滤波方法更精确 [^55]，因此成为视觉SLAM和视觉里程计领域的黄金标准。大规模单目SLAM在[^56]中通过滑动窗口BA实现，在[^57]中则通过双窗口优化和共视图实现。 基于这些思路，ORB-SLAM [^2]、[^3] 采用了定向快速特征点及旋转版BRIEF（ORB）特征，其描述符能够实现短期和中期数据关联；该系统还构建了共视图以降低跟踪与建图的复杂度，并利用词袋库DBoW2 [^9] 来执行回环检测与重定位，从而实现长期数据关联。迄今为止，它是唯一集成了这三种数据关联方式的视觉SLAM系统，我们认为这正是其具备卓越精度的关键。在本研究中，我们通过全新的Atlas系统提升了它在纯视觉SLAM场景下的鲁棒性——该系统会在跟踪丢失时启动新地图；同时借助召回率更高的新型地点识别方法，提高了它在回环场景中的精度。 直接法并不提取特征，而是直接使用图像中的像素强度，通过最小化光度误差来估计运动与结构。LSD-SLAM [^20] 能够利用高梯度像素构建大规模半稠密地图，但地图估计过程被简化为PG优化，因此其精度低于PTAM和ORB-SLAM [^2]。混合式系统半直接视觉里程计（SVO）[^23]、[^24] 会提取FAST特征，采用直接法来跟踪特征以及任意具有非零强度梯度的像素，并通过重投影误差来优化相机轨迹与三维结构。SVO的效率极高，但由于它属于纯粹的视觉里程计方法，仅能实现短期数据关联，这限制了其精度。直接稀疏里程计（DSO）[^27] 能够在特征点检测器表现不佳的场景下计算出精确的相机位姿，从而增强了在低纹理区域或模糊图像环境下的鲁棒性。它引入了局部光度BA技术，可同时优化最近七个关键帧以及点的逆深度。该工作的扩展研究包括立体视觉版本 [^29]、基于特征与DBoW2的回环检测功能 [^58]、[^59]，以及视觉惯性里程计 [^46]。直接稀疏建图（DSM）[^31] 提出了直接法中地图复用的理念，凸显了中期数据关联的重要性。在所有案例中，由于缺乏短期、中期和长期数据关联的整合，其精度均低于我们的方案（详见第VII节）。 ### B. 视觉惯性SLAM 视觉传感器与惯性传感器的结合能够提升系统在纹理匮乏、运动模糊及遮挡环境下的鲁棒性；对于单目系统而言，还能使尺度信息可被观测。 紧耦合方法的研究可追溯至多状态约束卡尔曼滤波器（MSCKF）[^33]，该技术通过特征边缘化避免了EKF中因特征数量增多而产生的二次成本问题。初始系统在 [^34] 中得到完善，并在 [^35] 和 [^36] 中被拓展至立体视觉领域。首个基于关键帧与BA的紧耦合视觉里程计系统是OKVIS [^38]、[^39]，它同样支持单目与立体视觉。虽然这些系统依赖特征，但ROVIO [^41]、[^42] 则利用直接数据关联将光度误差输入EFK中。 ORB-SLAM-VI [^4]首次提出了一种视觉-惯性SLAM系统，该系统能够利用短期、中期和长期的数据关联来复用地图，并将其应用于基于IMU预积分的精确局部视觉-惯性BA中 [^60], [^61]。不过，其IMU初始化技术速度过慢，耗时达15秒，这影响了系统的鲁棒性和精度。在 [^62] 和 [^63] 中提出了更快的初始化技术，这些技术基于闭式解来共同获取尺度、重力、加速度计偏差以及初始速度，还有视觉特征的深度信息。关键在于，它们忽略了IMU噪声特性，且最小化的是空间中点的三维误差，而非基于特征的计算机视觉中的黄金标准——重投影误差。我们之前的研究 [^64] 表明，这会导致较大的不可预测误差。 VINS-Mono [^7]是一种非常精确且鲁棒的单目-惯性里程计系统，具备回环检测功能，采用了DBoW2以及4自由度PG优化和地图合并技术。 --- 特征跟踪采用Lucas-Kanade跟踪器，其鲁棒性略优于描述符匹配方法。在VINS-Fusion [^44]中，该方法被扩展应用于双目及双目-惯性场景。 VI-DSO [^46]将DSO扩展为视觉-惯性里程计，提出了一种结合惯性观测与选定高梯度像素的光度误差的BA方法，从而获得了极高的精度。由于成功利用了高梯度像素的信息，在纹理较少的场景区域中系统的鲁棒性也得到了提升。其初始化方法依赖于视觉-惯性BA，收敛到1%的尺度误差需要20–30秒。 最近的BASALT [^47]是一种双目-惯性里程计系统，它从视觉-惯性里程计中提取非线性因子用于BA，并通过匹配ORB特征来实现回环检测，能够达到非常好乃至卓越的精度。Kimera [^8]是一种新颖出色的度量-语义建图系统，但其度量部分由双目-惯性里程计以及基于DBoW2和PG优化的回环检测组成，其精度与VINS-Fusion相近。 在这项工作中，我们以ORB-SLAM-VI为基础，将其扩展为双目-惯性SLAM系统。我们提出了一种基于MAP估计的新型快速初始化方法，该方法能恰当考虑视觉和惯性传感器的不确定性，在2秒内就能以5%的误差估算出真实尺度，15秒内即可收敛到1%的尺度误差。上述讨论的其他系统均为视觉-惯性里程计方法，其中一些还扩展了回环检测功能，但都不具备利用中期数据关联的能力。我们认为，这一点加上我们快速且精确的初始化技术，正是我们的系统即便在没有回环的序列中也能持续获得更高精度的关键。 ### C. 多地图SLAM 通过地图创建与融合来增强探索过程中对跟踪丢失情况的鲁棒性这一想法，最早是在[^65]中基于滤波方法提出的。最早基于关键帧的多地图系统之一是[^66]，但该系统的地图初始化需要人工操作，且无法合并或关联不同的子地图。多地图功能已被作为协同建图系统的一个组成部分加以研究，这类系统包含多个建图代理以及一个仅接收信息[^67]的中央服务器，或者像C2TAM [^68]那样采用双向信息流模式。MOARSLAM [^69]提出了一种用于多设备协同SLAM的健壮无状态客户端-服务器架构，但其主要侧重于软件架构，并未报告精度相关结果。 最近，CCM-SLAM [^70]、[^71]提出了一种基于ORB-SLAM构建的、面向多无人机的具有双向信息流的分布式多地图系统。他们的研究重点是克服带宽有限和分布式处理带来的挑战，而我们的重点则是精度和鲁棒性，在EuRoC数据集上取得了显著更好的结果。SLAM [^72]也提出了ORB-SLAM2的多地图扩展方案，但将子地图作为独立实体处理，而我们则能实现无缝地图合并，从而构建更精确的全局地图。 VINS-Mono [^7]是一种具备回环检测和多地图功能的视觉里程计系统，该系统依赖于地点识别库 ![Image](images/287067ef586382116540bc9af0b9869c654d001465e59396995876edb190320f.jpg)
Fig. 1. Main system components of ORB-SLAM3.
DBoW2 [^9]。我们的实验表明，在EuRoC数据集上，单目惯性单会话模式下ORB-SLAM3的精度是VINS-Mono的2.6倍，这得益于其利用中期数据关联的能力。我们的Atlas系统同样基于DBoW2构建，但提出了一种新颖的高召回率地点识别技术，并利用局部BA进行更细致、更精确的地图合并，这使得在EuRoC数据集的多会话模式下，其精度优势进一步扩大到比VINS-Mono高3.2倍。 ### 三、系统概述 ORB-SLAM3建立在ORB-SLAM2 [^3]和ORB-SLAM-VI [^4]的基础之上。它是一个完整的多地图和多会话系统，能够在纯视觉或视觉惯性模式下运行，支持单目、双目或RGB-D传感器，可适配针孔相机模型和鱼眼相机模型。图1展示了该系统的主要组件，这些组件与ORB-SLAM2的组件类似，但也存在一些重要的创新点，下文将对这些创新点进行总结。 1) Atlas是一种由一组互不相连的地图组成的多地图表示形式。其中存在一个活动地图，跟踪线程会在该地图中对传入的帧进行定位，本地建图线程会持续对该地图进行优化并添加新关键帧。我们将Atlas中的其他地图称为非活动地图。该系统会构建一个用于重定位、回环检测和地图合并的唯一DBoW2关键帧数据库。 2) 跟踪线程实时处理传感器信息，计算当前帧相对于活动地图的姿态，同时最小化匹配地图特征的重投影误差。该线程还会判断当前帧是否应成为关键帧。在视觉惯性模式下，通过将惯性残差纳入优化过程来估计机体速度和IMU偏差。一旦跟踪丢失，跟踪线程会尝试在Atlas的所有地图中重新定位当前帧。如果成功重新定位，跟踪便会恢复，必要时还会切换活动地图。否则，经过一定时间后，当前的活动地图会被标记为非活动状态，然后从头开始初始化新的活动地图。 --- 跟踪便会恢复，必要时还会切换活动地图。否则，经过一定时间后，当前的活动地图会被标记为非活动状态，然后从头开始初始化新的活动地图。 3) 本地建图线程会将关键帧和点添加到活动地图中，移除冗余部分，并利用视觉或视觉-惯性后端优化算法来细化地图，这一过程在靠近当前帧的局部关键帧窗口内进行。此外，在惯性情况下，映射线程会利用我们创新的MAP估计技术来初始化并优化IMU参数。 4) 回环与地图合并线程会以关键帧速率检测活动地图与整个地图集之间的共同区域。如果共同区域属于活动地图，则执行回环校正；如果它属于其他地图，则将两张地图无缝合并为一张，并使其成为活动地图。回环校正完成后，会在独立线程中启动完整的后端优化算法来进一步细化地图，而不会影响实时性能。 ### 四、相机模型 ORB-SLAM在所有系统组件中均采用针孔相机模型。我们的目标是将相机模型从整个SLAM流程中抽象出来，把与相机模型相关的所有属性和功能（投影与反投影函数、雅可比矩阵等）提取到独立模块中。这样一来，只要提供相应的相机模块，我们的系统就能使用任意相机模型。在ORB-SLAM3库中，除了针孔模型外，我们还提供了Kannala-Brandt [^12] 鱼眼模型。 由于大多数主流计算机视觉算法都基于针孔相机模型，许多SLAM系统会对整幅图像或特征坐标进行矫正，以使其适配理想的平面视网膜。然而，对于视场角（FOV）达到或超过$180^{\circ}$的鱼眼镜头而言，这种方法存在明显问题：图像矫正会导致边缘物体被放大，而中心物体的分辨率则会降低，进而影响特征匹配。若对特征坐标进行矫正，则只能使用小于$180^{\circ}$的视场角，这会给许多假设图像上重投影误差均匀的计算机视觉算法带来困扰——而这在矫正后的鱼眼图像中显然不成立。这种方式还会迫使人们裁剪掉图像的外部区域，从而丧失大视场角带来的优势：更快的环境建图速度以及更强的遮挡鲁棒性。接下来，我们将探讨如何克服这些难题。 ### A. 重定位 一个稳健的SLAM系统必须具备在跟踪失败时重新定位相机的能力。ORB-SLAM通过基于ePnP算法构建的透视n点求解器来解决重定位问题[^73]，该算法的所有推导过程均基于已标定的针孔相机模型。为了延续我们的思路，我们需要一种不依赖所用相机模型的PnP算法。因此，我们采用了最大似然透视n点算法[^74]，该算法完全独立于相机模型，它以投影射线作为输入。相机模型只需提供能将像素转换为投影射线的反投影函数，即可支持重定位功能。 ### B. 非矫正立体SLAM 大多数立体SLAM系统都假设立体帧已经过矫正，即两幅图像都通过相同的焦距转换为针孔投影，图像平面共面且极线水平对齐，这样就能通过查看另一图像中的同一行来轻松匹配某一图像中的特征。然而，这种对矫正后立体图像的假设限制极强，在许多应用场景中既不适用也不可行。例如，对发散式立体像对或立体鱼眼镜头进行矫正需要大幅裁剪图像，从而丧失大视场角的优势。 出于这个原因，我们的系统并不依赖图像校正，而是将立体相机组视为两台单目相机，它们具有以下特性： 1) 两者之间存在恒定的相对SE(3)变换关系； 2) 可选地，存在一个共同的图像区域，能够观测到场景的同一部分。 这些约束条件使我们能够在三角测量新路标以及进行BA优化时引入相关信息，从而有效估计地图的比例尺。基于这一思路，我们的SLAM流程会估算出6自由度刚体位姿，该位姿的参考系可以位于其中一台相机或IMU传感器上，同时用该刚体位姿来表示相机的位置。 如果两台相机存在有立体观测数据的重叠区域，我们就能在首次看到这些路标时便三角测量出它们的真实比例尺。两张图像中的其余部分仍包含大量有用信息，这些信息会在SLAM流程中作为单目信息使用。在这些区域首次出现的特征点会像单目情况一样，通过多视角进行三角测量。 ### V. 视觉-惯性SLAM ORB-SLAM-VI [^4]是首个能够实现地图复用的真正的视觉-惯性SLAM系统。不过它仅支持针孔单目相机，且初始化过程过于缓慢，在某些复杂场景下会失效。在本研究中，我们在ORB-SLAM-VI的基础上，提出了一种快速且精确的IMU初始化技术，同时开发了一个开源SLAM库，该库支持单目-惯性和立体-惯性SLAM，兼容针孔相机和鱼眼相机。 ### A. 基本原理 在纯视觉SLAM中，所估算的状态仅包含当前相机的位姿；而在视觉-惯性SLAM中，则需要计算额外的变量。这些变量包括世界坐标系下的机体位姿$\mathbf{T}_i = [\mathbf{R}_i, \mathbf{p}_i] \in \mathrm{SE}(3)$和速度$\mathbf{v}_i$，以及陀螺仪和加速度计偏差$\mathbf{b}_i^g$和$\mathbf{b}_i^a$，这里假设这些偏差遵循布朗运动规律变化。由此便得到了状态向量。 $$ \mathcal{S}_{i}\doteq\{\mathbf{T}_{i},\mathbf{v}_{i},\mathbf{b}_{i}^{g},\mathbf{b}_{i}^{a}\}. $$ --- ![Image](images/66f2e4c49c4a92273b76a5c2349a4d3865e0089203688e8081a8a5fea23104f7.jpg)
(c)
Fig. 2. Factor graph representation for different optimizations along the system.
对于视觉-惯性SLAM，我们按照$i + 1$中提出的理论，并在[^60]中以流形形式表述的方法，在连续的视觉帧i和[^61]之间对IMU测量数据进行预积分。我们得到预积分后的旋转、速度和位置测量结果，分别记为$\Delta \mathbf{R}_{i,i+1}$、$\Delta \mathbf{v}_{i,i+1}$和$\Delta \mathbf{p}_{i,i+1}$，以及整个测量向量的协方差矩阵$\Sigma_{\mathcal{I}_{i,i+1}}$。有了这些预积分项和状态$S_i$、$S_{i+1}$后，我们采用$\mathbf{r}_{\mathcal{I}_{i,i+1}}$中定义的惯性残差[^61]。 $$ \begin{aligned}\mathbf{r}_{\mathcal{I}_{i,i+1}}&=[\mathbf{r}_{\Delta\mathrm{R}_{i,i+1}},\mathbf{r}_{\Delta\mathrm{v}_{i,i+1}},\mathbf{r}_{\Delta\mathrm{p}_{i,i+1}}]\\\mathbf{r}_{\Delta\mathrm{R}_{i,i+1}}&=\operatorname{Log}\left(\Delta\mathbf{R}_{i,i+1}^{\mathrm{T}}\mathbf{R}_{i}^{\mathrm{T}}\mathbf{R}_{i+1}\right)\\\mathbf{r}_{\Delta\mathrm{v}_{i,i+1}}&=\mathbf{R}_{i}^{\mathrm{T}}\left(\mathbf{v}_{i+1}-\mathbf{v}_{i}-\mathbf{g}\Delta t_{i,i+1}\right)-\Delta\mathbf{v}_{i,i+1}\tag{2}\\ \mathbf{r}_{\Delta\mathrm{p}_{i,i+1}}&=\mathbf{R}_{i}^{\mathrm{T}}\left(\mathbf{p}_{j}-\mathbf{p}_{i}-\mathbf{v}_{i}\Delta t_{i,i+1}-\frac{1}{2}\mathbf{g}\Delta t^{2}\right)-\Delta\mathbf{p}_{i,i+1}\end{aligned} $$ 其中$\text{Log} : \text{SO}(3) \to \mathbb{R}^3$负责将李群映射到向量空间。除了惯性残差外，我们还使用帧i与三维点j之间的重投影误差$\mathbf{r}_{ij}$，该点位于$\mathbf{x}_j$位置。 $$ \mathbf{r}_{ij}=\mathbf{u}_{ij}-\mathrm{II}\left(\mathbf{T}_{\mathrm{CB}}\mathbf{T}_{i}^{-1}\oplus\mathbf{x}_{j}\right) $$ 其中$\Pi : \mathbb{R}^3 \to \mathbb{R}^n$是对应相机模型的投影函数，$\mathbf{u}_{ij}$是点j在图像i中的观测值，其协方差矩阵为$\Sigma_{ij}$；$\mathbf{T}_{\mathrm{CB}} \in \mathrm{SE}(3)$代表从机体-IMU到相机（左或右）的刚性变换，该参数可通过标定获得；$\oplus$则是$\mathrm{SE}(3)$群对$\mathbb{R}^3$元素的变换操作。 将惯性残差项和视觉残差项结合起来，视觉-惯性SLAM可以被表述为一个基于关键帧的最小化问题 [^39]。给定一组 $k + 1$ 个关键帧及其状态 $\bar{S}_k \doteq \{\mathcal{S}_0 \ldots \mathcal{S}_k\}$，以及一组三维点及其状态 $\mathcal{X} \doteq \{\mathbf{x}_0 \ldots \mathbf{x}_{l-1}\}$，视觉-惯性优化问题可以表述如下： $$ \min_{\bar{\mathcal{S}}_{k},\mathcal{X}}\left(\sum_{i=1}^{k}\left\|\mathbf{r}_{\mathcal{I}_{i-1,i}}\right\|_{\Sigma_{\mathcal{I}_{i,i+1}}^{-1}}^{2}+\sum_{j=0}^{l-1}\sum_{i\in\mathcal{K}^{j}}\rho_{\mathrm{Hub}}\left(\left\|\mathbf{r}_{ij}\right\|_{\Sigma_{ij}^{-1}}\right)\right) $$ 其中 $K^{j}$ 表示观测到三维点j的关键帧集合。这种优化过程可以用图2(a)所示的因子图来描述。需要注意的是，对于重投影误差，我们采用了鲁棒的Huber核函数 $\rho_{Hub}$ 来降低错误匹配的影响；而对于惯性残差而言则无需如此处理，因为不存在误关联的情况。为了在跟踪和建图过程中提高效率，这种优化算法需要做出相应调整，但更重要的是，它需要良好的初始参数才能收敛到精确的解。 ### B. IMU初始化 这一步骤的目标是获得惯性参数的良好初始值：机体速度、重力方向以及IMU偏差。像VI-DSO这样的系统 [^46] 试图从头开始求解视觉-惯性BA问题，从而跳过专门的初始化过程，但这会导致惯性参数的收敛速度很慢（甚至需要30秒之久）。 在本研究中，我们提出了一种快速且精确的初始化方法，该方法基于以下三个关键见解。 1) 纯单目SLAM能够提供非常精确的初始地图 [^2]，其主要问题在于尺度未知。先解决仅涉及视觉的问题将有助于改善IMU初始化效果。 2) 如 [^56] 所示，当把尺度显式地作为优化变量时，其收敛速度会比使用BA的隐式表示方式快得多。 3) 在IMU初始化过程中忽略传感器的不确定性会产生较大的不可预测误差 [^64]。 因此，在充分考虑传感器不确定性的基础上，我们将IMU初始化问题表述为一个MAP估计问题，该过程分为以下三个步骤。 1) 仅视觉的MAP估计：我们初始化纯单目SLAM [^2]，并让其运行2秒，期间以4 Hz的频率插入关键帧。这段时间过后，我们会得到一个包含 $k = 10$ 个相机位姿及数百个点的按比例缩放的地图，该地图会通过仅视觉的BA算法进行优化[图2(b)]。这些位姿会被转换到机体参考系中，从而得到轨迹 $\mathbf{T}_{0:k} = [\mathbf{R}, \mathbf{p}]_{0:k}$，其中的横线表示单目情况下的按比例缩放的变量。 2) 仅惯性的MAP估计：在这一步中，我们的目标是在MAP估计的意义下，仅利用 $\bar{T}_{0:k}$ 以及这些关键帧之间的惯性测量数据来获得惯性参数的最优估计。这些惯性参数可以堆叠成仅惯性的状态向量。 $$ \mathcal{Y}_{k}=\{s,\mathbf{R}_{\mathbf{w}g},\mathbf{b},\mathbf{\bar{v}}_{0:k}\} $$ where $s \in \mathbb{R}^+$ is the scale factor of the vision-only solution; $\mathbf{R}_{\mathrm{wg}} \in \mathrm{SO}(3)$ is a rotation matrix used to compute gravity vector $\mathbf{g}$ in the world reference as $\mathbf{g} = \mathbf{R}_{\mathrm{wg}} \mathbf{g}_1$, where $\mathbf{g}_1 = (0, 0, G)^\mathrm{T}$ and G is the gravity magnitude; $\mathbf{b} = (\mathbf{b}^a, \mathbf{b}^g) \in \mathbb{R}^6$ are the accelerometer and gyroscope biases assumed to be constant during initialization; and $\tilde{\mathbf{v}}_{0:k} \in \mathbb{R}^3$ is the up-to-scale body velocities from first to last keyframe, initially estimated from $\tilde{\mathbf{T}}_{0:k}$. At this point, we are only considering the set of inertial measurements --- $\mathcal{I}_{0:k} \doteq \{\mathcal{I}_{0,1} \ldots \mathcal{I}_{k-1,k}\}$。因此，我们可以构建一个MAP估计问题，需要最大化的后验分布为： $$ p(\mathcal{Y}_{k}|\mathcal{I}_{0:k})\propto p(\mathcal{I}_{0:k}|\mathcal{Y}_{k})p(\mathcal{Y}_{k}) $$ 其中，$p(\mathcal{I}_{0:k}|\mathcal{Y}_k)$ 表示似然函数，$p(\mathcal{Y}_k)$ 表示先验概率。考虑到各测量值之间的独立性，仅基于惯性数据的MAP估计问题可表示为： $$ \mathcal{Y}_{k}^{*}=\underset{\mathcal{Y}_{k}}{\arg\max}\left(p(\mathcal{Y}_{k})\prod_{i=1}^{k}p(\mathcal{I}_{i-1,i}|s,\mathbf{R}_{\mathrm{wg}},\mathbf{b},\mathbf{\bar{v}}_{i-1},\mathbf{\bar{v}}_{i})\right) $$ 取负对数并假设IMU预积分及先验分布服从高斯分布，最终可得到如下优化问题： $$ \mathcal{Y}_{k}^{*}=\underset{\mathcal{Y}_{k}}{\arg\min}\left(\|\mathbf{b}\|_{\Sigma_{b}^{-1}}^{2}+\sum_{i=1}^{k}\|\mathbf{r}_{\mathcal{I}_{i-1,i}}\|_{\Sigma_{\mathcal{I}_{i-1,i}}^{-1}}^{2}\right). $$ This optimization, represented in Fig. 2(c), differs from (4) in not including visual residuals, as the up-to-scale trajectory estimated by visual SLAM is taken as constant, and adding a prior residual that forces IMU biases to be close to zero. Covariance matrix $\Sigma_{b}$ represents prior knowledge about the range of values IMU biases may take. Details for preintegration of IMU covariance $\Sigma_{\mathcal{I}_{i-1,i}}$ can be found at [^61]. 由于我们在流形上进行优化，因此需要定义一个回缩映射[^61]，以便在优化过程中更新$\mathbf{R}_{\mathrm{wg}}$。由于绕重力方向的旋转并不会改变重力本身，因此该更新过程由两个角度$(\delta\alpha_{\mathbf{g}}, \delta\beta_{\mathbf{g}})$参数化。 $$ \mathbf{R}_{\mathrm{wg}}^{\mathrm{new}}=\mathbf{R}_{\mathrm{wg}}^{\mathrm{old}}\mathrm{Exp}(\delta\alpha_{\mathbf{g}},\delta\beta_{\mathbf{g}},0) $$ 其中，$\text{Exp}(.)$是从$\mathbb{R}^3$到$\text{SO}(3)$的指数映射。为确保优化过程中比例因子始终保持正值，我们将其更新方式定义为： $$ s^{\mathrm{n e w}}=s^{\mathrm{o l d}}\exp{(\delta s)}. $$ 仅基于惯性的优化完成后，帧位姿、速度以及三维地图点会按照估算出的比例因子进行缩放，并旋转以使z轴与估算出的重力方向对齐。随后更新偏差并重新进行IMU预积分，旨在减少后续的线性化误差。 3) 视觉-惯性MAP估计：一旦获得了惯性参数和视觉参数的良好估算结果，便可执行联合视觉-惯性优化以进一步精化求解结果。该优化过程可如图2(a)所示，但所有关键帧共享相同的偏差，且偏差的先验信息与仅基于惯性优化的步骤中一致。 我们在EuRoC数据集上进行的详尽初始化实验[^6]表明，这种初始化方法效率极高：仅需2秒即可将比例误差控制在5%以内。为改善初始估算结果，可在初始化后5秒和15秒分别执行视觉-惯性BA优化，最终如第VII节所述，比例误差可收敛至1%。经过这些BA优化后，我们称地图已成熟，即比例、IMU参数和重力方向均已被精准估算。 我们的初始化方法比那些通过求解一组代数方程的联合初始化方法[^62] [^63] [^64]要精确得多；同时也比ORB-SLAM-VI中的初始化方法[^4]快得多——后者需要15秒才能得出首个比例估算值；亦比VI-DSO中的初始化方法[^46]快，后者初始时比例误差极大，需20–30秒才能收敛至1%误差。不同初始化方法的对比结果可参见[^6]。 在某些特定情况下，当慢动作无法提供对惯性参数的良好观测时，初始化过程可能无法在短短15秒内收敛到精确解。为了增强对此类情况的鲁棒性，我们提出了一种基于改进型纯惯性优化的新颖尺度精化技术；该技术会纳入所有插入的关键帧，但仅将尺度和重力方向作为待估计参数[图2(d)]。需要注意的是，在这种情况下，关于偏差恒定的假设并不成立。因此，我们采用从映射中估算出的值并将其固定下来。这种计算效率极高的优化过程会在本地映射线程中每10秒执行一次，直至地图中的关键帧数量超过100个，或者自初始化以来已过去75秒。 最后，我们轻松地将单目惯性初始化方法扩展到了立体惯性初始化中：只需将尺度因子固定为1，并将其从纯惯性优化的变量中移除，就能提升其收敛性。 ### C. 跟踪与映射 在跟踪与映射方面，我们采用了[^4]中提出的方案。跟踪过程会求解一个简化的视觉-惯性优化问题，仅对最后两帧的状态进行优化，而地图点则保持固定。 在映射环节，若试图求解公式(4)所描述的完整优化问题，对于大型地图而言是无法实现的。我们将关键帧及其对应点的滑动窗口设为可优化变量，同时纳入这些点来自共视关键帧的观测数据，但保持这些关键帧的姿态固定。 ### D. 对跟踪丢失的鲁棒性 在纯视觉SLAM或VO系统中，相机暂时被遮挡或快速运动会导致视觉元素跟踪丢失，进而使系统迷失方向。ORB-SLAM率先采用了基于词袋模型的位置识别的快速重定位技术，但事实证明这些技术在EuRoC数据集上的复杂序列中仍不足以解决问题[^3]。当跟踪到的地图点少于15个时，我们的视觉-惯性系统会进入视觉丢失状态，并通过以下两个阶段来实现鲁棒性： 1) 短期丢失：根据IMU读数估算当前的机体状态，将地图点投影到估算出的相机姿态下，并在较大的图像窗口内搜索匹配点。得到的匹配结果会被纳入视觉-惯性优化过程。在大多数情况下，这足以恢复视觉跟踪功能；否则，5秒后便会进入下一阶段。 --- 2) 长期丢失：按照上述方法初始化新的视觉-惯性地图，并将其设为活动地图。 如果系统在IMU初始化后的15秒内就出现丢失情况，该地图会被丢弃。这一措施可防止积累不准确且无意义的地图。 ### VI. 地图合并与闭环检测 跟踪与映射线程会通过将地图点投影到估算出的相机姿态下，并在仅几像素大小的图像窗口内搜索匹配点，从而常规性地找到当前帧与活动地图之间的短期及中期数据关联。为了实现用于重定位和闭环检测的长期数据关联，ORB-SLAM采用了DBoW2词袋模型位置识别系统[^9]、[^75]。大多数实现了闭环检测的最新VO和SLAM系统也采用了该方法（见表I）。 与跟踪不同，地点识别并非从对相机姿态的初始猜测开始。相反，DBoW2会构建一个包含关键帧及其词袋向量的数据库；给定查询图像时，它能根据词袋高效找出最相似的关键帧。仅使用第一个候选结果时，原始DBoW2查询的精确度和召回率可达到50%–80% [^9]。为避免误匹配破坏地图，DBoW2引入了时间和几何一致性检查机制，从而使精确度提升至100%，召回率则达到30%–40% [^9]、[^75]。关键在于，时间一致性检查会使地点识别至少延迟三个关键帧。当我们尝试将其应用于Atlas系统时，发现这种延迟以及较低的召回率常常导致同一地图或不同地图中出现重复区域。 在这项工作中，我们提出了一种用于长期和多地图数据关联的、具有更高召回率的新型地点识别算法。每当建图线程创建新关键帧时，便会启动地点识别流程，试图与Atlas中已有的任意关键帧进行匹配。若找到的匹配关键帧属于当前活动地图，则执行闭环检测；否则就属于多地图数据关联，此时会将活动地图与匹配地图合并。我们方法的第二个创新点在于：一旦估算出新关键帧与匹配地图之间的相对姿态，我们便会定义一个以该匹配关键帧及其在共视图中的邻近关键帧为成员的局部窗口。在该窗口内，我们会密集搜索中期数据关联，从而提高闭环检测和地图合并的精确度。正是这两项创新，解释了为何在EuRoC实验中ORB-SLAM3的精确度优于ORB-SLAM2。接下来将详细介绍各项操作的具体内容。 ### A. 地点识别 为了实现更高的召回率，对于每一个新的活动关键帧，我们都会向Atlas中的DBoW2数据库查询多个相似的关键帧。为确保100%的精确度，每个候选关键帧都要经过多步几何验证。所有几何验证步骤的基本操作是：检查图像窗口内是否存在描述符与地图点的ORB描述符相匹配的ORB关键点，同时设定汉明距离的阈值。如果搜索窗口内有多个候选点，为了排除模糊匹配，我们会检查其与次近匹配点的距离比值 [^76]。我们的地点识别算法步骤如下。 1) DBoW2候选关键帧。我们用活动关键帧 $K_{a}$ 查询Atlas中的DBoW2数据库，以获取三个最相似的关键帧，但排除与 $K_{a}$ 共视的关键帧。我们将每个用于地点识别的匹配候选称为 $K_{m}$。 2) 局部窗口。对于每个 $K_m$，我们定义一个局部窗口，其中包含 $K_m$、其最佳共视关键帧以及它们共同观测到的地图点。DBoW2直接索引提供了 $K_a$ 与局部窗口关键帧之间的潜在匹配对。对于每一组这样的二维-二维匹配，我们也能获得其对应地图点之间的三维-三维匹配关系。 3) 三维对齐变换。我们利用RANSAC算法计算出变换矩阵$\mathbf{T}_{am}$，该矩阵能更好地将$K_m$局部窗口中的地图点与$K_a$中的对应点对齐。在纯单目视觉模式下，或者当地图尚未成熟时的单目惯性导航模式下，我们计算$\mathbf{T}_{am} \in \text{Sim}(3)$；其他情况下则计算$\mathbf{T}_{am} \in \text{SE}(3)$。在这两种情况下，我们都采用Horn算法[^77]，利用最少的三组三维-三维匹配来生成关于$\mathbf{T}_{am}$的假设。如果通过$K_a$对$\mathbf{T}_{am}$中的地图点进行变换后，其在$K_a$中的重投影误差低于某一阈值，则该假设会获得一次有效投票。获得投票数最多的假设会被选中，前提是投票数超过了设定阈值。 4) 引导式匹配优化。局部窗口中的所有地图点都会通过$T_{am}$进行变换，以寻找与$K_{a}$中关键点的更多匹配项。搜索过程也会反向进行，即在局部窗口的所有关键帧中寻找与$K_{a}$地图点的匹配项。利用所找到的所有匹配关系，通过非线性优化来优化$T_{am}$，优化目标函数为双向重投影误差，同时采用Huber影响函数来提高算法对异常匹配项的鲁棒性。若优化后的内点数量超过设定阈值，则会启动第二轮引导式匹配和非线性优化，此时使用的图像搜索窗口会更小。 5) 在三个共视关键帧中验证。为避免误判，DBoW2需要等待连续三个关键帧触发位置识别功能，这往往会导致位置识别被延迟或遗漏。我们的关键发现是：大多数情况下，验证所需的信息早已存在于地图中。为了验证位置识别结果，我们会在地图的活动区域内搜索两个与$K_a$共视的关键帧，且这两个关键帧中与局部窗口内点的匹配数量需超过设定阈值。如果找不到这样的关键帧，就会尝试用新传入的关键帧进行验证，而无需再次触发词袋模型。验证过程会持续进行，直到有三个关键帧确认$T_{am}$的有效性，或者连续两个新关键帧无法验证其有效性为止。 6) 视觉-惯性系统的重力方向验证。在视觉-惯性模式下，如果活动地图已经成熟，我们就能估算出$\mathbf{T}_{am} \in \mathrm{SE}(3)$。我们还会进一步检查俯仰角是否 --- 低于设定阈值，从而最终确认位置识别假设的合理性。 ### B. 视觉地图合并 当成功的位置识别产生了活动地图$K_a$中的关键帧$M_a$与存储在地图集$K_m$中的另一张地图的匹配关键帧$M_m$之间的多地图数据关联，且存在对应的对齐变换矩阵$T_{am}$时，我们便会启动地图合并操作。在此过程中必须特别注意，要确保$M_m$中的信息能被跟踪线程迅速复用，从而避免地图重复。为此，我们提出将$M_a$地图纳入$M_m$的参考系中。由于$M_a$可能包含大量元素，合并过程可能耗时较长，因此合并操作被分为两步：首先，在由$K_a$和$K_m$在共视图中的邻居节点定义的焊接窗口内完成合并；第二阶段，通过PG优化将校正结果传播到合并后的地图其余部分。合并算法的详细步骤如下。 1) 焊接窗口组装。焊接窗口包括 $K_{a}$ 及其共视关键帧、$K_{m}$ 及其共视关键帧，以及它们所观测到的所有地图点。在将它们纳入焊接窗口之前，属于 $M_{a}$ 的关键帧和地图点会先通过 $T_{ma}$ 进行变换，以便与 $M_{m}$ 对齐。 2) 地图合并。地图 $M_{a}$ 和 $M_{m}$ 会被融合成新的活动地图。为了去除重复点，系统会在 $M_{a}$ 的关键帧中主动搜索与 $M_{m}$ 中的点相匹配的点。每找到一组匹配点，就会移除来自 $M_{a}$ 的点，同时保留 $M_{m}$ 中的点，并累积该被移除点的所有观测数据。由于发现了新的中期点关联，共视图和本质图 [^2] 会通过添加连接 $M_{m}$ 和 $M_{a}$ 关键帧的边来更新。 3) 焊接束调整。执行局部束调整（BA），优化焊接窗口内来自 $M_{a}$ 和 $M_{m}$ 的所有关键帧，以及它们所观测到的地图点 [见图 3(a)]。为了固定尺度自由度，那些不属于焊接窗口但能观测到局部地图点的 $M_{m}$ 关键帧会被纳入 BA 过程，且其位姿被固定。优化完成后，焊接区域内包含的所有关键帧即可用于相机跟踪，从而实现地图 $M_{m}$ 的快速且精确的复用。 4) 本质图优化。利用整个合并地图的本质图执行图优化（PG），同时固定焊接区域内的关键帧。这种优化会将校正信息从焊接窗口传播到地图的其余部分。 ### C. 视觉-惯性地图合并 视觉-惯性合并算法与纯视觉情况步骤类似。其中步骤 1) 和 3) 经过修改，以便更好地利用惯性信息。 1) 视觉-惯性焊接窗口组装：如果活动地图已经成熟，我们会在将其纳入焊接窗口前，将现有的 $\mathbf{T}_{ma} \in \text{SE}(3)$ 应用于地图 $M_a$。如果活动 ![Image](images/afdac6b3d32d23096cb654f30d51672ee2c8248aab4003aab9a35bcf03143382.jpg)
Fig. 3. Factor graph representation for the welding BA, with reprojection error terms (blue squares), IMU preintegration terms (yellow squares), and bias random walk (purple squares). (a) Visual welding BA. (b) Visual-Inertial welding BA.
地图尚未成熟，我们就利用现有的 $M_a$ 来对 $\mathbf{T}_{ma} \in \text{Sim}(3)$ 进行对齐。 2) 视觉-惯性焊接束调整：关键帧 $K_{a}$ 和 $K_{m}$ 及其最后五个时间相邻关键帧的位姿、速度和偏差会被设为可优化变量。这些变量通过 IMU 预积分项相互关联，如图 3(b) 所示。对于 $M_{m}$，局部窗口之前的紧邻关键帧会被纳入但位姿固定；而对于 $M_{a}$，类似的关键帧也会被纳入，但其位姿仍可被优化。上述关键帧所看到的所有地图点都会被优化，同时被优化的还有 $K_{m}$ 和 $K_{a}$ 共视关键帧的位姿。所有关键帧和点之间通过重投影误差相互关联。 ### D. 闭环检测 闭环校正算法与地图合并类似，只不过适用于通过位置识别匹配到的两个关键帧均属于活动地图的情况。此时会从匹配的关键帧中组装出焊接窗口，检测并融合重复点，从而在共视图和本质图中创建新链接。下一步是执行图优化（PG），将闭环校正信息传播到地图的其余部分。最后一步是执行全局束调整（BA），在考虑闭环的中期和长期匹配后得出地图的最优估计。在视觉-惯性情况下， ---
TABLE II
PERFORMANCE COMPARISON IN THE EUROC DATASET (RMS ATE IN M., SCALE ERROR IN %). EXCEPT WHERE NOTED, WE SHOW RESULTS REPORTED BY THE AUTHORS OF EACH SYSTEM, FOR ALL THE FRAMES IN THE TRAJECTORY, COMPARING WITH THE PROCESSED GT
MH01MH02MH03MH04MH05V101V102V103V201V202V203Avg $^{1}$
MonocularORB-SLAM [^4]ATE $^{2,3}$0.0710.0670.0710.0820.0600.0150.020-0.0210.018-0.047 $^{*}$
DSO [^27]ATE0.0460.0460.1723.8100.1100.0890.1070.9030.0440.1321.1520.601
SVO [^24]ATE0.1000.1200.4100.4300.3000.0700.210-0.1100.1101.0800.294 $^{*}$
DSM [^31]ATE0.0390.0360.0550.0570.0670.0950.0590.0760.0560.0570.7840.126
ORB-SLAM3 (ours)ATE0.0160.0270.0280.1380.0720.0330.0150.0330.0230.029-0.041 $^{*}$
StereoORB-SLAM2 [^3]ATE0.0350.0180.0280.1190.0600.0350.0200.0480.0370.035-0.044 $^{*}$
VINS-Fusion [^44]ATE0.5400.4600.3300.7800.5000.5500.230-0.2300.200-0.424 $^{*}$
SVO [^24]ATE0.0400.0700.2700.1700.1200.0400.0400.0700.0500.0900.7900.159
ORB-SLAM3 (ours)ATE0.0290.0190.0240.0850.0520.0350.0250.0610.0410.0280.5210.084
Monocular InertialMCSKF [^33]ATE $^{5}$0.4200.4500.2300.3700.4800.3400.2000.6700.1000.1601.1300.414
OKVIS [^39]ATE $^{5}$0.1600.2200.2400.3400.4700.0900.2000.2400.1300.1600.2900.231
ROVIO [^42]ATE $^{5}$0.2100.2500.2500.4900.5200.1000.1000.1400.1200.1400.1400.224
ORBSLAM-VI [^4]ATE $^{2,3}$ scale error $^{2,3}$0.0750.0840.0870.2170.0820.0270.028-0.0320.0410.0740.075 $^{*}$
VINS-Mono [^7]ATE $^{4}$0.0840.1050.0740.1220.1470.0470.0660.1800.0560.0900.2440.110
VI-DSO [^46]ATE scale error0.0620.0440.1170.1320.1210.0590.0670.0960.0400.0620.1740.089
ORB-SLAM3 (ours)ATE scale error0.0620.0370.0460.0750.0570.0490.0150.0370.0420.0210.0270.043
Stereo InertialVINS-Fusion [^44]ATE $^{4}$0.1660.1520.1250.2800.2840.0760.0690.1140.0660.0910.0960.138
BASALT [^47]ATE $^{3}$0.0800.0600.0500.1000.0800.0400.0200.0300.0300.020-0.051 $^{*}$
Kimera [^8]ATE0.0800.0900.1100.1500.2400.0500.1100.1200.0700.1000.1900.119
ORB-SLAM3 (ours)ATE scale error0.0360.0330.0350.0510.0820.0380.0140.0240.0320.0140.0240.035
$^{1}$：成功序列的平均误差。未完成所有序列的系统用$*$表示，且不会以粗体标注。 $^{2}$：使用原始GT而非处理后的GT来报告误差。 $^{3}$：使用关键帧轨迹而非完整轨迹来报告误差。 $^{4}$：我们自行运行代码并采用默认配置得到的误差结果。 $^{5}$：在[^78]处报告的误差。 只有当关键帧数量低于某一阈值时才会执行全局BA，以避免巨大的计算开销。 ### 七、实验结果 整个系统的评估分为以下几个部分。 1) EuRoC数据集中的单会话实验[^79]：对11个序列逐一进行处理以生成地图，涉及四种传感器配置：单目、单目惯性、双目以及双目惯性。 2) 在极具挑战性的TUM-VI基准测试中，单目与双目视觉惯性SLAM在鱼眼相机下的性能表现[^80]。 3) 在两个数据集上的多会话实验。 与该领域惯例一致，我们使用rms ATE[^81]来衡量精度；在纯单目情况下，通过Sim(3)变换将估计轨迹与真实轨迹对齐，其余传感器配置则采用SE(3)变换。尺度误差通过Sim(3)对齐得到的s来计算，即$|1 - s|$。所有实验均在Intel Core i7-7700 CPU（3.6 GHz）、32 GB内存的平台上仅使用CPU运行。 ### A. EuRoC上的单会话SLAM 表二对比了ORB-SLAM3在其四种传感器配置下与现有最先进系统之间的性能。我们所报告的数值是10次运行后的中位数。如表所示，在所有传感器配置下，ORB-SLAM3得出的结果都比文献中现有的最佳系统更精确，多数情况下差距还相当明显。 在单目和双目配置下，由于我们的算法具备更优的位置识别能力，能够更早闭合回环并提供更多中期匹配点，因此系统精度高于ORB-SLAM2。有趣的是，紧随其后的较好结果来自同样利用中期匹配点的DSM，尽管它并不闭合回环。 在单目惯性配置下，ORB-SLAM3的精度比MCSKF、OKVIS和ROVIO高出五到十倍，更是达到VI-DSO和VINS-Mono的两倍以上， --- ![Image](images/97596e1346c675f31ac4784a2139a5e8a66f4f179209dfc22c73473452cc6246.jpg)
Fig. 4. Colored squares represent the rms ATE for ten different executions in each sequence of the EuRoC dataset.
这再次体现了中期和长期数据关联的优势。与ORB-SLAM VI相比，我们创新的快速IMU初始化技术使ORB-SLAM3能在几秒内完成惯性传感器的校准，并从一开始就加以利用，从而能够完成所有EuRoC序列的处理，获得更高的精度。 在立体-惯性配置下，ORB-SLAM3的精度比Kimera和VINS-Fusion高出三到四倍。只有近期的BASALT能在精度上与之媲美；不过BASALT作为原生立体-惯性系统，却无法完成序列V203的运算，因为该序列中某个摄像头的部分帧缺失了。对比我们的单目-惯性系统与立体-惯性系统，后者在大多数情况下表现更佳。仅在两个Machine Hall（MH）序列中，其精度略低。我们推测，MH序列中场景深度较大可能导致立体三角测量不够精确，进而影响尺度估计的准确性。 总结性能数据时，我们给出了每种传感器配置下十次运行结果的中位数。对于鲁棒性强的系统而言，中位数能准确反映系统的行为特征；但鲁棒性差的系统其结果差异会很大。通过图4可以分析这一点，图中用不同颜色标示出十次运行中产生的误差。与[^46]中公布的DSO、ROVIO和VI-DSO的相关数据对比，可证实我们方法的优越性。 在纯视觉配置下，多地图系统通过在跟踪丢失时创建新地图来提升对快速运动的鲁棒性，新地图随后会与全局地图合并。这一点在单目序列V103和立体序列V203中尤为明显：ORB-SLAM2无法解决这些序列的问题，而我们的系统在大多数运行中都成功解决了。不出所料，立体系统比单目系统更鲁棒，因为其特征初始化速度更快，且还能额外估算出真实的尺度。 不过，鲁棒性的巨大提升得益于我们新颖的视觉-惯性SLAM系统，无论单目还是立体配置均如此。立体-惯性系统相比单目-惯性系统有极微小的优势，尤其是在最具挑战性的V203序列中表现更为突出。
TABLE III
我们可以得出结论：惯性信息的融合不仅提升了精度，降低了与纯视觉方案相比的中位数ATE误差……
TUM-VI BENCHMARK [^80]: RMS ATE (M) FOR REGIONS WITH AVAILABLE GROUND-TRUTH DATA
Seq.Mono-InertialStereo-InertialLength\n(m)LC
VINS-MonoORB-SLAM3OKVISROVIOBASALTORB-SLAM3
corridor10.630.040.330.470.340.03305✓
corridor20.950.020.470.750.420.02322✓
corridor31.560.310.570.850.350.02300✓
corridor40.250.170.260.130.210.21114
corridor50.770.030.392.090.370.01270✓
magistrale12.190.563.494.521.200.24918✓
magistrale23.110.522.7313.431.110.52561✓
magistrale30.404.891.2214.800.741.86566
magistrale45.120.130.7739.731.580.16688✓
magistrale50.851.031.623.470.601.13458✓
magistrale62.291.303.91X3.230.97771
outdoors174.9670.79X101.95255.0432.232656
outdoors2133.4614.9873.8621.6764.6110.421601
outdoors336.9939.63 $^{{*}}$32.3826.1038.2654.771531
outdoors416.4625.2619.51X17.5311.61928
outdoors5130.6314.8713.1254.327.898.951168✓
outdoors6133.6016.8496.51149.1465.5010.702045
outdoors721.907.5913.6149.014.074.581748✓
outdoors883.3627.8816.3136.0313.5311.02986
room10.070.010.060.160.090.01146✓
room20.070.020.110.330.070.01142✓
room30.110.040.070.150.130.01135✓
room40.040.010.030.090.050.0168✓
room50.200.020.070.120.130.01131✓
room60.080.010.040.050.020.0167✓
slides10.680.970.8613.730.320.41289
slides20.841.062.150.810.320.49299
slides30.690.692.584.680.890.47383
我们给出的是三次运行结果的中位数。 对于其他系统，我们提供了[^82]中报告的数值。 $^{*}$指出，三次运行中有一次并未成功。 LC：该序列中可能存在回环现象。
TABLE IV
RMS ATE (M) OBTAINED BY ORB-SLAM3 WITH FOUR SENSOR CONFIGURATIONS IN THE ROOM SEQUENCES, REPRESENTATIVE OF AR/VR SCENARIOS (MEDIAN OF THREE EXECUTIONS)
Seq.MonoStereoMono-InertialStereo-Inertial
room10.0420.0770.0090.008
room20.0260.0550.0180.012
room30.0280.0760.0080.011
room40.0460.0710.0090.008
room50.0460.0660.0140.010
room60.0430.0630.0060.006
Avg.0.0390.0680.0110.009
TABLE V
MULTISESSION RMS ATE (M) ON THE EUROC DATASET. FOR CCM-SLAM AND VINS, WE SHOW RESULTS REPORTED BY THE AUTHORS OF EACH SYSTEM. OUR VALUES ARE THE MEDIAN OF FIVE EXECUTIONS, ALIGNING THE TRAJECTORIES WITH THE PROCESSED GT
RoomMachine HallVicon 1Vicon 2
SequencesMH01-03MH01-05V101-103V201-203
ORB-SLAM3 MonoATE0.0300.0580.0580.284
CCM-SLAM Mono [^71]ATE0.077---
ORB-SLAM3 StereoATE0.0280.0400.0270.163
ORB-SLAM3 Mono-InertialATE Scale error0.037 0.40.065 0.30.040 1.40.048 0.9
VINS [^7] Mono-InertialATE-0.210--
ORB-SLAM3 Stereo-InertialATE Scale error0.041 0.60.047 0.30.031 0.60.046 0.8
--- ![Image](images/d72dfcd1ab66b81ada3f96cd290366948c2b2a63983da37241b30096828fd672.jpg)
Fig. 5. Multisession stereo-inertial result with several sequences from TUM-VI dataset (front, side, and top views).
![Image](images/5541764ef4f98d68ffa78aa1b65d86901fb6708417eec805dfa42a76107cb24f.jpg)
Fig. 6. Multisession stereo-inertial. In red, the trajectory estimated after single-session processing of outdoors1. In blue, multisession processing of magistrale2 first and then outdoors1.
视觉解决方案相比，惯性融合不仅提升了精度，还赋予了系统极佳的鲁棒性，使其性能更加稳定。 ### B. TUM-VI基准测试上的视觉-惯性SLAM TUM-VI数据集[^80]包含6种不同环境下的28个序列，这些序列是使用手持式鱼眼立体惯性设备采集的。轨迹的真实值仅在序列的开头和结尾处可得，而对大多数序列而言，这部分仅占整个轨迹的极小部分。数据集中的许多序列不包含回环；即便起始点和结束点在同一房间内，视角方向也相反，因此地点识别无法检测到任何共同区域。利用这类真实值进行评估，相当于测量整个轨迹上的累积漂移量。 在单目惯性系统中，我们对每张图像提取1500个ORB特征点；在立体惯性系统中则提取1000个特征点，这些操作是在应用CLAHE均衡化处理以解决数据集中存在的曝光不足或过度问题之后进行的。对于室外序列而言，我们的系统难以处理来自多云天空的极远处的点，这类点在鱼眼相机中尤为明显。这些点的运动速度较慢，可能会导致相机位姿出现漂移。为防止这种情况，我们仅针对室外序列丢弃距离当前相机位姿超过20米的点。更精细的解决方案是使用图像分割算法来检测并剔除天空区域的点。 所得结果与文献中最相关的系统在表III中进行了比较，结果清楚地显示出ORB-SLAM3在单目惯性配置和立体惯性配置下的优越性。与之最接近的系统是VINS-Mono和BASALT，它们本质上属于带有回环闭合功能的视觉惯性里程计系统，但缺乏中期数据关联能力。 进一步详细分析我们系统的性能可知，它在中小型室内环境、房间及走廊序列中的误差最小，其中大部分场景的误差低于10厘米。在这些轨迹中，系统会持续访问并复用之前已映射的区域，这正是ORB-SLAM3的主要优势之一。此外，被跟踪的点通常距离小于5米，这更便于估算惯性参数，防止其出现发散现象。 在长度可达900米的复杂室内序列中，大多数被跟踪的点距离相对较近，ORB-SLAM3的误差约为1米，仅有一个序列的误差接近5米。相比之下，在一些较长的室外序列中，由于近距离视觉特征稀少，可能会导致惯性参数（尤其是尺度和加速度计偏差）发生漂移，进而产生10至70米量级的误差。不过，ORB-SLAM3仍是室外序列中表现最佳的系统。 该数据集还包含三个极具挑战性的滑梯序列，用户需沿着黑暗的管状滑梯下滑。 ---
TABLE VI
RUNNING TIME OF THE MAIN PARTS OF OUR TRACKING AND MAPPING THREADS COMPARED TO ORB-SLAM2, ON EUROC V202 (MEAN TIME AND STANDARD DEVIATION IN MS)
SettingsSystemORB-SLAM2ORB-SLAM3ORB-SLAM3ORB-SLAM3ORB-SLAM3
SensorStereoMonocularStereoMono-InertialStereo-Inertial
Resolution752\times480752\times480752\times480752\times480752\times480
Cam. FPS20Hz20Hz20Hz20Hz20Hz
IMU---200Hz200Hz
ORB Feat.12001000120010001200
RMS ATE0.0350.0290.0280.0210.014
TrackingStereo rect.3.07\pm0.80-1.32\pm0.43-1.60\pm0.74
ORB extract11.20\pm2.0012.40\pm5.1015.68\pm4.7411.98\pm4.7815.22\pm4.37
Stereo match10.38\pm2.57-3.35\pm0.92-3.38\pm1.07
IMU integr.---0.18\pm0.110.22\pm0.20
Pose pred2.20\pm0.721.87\pm0.682.69\pm0.850.09\pm0.410.15\pm0.71
LM Track9.89\pm4.954.98\pm1.656.31\pm2.858.22\pm2.5211.51\pm3.33
New KF dec0.20\pm0.430.04\pm0.030.12\pm0.190.05\pm0.030.18\pm0.25
Total37.87\pm7.4921.52\pm6.4531.48\pm5.8023.22\pm14.9833.05\pm9.29
MappingKF Insert8.72\pm3.609.25\pm4.628.03\pm2.9613.17\pm7.438.53\pm2.17
MP Culling0.25\pm0.090.09\pm0.040.32\pm0.150.07\pm0.040.24\pm0.24
MP Creation36.88\pm14.5322.78\pm8.8018.23\pm9.8430.19\pm12.9523.88\pm9.97
LBA139.61\pm124.92216.95\pm188.77134.60\pm136.28121.09\pm44.81152.70\pm38.37
KF Culling4.37\pm4.7318.88\pm12.2175.49\pm5.0926.25\pm17.0811.15\pm7.67
Total173.81\pm139.07266.61\pm207.80158.84\pm147.84191.50\pm80.54196.61\pm54.52
Map SizeKFs278272259332135
MPs14593968614245103069761
TABLE VII
RUNNING TIME OF THE MAIN OPERATIONS FOR LOOP CLOSING AND MAP MERGING FOR A MULTISESSION EXPERIMENT ON SEQUENCES V201, V202, AND V203 FROM EUROC DATASET (MEAN TIME AND STANDARD DEVIATION IN MS)
SettingsSensorMonocularStereoMono-InertialStereo-Inertial
Resolution752\times480752\times480752\times480752\times480
Cam. FPS20Hz20Hz20Hz20Hz
IMU--200Hz200Hz
ORB Feat.1000120010001200
RMS ATE0.2840.1630.0480.046
Place RecognitionDatabase query0.96\pm0.581.06\pm0.581.04\pm0.591.02\pm0.60
Compute Sim3/SE33.61\pm2.815.26\pm3.792.98\pm2.265.71\pm3.54
Total3.92\pm3.285.26\pm4.393.45\pm2.815.89\pm4.29
Map MergingMerge Maps152.03\pm45.8568.56\pm13.56129.08\pm8.2691.07\pm5.56
Welding BA52.09\pm14.0835.57\pm7.94103.14\pm6.0858.15\pm4.84
Opt. Essential Graph5.82\pm3.0110.98\pm9.7952.83\pm17.8136.08\pm17.95
Total221.90\pm58.73120.63\pm16.23287.33\pm15.58187.82\pm6.38
Merge info# Detected merges5422
Merge size (# keyframes)31\pm131\pm325\pm125\pm0
Merge size (# map points)2476\pm2072697\pm7182425\pm884260\pm160
LoopLoop Fusion311.82\pm333.4929.07\pm23.64-25.67
Opt. Essential Graph254.84\pm87.0384.36\pm37.56-95.13
Total570.39\pm420.77118.62\pm59.93-124.77
Loop info# Detected loops3401
Loop size (# keyframes)58\pm6027\pm9-60
Loop Full BAFull BA4010.14\pm1835.851118.54\pm563.75-1366.64
Map Update124.80\pm6.0713.65\pm12.86-163.06
Total4134.94\pm1829.781132.19\pm572.28-1529.69
BA size (# keyframes)345\pm147220\pm110-151
BA size (# map points)13511\pm377812297\pm4572-14397
几乎没有任何视觉特征。在这种情况下，纯视觉系统将会失效，但我们的视觉-惯性系统即便在无法检测到闭环的情况下，也能以具有竞争力的误差水平处理整个序列。有趣的是，使用Lukas-Kanade算法追踪特征的VINS-Mono和BASALT在某些序列中，其精度甚至超过了通过匹配ORB描述符的ORB-SLAM3。 最后，室内序列可以代表典型的AR/VR应用场景，即用户在狭小空间内手持或佩戴设备移动。对于这些序列，整个轨迹都有真实值可供参考。表III显示，ORB-SLAM3的精度明显优于其他竞争方法。表IV对比了使用我们四种传感器配置所获得的结果。单目视觉相比立体视觉的更高精度只是表面现象：单目解决方案是未标定尺度的，需通过7个自由度与真实值对齐；而立体视觉能提供真实的尺度，仅需6个自由度即可对齐。采用单目-惯性方案后，我们 --- 进一步将平均均方根ATE误差降至1厘米左右，同时还能获得真实的尺度。最终，我们的立体-惯性SLAM技术将误差控制在1厘米以内，因此非常适合AR/VR应用。 ### C. 多会话SLAM EuRoC数据集中的三个环境各自包含多个会话：MH环境有5个会话，Vicon1环境有3个会话，Vicon2环境也有3个会话。为了测试ORB-SLAM3的多会话性能，我们依次处理每个环境下对应的所有会话。同一环境中的各轨迹都以相同的世界坐标系为基准，因此可以进行单次全局对齐来计算ATE。 The first sequence in each room provides an initial map. Processing the following sequences starts with the creation of a new active map, which is quickly merged with the map of the previous sessions, and from that point on, ORB-SLAM3 profits from reusing the previous map. Table V reports the global multisession rms ATE for the four sensor configurations in the three rooms, comparing with the two only published multisession results in EuRoC dataset: CCM-SLAM [^71] that reports pure monocular results in MH01-MH03, and VINS-Mono [^7] in the five MH sequences, using monocular-inertial. In both cases, ORB-SLAM3 more than doubles the accuracy of competing methods. In the case of VINS-Mono, ORB-SLAM3 obtains 2.6 better accuracy in single-session, and the advantage goes up to 3.2 times in multisession, showing the superiority of our map merging operations. Comparing these multisession performances with the single-session results reported in Table II, the most notable difference is that multisession monocular and stereo SLAM can robustly process the difficult sequences V103 and V203, thanks to the exploitation of the previous map. We have also performed some multisession experiments on the TUM-VI dataset. Fig. 5 shows the result after processing several sequences inside the TUM building. $^{1}$ In this case, the small room sequence provides loop closures that were missing in the longer sequences, bringing all errors to centimeter level. Although ground-truth is not available outside the room, comparing the figure with the figures published in [^82] clearly shows our point: Our multisession SLAM system obtains far better accuracy than existing visual–inertial odometry systems. This is further exemplified in Fig. 6. Although ORB-SLAM3 ranks higher in stereo inertial single-session processing of outdoors1, there is still a significant drift ( $\approx$ 60 m). In contrast, if outdoors1 is processed after magistrale2 in a multisession manner, this drift is significantly reduced, and the final map is much more accurate. ### D. Computing Time Table VI summarizes the running time of the main operations performed in the tracking and mapping threads, showing that our system is able to run in real time at 30–40 frames and at 3–6 keyframes per second. The inertial part takes negligible time during tracking and, in fact, can render the system more efficient as the frame rate could be safely reduced. In the mapping thread, the higher number of variables per keyframe has been compensated with a smaller number of keyframes in the inertial local BA, achieving better accuracy, with similar running time. As the tracking and mapping threads work always in the active map, multimapping does not introduce significant overhead. Table VII summarizes the running time of the main steps for loop closing and map merging. The novel place recognition method only takes 10 ms per keyframe. Times for merging and loop closing remain below 1 s, running only a PG optimization. For loop closing, performing a full BA may increase times up to a few seconds, depending on the size of the involved maps. In any case, as both operations are executed in a separate thread (Fig. 1), they do not interfere with the real-time performance of the rest of the system. The visual–inertial systems perform just two map merges to join three sequences, while visual systems perform some additional merges to recover from tracking losses. Thanks to their lower drift, visual–inertial systems also perform less loop closing operations compared with pure visual systems. ### VIII. CONCLUSION Although it would be interesting, we do not compare running time against other systems since this would require a significant effort that is beyond the scope of this work. Building on [^2] [^3] [^4], we presented ORB-SLAM3, the most complete open-source library for visual, visual–inertial, and multisession SLAM, with monocular, stereo, RGB-D, pin-hole, and fisheye cameras. Our main contributions, apart from the integrated library itself, are the fast and accurate IMU initialization technique and the multisession map-merging functions that rely on a new place recognition technique with improved recall. Our experimental results showed that ORB-SLAM3 is the first visual and visual–inertial system capable of effectively exploiting short-term, mid-term, long-term, and multimap data associations, reaching an accuracy level that is beyond the reach of existing systems. Our results also suggested that, regarding accuracy, the capability of using all these types of data association overpowers other choices such as using direct methods instead of features or performing keyframe marginalization for local BA, instead of assuming an outer set of static keyframes as we do. The main failure case of ORB-SLAM3 is low-texture environments. Direct methods are more robust to low-texture but are limited to short-term [^27] and mid-term [^31] data association. On the other hand, matching feature descriptors successfully solves long-term and multimap data association but seems to be less robust for tracking than Lucas–Kanade, which uses photometric information. An interesting line of research could be developing photometric techniques adequate for the four data association problems. We are currently exploring this idea for map building from endoscope images inside the human body. About the four different sensor configurations, there is no question; stereo-inertial SLAM provides the most robust and accurate solution. Furthermore, the inertial sensor allows to estimate pose at IMU rate, which is orders of magnitude higher. --- than frame rate, being a key feature for some use cases. For applications where a stereo camera is undesirable because of its higher bulk, cost, or processing requirements, you can use monocular-inertial without missing much in terms of robustness and accuracy. Only keep in mind that pure rotations during exploration would not allow to estimate depth. In applications with slow motions, or without roll and pitch rotations, such as a car in a flat area, IMU sensors can be difficult to initialize. In those cases, if possible, use stereo SLAM. Otherwise, recent advances on depth estimation from a single image with convolutional neural networks (CNNs) offer good promise for reliable and true-scale monocular SLAM [^83], at least in the same type of environments where the CNN has been trained. ## REFERENCES [^1]: C. Cadena et al., “Past, present, and future of simultaneous localization and mapping: Toward the robust-perception age,” IEEE Trans. Robot., vol. 32, no. 6, pp. 1309–1332, Dec. 2016. [^2]: R. Mur-Artal, J. M. M. Montiel, and J. D. Tardós, “ORB-SLAM: A versatile and accurate monocular SLAM system,” IEEE Trans. Robot., vol. 31, no. 5, pp. 1147–1163, Oct. 2015. [^3]: R. Mur-Artal and J. D. Tardós, “ORB-SLAM2: An open-source SLAM system for monocular, stereo, and RGB-D cameras,” IEEE Trans. Robot., vol. 33, no. 5, pp. 1255–1262, Oct. 2017. [^4]: R. Mur-Artal and J. D. Tardós, “Visual-inertial monocular SLAM with map reuse,” IEEE Robot. Autom. Lett., vol. 2, no. 2, pp. 796–803, Apr. 2017. [^5]: C. Campos, R. Elvira, J. J. Gómez Rodríguez, J. M. M. Montiel, and J. D. Tardós, “ORB-SLAM3: An accurate open-source library for visual, visual-inertial and multi-map SLAM,” 2020. [Online]. Available: [^6]: C. Campos, J. M. M. Montiel, and J. D. Tardós, “Inertial-only optimization for visual-inertial initialization,” in Proc. IEEE Int. Conf. Robot. Autom., 2020, pp. 51–57. [^7]: T. Qin, P. Li, and S. Shen, “VINS-Mono: A robust and versatile monocular visual-inertial state estimator,” IEEE Trans. Robot., vol. 34, no. 4, pp. 1004–1020, Aug. 2018. [^8]: A. Rosinol, M. Abate, Y. Chang, and L. Carlone, “Kimera: An open-source library for real-time metric-semantic localization and mapping,” in Proc. IEEE Int. Conf. Robot. Autom., 2020, pp. 1689–1696. [^9]: D. Gálvez-López and J. D. Tardós, “Bags of binary words for fast place recognition in image sequences,” IEEE Trans. Robot., vol. 28, no. 5, pp. 1188–1197, Oct. 2012. [^10]: R. Elvira, J. D. Tardós, and J. M. M. Montiel, “ORBSLAM-Atlas: A robust and accurate multi-map system,” in Proc. IEEE/RSJ Int. Conf. Intell. Robots Syst., 2019, pp. 6253–6259. [^11]: R. Tsai, “A versatile camera calibration technique for high-accuracy 3d machine vision metrology using off-the-shelf TV cameras and lenses,” IEEE J. Robot. Autom., vol. 3, no. 4, pp. 323–344, Aug. 1987. [^12]: J. Kannala and S. S. Brandt, “A generic camera model and calibration method for conventional, wide-angle, and fish-eye lenses,” IEEE Trans. Pattern Anal. Mach. Intell., vol. 28, no. 8, pp. 1335–1340, Aug. 2006. [^13]: A. J. Davison, “Real-time simultaneous localisation and mapping with a single camera,” in Proc. IEEE Int. Conf. Comput. Vision, vol. 2, Oct. 2003, pp. 1403–1410. [^14]: A. J. Davison, I. D. Reid, N. D. Molton, and O. Stasse, “MonoSLAM: Real-time single camera SLAM,” IEEE Trans. Pattern Anal. Mach. Intell., vol. 29, no. 6, pp. 1052–1067, Aug. 2007. [^15]: H. Kim, “SceneLib2 - MonoSLAM open-source library.” Available: [^16]: G. Klein and D. Murray, “Parallel tracking and mapping for small AR workspaces,” in Proc. IEEE ACM Int. Symp. Mixed Augmented Reality, 2007, pp. 225–234. [^17]: G. Klein and D. Murray, “Improving the agility of keyframe-based SLAM,” in Proc. Eur. Conf. Comput. Vis., 2008, pp. 802–815. [^18]: G. Klein and D. Murray, “Parallel tracking and mapping on a camera phone,” in Proc. 8th IEEE Int. Symp. Mixed Augmented Reality, Oct. 2009, pp. 83–86. [^19]: G. Klein and D. Murray, “PTAM-GPL,” 2013. [Online]. Available: [^20]: J. Engel, T. Schöps, and D. Cremers, "LSD-SLAM: Large-scale direct monocular SLAM," in Proc. Eur. Conf. Comput. Vis., 2014, pp. 834–849. [^21]: J. Engel, J. Stueckler, and D. Cremers, “Large-scale direct SLAM with stereo cameras,” in Proc. IEEE/RSJ Int. Conf. Intell. Robots Syst., 2015, pp. 141–148. [^22]: J. Engel, T. Schöps, and D. Cremers, "LSD-SLAM: Large-scale direct monocular SLAM." Available: [^23]: C. Forster, M. Pizzoli, and D. Scaramuzza, “SVO: Fast semi-direct monocular visual odometry,” in Proc. IEEE Int. Conf. Robot. Autom., 2014, pp. 15–22. [^24]: C. Forster, Z. Zhang, M. Gassner, M. Werlberger, and D. Scaramuzza, "SVO: Semidirect visual odometry for monocular and multicamera systems," IEEE Trans. Robot., vol. 33, no. 2, pp. 249–265, Apr. 2017. [^25]: C. Forster, M. Pizzoli, and D. Scaramuzza, “SVO,” 2014. [Online]. Available: [^26]: R. Mur-Artal, J. D. Tardós, J. M. M. Montiel, and D. Gálvez-López, “ORB-SLAM2,” 2016. [Online]. Available: [^27]: J. Engel, V. Koltun, and D. Cremers, “Direct sparse odometry,” IEEE Trans. Pattern Anal. Mach. Intell., vol. 40, no. 3, pp. 611–625, Mar. 2018. [^28]: H. Matsuki, L. von Stumberg, V. Usenko, J. Stückler, and D. Cremers, “Omnidirectional DSO: Direct sparse odometry with fisheye cameras,” IEEE Robot. Autom. Lett., vol. 3, no. 4, pp. 3693–3700, Oct. 2018. [^29]: R. Wang, M. Schworer, and D. Cremers, “Stereo DSO: Large-scale direct sparse visual odometry with stereo cameras,” in Proc. IEEE Int. Conf. Comput. Vis., 2017, pp. 3903–3911. [^30]: J. Engel, V. Koltun, and D. Cremers, "DSO: Direct sparse odometry," 2018. [Online]. Available: [^31]: J. Zubizarreta, I. Aguinaga, and J. M. M. Montiel, “Direct sparse mapping,” IEEE Trans. Robot., vol. 36, no. 4, pp. 1363–1370, Aug. 2020. [^32]: J. Zubizarreta, I. Aguinaga, J. D. Tardós, and J. M. M. Montiel, “DSM: Direct sparse mapping,” 2019. [Online]. Available: [^33]: A. I. Mourikis and S. I. Roumeliotis, “A multi-state constraint Kalman filter for vision-aided inertial navigation,” in Proc. IEEE Int. Conf. Robot. Autom., 2007, pp. 3565–3572. [^34]: M. Li and A. I. Mourikis, “High-precision, consistent EKF-based visual-inertial odometry,” Int. J. Robot. Res., vol. 32, no. 6, pp. 690–711, 2013. [^35]: M. K. Paul, K. Wu, J. A. Hesch, E. D. Nerurkar, and S. I. Roumeliotis, “A comparative analysis of tightly-coupled monocular, binocular, and stereo VINS,” in Proc. IEEE Int. Conf. Robot. Autom., 2017, pp. 165–172. [^36]: M. K. Paul and S. I. Roumeliotis, “Alternating-stereo VINS: Observability analysis and performance evaluation,” in Proc. IEEE Conf. Comput. Vis. Pattern Recognit., 2018, pp. 4729–4737. [^37]: K. Chaney, “Monocular MSCKF,” 2018. [Online]. Available: [^38]: S. Leutenegger, P. Furgale, V. Rabaud, M. Chli, K. Konolige, and R. Siegwart, “Keyframe-based visual-inertial SLAM using nonlinear optimization,” in Proc. Robot. Sci. Syst., 2013. [^39]: S. Leutenegger, S. Lynen, M. Bosse, R. Siegwart, and P. Furgale, "Keyframe-based visual-inertial odometry using nonlinear optimization," Int. J. Robot. Res., vol. 34, no. 3, pp. 314–334, 2015. [^41]: M. Bloesch, S. Omari, M. Hutter, and R. Siegwart, “Robust visual inertial odometry using a direct EKF-based approach,” in Proc. IEEE/RSJ Intell. Robots Syst., 2015, pp. 298–304. [^42]: M. Bloesch, M. Burri, S. Omari, M. Hutter, and R. Siegwart, “Iterated extended Kalman filter based visual-inertial odometry using direct photometric feedback,” Int. J. Robot. Res., vol. 36, no. 10, pp. 1053–1072, 2017. [^43]: M. Bloesch, S. Omari, M. Hutter, and R. Siegwart, "ROVIO," 2015. [Online]. Available: [^44]: T. Qin, J. Pan, S. Cao, and S. Shen, “A general optimization-based framework for local odometry estimation with multiple sensors,” 2019, arXiv:1901.03638. [^45]: T. Qin, S. Cao, J. Pan, P. Li, and S. Shen, “VINS-Fusion: An optimization-based multi-sensor state estimator,” 2019. [Online]. Available: [^46]: L. von Stumberg, V. Usenko, and D. Cremers, “Direct sparse visual-inertial odometry using dynamic marginalization,” in Proc. IEEE Int. Conf. Robot. Autom., 2018, pp. 2510–2517. ## [^47]: V. Usenko, N. Demmel, D. Schubert, J. Stückler, and D. Cremers, “Visual-inertial mapping with non-linear factor recovery,” IEEE Robot. Autom. Lett., vol. 5, no. 2, pp. 422–429, Apr. 2020 --- [^48]: V. Usenko and N. Demmel, “BASALT,” 2019. [Online]. Available: [^49]: A. Rosinol, M. Abate, Y. Chang, and L. Carlone, “Kimera,” 2019. [Online]. Available: [^50]: A. J. Davison, “SceneLib 1.0.” Available: ajd/Scene/index.html [^51]: S. I. Roumeliotis and A. I. Mourikis, “Vision-aided inertial navigation,” U.S. Patent 9 766 0 74, Sep. 1 9, 2017. [^52]: J. Civera, A. J. Davison, and J. M. M. Montiel, “Inverse depth parametrization for monocular SLAM,” IEEE Trans. Robot., vol. 24, no. 5, pp. 932–945, Oct. 2008. [^53]: L. Clemente, A. J. Davison, I. D. Reid, J. Neira, and J. D. Tardós, “Mapping large loops with a single hand-held camera,” in Proc. Robot.: Sci. Syst., Atlanta, GA, USA, Jun. 2007. [^54]: J. Civera, O. G. Grasa, A. J. Davison, and J. M. M. Montiel, “1-point RANSAC for extended Kalman filtering: Application to real-time structure from motion and visual odometry,” J. Field Robot., vol. 27, no. 5, pp. 609–631, 2010. [^55]: H. Strasdat, J. M. M. Montiel, and A. J. Davison, “Visual SLAM: Why filter?” Image Vis. Comput., vol. 30, no. 2, pp. 65–77, 2012. [^56]: H. Strasdat, J. M. M. Montiel, and A. J. Davison, “Scale drift-aware large scale monocular SI AM” Robot · Sci Svet VI vol 2 2010 [^57]: H. Strasdat, A. J. Davison, J. M. M. Montiel, and K. Konolige, “Double window optimisation for constant time visual SLAM,” in Proc. IEEE Int. Conf. Comput. Vis., 2011, pp. 2352–2359. [^58]: X. Gao, R. Wang, N. Demmel, and D. Cremers, "LDSO: Direct sparse odometry with loop closure," in Proc. IEEE/RSJ Int. Conf. Intell. Robots Syst., 2018, pp. 2198–2204. [^59]: S. H. Lee and J. Civera, “Loosely-coupled semi-direct monocular SLAM,” IEEE Robot. Autom. Lett., vol. 4, no. 2, pp. 399–406, Apr. 2018. [^60]: T. Lupton and S. Sukkarieh, “Visual-inertial-aided navigation for high-dynamic motion in built environments without initial conditions,” IEEE Trans. Robot., vol. 28, no. 1, pp. 61–76, Feb. 2012. [^61]: C. Forster, L. Carlone, F. Dellaert, and D. Scaramuzza, “On-manifold preintegration for real-time visual-inertial odometry,” IEEE Trans. Robot., vol. 33, no. 1, pp. 1–21, Feb. 2017. [^62]: A. Martinelli, “Closed-form solution of visual-inertial structure from motion,” Int. J. Comput. Vis., vol. 106, no. 2, pp. 138–152, 2014. [^63]: J. Kaiser, A. Martinelli, F. Fontana, and D. Scaramuzza, “Simultaneous state initialization and gyroscope bias calibration in visual inertial aided navigation,” IEEE Robot. Autom. Lett., vol. 2, no. 1, pp. 18–25, Jan. 2017. [^64]: C. Campos, J. M. M. Montiel, and J. D. Tardós, “Fast and robust initialization for visual-inertial SLAM,” in Proc. IEEE Int. Conf. Robot. Autom., 2019, pp. 1288–1294. [^65]: E. Eade and T. Drummond, “Unified loop closing and recovery for real time monocular SLAM,” in Proc. 19th Brit. Mach. Vis. Conf., Sep. 2008. [^66]: R. Castle, G. Klein, and D. W. Murray, “Video-rate localization in multiple maps for wearable augmented reality,” in Proc. 12th IEEE Int. Symp. Wearable Comput., Sep. 2008, pp. 15–22. [^67]: C. Forster, S. Lynen, L. Kneip, and D. Scaramuzza, “Collaborative monocular SLAM with multiple micro aerial vehicles,” in Proc. IEEE/RSJ Int. Conf. Intell. Robots Syst., 2013, pp. 3962–3970. [^68]: L. Riazuelo, J. Civera, and J. M. M. Montiel, “C2TAM: A cloud framework for cooperative tracking and mapping,” Robot. Auton. Syst., vol. 62, no. 4, pp. 401–413, 2014. [^69]: J. G. Morrison, D. Gálvez-López, and G. Sibley, “MOARSLAM: Multiple operator augmented rslAM,” in Proc. Distrib. Auton. Robot. Syst., 2016, pp. 119–132. [^70]: P. Schmuck and M. Chli, “Multi-UAV collaborative monocular SLAM,” in Proc. IEEE Int. Conf. Robot. Autom., 2017, pp. 3863–3870. [^71]: P. Schmuck and M. Chli, "CCM-SLAM: Robust and efficient centralized collaborative monocular simultaneous localization and mapping for robotic teams," J. Field Robot., vol. 36, no. 4, pp. 763–781, 2019. [^72]: H. A. Daoud, A. Q. M. Sabri, C. K. Loo, and A. M. Mansoor, “SLAMM: Visual monocular SLAM with continuous mapping using multiple maps,” PLoS One, vol. 13, no. 4, pp. 1–22, 2018. [^73]: V. Lepetit, F. Moreno-Noguer, and P. Fua, "EPnP: An accurate O(n) solution to the PnP problem," Int. J. Comput. Vis., vol. 81, no. 2, pp. 155–166, 2009. [^74]: S. Urban, J. Leitloff, and S. Hinz, “MLPnP—A real-time maximum likelihood solution to the perspective-N-Point problem,” ISPRS Annal. Photogramm. Remote Sens. Spatial Inform. Sci., vol. III-3, pp. 131–138, 2016. [^75]: R. Mur-Artal and J. D. Tardós, “Fast relocalisation and loop closing in keyframe-based SLAM,” in Proc. IEEE Int. Conf. Robot. Autom., 2014, pp. 846–853. [^76]: D. G. Lowe, “Distinctive image features from scale-invariant keypoints,” Int. J. Comput. Vis., vol. 60, no. 2, pp. 91–110, 2004. [^77]: B. K. Horn, “Closed-form solution of absolute orientation using unit quaternions,” J. Opt. Soc. Amer. A, vol. 4, no. 4, pp. 629–642, 1987. [^78]: J. Delmerico and D. Scaramuzza, “A benchmark comparison of monocular visual-inertial odometry algorithms for flying robots,” in Proc. IEEE Int. Conf. Robot. Autom., 2018, pp. 2502–2509. [^79]: M. Burri et al., “The EuRoC micro aerial vehicle datasets,” Int. J. Robot. Res., vol. 35, no. 10, pp. 1157–1163, 2016. [^80]: D. Schubert, T. Goll, N. Demmel, V. Usenko, J. Stückler, and D. Cremers, “The TUM VI benchmark for evaluating visual-inertial odometry,” in Proc. IEEE/RSJ Int. Conf. Intell. Robots Syst., 2018, pp. 1680–1687. [^81]: J. Sturm, N. Engelhard, F. Endres, W. Burgard, and D. Cremers, “A benchmark for the evaluation of RGB-D SLAM systems,” in Proc. IEEE/RSJ Int. Conf. Intell. Robots Syst., 2012, pp. 573–580. [^82]: D. Schubert, T. Goll, N. Demmel, V. Usenko, J. Stückler, and D. Cremers, “The TUM VI benchmark for evaluating visual-inertial odometry,” Mar. 2020, arXiv:1804.06120v3. [^83]: N. Yang, L. v. Stumberg, R. Wang, and D. Cremers, “D3VO: Deep depth, deep pose and deep uncertainty for monocular visual odometry,” in Proc. IEEE/CVF Conf. Comput. Vis. Pattern Recognit., 2020, pp. 1281–1292. ![Image](images/8ddfc4416747dc5e68ebe9e63712e1510bcae95ab26a77c7c4d1a7e8247a86bb.jpg) Carlos Campos received B.S. and M.S. degrees in industrial engineering (mention in robotics) from Universidad de Zaragoza, Zaragoza, Spain, in 2014 and 2016 respectively, and the M.S. degree in electronics (mention in signal processing) from INP-Toulouse, Toulouse, France, in 2017. He is currently working toward the Ph.D. degree with the I3A Robotics, Perception and Real-Time Group, Universidad de Zaragoza. His research interests include visual–inertial simultaneous localization and mapping for robotics and AR/VR applications. ![Image](images/87ac54936f03f8a997207dbe4ad2f78b9943809c4db0784956b09de06eab1346.jpg) Richard Elvira received the bachelor's degree in informatics engineering (mention in computing) in 2016 and the master's degree in biomedical engineering (mention in information and communication technologies in biomedical engineering) in 2018 from Universidad de Zaragoza, Zaragoza, Spain, where he is currently working toward the Ph.D. degree at the I3A Robotics, Perception and Real-Time Group. His research interests include real-time visual SLAM and place recognition in rigid environments. ![Image](images/88964389de7553075341a1f0611b219cd600f74d8190d3f73089bfd09d419bea.jpg) Juan J. Gómez Rodríguez (Graduate Student Member, IEEE) received the bachelor's degree in informatics engineering (mention in computing) in 2018 and master's degree in biomedical engineering (mention in information and communication technologies in biomedical engineering) in 2019 from Universidad de Zaragoza, Zaragoza, Spain, where he is currently working toward the Ph.D. degree at the I3A Robotics, Perception and Real-Time Group. His research interests include real-time visual SLAM for both rigid and deformable environments. --- ![Image](images/02725fe845ed862ef4dd1f4826f75a143cecf511ef7695fa9e470f71491b3ab9.jpg) J. M. Martínez Montiel (Member, IEEE) was born in Arnedo, Spain, in 1967. He received the M.S. and Ph.D. degrees in electrical engineering from Universidad de Zaragoza, Zaragoza, Spain, in 1992 and 1996, respectively. He is currently a Full Professor with the Departamento de Informática e Ingeniería de Sistemas, Universidad de Zaragoza, where he is in charge of perception and computer vision research grants and courses. His interests include real-time visual SLAM for rigid and nonrigid environments, and the transfer ence of this technology to robotic and nonrobotic application domains. Dr. Montiel has been awarded several Spanish MEC grants to fund research with the University of Oxford, Oxford, U.K., and Imperial College London, London, U.K. He has received several awards, including the 2015 King-Sun Fu Memorial IEEE Transactions on Robotics Best Paper Award. Since 2020, he has been coordinating the EU FET EndoMapper grant to bring visual SLAM to intracorporeal medical scenes. ![Image](images/4c609f5265a6cc6c24593d20cb4d2e486ca551f67d7c51e272ddd8cf8324dbbc.jpg) Juan D. Tardós (Senior Member, IEEE) was born in Huesca, Spain, in 1961. He received the M.S. and Ph.D. degrees in electrical engineering from the University of Zaragoza, Zaragoza, Spain, in 1985 and 1991, respectively. 他目前是萨拉戈萨大学计算机与系统工程系的全职教授，负责教授机器人学、计算机视觉和人工智能相关课程。他的研究方向包括SLAM、感知技术和移动机器人。 Tardós博士因发表关于单目SLAM系统ORB-SLAM的论文，荣获2015年IEEE机器人学报King-Sun Fu纪念最佳论文奖。