这篇博客

最近ORB-SLAM3全能王横空出世，在看完ORBSLAM-Atlas系统论文后，就直接莽ORB-SLAM3的论文了。关于系统中$IMU$初始化的相关论文之后在慢慢理解（概率好头疼），剩下的就直接莽了！
【转载声明】本篇文字均为原创，如转载请注明出处

ORB-SLAM3系统

ORB-SLAM3是基于 VI ORB-SLAM 和 ORBSLAM-Atlas 两个系统（点击可查看我写的关于这两个系统论文的笔记）实现的，它相当于是这两个系统的融合升级版，是在视觉、视觉+惯性SLAM中的全能王。下图是ORB-SLAM3的流程框图：

 这里大致介绍一下各个线程的工作内容：
 a) Atlas：Atlas相当于一个子系统，它保存了许多ORB-SLAM3所创建的子地图。所有子地图可以分成两类：Active Map(跟踪定位所使用的地图)，Non-active Map(之前保留下来的地图)。
 b) Tracking：处理传感器的输入信息，通过Active Map来实时地对机器进行跟踪定位，同时还会决定是否创建新的关键帧 KF 。如果跟踪丢失，跟踪线程会先在 Atlas 的所有地图上进行重定位：重定位成功，则继续跟踪；否则将当前 Active Map 保存为 Non-active Map ，并重新创建地图。
 c) Local Mapping：维护 Active Map 中的一小部分地图，可以帮助提高跟踪定位的精度。如果是惯性系统，则该线程还承担着 IMU 初始化的任务。
 d) Loop and Map Merging：检测地图内或地图之间存在的闭环，根据闭环对地图进行修正，降低系统累积误差。

通过整个工作流程图以及各线程的功能，我们可以看出，相比于普通的视觉SLAM、视觉惯性SLAM（这里可用ORB-SLAM2和VI ORB-SLAM作为对比对象），ORB-SLAM3主要有以下几个创新点：
（1）能够进行单目、双目的视觉、视觉+惯性跟踪定位，使用最大后验估计（MAP）方法计算位姿。同时在IMU初始化方面也采用了一种精度、效率更好的新方法；
（2）使用一种新的位置识别方法，来完成重定位和闭环检测操作。这个方法耗时更少，且在保证100%准确率的同时，能够提高闭环的召回率；
（3）加入了 ORBSLAM-Atlas 系统的多地图特性，并使用（2）中的方法完成地图的位置识别+闭环检测；
（4）系统对相机模型进行了抽象表达，把系统中与相机模型有关的所有特性、函数（如投影、反投影函数等）都提取出来，组成一个独立的相机类。这样系统就能适用于不同的相机模型。

相机模型的抽象(Camera Model)

为了让ORB-SLAM3能适应于不同类型相机，这里将相机模型从系统中独立了出来，构建抽象类对象(代码实现是这样的)。用户可通过类继承的方法，创建想要的相机模型类对象。之后就能在ORB-SLAM3代码里生成相应实例并使用。论文里有针孔模型和鱼眼相机模型。这种作法会导致系统使用的相机模型不再是确定的，因此在算法等方面会存在一些问题。

重定位的问题

在重定位方面，ORB-SLAM 是通过 EPnP 算法来计算位姿的。而 EPnP 算法的公式是基于针孔相机模型的。如果系统使用的是鱼眼相机那么 EPnP 就失效了。因此系统需要使用一个不依赖某一个相机模型的PnP算法，即最大似然 PnP（MLPnP) 。

图片矫正的问题

在多数双目SLAM中，都假设双目图片能够矫正好，然后就能通过极线搜索的方法轻松地找到图片间的匹配特征。但这种假设具有较大局限性，且图片矫正在某些时候并不实用，比如矫正鱼眼相机的图片要去掉其中大部分区域。这就丢掉了很多环境信息，会降低系统的鲁棒性。所以ORB-SLAM3不依靠图片矫正，而将双目看作是两个满足下述条件的单目相机：

1）两个单目相机间存在一个固定的相对位姿变换矩阵。
 2）（可选）两个相机间存在共视区域（当两个相机一前一后时就没有共视了）。

由于不依靠矫正后的图片，所以 ORB-SLAM3 采用的是暴力匹配的方式，在全局使用knn搜索为两个相机寻找匹配特征。
 在跟踪和建图时，使用一个单目相机作为视觉上的参考坐标系，另一个则用来帮助构建地图点。可以通过三角化构建两相机间的共视部分中特征点对应的地图点。而共视部分以外的图片区域则按照单目相机图像的方式来处理（单目建图）。

视觉惯性SLAM的工作原理

相关公式

在视觉惯性SLAM中的状态变量有（用下标C(Camera)表示相机坐标系，下标B(Body)表示惯性元件坐标系）:
1）$T_i=[R_i,p_i]\in SE(3)$：第 i 时刻Body的位姿；
2）$v_i$：每帧的速度（在地图中第一参考坐标系下表示）；
3）$b_i^g$：第 i 时刻 IMU 元件的陀螺仪偏差；
4）$b_i^a$：第 i 时刻 IMU 元件的加速度偏差；
 将这些变量整合起来，用一个向量 S 来公式 (1)表示：

因为是视觉惯性SLAM，所以系统通过优化惯性误差、视觉误差来获得高精度结果。其中IMU误差如公式 (2)所示：

其中$\Delta R_{i,i+1},\Delta v_{i,i+1},\Delta p_{i,i+1}$ 是两帧间 IMU 测量值预积分后的结果。
是所有测量向量的信息矩阵。$R_i,p_i$ 是系统估计（视觉估计）出来的结果，$v_i$ 由 IMU 的速度计算公式求出。
 视觉误差就是重投影误差，如公式 (3)所示：

其中 $r_{ij}$ 是第 j 个地图点到第 i 帧上的重投影误差。Π 是相应的相机投影模型，$u_{ij}$是地图点对应的像素坐标，它具有一个信息矩阵 ${\Sigma }_{ij}$。$T_{CB}$ 表示Body–>Camera坐标系的变换矩阵，可以提前标定。
 将公式(2)和公式(3)的误差联合起来，就是系统所优化的总误差，如公式 (4)所示：

其中$K^j$ 是能够观测到第 j 个地图点的所有 KFs 。${\rho }_{Hub}$ 是鲁棒核函数。公式 (4) 的优化问题可由下图表示。

（图中的展示的优化方法在跟踪和建图过程中，会依据情况做出相应调整）

IMU初始化

因为有些IMU的参数：速度 $v$、加速度偏差 $b^a$、陀螺仪偏差 $b^g$、重力方向 $g$ 是未知的，所以要通过初始化将它们估计出来。如果用的是单目相机，还需要估计出尺度 $s$。
 论文用的初始化方法速度更快、精度更高。由于考虑了传感器的不确定性，所以将初始化当作一个最大后验估计（MAP）问题来求解。以单目系统为例，初始化的步骤为：

 1）Vision-only MAP Estimation(这里和普通单目初始化一样)：将系统以 4Hz 插入 KF 的频率运行 2s ，构建一个尺度不确定的视觉地图。该地图内应含有 10 个 KFs 和几百个地图点。然后通过Visual-Only BA 来优化各帧的位姿（如图 2b 所示）。通过各帧位姿，获得对应的 Body 位姿${\bar{T}}_{0:k}=[R,\bar{p}]$。
 2）Inertial-only MAP Estimation：通过${\bar{T}}_{0:k}$和 KFs 间 IMU 预积分的值，来估计 IMU 的参数和尺度。将这些待估计变量用下式表示：

其中 $s$ 是地图的尺度。$R_{wg}\in SO(3)$ 是重力的旋转矩阵，它表示的是地图中第一参考坐标系到真实世界坐标系的旋转关系。$b$ 则是加速度和陀螺仪偏差（在初始化过程中被认为是定值）。${\bar{v}}_{0:k}$ 是基于尺度的速度。

（下面部分涉及到论文：Inertial-Only Optimization for Visual-Inertial Initialization）

 此时，如果只考虑相邻两帧间的惯性测量值：

那么，根据就能建立以下 MAP 估计问题：

考虑到每个测量值的独立性，MAP 问题可以写成 公式 (7)：

再进行一些假设，就能获得最终的优化问题，如公式 (8) 所示：

公式中不包含视觉误差，仅优化惯性误差。$r_p$ 表示先验误差。这个优化过程如 图2c 所示：
其中关于$R_{wg},s$ 两个待估计参数的更新公式如下：


 优化完后，地图的尺度就确定了。之后通过估计的重力方向，将地图中所有元素变换到真实世界坐标系下表示。
 3）Visual-Inertial MAP Estimation：完成上述两步后，同时优化视觉误差和惯性误差。这一步和图 2c 很相似，但要保证各KFs都有相同的偏差$b^a,b^g$，以及和第二步具有一样的先验信息。

通过上述步骤就能仅根据 2s 内产生的轨迹，获得尺度误差为 5% 的估计结果。在初始化后的 5、15s 会各执行一次视觉 + 惯性 BA（代码中类似于又执行了两次初始化，但稍微有些不同）。这样可将尺度误差降低到 1% 。完成上述所有操作后，就认为地图已经成熟。
 双目情况下的初始化和上述类似，只需要将尺度 $s$ 固定为 1，且不对它进行优化。

跟踪和建图

跟踪定位当前帧可以采用两种方法：通过运动模型跟踪（相邻两帧间的运动矩阵已知）和通过当前参考 KF 跟踪。在完成跟踪之后，需要进行位姿的优化，其方法和VI ORB-SLAM类似(单目系统是一样的优化方法，而双目系统需要考虑地图点是在哪一个相机上成像的。如果在右边相机，那在构建g2o误差边时要考虑两相机间的相对变换矩阵。关于VI ORB-SLAM可以参考这篇博客）。优化过程中只优化两帧的状态变量，且保持地图点不变。但当地图刚更新完毕时，优化方法会有所改变，具体可见VI ORB-SLAM中相关部分。
 建图环节的优化主要考虑局部地图优化，优化局部地图中的 KFs 和地图点，其他的共视 KFs （局部地图以外）可以提供约束（重投影误差），但不会被优化（这里和代码实现不一样，也许是自己理解没有到位。代码中共视 KFs 的位姿也会被优化。）
 在某些特殊情况下，比如机器移动很慢，IMU参数的可观测性较差。此时使用之前的初始化方法无法在15s内获得精度较好的结果。所以需要采用一个新的尺度提炼方法来预防这种特殊情况。这个方法是 Inertial-only 优化的修改版，它只优化尺度和重力方向。此时不再认为各 KFs 的偏差相同。该优化方法如下图所示（即图 2d ）：

这个尺度提炼的方法将在 Local Mapping 线程中每隔10s执行一次，直到地图中已有100个以上KFs或者在初始之后系统已经运行了75s。

系统对跟踪丢失的应对

一般的视觉SLAM，如ORB-SLAM2在跟踪丢失时会直接进行重定位操作。此时地图停止更新，直到重定位完成。而ORB-SLAM3有两种方法来应对跟踪丢失的情况：

 1）在视觉上发生跟踪丢失时，则只使用IMU进行位姿估计。根据估计的位姿，将地图点投影到当前图像上以寻找匹配点。然后通过视觉+惯性优化获得最终位姿结果。
 2）如果通过上面方法在5s内无法重新定位，则重新构建地图。当前使用的地图根据情况决定是否保存。
（如果当前地图中信息较多（ KFs 数量多）则认为其比较重要，将其保留；否则直接残忍抛弃）

多地图的闭环检测和地图融合

由于系统具有多地图特性，所以会构建出多个子地图。为了更有效地检测地图内和地图之间的闭环，ORB-SLAM3使用一种新的位置识别方法和闭环融合方法。

位置识别

将跟踪时正在使用的地图称为Active Map，其余子地图为Non-active Map。在Active Map中的关键帧记为 $K_a$ 。整个位置识别的大致步骤为：

 1）为当前关键帧 $K_a$ ，找出三个候选者 $K_m$（不包括之前就与 $K_a$ 有共视关系的 KFs ）；
 2）为每个 $K_m$ 构建一个局部窗口，其中包含了 $K_m$ 和与之共视程度较高的一些 KFs ，以及它们观测到的所有地图点；
 3）先进行几何一致性检测。通过 $RANSAC$ 算法，估计 $K_a$ 和 $K_m$ 间适应性最高的变换矩阵 $T_{am}$；
 4）将 $K_m$ 的局部窗口中所有地图点通过 $T_{aw}=T_{am}{T}_{mw}$投影到 $K_a$ 中，寻找其匹配点。反过来也一样，可以把 $K_a$ 观测到的地图点投影到 $K_m$ 的局部窗口中的KFs上。根据这些匹配关系，通过非线性优化来提高 $T_{am}$ 的精度。
 5）在 Active Map 中找 3 个和 $K_a$ 共视程度大于一定阈值的 KFs（与$K_a$观测到相同的局部地图中的地图点的数量较多） 。如果没找到，就等待在 $K_a$ 后面的两个连续KFs**（不需要对这两个KFs进行DBoW2方式的位置识别，所以能节约时间）**。此时用 $K_a$ 和其他 3 个KFs来验证$T_{am}$。（验证方法应该就是看这两个KFs在经过$T_{am}$变换后能否看到一定数量的地图点（地图点在 $K_m$ 的局部地图中））
 6）基于重力方向的验证。在视觉惯性系统中，如果地图已初始化完毕，那么 $T_{am}\in SE(3)$。检查$T_{am}$中 $pitch、roll$ 方向的旋转角度是否低于一定阈值。低于则认为这次位置识别成功。

（对第(6)条的个人理解：如果 $K_a$ 和$K_m$形成闭环，那么他们的重力方向应该是差不多的，因为他们只有在 $pitch、roll$ 方向上差不多的位置观测同一区域时，才会形成闭环）

视觉地图融合方法

将系统正在使用的地图定义为Active map，记为 $M_a$，其中的 KF 记为$K_a$；其余子地图记为Non-active map，记为$M_m$；与$K_a$匹配的闭环帧记为$K_m$；$K_a$和$K_m$间相对变换矩阵为$T_{ma}$。

 1）用$K_a$+$K_m$+他们俩的共视图（系统默认$K_a$选取 15个，$K_m$也一样）和观察到的地图点来构建接合窗口。构建前，先将属于$M_a$的元素通过$T_{ma}$变换到 $M_m$ 地图坐标系中。
 2）第一步后，Ma和Mm就完成了融合，一起作为新的Active map。此时会有重复的地图点产生，它们是$M_a$与$M_m$间的匹配点。对它们进行融合：把在$M_a$地图中的地图点信息（它的观测KFs和描述子之类的）转移到与其匹配的$Mm$中的地图点上。最后再更新融合后地图的共视图和本质图。
 3）对融合地图中的接合窗口部分进行局部 BA 优化。其中，对于$M_m$地图中剩余的KFs，只要能观测到接合窗口中的地图点，就能参与局部 BA 优化。但它们只是提供约束，不会被优化。这个过程如下面的 图(a) 所示：
 4）以优化好的接合窗口为基础（固定其中KFs），通过位姿图优化来优化融合地图中剩余的 KFs 。

PS：融合后的地图直接用于跟踪定位，成为新的Active Map。在融合之前的Active Map中所使用的Local Window（跟踪线程使用的局部窗口），在融合之后的地图中继续使用（仍然是那些 KFs 构建局部窗口）。

视觉+惯性地图的融合方法

这里和视觉地图融合的方法相似，只是原来的第（1）和（3）步做了修改（接合窗口中KFs的数量和选取方法变了）：

1）(与上面的第(1)步相比较)如果当前地图成熟了（尺度、重力方向估计优化完成），$T_{ma}\in SE(3)$，否则$T_{ma}\in Sim(3)$。之后的操作和之前一样。
2）视觉惯性接合窗口的局部 BA 优化：将$K_a+K_a$前面的5个 KFs记为 $K_a$ 的局部窗口、$K_m+K_m$附近5个 KFs 记为$K_m$ 的局部窗口。将在$K_m$ 局部窗口之前的 KF 记为$K_{bm}$，同理 $K_a$ 那边则记为$K_{ba}$ 。找出上面 KFs 观测到的地图点，记为$M_p$，并在$M_a$ 和 $M_m$ 地图中找到这些地图点其余的观测帧 KFs，记为$K_c$ 。

最后将上述所有 KFs 和地图点都包含到优化算法中，其中各部分的优化情况为：
 1）$K_a$局部窗口中的 KFs 的所有状态变量都会优化；
 2）$K_m$局部窗口中的 KFs 的所有状态变量都会优化；
 3）$K_c$中的 KFs 只会优化位姿状态变量会被优化；
 4）$M_p$中的所有地图点都优化；
 5）$K_{ba}$只优化其位姿状态变量，其余状态变量保持不变；
 6）$K_{bm}$提供约束，但不会被优化。
这个优化过程的示意图如下：


疑问：在查看代码时，代码中进行视觉惯性地图融合的函数应该是 MergeLocal2()，但是该函数是将 $M_m$ 地图中的元素变换到了 $M_a$ 中，而且最后只完成了接合窗口部分优化，而没有对融合地图中剩余部分进行全局位姿优化。看来仍需要继续深入学习。

单个地图中的闭环融合

这里形成闭环的两个KFs都属于Active map。处理方式和上面的地图融合是相似的，即：

构建接合窗口(闭环接合处)-->优化接合窗口-->优化地图剩余KFs

在这之后也会进行一次全局的BA优化（优化地图中所有KFs），以获得 MAP 估计结果。如果是视觉地图，最后的全局 BA 优化一定会使用；如果是视觉惯性地图，只有当地图中的 KFs 数量较少时 (代码中默认为200以内) 才会执行最后一步（为了防止计算量过大）。

结尾

上面是自己结合代码对 ORB-SLAM3 论文的初步理解 ，如有错误，请大家指出，谢谢！之后有新的思路会对这篇博客进行更新（路漫漫其修远兮，吾将上下而求索！）。

参考资料：
1、ORB_SLAM3和之前版本有什么不同？
2、ORB-SLAM3: An Accurate Open-Source Library for Visual, Visual-Inertial and Multi-Map SLAM