《论文笔记》ORB-SLAM3：An Accurate Open-Source Library for Visual, Visual-Inertial and Multi-Map SLAM》

《ORB-SLAM3: An Accurate Open-Source Library for Visual, Visual-Inertial and Multi-Map SLAM》

 

作者：Carlos Campos∗, Richard Elvira∗, Juan J. Gómez Rodr´ ıguez, José M.M. Montiel and Juan D. Tardós

单位：西班牙萨拉戈撒大学

期刊：目前只挂到了arXiv上

时间：2020

摘要：

        本文介绍了ORB-SLAM3系统，这是第一个能够用单目、双目和RGB-D相机执行视觉、视觉惯导和多地图SLAM的系统，支持针孔和鱼眼相机模型。

        第一个创新点是提出了一种基于特征点法的视觉惯导紧耦合SLAM系统，它完全依赖于最大后验概率(MAP)估计对状态量进行求解，即使在IMU初始化阶段也是如此。其结果是，该系统能够在大/小/室内/室外各种环境中实时可靠地运行，并且比以前的方法精度提高2到5倍。

        第二个主要的创新是一个多地图系统，它依赖于一种新的高召回率的场景识别算法。多亏了它，ORB-SLAM3能够在视觉信息不佳的情况下运行很长一段时间：当它迷路时，它重新开始建图并在当经过之前走过的地点时与原来的地图无缝融合。与仅使用最后几秒信息的VO系统相比，ORB-SLAM3是第一个能够在所有算法阶段重用所有先前信息的系统。这允许在BA中包括提供高视差观测以提高精确度的共视关键帧，即使它们在时间上相隔很远，或者它们来自先前阶段的地图。

        我们的实验表明，在所有传感器配置中，ORBSLAM3与文献中可用的最好的系统一样鲁棒，而且精度要高得多。值得注意的是，我们的双目-惯导SLAM在EuRoc无人机上的平均精度为3.6厘米，在TUM-VI数据集(代表AR/VR场景)中的室内快速手持运动下的平均精度为9毫米。我们将源代码公之于众。

 

介绍:

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

        短期的数据关联：在最新的几秒中匹配地图元素。就像是VO中做的一样，丢掉那些已经看不到的帧，这会导致有累计的漂移。

        中期的数据关联：匹配相机累计误差小的地图，这也可以用在BA中，当系统在已经建好的地图中运行的时候可以达到零漂移。

        长期的数据关联：利用场景重识别来匹配当前的观测和先前的观测，不用管累计误差而且即使跟踪失败也可以实现,长期的匹配可以利用位姿图优化重新设置漂移，为了更准确也可以利用BA。这是SLAM在大场景中精度保证的关键。

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

本文主要贡献：

        1）一个单目和双目的视觉惯导SLAM系统：全部依赖于MAP(最后后验概率估计)，即使是在IMU初始化的时候。

       2） 高召回率的场景重识别算法:DBoW2需要匹配三个连续的关键帧，太慢了。作者的方法是：候选的关键帧第一次就进行几何一致性检测，然后利用三个共视的关键帧进行局部的一致性检验，这种策略提升了召回率，并简化了数据关联，从而提高了地图准确性，但计算成本变高。

        3）第一个可以解决纯视觉或者视觉惯导的完整的混合地图的SLAM系统。在单目或者双目的系统中,Atlas代表的是一系列不连续的地图，而且可以把他们应用到所有的建图过程中：场景重识别、相机重定位、闭环检测和精确的地图融合。这就允许地图是在不同的时间构建的(增量的SLAM系统)，纯视觉的Atlas是参考的2019年IROS的一篇文章：ORBSLAM-atlas: a robust and accurate multi-map system，本文又添加了视觉惯导的混合地图系统来实现场景重识别。

        4）抽象的相机表示：使SLAM系统与所使用的相机模型无关。并允许通过提供其投影，非投影和Jacobian函数来添加新模型。我们提供了针孔和鱼眼模型的实现。

 

相关工作：

视觉SLAM：

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

视觉惯导系统：

• MSCKF:基于EKF的利用特征边缘化来简化计算(状态向量中没有地图点)
• OKVIS:第一个紧耦合的基于关键帧优化的VIO系统
• ROVIO利用EKF的光度误差
• VINS->VINS-Fusion->Kimera
• VI-DSO:初始化需要20-30s

多地图SLAM：

• MOARSLAM：多设备协同建图，但主要关注软件架构，精度不够
• CCM-SLAM：基于ORB-SLAM的分布式多机建图系统，主要把精力放在了克服带宽限制和分布式过程上（我们是放在精度和鲁棒性上），在EuRoc数据集上我们表现更好
• ORB-SLAMM：基于ORB-SLAM2，但只是把子图作为单独的实体（而我们进行了无缝地图融合，建立了一个更精确的全局地图）
• VINS-Mono：有回环检测和多地图能力的VO。实验证明，在EuRoc数据集上，我们的单段单目+imu建图是VINS-Mono精度的2.6倍，多段精确度达到了3.2倍。

 

具体实现：

一.系统概述

        图中画圈的部分是相比于orb2有区别的，主要包括以下几个部分：

1.1 Atlas

        Atlas就是一种多地图的表示形式，多地图由不相连的地图组成。分为active map和non-active map。其中active map被Tracking线程用作定位，被local mapping线程优化和插入新关键帧。（个人理解，这个active map就是当前的局部地图，也就是orb2中的map）

1.2 Tracking thread

        与ORB-SLAM2相比主要是加入了imu信息。该线程用来处理传感器信息，计算当前帧相对于active map的位姿以及最小化匹配到的地图点的重投影误差（在Track local map中针对某一帧做位姿BA）。该线程根据跟踪结果，决定是否将当前帧判定为关键帧。在VI模式下，机体的速度以及IMU的bias通过优化惯导残差被估计。当系统追踪丢失后，会触发重定位模式，即当前帧在所有的Altas中的map进行重定位；若重定位成功，当前帧恢复追踪状态；否则，经过一段时间（超过5秒），当前的active map会被存储为non-active map，同时开启新的建图过程。

1.3 local Mapping thread

        向active map中新增/删减/优化关键帧以及地图点，上述的操作是通过维护一个靠近当前帧的关键帧局部窗口进行实现。另外，在VI模式下，IMU的参数被初始化然后被该线程通过本文提出的最大后验概率估计技术进行求解。

1.4 Loop and map merging thread

        该线程检测active map与整个Atlas是否有共同的区域，若共同的区域同属于active map，此时进行闭环矫正；若共同的区域属于不同的map，那么这些map就会被融合（融合为1个）,并变为active map。（疑问：当前地图与地图集的公共区域，怎么会属于另一个地图？个人理解：重定位失败时，会重新建图，应该是这种情况下才会？）回环矫正之后，全局BA被一个独立的线程执行（不影响系统的实时性），用来修正地图。

二.相机模型

        ORB-SLAM假设所有系统组件都符合针孔模型。本文的目标是：系统可以通过提供相应的摄像头模块来使用任何型号的摄像头。（疑问：ORB2不是已经可以使用双目/RGB-D了吗？或许本文的这块工作能替代orb2中的输入预处理模块，使得无论何种相机输入都不用预处理了？）

2.1 重定位

        ORB-SLAM解决重定位问题：基于EPnP算法建立PnP求解器进行解算位姿，其中所有的公式都假设相机为标定好的针孔相机。本文使用了一种独立于相机模型的求解器：Maximum Likelihood Perspective-n-Point algorithm (MLPnP)（最大后验PnP算法）。该算法实现了相机模型与求解算法的解耦。相机模型只需要提供一个 从像素到投影射线的反投影函数（？） 作为输入。

2.2 非矫正双目SLAM

        几乎所有的SLAM系统都假设双目图像是已经被矫正的，这里的矫正是指：使用相同的焦距将两个图像转换为针孔投影，且成像平面在同一平面上，同时与水平对极线对齐，从而可以通过查看图像中的同一行进行特征匹配。然而，这个假设是及其严格的，在很多应用中通常难以满足或不可用。例如，矫正（两个摄像头距离很大的）双目相机或双目鱼眼镜头将需要对其拍摄的图像进行严格的裁剪，从而失去了大视场角的优势：更快的环境建图速度和更强的抗遮挡能力。  

        本文设计的系统并不依赖于图像的矫正，而是将双目设备看作有如下约束的两个单目相机：

        1) 两个单目相机之间存在一个固有的SE(3)变换（即双目外参）；

        2) 可选，两个单目相机之间存在共视；

        上述两个约束可以用来有效地估计地图尺度。按照这种思路，ORB-SLAM3可估计6DOF刚体机体位姿，先在双目与 IMU 中的一个上做，其他的传感器通过刚体位姿变换即可。另外，处于双目共同观测区域的路标点通过双目进行三角化（仅在首次看到时）以恢复路标点的尺度，不在共视区域的路标点就正常的multi-view进行三角化即可。

 

三、VI SLAM

3.1 原理

        构造imu残差、重投影误差等。

        给定k+1个关键帧及其状态{S0…Sk}，l个3D点及其状态{x0…xl-1}，}VI优化问题最终表示为：

其中， Kj是能观察3D路标点 j 的关键帧的集合。重投影误差中的鲁棒核函数ρ用来减少误匹配的影响，在惯导残差中不需要，因惯导不会存在错误联系。

        优化问题用因子图表示如下：

 

3.2 IMU初始化

        初始化的目的是为了给惯导的变量提供良好的初始值：机体速度、重力方向还有IMU的偏置。在本文的工作中我们提出了基于三个关键insights的快速精准的初始化方法：

        1） 纯单目SLAM可以提供非常精确的初始地图，但不知尺度。先解决视觉问题将增强IMU的初始化。

        2）当尺度被明确地表示为一个优化变量，而不是使用BA的隐式表示时，它收敛得更快。

        3）在IMU初始化过程中忽略传感器的不确定性会产生较大的不可预测的误差

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

        1）纯视觉MAP估计：采用ORB-SLAM经典框架纯视觉初始化流程，即进行单目SLAM。按照关键帧速率4Hz持续运行2s，然后我们可以得到按比例缩放的地图，包括10个关键帧以及上百个地图点，然后通过Visual-Only BA进行优化。因子图如下图所示，实际上即为优化特征点重投影误差。可以得到优化后的轨迹 ，其中上划线表示按比例缩放的变量（即尺度未定）。

        2）纯惯导MAP估计：这一步的目的是获得IMU参数最优估计。利用前述单目视觉SLAM初始化后稳定运行的数据（包含10个关键帧的地图），以及这些关键帧之间的IMU测量，包括：纯视觉问题的尺度因子、重力方向、IMU传感器偏置参数（bias，加速度计和陀螺仪各3个轴，共6个参数）、关键帧的无尺度速度。这些变量，放在一起构成状态向量，构建优化问题求解。这个优化问题的因子图表示为下图，不包含视觉残差，而是多了一个先验残差项 用来约束IMU的bias需要接近于0。

 

        具体推导过程见下图（摘自3D视觉工坊公众号）。一旦惯性优化完成，帧位姿、速度和3D路标点将按照估计的尺度因子进行缩放，并进行旋转使z轴与估计的重力方向对齐。对偏置进行更新，并重复计算IMU预积分，以减少未来的线性化误差。

 

        3）视觉-惯导MAP估计：一旦视觉以及IMU有了较好的估计后，进行一个VI联合优化进一步对这些参数进行精化，优化因子图如下。注意所有关键帧的bias都相同，而且先验信息也相同（？）。

        通过将尺度因子固定为1，并将其从惯性优化变量中提取出来，我们可以很容易地将单目惯性初始化扩展到双目惯性，从而增强其收敛性（个人理解是因为双目本身就有尺度了，不再需要imu的尺度信息了。但增强其收敛性是？）。

3.3 追踪和建图

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

        1） 跟踪线程只优化最新两帧的状态而地图点位置保持不变。

        2）建图使用 关键帧及其点 的滑动窗口作为可优化变量，包括可共视的关键帧，但保持其固定。

        在某些情况下，当慢速运动不能提供良好的惯性参数观测时，初始化可能无法在15秒内收敛到精确解。为了使系统更鲁棒，本文基于改进的纯惯导优化方法，提出一个新的尺度优化的方法：所有插入的关键帧都参与优化，但优化量只有重力方向以及尺度因子(图2d)。在这种情况下bias为常数的假设就不再成立。我们使用每一帧的估计值来修正biases。这种优化的计算效率非常高，在Local Mapping线程每隔10秒执行一次，直到Map中有100个关键帧或者从初始化起已经过了75秒。

3.4 对跟踪丢失的鲁棒性

        在纯视觉SLAM或VO系统中，短暂的摄像机遮挡和快速的运动会导致视觉元素失去轨迹，导致系统丢失。当跟踪点数小于15时，我们的系统进入视觉迷失状态，之后分两个阶段：

        短期丢失：利用IMU的读数估计位姿，把地图点投影到估计的相机位姿上，然后在一个大的image窗口中做匹配，匹配的结果包含在VI优化中。在大多说情况下可以恢复视觉跟踪，但如果超过5s还没有恢复，进入下一个状态。

        长期丢失：重新进行视觉惯导的初始化构建一个地图，这个地图成为active地图。

四.地图融合和回环检测

        短期以及中期数据关联可以通过Tracking以及Local Mapping进行完成。而对于长期的数据关联，可通过重定位以及闭环检测实现。

        ORB-SLAM采用的基于视觉词袋场景识别的重定位，若候选关键帧只有1个，召回率为50-80%。为了应对假阳性的出现，算法使用了时间一致性校验以及几何校验，这两种手段能够使精确率达到100%的同时召回率为30-40%。至关重要的是，时间一致性检测将使场景识别滞后至少3个关键帧，同时召回率较低，这都是目前存在的问题。

        为了应对这个问题，本文提出一种新的场景识别（召回率得到改善）以及多地图数据关联算法。一旦Local Mapping线程创建了关键帧，场景识别算法就会被激活并且寻找该帧在Atlas中的匹配。若匹配的关键帧在active map中，则进行闭环；否则，则进行多地图间的数据关联，即将active map与匹配的map进行融合。（本方法第二个创新点）一旦这个新的关键帧与匹配地图间的相对位姿被计算出，就定义一个局部窗口，这个局部窗口包括匹配到的关键帧以及这个关键帧的共视关键帧。在这个局部窗口中，我们会寻找中期数据关联，以提高闭环以及地图融合的精度。这个改进使得ORB-SLAM3比ORB-SLAM2具有更高的精度。

4.1 场景识别

        为了达到高的召回率，每个新来的关键帧都会利用DBoW2数据库在Altas中检测几个相似的关键帧。为了达到百分之百的准确度，每个候选帧都要进行几何验证。几何检验包括使用它们之间的汉明距离的阈值，去检查图像窗口中是否有一个ORB关键点，其描述子与路标点的ORB描述子匹配。如果有几个匹配候选帧，为了放弃模糊匹配，我们检查与第二接近匹配的距离比。场景识别的过程如下：

        1）DBoW2候选关键帧：利用active关键帧在Altas的DBoW2数据库中检索三个候选的相似帧，但不包括与Ka可共视的关键帧，我们把每个候选匹配帧称为Km。

        2）局部窗口：对与每一个Km，我们定义一个局部的窗口，包括Km和其最好的共视帧，以及他们观测到的所有的地图点。DBoW2直接索引提供了Ka和局部窗口关键帧中的关键点之间的一组假定匹配。这些2D-2D匹配，也有相应地图点之间的3D-3D匹配。

        3）3D对齐变换：利用RANSAC计算出Tam，来更好的对齐Km局部窗口中的和Ka的地图点。

        在单目或者单目惯导的系统中，如果地图还没初始化成功，我们计算Sim(3)变换（相似变换群，有尺度）；如果初始化成功，我们计算SE(3)变换（特殊欧式群，无尺度）。

        在计算两种变换的时候，我们都使用Horn算法[使用一组最小的3D - 3D匹配]，为 Tam找到每个假设。这种假定匹配，如果计算出来的Tam把Ka中的点变换到Km中重投影误差小于某个阈值，就给该假设一张正选票，选择正选票最高的假设（前提是还要该假设满足一定的阈值）。

        4）匹配优化：将局部窗口中的所有地图点通过Tam进行转换，以找到更多与Ka中的关键点匹配的地图点。查找也可以反向，也可以把Ka转换到局部窗口中找到与之的匹配点。利用找到的所有匹配来进行非线性优化修正Tam，其中目标函数为双向重投影误差（也利用Huber函数对伪匹配提供鲁棒性）。如果优化后inliers数量超过一定的阈值，就会在一个更小的图像窗口中进行第二段的匹配和非线性优化。

        5）在三个共视关键帧中验证：为了避免假阳性的结果，DBoW2在连续三个关键帧中等待位置识别触发，从而开启、延迟或错过位置识别。这个方法的关键是：在大多数情况下我们需要验证的信息已经都在地图中了，为了验证位置重识别，我们在active地图中搜索和Ka共视的两个关键帧（共视帧中共视的地图点超过一定的阈值）。如果没有找到这样的共视关键帧，这种验证就会在新来的关键帧中进行，但是不需要再次启动词袋。验证一直持续，直到有三个关键帧验证Tam，或者连续两个关键帧验证失败。

        6）重力方向验证：在VI的情况下，如果active地图成熟，我们已经估计了Tam属于SE(3)。我们就要进一步检验两帧的pitch和roll角低于某个阈值，以确定是不是接受位置识别假设。

4.2 视觉地图融合

        当位置识别成功，即active地图Ma中的关键帧Ka和一个在Altas地图Mm中的关键帧Km利用Tma完成数据关联时，融合地图。方式是：把Ma地图放到以Mm为参考的地图中。地图融合分成两部分：首先，在一个由Ka和Km邻域定义的连接窗口中进行合并，在第二阶段，通过位姿图优化将校正传播到合并图的其余部分。合并算法的具体步骤为:

        1）连接窗口集合：连接窗口包括Ka和他的共视的关键帧，Km和他的共视关键帧，以及所有他们观测到的地图点。把Ma中的地图点和关键帧用Tma转换，以和Mm对齐，然后再放到连接窗口中。

        2）融合地图：Ma和Mm融合组成一个新的active地图。为了删除重复的点，在Mm中的关键帧中主动搜索Ma的匹配点，对于每个匹配都删除Ma中的点，并把其观测转给Mm中的匹配点。利用中期点关联，添加Mm和Ma的连接边，来更新共视图和本质图。

        3）连接窗口的BA：在连接窗口中把所有来自Mm和Ma中的关键帧进行局部优化。为了保证滑窗中的关键帧数，Mm的共视帧保持固定（为什么要固定？为什么Ma的不用？）。一旦优化完成，连接窗口中的所有关键帧都可以用于跟踪，实现快速准确的重用地图Mm。

        4）位姿图优化：利用整个融合地图的本质图进行位姿图优化，保持连接区域的关键帧固定。这个优化将修正从连接窗口传播到地图的其余部分。

4.3 VI地图融合

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

        1）VI连接窗口集合：如果active成熟了，在把Ma包含在连接窗口前把地图Ma利用Tma∈SE(3)进行变换。如果active没成熟，利用Tma∈Sim(3)来对齐Ma。

        3) VI连接窗口BA：active关键帧Ka及其最新的5个关键帧的位姿、速度和偏置都是可优化的。这些变量通过IMU预积分进行关联。对于Mm进行类似的处理，包括Km和它的5个时间邻域的位姿、速度和偏置，如图3b所示。对于Mm，包含了紧靠 局部窗口前的关键帧，但是固定的；而对于Ma ，包含了类似的关键帧，但其姿态仍然是可优化的（见因子图最左侧方框，为什么？）。所有关键帧所能看到的所有点，以及观察这些点的关键帧姿态也得到了优化。利用重投影误差将关键帧和关键点关联起来。因子图见下方，有重投影误差项(蓝色方块)、IMU预积分项(黄色方块)和偏置随机游走(紫色方块)。

4.4 回环

       闭环检测和地图融合类似，但是是在场景重识别的两个关键帧都在active地图中。

       1）根据匹配的关键帧组成连接窗口；

       2）重复的点被检测融合然后在共视图和本质图中构建新的边。

       3）然后进行位姿图优化来传播闭环校正的结果到剩余的地图中。

       4）最后一步是全局BA，在考虑闭环检测中期和长期的匹配后得到MAP估计。在视觉-惯导的情况下，只有在关键帧数量低于阈值的时候才进行全局BA，以避免巨大的运算成本。

 

 

结论：

        在ORB-SLAM、ORB-SLAM2以及ORB-SLAM-VI的基础上，我们提出了ORB-SLAM3，这是一个最完整的开放源码库，用于视觉、视觉惯性和多会话SLAM，支持单目、立体、RGB-D、针孔和鱼眼摄像机。除了集成库本身，我们的主要贡献是快速准确的IMU初始化技术和多会话地图合并功能，这些技术依赖于一种新的位置识别技术，提高了召回率，使ORB-SLAM3非常适合于实际应用中的长期和大规模SLAM。

        我们的实验结果表明，ORB-SLAM3是第一个能够有效利用短期、中期、长期和多地图数据关联的视觉和视觉惯性系统，达到了现有系统无法达到的精度水平。我们的结果还表明，在准确性方面，使用所有这些类型的数据关联的能力压倒了其他选择，例如使用直接方法而不是特征点法，或者对局部BA执行关键帧边缘化，而不是像我们那样假设一组外部静态关键帧。

        关于稳健性，直接方法在低纹理环境中可以更健壮，但仅限于短期[27]和中期[31]数据关联。另一方面，匹配要素描述符成功地解决了长期和多地图数据关联，但在跟踪方面似乎不如使用光度信息的Lucas-Kanade。一个有趣的研究方向可能是开发适合于这四个数据关联问题的光度学技术。我们目前正在探索利用人体内的内窥镜图像构建地图的想法。

        在四种不同的传感器配置中，双目惯导SLAM提供了最可靠，最准确的解决方案。此外，惯性传感器允许以IMU速率估算姿势，IMU速率比帧速率高几个数量级，这可能也会在某些领域发挥优势（如AR/MR等领域）。对于设备体积/成本/处理要求受限的应用，可以选择使用单目惯导方案，精度与鲁棒性并不会下降多少。只需记住，在勘探过程中纯粹的旋转是不能估计深度的。

        在慢速运动或没有滚转、俯仰的应用中，例如平坦区域中的汽车，IMU传感器可能难以初始化。在这些情况下，推荐使用双目SLAM。或者，使用CNN进行单目深度恢复的最新研究成果为单目SLAM恢复尺度提供了良好的前景，但是需要保证在同样的环境中对网络进行了训练。