ORB-SLAM3论文解读

╰+哭是因爲堅強的太久メ 2022-12-19 11:42 619阅读 0赞

**目录**

ORB-SLAM3论文解读

摘要

I. 介绍

主要的创新点

数据关联模型

II. 相关工作

A. Visual SLAM

B. Visual-Inertial SLAM

C. Multi-Map SLAM

III. 系统整体

Atlas地图集

跟踪线程

局部建图线程

回环和地图合并线程

IV. 相机模型

A. 相机模型带来的重定位问题

B. 相机模型带来的双目问题

V. 视觉惯性SLAM部分

A.基本原理

B. IMU初始化

1) 纯视觉最大后验估计(Vision-only MAP Estimation)

2) 纯惯性最大后验估计(Inertial-only MAP Estimation)

3) 视觉惯性联合最大后验估计

C. 跟踪和建图

D.追踪损失的稳健性

VI. 地图合并和回环检测

A. 场景识别(Place Recognition)

B. 纯视觉地图合并

地图缝合

地图优化

C. 视觉惯性地图合并

D. 闭环

VII. 实验和结论

参考

--------------------

# ORB-SLAM3论文解读 #

ORB-SLAM1和2的作者**Juan**已经毕业了，其工作仍然由同一课题组的**Carlos**继续完成并撰写论文和代码，这次论文同样对应着开源代码，youtube演示效果惊人，很多场景下无需大量修改即可直接使用，可谓是工作党的KPI，学生党的SCI。  
废话少说，直入主题。ORB-SLAM3与前两次作品相比，引入和**视觉惯性**和**多地图模式**的SLAM。其算法支持的传感器也有单目、双目、RGBD相机等。在相机的成像模型中 ，采用了针孔、**鱼眼相机**模型（见摘要的第一段）。  
代码传送门：链接: [https://github.com/UZ-SLAMLab/ORB\_SLAM3][https_github.com_UZ-SLAMLab_ORB_SLAM3].

参考ORB\_SLAM2代码注释：

*  [https://github.com/electech6/ORB\_SLAM2\_detailed\_comments][https_github.com_electech6_ORB_SLAM2_detailed_comments]
 *  [https://github.com/skyjhyp11/ORB\_SLAM2\_detailed\_comments][https_github.com_skyjhyp11_ORB_SLAM2_detailed_comments]

论文传送门：链接: [https://arxiv.org/pdf/2007.11898.pdf][https_arxiv.org_pdf_2007.11898.pdf].

# 摘要 #

本文介绍了ORB-SLAM3系统，这是第一个能够使用单眼、立体和RGB-D摄像机进行视觉、视觉惯性和多地图SLAM的系统，使用针孔镜头和鱼眼镜头模型。

*  第一个主要的新颖性是一个基于特征的紧耦合的视觉惯性SLAM系统，它完全依赖**最大后验(MAP)估计**，即使在**IMU初始化阶段**也是如此。 其结果是，该系统可以在大大小小的、室内和室外环境中实时运行，并且比以前的方法**精度高2到5倍**。
 *  第二个主要的创新是一个**多地图系统**，它依赖于一种新的地点识别方法和改进的召回率recall。 多亏了它，ORB-SLAM3能够在长时间的视觉信息不完善的情况下持续进行:当它丢失时，它会启动一个新的地图，并在重新访问之前的地图时，无缝地与之前的地图合并。

与只使用最后几秒钟的信息的视觉测程系统相比，ORB-SLAM3是第一个能够在所有算法阶段**重用之前所有信息**的系统。 这允许包括在束调整共可见关键帧，提供高视差观测提高精度，即使他们是在时间上广泛分离或者即使他们来自以前的映射会话。

我们的实验表明，在所有传感器配置中，ORBSLAM3与文献中可用的最佳系统一样健壮，而且更加精确。 值得注意的是，我们的立体惯性SLAM在EuRoC无人机上实现了**3.6 cm**的平均精度，在tumi - vi数据集(AR/VR场景的代表)的房间中实现了**9 mm**的快速手持运动。为了社区的利益，我们公开源代码。

# I. 介绍 #

## 主要的创新点 ##

大佬的自吹自擂部分中也提到，他们的主要创新点有两点：

1.  **“完全”基于最大后验估计MAP的VI-SLAM** ，无论在初始化阶段还是运行阶段，都采用了MAP进行状态估计，因此ORB-SLAM3在室内外、大小场景中鲁棒性很好，且精确度是其他方法的2~5倍；如文中所讲，本文的IMU和视觉的组合系统是extremely robust的。
2.  **多地图系统**，当定位丢失即lost时，ORB-SLAM3会自动建立一个新的小地图，并在revisit两张地图上的**同一地点**的时候进行地图的seamlessly merge，因此，这一算法能够使用不仅仅几帧之前的信息，而是运用了全局的信息，能够在bundle adjustmen中利用**视差较大**的帧来增加BA求解的准确性（因为当视察较小时，求解不准确，且优化容易进入局部极值）。这种方法里对同一特征的观测可能在时间上的间隔较大，因为我们对其上一次的观测甚至可能出现在之前一张小地图中（中间过程里lost了至少一次）。

## 数据关联模型 ##

1.  **short-term**，在短程模式下，路标点一旦退出我们的视野，我们将这个路标点丢弃，其在优化时计算效率高，但存在累计误差，即使重新回到同一地点，也无法实现重定位或者回环检测，位姿图无法构成闭环。
2.  **mid-term**，在中程模式下，和短程模式类似，累计误差只有在重新回到地图中已知区域才有可能实现重定位，建立一个封闭的环状位姿图以消除累计误差。这与大多数SLAM系统中的方法类似。这一方法能够保证局部定位的**准确性**。
3.  **long-term**，长期模型中，采用**place recognition**的方式，例如使用基于**Dbow词袋库**的回环检测与重定位，来处理累计误差和跟踪丢失，直接将当前位姿和之前一记录位姿对齐。并通过图优化的方式消除（平摊）累计误差。这一方法能够保证大环境重定位的**稳定性**。在运用词袋库的回环检测模型中，需要被检测帧具有**时间一致性**，即需要连续三帧均能触发词袋库召回匹配。在此之后，还需要检查是否具有**几何一致性**，因此，召回的准确性得到了较大提升，然而其召回率却难以得到保证，这种方法的计算过于缓慢，难以将之前建立的小地图充分应用起来。因此ORB-SLAM3先进行几何一致性的检测，随后再对**局部一致性**（即local window中连续三个存在共视的帧的匹配性）进行检测。其中，时间一致性将连续三个关键帧进行比较，而此处局部一致性将连续三个共视关键帧进行了检查。由于其存在共视，关键点的匹配会更多，所以其准确性和召回率得到了提升，而计算量会少许增大。

# II. 相关工作 #

## A. Visual SLAM ##

//TODO

## B. Visual-Inertial SLAM ##

视觉传感器和惯性传感器的组合，在纹理较差，运动模糊和遮挡时提供了鲁棒性，并且在单眼系统的情况下，可以观察到尺度 （scale）。紧密耦合方法的研究可以追溯到MSCKF \[33\]，其中通过**特征边缘化**避免了特征数量上的EKF二次成本。最初的系统在\[34\]中得到完善，并在\[35\]，\[36\]中扩展到双目（stereo）。第一个基于关键帧和束调整（bundle adjustment）的紧密耦合的视觉里程计系统是OKVIS \[38\] \[39\]，它也能够使用单眼和立体视觉。尽管这些系统依赖于**特征**，但ROVIO \[41\]，\[42\]使用直接数据关联的方法，在EFK中引入了**光度误差**。

**ORB-SLAM-VI** \[4\]首次提出了一种 能够在短期，中期和长期数据关联中，重用地图视觉惯性SLAM系统，并且将其用于基于IMU的预积分(preintegration)\[61\]，\[62\]的 local visual-inertial BA过程中。但是，其IMU初始化技术太慢，耗时15秒，这损害了鲁棒性和准确性。在\[63\]，\[64\]中提出了一种更快的初始化技术，该技术基于一种封闭形式的解决方案，可以共同获取比例尺，重力，加速度计偏差和初始速度以及视觉特征深度。至关重要的是，它们忽略了IMU噪声属性，并最小化了空间点的3D误差，而不是其重投影误差，这是基于特征的计算机视觉的黄金标准。我们以前的工作\[65\]表明，这会导致较大的不可预测的错误。

VINS-Mono \[7\]是一种非常准确和强大的单眼惯性里程计系统，使用**DBoW2**和**4 DoF姿态图优化**以及**地图合并**实现闭环。使用**Lucas-Kanade跟踪器**执行特征跟踪，它比描述符匹配要健壮得多。在**VINS-Fusion** \[44\]中，它已扩展到双目和双目惯导（stereo-inertial）。 Kimera \[8\]是一种新颖的杰出的度量-语义映射系统，但其度量部分包括立体惯性测距，DBoW2闭环和姿态图优化，实现了与VINS-Fusion相似的准确性。最新的BASALT \[47\]是一种立体视觉惯性里程计系统，该系统从视觉惯性里程计中提取非线性因素以在BA中使用它们，并关闭与ORB功能匹配的回路，从而实现了非常好的精度。

最近，VI-DSO \[66\]将DSO扩展到视觉惯性里程表。他们提出了一种捆绑调整(BA)，将惯性观测与所选高梯度像素中的光度误差结合在一起，从而提供了非常好的准确性。由于成功利用了高梯度像素中的信息，因此具有较差纹理的场景区域中的鲁棒性也得到了增强。他们的初始化方法依赖于视觉惯性BA，并花费20-30秒的时间在1％的比例误差内收敛。

在这项工作中，我们**以ORB-SLAM-VI为基础**，并将其扩展为双目惯性SLAM。我们提出了一种基于**最大后验（MAP）估计**的新颖**快速初始化**方法，该方法适当考虑了视觉和惯性传感器的不确定性，并在2秒内估计出5％的真实标度，在15秒内收敛到1％的标度误差秒。上面讨论的所有其他系统都是视觉惯性测距法，其中一些系统通过闭环进行了扩展，并且缺乏使用中期数据关联的能力。我们相信，这与我们快速精确的初始化一起，是即使在没有循环的序列中，系统始终如一地获得更高精度的关键。

## C. Multi-Map SLAM ##

//TODO

# III. 系统整体 #

这部分就直接按照论文中的部分进行依次解读 (fan)(yi)。  
![ORB-SLAM3系统流程图][ORB-SLAM3]

## Atlas地图集 ##

整个ORB3最牛逼的地方就在这里了，最早在IROS2019的一篇文章，即参考文献\[9\]中提到。在ORB3中，Atlas地图集由一系列不连续的小地图构成，并能够无缝连接，实现重定位、回环检测、地点识别等功能。当每一个新的视觉帧进入流程，跟踪线程立即追踪并定位新一帧的位姿。地图集本身也随着时间逐步优化且会将新的视觉帧经过挑选作为关键帧。在Atlas中，**DBoW2**被用于**重定位、回环检测、地图合并**。地图的状态也分为active和non-active两个状态，在跟踪线程的介绍里将会详细介绍这一机制。

## 跟踪线程 ##

跟踪线程被用来根据当前的active地图，实时地跟踪最新一帧的位置，利用**最小化重投影误差**的方式实现位姿的最大后验估计MAP。决定新一帧是否作为关键帧加入地图也是在这个线程中完成的。跟踪线程接受IMU、Frame输入，IMU 被预积分处理，而Frame被提取ORB特征，整体跟踪流程中规中矩，没什么值得一提的地方。  
当定位跟踪失败时：首先会在所有的Atlas地图集中搜索当前位置的匹配，如果成功，则将当前的地图设置为non-active，而将在Atlas搜索到的地图设置为active，并继续在active的地图上进行局部跟踪。如果在全图搜索失败，则在一定时间后，当前的active地图会强行被设置为non-active，新的小地图将被构建，并设置为active。这样，就的地图的正确性不会被局部定位的失败影响，新旧地图之间的位姿变换关系不确定性也有希望被之后的共视减小或者消除。

## 局部建图线程 ##

局部建图线程中规中矩，只有active的地图被添加关键帧，在添加关键帧的时候，重复的关键点被移除。VI-Bundle Adjustment在当前被插入帧前后窗口内被用来提升地图的质量。但是，在Local mapping框图中的后三项，ORB3加入了一些改进，用了他们的novel MAP，后续会介绍。

## 回环和地图合并线程 ##

当每一个关键帧被插入，地点识别都被启动，这一关键帧被和整个Atlas地图集关键帧进行比较，如果检测到两关键帧是同一地点，分为两种情况：若被召回帧是当前的active地图中的一部分，则需要进行loop correction回环矫正，在回环矫正之后，Full BA在另一个线程中被悄悄进行，全局位姿图得到优化，整体的地图一致性得到了提升；若被召回帧是属于一个non-active的map，两个地图则会被合并成一个地图并将大地图设置为当前的active地图。这一过程和跟踪并行，不会影响跟踪的实时性。

# **IV. 相机模型** #

## A. 相机模型带来的重定位问题 ##

相机模型这块，ORB3将系统中所有与相机模型相关的部分（投影和反投影公式以及其雅可比）提取了出来，让相机模型成为了一个独立的模块儿，以便随时替换其他的相机模型。在重定位中，由于需要使用PnP（Perspective n points）算法，所以需要一个标定好的针孔模型，因此，其他相机模型不再适用，在此我们选择MLPnP（Maximum Likelihood PnP）来作为求解算法。这种算法直接应用光线投影作为输入，只需要使用者提供一个将像素反投影为光束的unprojection公式以及相应像素点的坐标就可以求解相机的位姿。

## B. 相机模型带来的双目问题 ##

在针孔相机双目视觉中，在寻找特征点匹配时，可以直接在另一个相机的同一行像素上进行搜索，实现快速的匹配，但这一加速技巧在鱼眼相机或者两个不同的针孔相机上难以实现。因此我们将一个双目相机看作是两个具有固定位姿变化SE3的单目相机，且两个相机帧之间存在较大的共视区域。两个单目被分别加入BA优化。为了利用双目的优势，如果两个相机之间存在共视区域，这一区域的特征点可以在第一次被看见的时候被三角化，且尺度的不确定性被双目SE3的尺度信息所消除。在没有共视的区域，特征点依旧按照多视图几何的方式进行三角化。

# V. 视觉惯性SLAM部分 #

ORB-SLAM-VI\[4\]是第一个具有地图重用能力的真正视觉惯性SLAM系统。然而，它

*  仅限于针孔单目摄像机，
 *  初始化太慢，
 *  在一些具有挑战性的场景中失败。

在这项工作中，我们建立在ORB-SLAM-VI上，

*  提供了一种快速准确的IMU初始化技术，以及一个开源的SLAM库，
 *  可以使用 monocular-inertial 和 stereo-inertial SLAM，
 *  带有针孔和鱼眼相机。

## A.基本原理 ##

关于预积分和视觉重投影误差的部分，分别将两个残差的**信息矩阵**加权**马氏距离**进行求和最小化即是最终的损失函数。在优化时，调整路标点的**三维坐标**以及**机器人状态（R，p，v，q，b）**使目标损失函数最小。

在纯视觉SLAM中，估计状态仅包括当前**相机姿态**，而在**视觉惯性SLAM**中，需要计算其他变量。这些包含主体body姿态![20210330134846662.png][]和在世界坐标系下的**速度vi**，以及**陀螺仪的**偏差![20210330134950922.png][]和**加速度计的偏差**和![20210330135005292.png][]——假定根据布朗运动而演化。这导致状态向量的表示方式为：

![20210330135043940.png][]

对于视觉惯性SLAM，我们按照\[61\]中开发的理论，并在\[62\]中的流形上，对连续的 **视觉帧i** 和 **i+1** 之间的IMU测量值进行了**预积分**（preintegrate）。我们获得预积分的**旋转**，**速度**和**位置测量值**，分别表示为![20210330135203148.png][]，以及整个测量矢量的**信息矩阵**![20210330135305198.png][]。给定这些预积分项，和状态Si和Si + 1，我们从\[62\]中采用**惯性残差**![20210330135331596.png][]的定义：

![20210330135351568.png][]

连同惯性残差，我们还使用帧i和3D点j在位置xj之间的**重投影误差rij**：

![20210330135441748.png][]

其中![20210330135458965.png][]是对应相机模型的投影函数，uij是图像i上点j的观测值，具有信息矩阵Σij。![20210330135540371.png][]代表从**机身IMU到摄像机（左或右）的刚性转换**，这从校准中可知。 ⊕是SE(3)组在![20210330135628988.png][]元素上的转换操作。

结合惯性和视觉残差项，可以将视觉惯性SLAM视为**基于关键帧的最小化问题**\[39\]。给定一组![202103301358435.png][]个关键帧及其状态![20210330135756279.png][]，以及一组 ![2021033013592245.png][]个3D点及其状态![20210330135808925.png][]，视觉惯性优化问题可以表示为：  
![20210330135942288.png][]  
其中![20210330135959872.png][]是观察3D点j的关键帧集合。

这种优化可以概括为图2a所示的因子图。请注意，对于重投影误差，我们使用鲁棒的Huber核![20210330140103850.png][]来减少虚假匹配的影响，而对于惯性残差则不需要，因为不存在缺失关联。这种优化需要在跟踪和映射过程中进行调整，以提高效率，但更重要的是，它需要**良好的初始种子**才能收敛到准确的解决方案。

![watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3NoeWpoeXAxMQ_size_16_color_FFFFFF_t_70][]

## **B. IMU初始化** ##

此步骤的目标是为**惯性变量**获取良好的初始值：**机体速度v，重力方向g**和**IMU偏差bias**。诸如VI-DSO \[46\]之类的某些系统尝试从头解决视觉惯性BA，避开特定的初始化过程，获得惯性参数的缓慢收敛（最多30秒）**。**

ORB3中的初始化主要在accuracy和efficiency上进行了改进，以获得一个准确的IMU状态初始值。主要有**三点改进**的insight：

*  1. 纯单目SLAM可以提供非常准确的初始地图\[2\]，但其主要问题是比例尺未知。**先解决仅视觉问题**将会促进(enhance)IMU的初始化。
 *  2. 如\[77\]所示，当尺度**显式地表示为优化变量**，而不是使用BA的隐式表示时，规模收敛更快。（尺度应该作为一个单独的优化变量而不是作为一个隐含变量存在于逆深度、相机位移等包含尺度的量中，这样的优化具有更快的收敛性。）
 *  3. 在IMU初始化过程中忽略**传感器不确定性**，会产生较大的不可预测的错误\[65\]（因此在初始化的时候应该考虑传感器的uncertainty）。

因此，在适当考虑传感器不确定性的情况下，我们将IMU初始化表示为MAP估计问题，分为三个步骤：

### 1) 纯视觉最大后验估计(Vision-only MAP Estimation) ###

我们初始化纯单眼SLAM \[2\]，并在2秒钟内运行它，以4Hz频率插入关键帧。在此期间之后，我们得到了一个由k = 10个相机姿态和数百个点组成的up-to-scale（不含尺度的）的最新比例地图，此时该地图已使用Visual-Only BA进行了优化（图2b）。将这些姿态转换为body参考系，获得轨迹![20210330143120416.png][]，其中的**横条**表示up-to-scale的变量。

### 2) 纯惯性最大后验估计(Inertial-only MAP Estimation) ###

在这一步中，我们的目标是在**MAP估计**的意义上，仅使用![20210330143653873.png][]和这些关键帧之间的**惯性测量值**来获得**惯性变量**的最佳估计。这些惯性变量可以堆叠在仅**惯性状态向量**中：

![20210330143741970.png][]  
其中![20210330143801427.png][]是仅**视觉解决方案的比例因子scale**，Rwg是**重力方向，**用两个角度表示，因此世界参考中的重力矢量为![20210330143942551.png][]，![20210330144020748.png][]为G的重力大小**；**，![20210330144127918.png][]是在初始化期间假设加速度计和陀螺仪恒定的偏置值，![2021033014422887.png][]是从第一个关键帧到最后一个关键帧的up-to-scale的body速度，最初是根据![20210330144302197.png][]估算的。在这一点上，我们仅考虑**惯性测量量**的集合![20210330144411757.png][]，因此，我们可以表述为一个MAP估计问题，其中要最大化的**后验分布**是：  
![20210330144817388.png][]  
其中![20210330144915769.png][]代表似然（在某状态值y下观测值I的概率），![20210330145001853.png][]为先验（某状态出现的概率）。考虑到测量的独立性，仅惯性MAP估计问题可写为（其中公式就是贝叶斯公式的去掉分母的版本，后验=似然\*先验。其中花体的I就是IMU测量值，我们将公式搞进最大似然argmax有：）  
![20210330145027642.png][]  
取**负对数**，并假设**IMU预积分**和**先验分布**概率密度函数，服从高斯误差分布，最终会导致优化问题，argmax变argmin：  
![20210330145904392.png][]  
如图2c所示，此优化与等式4的不同之处在于：这里不包括视觉残差，但包含施加给IMU bias的、接近于零的先验残差![20210330150044342.png][]，它的协方差由IMU的特性给出。（rp就是先验，后面那个就是针对每一次IMU预积分的求和。这个y函数的最小值就是我们所需要估计的参数的**最大后验估计**）。

当我们在流形中进行优化时，我们需要定义一个回缩 retraction \[62\]以在优化过程中**更新重力方向**估计（在优化上为了保证旋转矩阵还在流形上，我们整了指数变换来更新这一李群）：  
![20210330150213263.png][]  
为了确保**比例因子**在优化过程中保持正值，我们将其更新定义为（用指数更新尺度）：  
![20210330150225519.png][]

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

这一下纯IMU部分就完全ok了。值得一提的是，**先验**选取中，IMU的bias设置为0，使之不会偏离0太远，且**初始的协方差矩阵**按照IMU的datasheet设定。当完成了IMU的MAP估计后，纯视觉的尺度被对齐，并且重力方向上也与Rwg旋转后的方向对齐，Bias值也得到了一个合理的估计。

### 3) 视觉惯性联合最大后验估计 ###

一旦我们对惯性和视觉参数有了良好的估计，就可以执行**联合视觉惯性优化**以进一步完善解决方案。该优化可以表示为图2a——对于所有关键帧，具有共同的偏差；与仅惯性步骤相比，包括相同的先验信息。

这就没啥好说的了，和其他VIO算法一样，做一个VIO的联合BA，如**图2a**所示。但bias是不变的。且IMU方面的先验是和纯惯性估计的先验是一样的。

我们在EuRoC数据集\[6\]上进行的详尽初始化实验表明，这种初始化非常有效，在2秒的轨迹上实现了5％的比例误差。为了改善初始估计，在初始化后的5到15秒执行视觉惯性BA，收敛到1％的比例误差，如VII节所示。在获得这些BA之后，我们说地图已经成熟，这意味着比例，IMU参数和重力方向已得到准确估计。(初始化在2s时，尺度误差已经收敛到5%以内，**此时我们就称初始化完成了**。但VI联合MAP优化是在延时5-15s之后才运行的，为的是保证初始化的效率。VI联合优化后，地图才被称为时mature的，scale的误差可以收敛到1%以内。)

我们的初始化比解决一组代数方程\[63\]-\[65\]的联合初始化方法更加**准确**，并且比ORB-SLAM-VI \[4\]中使用的初始化（需要15秒才能获得第一个标度）要**快**得多。而且快于VI-DSO \[66\]中使用的估计--会产生巨大的比例误差，并且需要20-30秒才能收敛到1％的误差。

通过将**比例因子**固定为1并将其从仅用于惯性的优化变量中删除，可以轻松地将**monocular-inertial初始化**扩展为**stereo-inertial初始化**，从而增强其收敛性。

## C. 跟踪和建图 ##

对于跟踪和建图，我们采用**\[4\]**中提出的方案。跟踪解决了简化的视觉惯性优化，其中仅优化了最后两帧的状态，而地图点保持固定。对于建图，尝试解决等式4的整个优化问题对于大型地图而言将是棘手的。我们使用关键帧及其点的滑动窗口作为可优化变量，包括可见的关键帧，但保持其固定不变。

在某些特定情况下，当慢动作不能提供惯性参数的良好可观察性时，初始化可能仅在15秒内无法收敛到精确解。为了在这种情况下获得鲁棒性，我们基于改进的仅惯性优化提出了一种新颖的比例尺细化技术，其中包括所有插入的关键帧，但比例尺和重力方向是唯一要估计的参数（图2d）。请注意，在那种情况下，恒定偏差的假设将是不正确的。取而代之的是，我们使用每个帧的估计值，然后对其进行修复。这种优化的计算效率非常高，它每十秒钟在本地映射线程中执行一次，直到映射自初始化以来已超过100个关键帧或超过75秒为止。

## D.追踪损失的稳健性 ##

在纯视觉SLAM或VO系统中，暂时的相机遮挡和快速运动会导致失去视觉元素的跟踪，从而使系统迷路。 ORB-SLAM率先使用了基于**词袋位置识别**的快速重定位技术，但事实证明它们不足以解决EuRoC数据集中的困难序列\[3\]。当跟踪少于15个点的地图时，我们的视觉惯性系统会进入视觉丢失状态，并在两个阶段实现鲁棒性：

*  短期丢失：根据IMU读数估算当前的body状态，并在估算的摄像机姿态中投影地图点，并在大图像窗口中搜索匹配项。所得的匹配项包括在视觉惯性优化中。在大多数情况下，这可以恢复视觉跟踪。否则，在5秒钟后，我们进入下一个阶段。
 *  长期丢失：如上所述，一个新的视觉惯性图将被初始化，并成为活动图(active map)。

[闭门即深山][Link 1]**评价：跟踪**方面和ORB-SLAM2一样，没有什么特别的地方，实际上跟踪问题就是最新两帧的VI-BA优化问题，而**t-1帧路标点**是fix的。在**建图**时，需要在滑动窗口和共视图里做一个范围更大的BA，以获得更高的地图准确性，参与建图BA优化的帧由于计算量限制不能把所有帧加进来，而是仅仅包含窗口内的帧and与当前帧有共同观测的帧。有的时候，初始化并不理想，这时候我们把头100帧或者前75s的keyframe全部纳入初始化优化阶段，用于获得一个较好的重力旋转向量和尺度。实际上这个过程就是“**初始化不好的时候就让他初始化久一点**”，但这一过程中只优化图2d框框外的变量。当跟踪过程中小于15个可辨认特征点时，进入**visually lost**即视觉**丢失状态**。并采用应对策略：从IMU估计当前位姿，并扩大滑动窗口的长度，将IMU获得的位姿以及当前帧特征点投影到这个**大一点的滑动窗口**中去寻找匹配。如果这一过程5s后仍没有成功重定位，则**开启新的小地图**，并将新图切换为active状态。从而进入咱们的下一章节：

# VI. 地图合并和回环检测 #

作者又说了，这块儿是他最牛逼的地方，牛逼之处主要有两块儿，第一点是我们前面所说的回环检测与地图合并机制，除此之外，他们用在共视图local window上的一致性检测，代替了时间上的一致性检测，理论上讲，graph上的距离范围内的一致性确实比单纯时间上的一致性要有道理地多，实验效果反正是又好又快:-)

## A. 场景识别(Place Recognition) ##

首先，我们用DBoW操作一番得到三个Ka的匹配候选帧Km，然后将Km、Km的**n**个最佳共视帧、所有这些帧看到的路标点放进local window里，通过词袋库的正向索引得到各个匹配点的2D匹配关系，从而建立匹配点的3D匹配关系。然后需要我们计算Km到Ka的一个相似变换Sim(3)若尺度不确定。计算这一变换时，我们采用Horn algorithm（霍恩法），同时还要做一个RANSAC操作：为得到候选变换，我们把每个匹配的点对重投影误差计算出来，当小于一定阈值，就给这个变换投一票，采用民主共和的方式选出最好的位姿变换T。  
得到T后，由于前面是基于采样计算的位姿变换，没有用到全部的特征点匹配信息，我们需要用上所有信息refine一下这个位姿变换。我们在local window里找所有与这帧图像中特征点匹配的路标，并使用双向转移误差作为误差目标函数来优化这一位姿变换，还用一下Huber来增强一波稳定性。如果inlier的个数ok，我们再在更小的local window（减小前面的**n**）中开始第二次的引导匹配。为了验证这一结果，我们把后续进来的3帧都做一下检验：在Ka的active map中找两个共视帧，然后看这两帧与local windows的匹配点数目是否超过阈值。这一过程**持续三帧成功**或者**连续两帧失败**来作为验证成功失败的判断条件。最最最最后用IMU重力再验证一波pitch和roll才算最终通过考验，成为一个*place recognition*。

## B. 纯视觉地图合并 ##

地图合并主要分为缝合和优化两部分

### 地图缝合 ###

当我们发现了Ka和Km的匹配发生在**不同小地图**之间，就需要对他们进行缝合（wlding）操作。为此我们将Ka、Km两帧、Ka和Km所有共视帧、所有这些帧的特征点都放进**缝合窗口**（welding window）。在将地图Ma（最新的地图）里面的特征点放进窗口前，我们见所有特征点均通过Tma变换到Mm地图（被召回的地图）下。在地图缝合时，需要将两图中重复的特征点进行剔除：对Ma中的每个特征点都在Mm中寻找其匹配点，如果找到则把Ma中的该点去除，并将这次观测放入Mm中去。同时，共视图也应当进行更新。  
![在这里插入图片描述][watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3dlaXhpbl80NzY1ODc0Mw_size_16_color_FFFFFF_t_70]

### 地图优化 ###

缝合完之后需要进行优化，调整大的内部关系。优化分为两块：缝合优化和整体优化。首先我们进行缝合优化：我们将缝合窗口之外的部分进行固定，再在缝合窗口内部进行一个BA优化，到此我们的缝合后的地图就可以用于追踪新来的帧了。但是为了提升窗口内外的一致性，利用地图缝合减小整体的累计误差，我们需要进行整体的位姿图优化：在优化时，缝合窗口内被固定，窗口外采用essential graph进行优化。此时，回环矫正从缝合窗口传递向整个地图，缝合优化就完成了。

## C. 视觉惯性地图合并 ##

![在这里插入图片描述][watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3dlaXhpbl80NzY1ODc0Mw_size_16_color_FFFFFF_t_70 1]

和上一节讲的差不多，区别有两点：

1.  如果active的地图是mature的，一切照旧；如果不mature，说明尺度信息暂时不可靠，在缝合时，需要用相似变换Sim(3)变换而不是SE(3)将Ma中的点对齐到Mm。
2.  优化的变量和固定的变量如图所示，Easy

## D. 闭环 ##

# VII. 实验和结论 #

效果非常好 :-）  
结论非常硬 :-）

# 参考 #

*  [ORB-SLAM3论文解读][ORB-SLAM3 1]
 *  [论文翻译 | ORB-SLAM3：一个用于视觉、视觉惯性和多地图SLAM系统][_ ORB-SLAM3_SLAM]

[https_github.com_UZ-SLAMLab_ORB_SLAM3]: https://github.com/UZ-SLAMLab/ORB_SLAM3
[https_github.com_electech6_ORB_SLAM2_detailed_comments]: https://github.com/electech6/ORB_SLAM2_detailed_comments
[https_github.com_skyjhyp11_ORB_SLAM2_detailed_comments]: https://github.com/skyjhyp11/ORB_SLAM2_detailed_comments
[https_arxiv.org_pdf_2007.11898.pdf]: https://arxiv.org/pdf/2007.11898.pdf
[ORB-SLAM3]: /images/20221120/9446a8d731b944d29a8fad976e203656.png
[20210330134846662.png]: https://img-blog.csdnimg.cn/20210330134846662.png
[20210330134950922.png]: https://img-blog.csdnimg.cn/20210330134950922.png
[20210330135005292.png]: /images/20221120/f78328eae41a4c7e9266a9444f91524c.png
[20210330135043940.png]: /images/20221120/c7cd81ce82814dc09d1760411d066ad0.png
[20210330135203148.png]: https://img-blog.csdnimg.cn/20210330135203148.png
[20210330135305198.png]: https://img-blog.csdnimg.cn/20210330135305198.png
[20210330135331596.png]: /images/20221120/02fc758f8c734dbf801c61aa9d207634.png
[20210330135351568.png]: /images/20221120/7b7901be0e5a4e31a74e337bca96ea9f.png
[20210330135441748.png]: /images/20221120/c4fe8a9d71e24cdaab085a68adb6de9f.png
[20210330135458965.png]: https://img-blog.csdnimg.cn/20210330135458965.png
[20210330135540371.png]: https://img-blog.csdnimg.cn/20210330135540371.png
[20210330135628988.png]: /images/20221120/1b6c1fa190f245d5b0a8f4e03987f72d.png
[202103301358435.png]: https://img-blog.csdnimg.cn/202103301358435.png
[20210330135756279.png]: https://img-blog.csdnimg.cn/20210330135756279.png
[2021033013592245.png]: https://img-blog.csdnimg.cn/2021033013592245.png
[20210330135808925.png]: /images/20221120/52513246e7124e98a5b1247f6b9766ce.png
[20210330135942288.png]: /images/20221120/56907418860d4d5bb872ea690e53ff67.png
[20210330135959872.png]: /images/20221120/a7d656ff675c4acba34c21b84932d8d5.png
[20210330140103850.png]: /images/20221120/2e684e20296744698443250e2e775e8b.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3NoeWpoeXAxMQ_size_16_color_FFFFFF_t_70]: /images/20221120/6b4bba877e0749cfbdceabeaea0a725d.png
[20210330143120416.png]: /images/20221120/c061c4790fc34b67aa8e8298cc3e0d06.png
[20210330143653873.png]: /images/20221120/7a7f4ec9979d44d0839d1caf72d6ab61.png
[20210330143741970.png]: /images/20221120/34d006699f144bd0ab61064100d37c33.png
[20210330143801427.png]: https://img-blog.csdnimg.cn/20210330143801427.png
[20210330143942551.png]: https://img-blog.csdnimg.cn/20210330143942551.png
[20210330144020748.png]: https://img-blog.csdnimg.cn/20210330144020748.png
[20210330144127918.png]: https://img-blog.csdnimg.cn/20210330144127918.png
[2021033014422887.png]: https://img-blog.csdnimg.cn/2021033014422887.png
[20210330144302197.png]: https://img-blog.csdnimg.cn/20210330144302197.png
[20210330144411757.png]: /images/20221120/f92e475a50064385a9600fca0774cdca.png
[20210330144817388.png]: /images/20221120/e419e213b495460e8e0d1714a23af903.png
[20210330144915769.png]: https://img-blog.csdnimg.cn/20210330144915769.png
[20210330145001853.png]: /images/20221120/5b52ee2319374b13adca14d0a3c7a456.png
[20210330145027642.png]: /images/20221120/8ecd2c6d37fd43259c404e6845e17004.png
[20210330145904392.png]: /images/20221120/b2b0f76d9a824f0a8be1ad9f8e0bd748.png
[20210330150044342.png]: /images/20221120/e5b8c6b30457449e8912ecb5f508371b.png
[20210330150213263.png]: /images/20221120/9ace8ae42e044a839e14b1150b6b944c.png
[20210330150225519.png]: /images/20221120/80fe1c6f1ee24488aff19a8ec70a0db9.png
[Link 1]: https://blog.csdn.net/weixin_47658743
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3dlaXhpbl80NzY1ODc0Mw_size_16_color_FFFFFF_t_70]: /images/20221120/20a72210102542bab06e914552fa3b6a.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3dlaXhpbl80NzY1ODc0Mw_size_16_color_FFFFFF_t_70 1]: /images/20221120/c2f16a02c1d94ce1817fcfef6a6dad8c.png
[ORB-SLAM3 1]: https://blog.csdn.net/weixin_47658743/article/details/107604358
[_ ORB-SLAM3_SLAM]: https://blog.csdn.net/shyjhyp11/article/details/110621998