ORB-SLAM3理解

长期更新

概要

ORB_SLAM3提供了单目、双目以及RGB-D相机，在针孔和鱼眼模型上的VO、VIO以及多地图SLAM的实现。

第一个主要的创新是一个基于特征的紧密集成视觉惯性SLAM系统,它完全依赖于最大后验概率（MAP）估计,甚至在IMU初始化阶段也是如此。其在小型和大型,室内和室外环境中都能稳定地运行,并且比以前的方法精确2到5倍.

第二个主要的创新是一个多地图系统(multi-map),它依赖于一种新的提升了recall的位置识别方法。多亏了它，ORB-SLAM3能够在长时间的恶劣视觉信息下生存;当它丢失时，它会启动一个新的地图，当重新访问地图区域时，它将与以前的地图无缝地合并。

与只使用最后几秒信息的视觉里程计系统相比，ORB_SLAM3是第一个能够在所有算法阶段重用所有先验信息的系统。这允许包含BA调整共视关键帧,这些关键帧提供了高视差观测,提高了精度.

我们的实验表明,在所有的传感器配置中,ORB_SLAM3与文献中可用的最佳系统一样鲁棒，并且更精确.值得注意的是,我们的立体惯性SLAM在EuRoC无人机上的平均精度为3.6cm,在TUM-VI数据集(AR/VR场景的一个典型设置)中,快速手持移动时的平均精度为9mm,最后,我们开源了代码.

1. 概述

SLAM地图的最大优势是，它允许匹配和使用BA之前的观测数据执行三种类型的数据关联：

• 短期数据关联：匹配在最后几秒内获得的地图元素—这是被大多数 VO 系统所使用的数据关联，这会丢掉那些在视野外的环境元素，导致系统移动到了相同的位置后，会有连续的估计漂移。（比如用光流得到的相对位姿，用这个做 VO，绝逼产生很大的 drift）
• 中期数据关联：匹配相邻相机且累积漂移小的地图元素。与短期观测相比，这些可以在 BA 中匹配和使用，当系统在已构建地图的区域移动时，允许达到零漂移。这是我们的系统与环路检测的VO系统相比获得更好精度的关键。
• 长期数据关联：使用位置识别技术将观测结果与以前访问过的区域中的元素匹配：漂移累积(回环检测)、当前区域先前在断开连接的地图中绘制(地图合并)、跟踪丢失(重定位)。长期匹配可以重置漂移并使用姿态图(PG)优化校正地图，或使用更准确的BA。这是中、大型环状环境中SLAM精度的关键。

在该项目中，除了上述所说的三个数据关联方式外，进一步的提出了 多地图数据融合----这允许我们匹配和使用来自以前的地图会话的BA地图元素，实现SLAM系统的真正目标:构建一个地图，以便以后可以使用，以提供准确的定位。

ORB-SLAM 的新颖点：

• 一个单双目视觉惯导 SLAM 系统： 全部依赖于 MAP(最大后验估计) ，即使实在 IMU 初始化相位的时候。
• 高召回率位置识别： 许多最近的视觉 SLAM 和 VO 系统使用词袋库做地点识别。DBoW2 需要时间一致性，在检查几何一致性之前将三个连续的关键帧匹配到同一区域，以牺牲召回率来提高精度----结果是系统在闭环和之前地图的重用时太慢了。提出了一种新的位置识别算法，该算法首先检查候选关键帧的几何一致性，然后利用3个在大多数情况下已经存在于地图中的共可见关键帧进行局部一致性检测。这一策略增加了召回率和数据关联的密度，提高了地图的准确性，但代价是略微增加了计算成本。召回率：本身为 positive的，预测为 positive 的概率
• ORB-SLAM 地图集： 第一个能够处理 visual 和 visual-inertial 系统的完整的多地图 SLAM 系统。
• 该图集可以表示一组互不连接的地图，并能顺利地对这些地图进行位置识别、相机重新定位、闭环和精确无缝地图合并等映射操作。这允许自动使用和合并在不同时间构建的映射，执行递增的多会话SLAM。文中增加了新的地点识别系统、视觉惯性多地图系统及其对公共数据集的评估。

3. ORB-SLAM3 子线程

3.1 Tracking

Tracking 线程用来处理传感器信息(图像&&IMU)，决定何时当前帧被判定为关键帧。

• 计算当前帧相对于 Active map(要进行操作的地图) 的位姿以及最小化匹配到的地图点的重投影误差。
• 在 VI 模式下，相机的速度以及 IMU 的 bias 被通过优化惯性残差来估计。
• 当系统追踪丢失后，会触发重定位模式，即当前帧在所有的 Altas 进行重定位。
  • 若重定位成功，当前帧恢复追踪状态
  • 若重定位失败，经过一段时间(超过 5s)，当前的 Active map 会被存储为 Non-active map，同时开启新的建图过程。

3.2 Local Mapping

Local mapping 线程主要用来构建地图点云。

• 向 Active map 中 新增/删减/优化 关键帧以及地图点，上述的操作是通过维护一个靠近当前帧局部窗口的关键帧进行 BA 实现
• VI 模式中，利用最大后验估计 对IMU的参数进行初始化与更新

3.2.1 IMU 初始化

IMU 初始化目的在于获得较好的 IMU 初始值：速度、重力方向和偏置。

• Vision-Only MAP Estimation
  采用 ORB-SLAM(单目)的初始化流程，按照关键帧速率 4Hz 运行 2s 插入关键帧，然后按比例缩放的地图，包括 10 个关键帧以及上百个地图点，然后通过 Visual-Only BA 进行优化。
• Inertial-Only MAP Estimation
  为了获得 IMU 参数的最优估计。利用前述单目视觉 SLAM 初始化后稳定运行的数据，以及这些关键帧之间的 IMU 测量，包括：尺度因子、重力方向、IMU 偏置、关键帧的无尺度速度。这些 IMU 测量放在一起构成状态向量，构建优化问题求解。一旦惯性优化完成，帧的姿态、速度以及 3D 地图点就会以估计的尺度进行缩放，同时旋转以使 $z$ 轴与估计的重力方向对齐。
• Visual-Inertial MAP Estimation
  一旦视觉以及 IMU 有了较好的估计后，进行一个联合优化进一步对这些参数进行精化。结合惯性残差和视觉残差，视觉-惯性 SLAM 可以看作是基于关键帧的最小化问题。基于下式进行计算优化：
  $$\begin{array}{r}\underset{{\bar{\mathcal{S}}}_k,\mathcal{X}}{\min }\left(\sum _{i=1}^k{\Vert {\mathbf{r}}_{{\mathcal{I}}_{i-1,i}}\Vert }_{{\Sigma }_{i,i+1}^{-1}}^2+\sum _{j=0}^{l-1}\sum _{i\in {\mathcal{K}}^j}{\rho }_{\text{Hub}}\left({\Vert {\mathbf{r}}_{ij}\Vert }_{{\Sigma }_{ij}^{-1}}\right)\right)\end{array}$$其中 ${\sum }_{i=1}^k{\Vert {\mathbf{r}}_{{\mathcal{I}}_{i-1,i}}\Vert }_{{\Sigma }_{i,i+1}^{-1}}^2$ 为惯性残差(图中黄色方框 Inertial residual)，${\sum }_{j=0}^{l-1}{\sum }_{i\in {\mathcal{K}}^j}{\rho }_{\text{Hub}}\left({\Vert {\mathbf{r}}_{ij}\Vert }_{{\Sigma }_{ij}^{-1}}\right)$ 为视觉残差(图中红色方框 Reproj. residual)。

ORB-SLAM3 采用了滑动窗口思想，把关键帧及其地图点的滑动窗口作为优化变量。

3.3 Loop & Map Merging 子线程

• 每当加入一个新的关键帧，将该关键帧在整个 Atlas 地图中检测公共区域
• 检测若存在一个公共区域：如果该公共区域属于 Active Map，它就会执行闭环矫正；如果该公共区域属于 Non-active Map，就会把它们融合为一个地图，并把这个融合地图作为新的激活地图
• 在闭环矫正以后，一个独立线程就会进行全局BA，进一步优化地图，同时并不影响实时性能

3.3.1 位置识别

• 数据库查询。首先我们计算关键帧和在共视图中与其相连的关键帧之间的词袋(BOW)的相似度。通过离线训练了大量的基于 ORB 描述的词袋(对该关键帧的描述)，将闭环检测转变为一个类似于模式识别的问题。当相机再次来到之前到过的场景时，就会因为看到相同的景物，而得到类似的词袋描述，从而检测到闭环。
• 计算 Sim3。当检测到闭环候选帧的时候，就需要对当前关键帧和对应的闭环候选帧之间计算其变换关系，即 Sim3 求解。通过 Sim3(g2o::sim3) 变换解出当前关键帧和闭环候选帧的匹配 MapPoint 之间的旋转矩阵R、平移向量t、尺度变换s，也就能对当前关键帧进行位姿矫正(当然也要对关键帧对应的 MapPoints 以及其共视的关键帧进行矫正)。

3.3.2 闭环矫正

闭环矫正的第一步是融合重复的地图点，并且在共视图中插入新的边以连接闭环。

首先当前帧的位姿会根据相似变换而被矫正，同时所有与其相连的关键帧也会被矫正。所有的被闭环处的关键帧观察到的地图点会通过映射在一个小范围里，然后去搜索它的近邻匹配。这样就可以对所有匹配的地图点进行更加有效的数据融合，并更新关键帧位姿，以及在图中的边。

为了有效地完成闭环，使用本质图(Essential Graph)去优化位姿图。这样就可以将闭环的误差分散到整个图中。

3.3.3 地图合并

当检测到闭环候选帧的时候，就需要对当前关键帧和对应的闭环候选帧之间计算其变换关系，即 Sim3 求解。通过 Sim3 变换解出当前关键帧和闭环候选帧的匹配 MapPoint 之间的旋转矩阵 R、平移向量 t、尺度变换 s，也就能对当前关键帧进行位姿矫正(当然也要对关键帧对应的 MapPoints 以及其共视的关键帧进行矫正)。

3.3.3.1 Visual Map Merging

当场景识别成功后(即认为闭环是存在的)，位于 Active map $M_a$ 中的当前关键帧 $K_a$ 与存于 Atlas $M_m$ 中不同地图的匹配关键帧 $K_m$ 产生了多地图的数据关联，此时会进行地图融合。

由于 $M_a$ 包含许多元素，融合它们可能需要很长时间。因此融合分为两个步骤：

1. 在由 $K_a$ 和 $K_m$ 的邻域定义的 Welding window(焊接窗口) 中执行融合(通过滑窗的思想，将数据的处理缩小到一定空间，进行一步步的融合。若一次性完全融合，这样太耗时了，会导致 SLAM 没法去定位建图)；
2. 通过位姿图优化将校正传播到融合图的其余部分。

Visual Map Merging 的具体过程如下：

1. Welding window assembly：匹配上的关键帧、共视关键帧、及其地图点，使用平移矩阵或相似变换矩阵对齐。
   Welding window 包括当前关键帧 $K_a$ 及其共视帧、$K_m$ 及其共视帧，以及被这些关键帧观测的地图点。在将它们包含在 Welding window 中之前，属于 $M_a$ 的关键帧和地图点通过 $T_{ma}$ 进行变换，以使其与 $M_m$ 对齐 。

2. Merging maps：$M_a$ 与 $M_m$ 融合为一个新的 Active map。为了删除重复的点，$M_m$ 关键帧主动搜索匹配 $M_a$ 中的点。对于每个匹配点，都会删除 $M_a$ 中的点，并保留 $M_m$ 中的点，同时更新共视图以及本质图。

3. Welding BA：Welding window 范围内的所有关键帧进行局部 BA 优化。为了确定尺度自由度，观测到 $M_m$ 的那些关键帧需要保持固定。优化完成后，Welding window 区域中包含的所有关键帧都可以用于跟踪，实现地图 $M_m$ 的准确、快速复用。

4. 本质图优化：保持整个 Welding window 范围内关键帧固定，对整个合并地图的本质图执行位姿优化。

Visual-Inertial Map Merging 的具体过程如下：

1. VI Welding window assembly：若 Active map 成熟(成熟：尺度、IMU 参数、重力方向已经被准确估计)，与纯视觉类似，对地图 $M_a$ 进行 $T_{ma}\in SE(3)$ 变换，与 $M_m$ 对齐；若 Active map 未成熟，对地图进行 $T_{ma}\in Sim(3)$ 变换，与 $M_m$ 对齐。

2. Merging maps：$M_a$ 与 $M_m$ 融合为一个新的 Active map。为了删除重复的点，$M_m$ 关键帧主动搜索匹配 $M_a$ 中的点。对于每个匹配点，都会删除 $M_a$ 中的点，并保留 $M_m$ 中的点，同时更新共视图以及本质图。

3. VI Welding BA：优化的对象包括关键帧 $K_a$ 和 $K_m$，以及它们前 $5$ 个连续关键帧的位姿、速度、偏置。

   对于 $M_m$，优化中包括但不固定在紧接局部窗口之前的那一个关键帧位姿。

   而对于 $M_a$，局部窗口之前的那一个关键帧，但其姿势仍然参与优化。所有这些关键帧看到的所有地图点以及观测这些地图点以及观测这些地图点的关键帧位姿也都进行优化。所有关键帧和地图点都通过重投影误差进行关联。

3.3.4 闭环矫正

闭环矫正算法类似于地图融合，但是在这种情况下，当前关键帧以及匹配关键帧都同属于 Active map。Welding window 是由匹配的关键帧组合而成、重复的 3D 点被融合、更新共视图以及本质图的连接关系。下一步是位姿图优化，以均分误差。由于闭环增加了中期/长期数据关联，此时进行全局 BA。在 VI 情况下，仅在关键帧的数量低于阈值时才执行全局 BA，以避免巨大的计算量。