[论文笔记|slam]ORB-SLAM3: An Accurate Open-Source Library for Visual, Visual-Inertial and Multi-Map SLAM

ORB-SLAM3建立在ORB-SLAM2和ORBSLAM-VI之上，是一个完整的多地图和多会话系统，能够工作在纯视觉或视惯性模式。带有单目、双目或RGB-D传感器，可使用针孔和鱼眼相机模型。图1显示了主要的系统组件，这些组件与ORB-SLAM2的组件并行，具有一些重要的新特性。

1. 地图集是由一组未合并的地图组成的多地图集。active map就是当前在用的 map，由 tracking thread 喂进数据，并通过局部mapping线程不断地使用新的关键帧进行持续优化与扩展地图。将图集中的其他地图称为non-active maps。Atlas 是基于作者构建的一个独特的 DBoW2 keyframes database 用来做重定位、回环与地图合并。
2. 跟踪线程实时处理图像和IMU信息，通过最小化路标点的重投影误差去计算当前帧相对于active map的相对位姿。它还决定当前帧是否成为关键帧。在视觉-惯性模式下，通过将惯性残差包含在优化中来估计body速度和IMU偏差。当跟踪丢失时，跟踪线程尝试在地图集中重新定位当前帧。如果重新定位，则跟踪被恢复，如果需要则可以切换active map。否则，在一段时间后，active map将存储为non-active maps，并重新初始化一个新的active map。
3. Local mapping线程向active map添加关键帧和点，并删除冗余项，使用视觉或视觉-惯性BA来细化地图，这一系列操作在靠近当前帧的关键帧局部窗口中进行。此外，在惯性情况下，使用新的最大后验(MAP)估计，由此线程进行初始化和细化IMU参数。
4. 回环和地图合并线程将active map 与整个 Atlas 基于 keyframe 的速度进行相似性度量。如果相似场景区域在当前的 active map 中，将进行 loop correction；当这个区域也属于其他地图，将进行合并操作并将合并后的地图置为当前的 active map。在进行 loop correction 的时候，一个 full BA 将再开一个线程进行，以来追求更高的精度，但这个并不要求实时性。

1. 相机模型

A. 重定位

一个鲁棒的SLAM系统需要具有在跟踪失败时重新定位相机的能力。ORB-SLAM是通过ePnP算法解决了重定位问题，但该算法需要有一个经过校准的针孔摄像机。因此，为了兼顾鱼眼相机模型，采用最大似然PnP算法(MLPnP)，该算法使用反投影射线作为输入，与相机模型完全解耦，所以提供个反投影模型鱼眼相机就可以使用。相机模型只需要提供一个从像素传递到投影射线的unprojection函数，以便重定位。

B. Non-rectified Stereo SLAM

大多数双目SLAM假定双目帧是被校正的。然而，这有很大的局限性，在许多应用中不适合也不可行。例如，矫正一个发散的双目图像对或双目鱼眼相机将需要严重的图像裁剪，这会失去大场景的优势:更快的环境映射和更好的遮挡鲁棒性。

由于这个原因，我们的系统不依赖于图像校正，考虑到两个单目相机所拥有的双目配置:
1)双目之间采用一个常数 SE(3) 变化来描述两者变换关系，
2)双目有共同观测区域。

这些约束允许我们在三角化新路标和BA优化中引入该信息，从而有效地估计地图的比例尺。根据这个想法，在SLAM pipeline中估计了一个6自由度刚体姿态，它的参考系可以位于其中一个摄像机或IMU传感器，并根据刚体姿态表示其他传感器。

处于双目共同观测区域的路标点通过双目进行三角化，不在共同区域的路标点就正常的 multi-view 进行三角化即可。

2. VI-SLAM

A. Fundamentals

1. 状态向量
   
2. IMU残差
   
3. 视觉残差-重投影误差
   帧$i$与位置是$x_j$的3D路标点$j$之间的重投影误差$r_{ij}$：
   
4. 优化问题
   给定一组$k+1$个关键帧及其状态 S ˉ k = { S 0 . . . S k } , \bar{S}_k = \{S_0 . . . S_k\}, Sˉk​={S0​...Sk​},和一组$l$个3D路标点及其状态 X = { x 0 … x l − 1 } X = \{x_0…x_{l-1}\} X={x0​…xl−1​}，优化问题可表示为:
   
   其中，$K_j$是能观察3D路标点$j$的关键帧的集合。对于重投影误差，我们使用鲁棒核函数${\rho }_{Hub}$来减少误匹配的影响，而对于惯性残差，这是不必要的，因为错误关联不存在。下图是优化的因子图表示。
   
   图(a)对应公式(4)，表示视觉和惯性联合优化；图©所示的优化与公式(4)的不同之处在于不包括视觉残差，对应公式(8)；

B. IMU Initialization

这一步的目标是获得良好的惯性变量初始值:body速度，重力方向，IMU偏置。

解决问题的关键：

• 纯单目SLAM可以提供非常精确的初始地图，其主要问题是比例尺未知。首先解决视觉问题将增强IMU的初始化。
• 将尺度明确表示为优化变量比使用BA的隐式表示收敛得快得多。
• 在IMU初始化过程中忽略传感器的不确定性会产生较大的不可预测的误差。

因此，适当考虑传感器的不确定性，我们将IMU初始化问题描述为一个MAP(最大后验估计)问题，分为三个步骤：

1. 纯视觉MAP估计

初始化纯单目SLAM，并在2秒内运行，以4Hz插入关键帧。经过这段时间，会得到一个含有$k=10$个相机位姿和数百个点组成的最新地图，使用纯视觉BA进行优化(图2b)。将这些姿态转换为IMU坐标系，得到轨迹${\bar{T}}_{0:k}=[R,\bar{p}{]}_{0:k}$，其中上横线表示是最新的变量。

2. 纯惯性MAP估计

这一步的目标是获得最优的惯性变量估计，基于MAP，仅使用${\bar{T}}_{0:k}$和这些关键帧之间的惯性测量。这些惯性变量可以叠加在惯性状态向量中:

其中，$s$是纯视觉问题的尺度因子，$R_{wg}\in SO(3)$为重力方向，用两个角度表示。世界坐标系下的重力矢量为$g=R_{wg}{g}_I，其中g_I=(0,0,G{)}^T$，$G$是重力的大小。$b=(b_a,b_g)\in R^6$为初始化时假设为常数的加速度计和陀螺仪偏置；${\bar{v}}_{0:k}\in R^3$是从第一帧到最后一帧的最新body速度，最初由${\bar{T}}_{0:k}$估计。此时，我们只考虑惯性测量的集合 I 0 : k . = { I 0 , 1 … I k − 1 , k } I_{0:k} .= \{I_{0,1}…I_{k-1,k}\} I0:k​.={I0,1​…Ik−1,k​}。最大化的后验分布为：

式中，$p(I_{0:k}|y_k)$表示似然，$p(y_k)$表示先验，又可以表示成下式：

对IMU预积分和先验分布取负对数并假设高斯误差，最终得到优化问题：

用来施加IMU偏置的先验残差$r_p$应该接近零，这也是结果十分精确的原因。其协方差由IMU特性给定。

• 更新优化过程中的重力方向估计：
  
  Exp(.)表示从$so(3)$到$SO(3)$的指数映射。
• 为了保证优化过程中的尺度因子为正，我们将其更新定义为：
  

一旦惯性优化完成，帧位姿、速度和3D路标点将按照估计的尺度因子进行缩放，并进行旋转使z轴与估计的重力方向对齐。对偏置进行更新，并重复计算IMU预积分，以减少未来的线性化误差。

3. 视觉-惯性MAP估计

通过上面两步，可以得到良好估计的惯性和视觉参数，因此可以执行一个联合的视觉-惯性优化，以进一步细化解决方案。需要注意的是，所有关键帧都有共同的偏置，并且具有与纯惯性步骤相同的先验信息。

在EuRoC数据集上进行的初始化实验表明，这种初始化非常有效，在2秒轨迹的情况下实现了5%的尺度误差。为提高初始估计，在初始化后5秒和15秒进行视惯性BA，收敛到1%尺度误差。在这些BA之后，我们说地图已经"成熟"了，也就是说比尺、IMU参数和重力方向已经被准确地估计出来了。

通过将尺度因子固定为1，并将其从惯性优化变量中提取出来，可以将单目惯性初始化扩展到双目惯性，从而增强其收敛性。

C. Tracking and Mapping

对于跟踪和绘图，采用ROVIO中的方案。跟踪解决了一个简化的视觉-惯性优化，其中只有最后两帧的状态被优化，而地图点保持固定。我们使用一个包含关键帧及其帧上的点、保持固定的共视关键帧的滑动窗口作为可优化变量。

在某些特定情况下，当慢速运动不能提供良好的惯性参数观测能力时，初始化可能无法在短短15秒内收敛到精确解。为了在这种情况下获得鲁棒性，提出了一种基于改进的惯性优化的尺度细化技术，其中所有插入的关键帧都被包含，尺度和重力方向是唯一需要估计的参数(图d)。注意，在这种情况下，常数偏置的假设是不正确的。相反，我们使用每一帧的估计值。这种优化的计算效率非常高，每10秒在local mapping线程中执行一次，直到map有超过100个关键帧，或者自初始化以来经过了超过75秒则中止。

D.对跟踪丢失的鲁棒性

在纯视觉SLAM或VO系统中，短暂的摄像机遮挡和快速的运动会导致视觉元素失去轨迹，导致系统丢失。ORB-SLAM率先使用了基于词袋位置识别的快速重定位技术，但它们被证明不足以解决EuRoC数据集中的复杂序列。当跟踪点数小于15时，系统进入视觉迷失状态，实现了两阶段鲁棒性：

• 短期丢失：当前的IMU状态是根据IMU读数估计的，地图点被投影到估计的相机姿态中，并在一个大的帧窗口中搜索匹配。结果匹配包含在视觉-惯性优化中。在大多数情况下，允许恢复视觉跟踪。否则，5秒后，我们进入下一阶段。
• 长期丢失：一个新的视觉-惯性map被初始化，并成为active map。

3. MAP MERGING AND LOOP CLOSING

帧和active map之间的短期和中期数据关联通常是通过跟踪和mapping线程发现的，通过将地图点映射到估计的相机位姿中，并在只有几个像素的图像窗口中搜索匹配。

为了实现重定位和闭环的长期数据关联，本文提出了一个新的有着改进的长期召回和多地图数据关联的位置识别算法。

1. 每当mapping线程创建一个新的关键帧，则启动位置识别，尝试检测与地图集Atlas中已经存在的任何关键帧的匹配。如果找到的匹配关键帧属于active map，则执行闭环；否则，它将是一个多地图数据关联，然后将active map和匹配地图合并。
2. 该方法的第二个新颖之处是，一旦估计出了新关键帧和匹配地图之间的相对位姿，我们就可以在共视图中定义一个带有匹配关键帧及其邻域的局部窗口。在这个窗口中，我们集中搜索中期数据关联，提高闭环和地图合并的准确性。

A. Place Recognition

为了实现高召回率，对于每一个新的active关键帧，都在DBoW2数据库中查询Atlas中几个类似的关键帧。为了达到100%的精度，每一个候选对象都要经过几个几何验证步骤。所有几何验证步骤的基本操作包括：使用它们之间的汉明距离的阈值，去检查图像窗口中是否有一个ORB关键点，其描述子与路标点的ORB描述子匹配。如果搜索窗口中有几个候选帧，为了放弃模糊匹配，我们检查与第二接近匹配的距离比。具体的位置识别算法的步骤是：
1. DBoW2候选关键帧
使用active关键帧$K_a$查询Atlas中的DBoW2数据库，以检索三个最相似的关键帧，但不包括与$K_a$可共视的关键帧–将每个需要进行位置识别的匹配候选帧称为$K_m$。
2. 局部窗口
对于每组匹配候选帧$K_m$，定义一个局部窗口，其中包括$K_m$，它的最佳共视关键帧，以及所有这些关键帧观察到的路标点。DBoW2直接索引提供了$K_a$和局部窗口关键帧中的关键点之间的一组假定匹配。这些2D-2D匹配，也有相应地图点之间的3D-3D匹配。
3. 3D校准变换
我们利用RANSAC计算了变换$T_{am}$，能较好地将$K_m$局部窗口中的路标点与$K_a$的路标点对齐。在单目或单目惯性下,当地图仍然不“成熟”时，计算$T_{am}\in$Sim(3),否则计算$T_{am}\in SE(3)$。在这两种情况下,我们使用Horn算法[使用一组最小的3D - 3D匹配]，为$T_{am}$找到每个假设。这种假定匹配，通过利用$T_{am}$将$K_a$中的路标点进行变换后,在阈值下实现了$K_a$的一个重投影误差，对该假设进行了积极的评分。选择评分更多且超过阈值的假设。
4. Guided matching refinement
将局部窗口的所有路标点都通过$T_{am}$进行转换,以找到与$K_a$上的关键点更多的匹配。查找也可反向,在局部窗口的所有关键帧中查找$K_a中$路标点的匹配。利用所有找到的匹配，通过非线性优化去优化$T_{am}$，其中目标函数为双向重投影误差，利用Huber函数对伪匹配提供鲁棒性。如果优化后的内点数量超过阈值，则启动第二次迭代使用较小的图像搜索窗口进行匹配和非线性优化。
5. Verification in three covisible keyframes
为了避免假阳性，DBoW2在三个连续的关键帧中等待位置识别触发，延迟或丢失。大多数时候，需要验证的信息已经在地图中。为了验证位置识别，在地图的active部分搜索两个与$K_a$共视的关键帧，这两帧与局部窗口匹配的点的数量超过了阈值。如果没有找到关键帧，则使用新的关键帧进一步尝试验证，而不需要再次启动词袋。验证一直持续，直到有三个关键帧验证$T_{am}$，或者连续两个关键帧验证失败。
6. 重力方向验证
在视-惯性情况下，如果active map是成熟的，则估计$T_{am}\in SE(3)$。我们进一步检查俯仰和滚动角是否低于一个阈值，以确定是否接受位置识别假设。

B. Visual Map Merging

当一个成功的位置识别在active map(活动地图)$M_a$中的关键帧$K_a$和存储在Atlas(地图集)$M_m$中的不同地图的匹配关键帧$K_m$之间产生多个地图数据关联时($???$)，我们启动Map Merging(地图合并)操作。在这一过程中，必须确保$M_m$中的信息能够被跟踪线程及时重用，以避免地图重复。为此，我们建议将$M_a$地图引入到$M_m$参考中。由于$M_a$可能包含许多元素，并且合并它们可能需要很长时间，因此合并分为两个步骤。首先，在由$K_a$和$K_m$的邻域定义的welding window(合并窗口)中进行合并，然后通过位姿图优化将校正传播到合并后的其余部分。合并算法的具体步骤是：
1. Welding window assembly
合并窗口包括$K_a$和它的共视关键帧，$K_m$和它的共视关键帧，以及它们所观察到的所有路标点。在焊接窗口被纳入之前，$M_a$的关键帧和路标点通过$T_{ma}$进行变换,以使它们与$M_m$相一致。
2. Merging maps
地图$M_a$和$M_m$被合并成新的活动地图active map。若要删除重复点，将在$M_m$关键帧中主动搜索匹配项以查找$M_a$点。对于每一个匹配，从$M_a$中移除点，而$M_m$中的点要持续累积移除点的所有观测值。由于发现了新的中期点关联，通过添加连接$M_m$和$M_a$关键帧的边来更新共视性和基本图。
3. Welding bundle adjustment
在合并窗口（图3a）中，对$M_a$和$M_m$的所有关键帧进行局部BA优化。为了固定gauge自由度，与$M_m$中的关键帧共视的关键帧保持固定。优化完成后，合并区域中包含的所有关键帧都可以用于相机跟踪，实现快速准确地重用地图$M_m$。
4. 位姿图优化
利用合并图进行姿态图优化，使合并区域的关键帧保持固定。这个优化将修正从Welding window传播到地图的其余部分。

C. Visual-Inertial Map Merging

步骤与B部分相似。修改步骤1)和步骤3)，以更好地利用IMU信息。
1. VI welding window assembly
如果active map是“成熟”的，则使用$T_{ma}\in SE(3)$在$M_a$包含在合并窗口之前去map $M_a$。如果active map不"成熟"，则使用$T_{ma}\in Sim(3)$对齐$M_a$。
2. VI welding bundle adjustment
active关键帧$K_a$及其最后5个时间关键帧的位姿、速度和偏置都是可优化的。这些变量通过IMU预积分进行关联。对于map $M_m$，进行类似的处理，包括$K_m$和它的5个时间邻域的位姿、速度和偏置，如图3b所示。对于$M_m$，包含了紧靠局部窗口前的关键帧，但是固定的；而对于$M_a$，包含了类似的关键帧，但其姿态仍然是可优化的。所有关键帧所能看到的所有点，以及观察这些点的关键帧姿态也得到了优化。利用重投影误差将关键帧和关键点关联起来。

图3:welding BA的因子图表示，有重投影误差项(蓝色方块)、IMU预积分项(黄色方块)和偏置随机游走(紫色方块)。

D. Loop Closing

闭环校正算法类似于地图合并，但位置识别匹配的两个关键帧都属于active map。

1. 由匹配的关键帧组建合并窗口，检测并融合重复点，并在共视图和基本图中创建新的链接。
2. 位姿图优化，将loop校正传播到map的其余部分。
3. 全局BA，在考虑闭环中期和长期匹配后，找到MAP估计。在视觉-惯性情况下，只有在关键帧数量低于阈值时才执行全局BA，以避免巨大的计算代价。