一文详解ORB-SLAM3-技术圈

点击上方“3D视觉工坊”，选择“星标”

干货第一时间送达

摘要

ORB-SLAM3是一个支持视觉、视觉加惯导、混合地图的SLAM系统，可以在单目，双目和RGB-D相机上利用针孔或者鱼眼模型运行。

他是第一个基于特征的紧耦合的VIO系统，仅依赖于最大后验估计(包括IMU在初始化时)。这样一个系统的效果就是：不管是在大场景还是小场景，室内还是室外都能鲁棒实时的运行，在精度上相比于上一版提升了2到5倍。本文的第二个创新点是根据改进recall的新的重定位模块来构建的混合地图，因为这个模块他可以让ORB-SLAM3在特征不是很好的场景中长期运行：当里程计失败的时候，系统会重新构建地图并将这个地图和原来构建的地图对齐。和那些仅利用最新的几帧数据的里程计相比，ORB-SLAM3是第一个能够在所有算法阶段重用所有先前信息的系统。这样的机制就可以在BA的时候用有共视关系的关键帧，即使两帧在时间相差很远，或者来自原来的建图过程。

这个系统在EuRoC数据集上达到了平均3.6cm的精度，在TUM-VI这种利用手持设备快速移动的数据集(AR/VR场景)上达到了9mm的精度。作者已经开源了代码：https://github.com/UZ-SLAMLab/ORB_SLAM3

一、介绍

SLAM建图的最大优势在于，它允许在BA中匹配并使用执行三种数据关联的先前观测值：

短期的数据关联：在最新的几秒中匹配地图元素。就像是VO中做的一样，丢掉那些已经看不到的帧，这会导致有累计的漂移。
中期的数据关联：匹配相机累计误差小的地图，这也可以用在BA中，当系统在已经建好的地图中运行的时候可以达到零漂移。
长期的数据关联：利用场景重识别来匹配当前的观测和先前的观测，不用管累计误差而且即使跟踪失败也可以实现,长期的匹配可以利用位姿图优化重新设置漂移，为了更准确也可以利用BA。这是SLAM在大场景中精度保证的关键。

这是第一个可能对短期、中期、长期数据进行数据关联的视和视觉惯导的系统。在已知地图的环境中可以没有漂移的运行，其中混合地图数据关联－这个可以保证我们进行地图匹配和进行BA优化，这也达到了一个目的：构建一个地图，然后可以在地图中进行精确的定位。

主要贡献：

一个单目和双目的视觉惯导SLAM系统：全部依赖于MAP(最后后验概率估计)，即使是在IMU初始化的时候。
高召回率的场景重识别算法:DBoW2需要匹配三个连续的关键帧，太慢了。作者的方法是：候选的关键帧第一次就进行几何一致性检测，然后利用三个共视的关键帧进行局部的一致性检验，这种策略提升了召回率，并简化了数据关联，从而提高了地图准确性，但计算成本变高。
第一个可以解决纯视觉或者视觉惯导的完整的混合地图的SLAM系统。在单目或者双目的系统中,Atlas代表的是一系列不连续的地图，而且可以把他们应用到所有的建图过程中：场景重识别、相机重定位、闭环检测和精确的地图融合。这就允许地图是在不同的时间构建的(增量的SLAM系统)，纯视觉的Atlas是参考的2019年IROS的一篇文章：ORBSLAM-atlas: a robust and accurate multi-map system，本文又添加了视觉惯导的混合地图系统来实现场景重识别。
抽象的相机表示：使SLAM系统与所使用的相机模型无关。并允许通过提供其投影，非投影和Jacobian函数来添加新模型我们提供了针孔和鱼眼模型的实现。

二、相关工作

表一中展示位姿估计和数据关联的工作。

视觉SLAM

MonoSLAM:第一个基于EKF和ShiTomasi特征的SLAM系统
PTAM:分离tracking和mapping、基于关键帧
LSD-SLAM:构建大场景的半稠密地图，但是没有对地图进行优化，精度低
SVO检测FAST特征，利用直接法跟踪特征，利用重投影误差模型来优化位姿，但是只有短期的数据关联，限制了他的精度。
DSO在检测不到特征点的场景也可以得到精准的相机定位，利用7个关键帧的局部光度误差的BA来优化位姿，利用逆深度来存储点。

视觉惯导系统

MSCKF:基于EKF的利用特征边缘化来简化计算(状态向量中没有地图点)
OKVIS:第一个紧耦合的基于关键帧优化的VIO系统
ROVIO利用EKF的光度误差
快速的IMU初始化方法：Closed-form solution of visual-inertial structure from motionSimultaneous state initialization and gyroscope bias calibration in visual inertial aided navigation
VINS->VINS-Fusion->Kimera
VI-DSO:初始化需要20-30s

三、系统概述

ORB-SLAM3是基于ORB-SLAM2和ORB-SLAM-VI构建的系统，他可以在纯视觉或者视觉惯导的系统中鲁棒的运行（单目、双目和RGB-D利用针孔或者鱼眼模型，你也可以自己定义模型）。

Atlas是一个由一系列离散的地图组成的混合地图。这里会维护一个active map来定位来的新的关键帧，local mapping线程不断的优化更新这个地图。Atlas里面还有non-active地图。这个系统基于词袋模型给系统编码用于重定位、闭环检测和地图融合。
跟踪线程处理传感器信息、实时的计算当前帧和active map的位姿，最小化匹配特征点的重投影误差，这个线程还有一个关键帧筛选的过程。在VIO的模式下，机体的速度和IMU的bias利用惯导的参差来=进行优化。当跟踪线程跟丢的时候，跟踪线程尝试在所有的Alats中完成当前帧的重定位，如股票重定位成功，跟踪就被拉回来了，否则过一段时间activate就会重新存储为non-active，一个新的active map就会被初始化。
局部建图线程添加关键帧和点到active map中，删除多余的帧并使用视觉或者视觉惯导的BA来优化地图，这些都是在一个局部的滑窗中做的。除此之外，在有惯导的情况下，IMU的参数是利用MAP估计来初始化和优化。
闭环检测和地图融合线程在active map和整个Atlas中检测相同的区域。如果相同的区域是在active中，就会执行闭环的过程，如果属于不同的map，他们就会被融合为一个地图。在闭环纠正后，一个全局的BA在一个线程中被触发来优化地图。

四、相机模型

提供了针孔模型和鱼眼模型。系统中把相机模型单独的抽象出来(重投影和反投影方程，Jacobian方程等)。这就允许我们在系统中使用自己的相机模型。在抽象的过程中也遇到了一些困难，在下边讨论。

A.重定位

ORB-SLAM解决重定位是用的Epnp；但是他是基于一个标定好的针孔相机模型的，为了继续我们的工作，我们采用独立于相机的ML-pnp算法可以完全从相机模型中解耦，因为他利用投影光线作为输入。相机模型只需要提供一个从像素传递到投影光线的反投影函数，以便能够使用重定位。

B.未矫正的双目SLAM

很多的SLAM系统都假设双目相机已经矫正了(两幅图像都转换为使用相同焦距的针孔投影，图像平面共面，并与水平极线对齐，这样，通过观察另一幅图像中的同一行，可以很容易地匹配其中一幅图像中的特征)。然而，对立体图像进行校正的假设有很大的局限性，在许多应用中不适合也不可行。例如矫正一个鱼眼相机需要需要很严重的裁剪。这就失去了广角相机大视角，快速建图、处理遮挡鲁棒的优点。因此本文的系统不依赖于图像矫正，而是把双目相机看成两个单目相机：

有一个恒定的SE(3)变换
两幅图像有部分相同的观测

这些约束在我们三角化新的路标点和进行BA优化的时候很有效率。SLAM估计6自由度刚体位姿，其参考系可以位于其中一个摄像机中，也可以位于IMU传感器中，根据刚体位姿表示摄像机。我们可以利用双目初始化第一次看到的路标点。双目还有很多重叠的区域，我们把他看成单目相机来使用。

视觉惯导SLAM

ORB-SLAM-VI是第一个有能力地图重用的视觉惯导的系统，但是他只能基于单目的针孔模型，初始化很慢，在本文，系统中使用快速精准的IMU初始化，通过了一个开源的SLAM库利用针孔或者鱼眼模型快速的完成单目惯导或者双目惯导的初始化。

B.IMU初始化

初始化的目的是为了给惯导的变量提供良好的初始值：机体速度、重力方向还有IMU的偏置。VI-DSO尝试不进行初始化，直接利用BA来进行优化导致初始化长达30s。在本文的工作中我们提出了基于三个关键insights的快速精准的初始化方法：

纯单目SLAM可以提供非常精确的初始地图，但是不知道尺度。所以可以加入Imu获得尺度信息。
Scale drift-aware large scale monocular SLAM证明当尺度被明确地表示为一个优化变量，而不是使用BA的隐式表示时，它收敛得更快。
在IMU初始化过程中忽略传感器的不确定性会产生较大的不可预测的误差(Fast and robust initialization for visual-inertial SLAM_2019ICRA)

所以我们需要考虑初始化的过程中传感器的不确定性，把IMU初始化看成一个MAP的问题，分为三个步骤：

纯视觉的MAP估计：在ORB-SLAM中初始化纯单目相机仅用了2s，以4HZ的速度插入关键帧。初始化后我们有按一定尺度比的由十个相机位姿和数百个3D点组成的地图。利用图2中纯视觉的模型进行BA优化。这些位姿被转换到机体坐标系下获得轨迹。

纯惯导的MAP估计：惯导的状态向量为：

一旦惯性优化完成，框架的姿态和速度和3D地图点将与估计的比例进行缩放，并旋转使z轴与估计的重力方向对齐。对偏差进行更新，并重复IMU预积分，以减少未来的线性化误差。

视觉惯导的MAP估计：一旦我们有了对视觉和惯导好的参数，我们可以执行一个联合的视觉惯导优化老进一步优化参数。这个图在2a中但是所有关键帧的bias都相同，而且先验信息也相同。我们在EuRoC数据集上进行的详尽的初始化实验表明，这种初始化非常有效，在2秒轨迹的情况下实现了5%的尺度误差。为了改进初始估计，在初始化后5秒和15秒进行视惯性BA，收敛到1%尺度误差，如第七节所示。在这些BAs之后，我们说地图已经成熟了，也就是说尺度、IMU参数和重力方向已经被准确地估计出来了，这种初始化方法比ORB-SLAM-VI和VI-DSO都好的多。通过将尺度因子固定为1，并将其从惯性优化变量中提取出来，我们可以很容易地将单目惯性初始化扩展到立体惯性，从而增强其收敛性。

C.跟踪和建图

跟踪和建图采用的是Visual-inertial monocular SLAM with map reuse中的方案。

跟踪线程只优化最新两帧的状态而地图点位置保持不变。
建图使用关键帧及其点的滑动窗口作为可优化变量，包括可共视的关键帧，但保持其固定。

在某些情况下，当慢速运动不能提供良好的惯性参数观测能力时，初始化可能无法在15秒内收敛到精确解。为了使系统更鲁棒，本文提出一个新的尺度优化的方法，这种方法基于改进的单惯导的优化方法，其中插入所有关键帧，但尺度和重力方向是唯一的估计参数(图2d)。在这种情况下，biases不变是不对的假设。可以使用每一帧的估计值来修正biases。这种优化的计算效率非常高，每10秒在局部建图线程中执行一次，直到建图超过100个关键帧，或者初始化超过75秒。

D.跟踪丢失的鲁棒提升

本文的VIO系统在系统跟踪少于15个点的时候就进入视觉跟踪失败的状态，然后执行：

短期的失败：利用IMU的读数估计位姿，把地图点投影到估计的相机位姿上，然后在一个大的image窗口中做匹配，匹配的结果包含在VIO优化中。在大多说情况下可以恢复视觉跟踪，但是如果超过5s还没有恢复。进入下一个状态。
长期的失败：重新进行视觉惯导的初始化构建一个地图，这个地图成为active地图。

地图融合和闭环检测

在图像帧和active地图间建立的短期和中期的数据关联是在跟踪和建图线程利用地图点投影到估计的位姿上，然后在小的窗口中匹配得到匹配关系。为了长期的数据关联来进行重定位和闭环检测，ORB-SLAM是用的词袋模型。与跟踪不同，位置识别是利用DBoW2使用其词袋矢量构建关键帧的数据库，并且给定查询图像能够根据其词袋有效地提供最相似的关键帧。仅使用第一个候选帧，原始DBoW2查询就可以达到50-80％的精度和召回率。为了防止假阳性的观测，DBoW2实施时间和几何一致性检查，将工作点的精度提高到100％，召回率达到30-40％。至关重要的是，时间一致性检查至少在3个关键帧期间延迟了位置识别。当尝试在我们的Atlas系统中使用它时，我们发现这种延迟和较低的召回率经常是在相同或不同地图的重复区域中造成的。在本文的工作中我们提出了一个在长期和混合地图数据关联的时候有改进召回率的新的场景重识别的算法。当建图线程筛选出一个关键帧，场景重识别就尝试检测在Atlas中的关键帧进行匹配。如果匹配的关键帧在active地图中，这就确定了一个闭环；否则就是混合地图的数据关联，执行匹配的地图和当前的active地图的融合。这个方法的第二个特点是一旦当前帧和匹配的地图帧的位姿估计出来了，我们就在匹配帧和其在共视图中的相邻帧构建一个局部的窗口。在此窗口中，我们集中搜索中期数据关联，从而提高了闭环检测和地图融合的准确性。

A.场景重识别

为了达到高的召回率，每个新来的关键帧都会利用DBoW2数据库在Altas中检测几个相似的关键帧。为了达到百分之百的准确度。每个候选帧都要进行几何验证。几何检验包括检验图像窗口中是否有和地图点描述子匹配的上的ORB特征点(汉明距离)。如果有几个匹配候选帧，首先去除不正确的匹配，还需要检验和排第二的候选帧比较距离比。场景重识别的过程如下：

DBoW2候选关键帧：利用active关键帧在Altas的DBoW2数据库中检索三个候选的相似帧，包括Ka的共视帧，我们把匹配帧称为Km。
局部窗口：对与每一个Km我们定义一个局部的窗口包括Km和他最好的共视帧，以及他们观测到的所有的地图点。DBoW2直接索引提供了Ka和局部窗口关键帧的特征点匹配，我们可以得到2D-2D和3D-3D的匹配关系。
3D对齐变换：利用RANSAC->Tam来来更好的对齐局部窗口中的Km和Ka的地图点。在单目或者单目惯导的系统中，如果地图还没初始化成功，我们计算Sim3变换，如果初始化成功我们计算SE3变换。在计算两种变换的时候我们都利用3点的Horn算法来找到Tam。如果计算出来的Tam把Ka中的点变换到Km中重投影误差小于某个阈值，就把这个匹配当成正确的。拥有最多选票的假设被选择（前提是该假设满足一定的阈值）。
匹配优化：将局部窗口中的所有地图点通过Tam进行转换，以找到更多与Ka中的关键点匹配的地图点。同时也要把Ka转换到局部窗口中找到与之的匹配点。利用所有的匹配点来计算Tam，利用双向带有鲁棒核函数的重投影误差的非线性优化来优化Tam，如果优化后inliers数量超过一定的阈值，就会在一个更小的图像窗口中进行第二段的匹配和非线性优化。
在三个共视关键帧中验证：为了避免假阳性的结果，DBoW2在连续三个关键帧中决定是否触发、延时或丢失位置重识别。这个方法的关键是：在大多数情况下我们需要验证的信息已经都在地图中了，为了验证位置重识别，我们在active地图中和Ka共视的两个关键帧（共视帧中共视的地图点超过一定的阈值。如果没有找到这样的共视关键帧，这种验证就会在新来的关键帧中进行，但是不需要再次启动词袋。验证一直持续，直到有三个关键帧验证Tam，或者连续两个关键帧验证失败。
重力方向验证：在有IMU的情况下，如果active地图成熟，我们已经估计了Tam。我们就要确定两帧的pitch和roll角低于某个阈值来确定是不是进行位置重识别。

B.视觉地图融合

当位置重识别成功的时候产生active地图Ma中的关键帧Ka和一个在Altas地图Mm中的关键帧Km利用Tma$进行混合地图的数据关联来融合地图。这里需要注意的是Altas中的地图的信息需要被tracking线程重用来避免地图复制。这里把Ma地图放到以Mm为参考的地图中，由于Ma中有很多的组件，所有融合可能需要一段时间。地图融合分成两部分：首先，在一个由Ka和Km相邻点定义的连接窗口中进行合并，在第二阶段，通过位姿图优化将校正传播到合并图的其余部分。合并算法的具体步骤为:

连接窗口集合：连接窗口包括Ka和他的共视的关键帧，Km和他的共视关键帧，以及所有他们观测到的地图点。利用Tma把Ma中的地图点和关键帧和Mm对齐然后再放到连接窗口中。
融合地图：Ma和Mm融合组成一个新的active地图。为了删除重复的点，在Mm中的关键帧中主动搜索Ma的匹配点，对于每个匹配都删除Ma中的点，Mm中的点保存下来了所有的观测。利用中期的点关联来更新共视和基本图添加Mm和Ma的连接边。
连接窗口的BA：在连接窗口中把所有来自Mm和Ma中的关键帧进行局部优化。为了保证滑窗中的关键帧数，Mm的共视帧保持固定。一旦优化完成，连接窗口中的所有帧都可以进行跟踪，来快速准确的重用地图Mm。
位姿图优化：利用整个融合地图的本质图进行位姿图优化，保持连接区域的关键帧固定。这个优化将修正从连接窗口传播到地图的其余部分。

C.视觉惯导地图融合

视觉-惯性合并算法的步骤与纯视觉合并相似。改进纯视觉中的步骤1和步骤3，以更好地利用惯性信息:

VI连接窗口集合：如果active成熟了，在把Ma包含在连接窗口前把地图Ma利用Tma(SE3)进行变换。如果active没成熟，利用Tma(Sim3)来对齐Ma。
VI连接窗口BA：active关键帧Ka以及最新的五个关键帧的位姿，速度和偏置是可以优化的。这些变量通过IMU预积分来进行关联。对于Mm，我们也对Km和他时序上的五个相邻帧的位姿、速度和偏置进行优化，如下图所示：

对于Mm，包含了紧靠局部窗口前的关键帧，但是固定的;而对于Ma，包含了类似的关键帧，但其姿态仍然是可优化的。所有关键帧所能看到的所有点，以及观察这些点的关键帧姿态也得到了优化。利用重投影误差将关键帧和关键点关联起来。

D.闭环检测

闭环检测和地图融合类似，但是是在场景重识别的两个关键帧都在active地图下。根据匹配的关键帧组成连接窗口，重复的点被检测融合然后叜共视图和本质图中构建新的边。然后进行位姿图优化来传播闭环校正的结果到剩余的地图中。最后一步是全局BA，在考虑闭环检测中期和长期的匹配后得到MAP估计。在视觉惯导的情况下，只有在关键帧数量低于阈值的时候才进行以避免巨大的运算成本。

参考文献

[1] Campos C , Elvira R , Rodríguez, Juan J. Gómez, et al. ORB-SLAM3: An Accurate Open-Source Library for Visual, Visual-Inertial and Multi-Map SLAM[J]. 2020.

[2] Campos C , Montiel, José M. M, Tardós, Juan D. Inertial-Only Optimization for Visual-Inertial Initialization[J]. 2020.

[3] https://blog.csdn.net/qq_40878688/article/details/105291348(ORBSLAM-Atlas a robust and accurate multi-map system介绍)

本文仅做学术分享，如有侵权，请联系删文。

下载1

在「3D视觉工坊」公众号后台回复：3D视觉，即可下载 3D视觉相关资料干货，涉及相机标定、三维重建、立体视觉、SLAM、深度学习、点云后处理、多视图几何等方向。

下载2

在「3D视觉工坊」公众号后台回复：3D视觉github资源汇总，即可下载包括结构光、标定源码、缺陷检测源码、深度估计与深度补全源码、点云处理相关源码、立体匹配源码、单目、双目3D检测、基于点云的3D检测、6D姿态估计汇总等。

下载3

在「3D视觉工坊」公众号后台回复：相机标定，即可下载独家相机标定学习课件与视频网址；后台回复：立体匹配，即可下载独家立体匹配学习课件与视频网址。

重磅！3DCVer-学术论文写作投稿交流群已成立
扫码添加小助手微信，可申请加入3D视觉工坊-学术论文写作与投稿微信交流群，旨在交流顶会、顶刊、SCI、EI等写作与投稿事宜。

同时也可申请加入我们的细分方向交流群，目前主要有3D视觉、CV&深度学习、SLAM、三维重建、点云后处理、自动驾驶、CV入门、三维测量、VR/AR、3D人脸识别、医疗影像、缺陷检测、行人重识别、目标跟踪、视觉产品落地、视觉竞赛、车牌识别、硬件选型、学术交流、求职交流等微信群。

一定要备注：研究方向+学校/公司+昵称，例如：”3D视觉 + 上海交大 + 静静“。请按照格式备注，可快速被通过且邀请进群。原创投稿也请联系。
▲长按加微信群或投稿
▲长按关注公众号

3D视觉从入门到精通知识星球：针对3D视觉领域的知识点汇总、入门进阶学习路线、最新paper分享、疑问解答四个方面进行深耕，更有各类大厂的算法工程人员进行技术指导。与此同时，星球将联合知名企业发布3D视觉相关算法开发岗位以及项目对接信息，打造成集技术与就业为一体的铁杆粉丝聚集区，近2000星球成员为创造更好的AI世界共同进步，知识星球入口：

一文详解ORB-SLAM3

摘要