ORB-SLAM3系列：论文重点导读及整体算法流程梳理

智星崛起

深度分享机器人干货

本文是ORB-SLAM3深度系列的第二篇内容，我们将聚焦ORB-SLAM3的整体算法代码流程梳理及论文重点翻译和解读。

ORB-SLAM3系统优势

ORB-SLAM3是第一个对短期、中期、长期数据关联的视觉、视觉惯性系统。和其他具有代表性的视觉和视觉惯性系统相比，ORB-SLAM3系统都优势明显。

具体表现在四个方面：

1）它是一个单目和双目视觉惯导SLAM系统，完全依赖于最大后验估计(MAP)。在小型和大型、室内和室外环境中实时稳定运行，比其他方法准确2-10倍。

2）召回场景识别采用先检查候选关键帧的几何一致性，然后检查与三个共视关键帧的局部一致性策略。以略高的计算成本为代价，增加了召回率并增加了数据关联，提高了地图准确性。

3）使用Atlas表示一组断开的地图集，并将所有的地图平滑地应用于位置识别、相机重新定位、回环检测和地图无缝拼合。

4）抽象的相机表示，使SLAM代码与所用的相机模型无关，并允许通过提供它们的投影、非投影和雅可比函数来添加新模型。

ORB-SLAM3系统概述

ORB-SLAM3系统基于ORB-SLAM2和ORB-SLAM-VI建立，主要组成如下：

图1：ORB-SLAM3系统的主要内容

Atlas

由一些不连续的地图组成的集合表示，分为活跃地图和非活跃地图。其中，跟踪线程在活跃地图定位传入帧，并通过局部建图线程随着新的关键帧不断优化和增长。Atlas构建了一个基于关键帧的唯一DBoW2数据库，用于重定位、回环检测以及地图合并。

跟踪线程

处理传感器信息并实时计算当前帧相对于活跃地图的位姿，使匹配地图特征的重投影误差最小化。在视觉惯性模式下，通过在优化过程中引入惯性残差来估计本体速度和IMU偏置。

跟踪丢失后，跟踪线程会尝试在Atlas地图中重定位当前帧。如果重定位成功，将恢复跟踪并在需要时切换为活跃地图。否则一段时间后，活跃地图被存储为非活跃地图，新的活跃地图需从头进行初始化。

局部建图线程

在靠近当前帧的关键帧局部窗口中，添加关键帧及特征点到活跃地图中，并移除冗余关键帧与地图点，使用视觉或者视觉惯导BA来优化地图。而在惯性情况下，建图线程中的IMU参数通过最大后验估计技术来初始化和优化。

回环和地图合并线程

用关键帧速率检测活跃地图和整个Atlas之间的公共区域。公共区域如果属于活跃地图，就执行回环纠正；如果属于不同地图，两个地图就被无缝合并成一张地图，这个地图就成了活跃地图。回环纠正后，在不影响实时性能的情况下，独立线程会启动一个完全的BA来进一步优化地图。

视觉-惯导SLAM

ORB SLAM3建立在ORB-SLAM-VI的基础上，并提供一种快速准确的IMU初始化技术和开源SLAM库，支持使用针孔和鱼眼相机进行单目惯性和立体惯性SLAM。

基本原理

纯视觉SLAM只估计当前相机位姿，而视觉-惯导SLAM还会估计其他变量，包括世界坐标系下的本体位姿T_i=[R_i,p_i]∈ SE(3)、速度v_i，陀螺仪和加速度计偏置以及 ,状态变量表示为：

对于视觉-惯性SLAM，在连续两帧i和i+1之间进行IMU预积分。获得的预积分旋转、速度以及位置测量分别用∆R_i,i+1，∆v_i,i+1，∆P_i,i+1表示，整个测量向量的协方差矩阵表示为。给出这些预积分项以及状态S_i和S_i+1，我们采用惯导残差的定义：

Log：SO(3)→R³，从一个李群空间到向量空间。重投影误差r_ij在第i帧和在位姿x_j的3D点j之间：

其中，∏是对应相机模型的从R³到R²的投影模型。U_ij是图像i中对点j的观测，这里有一个信息矩阵Σ_ij，T_CB∈SE(3)表示从机体（IMU）坐标系到相机坐标系的变换矩阵（在标定的时候得到的外参），⊕表示SE(3)到R³的变换。（第i帧的X_j变换到参考帧→再从机体坐标系变换到相机坐标系→投影→和匹配点的像素位置做差得到误差）。

结合惯性和视觉残差项，视觉-惯导SLAM可以称为一个基于关键帧的最小化问题。给定K+1个关键帧的集合及其状态 ≐{S₀ . . . S_k },以及一个包含l个3D点的集合及对应的状态X≐{x₀ . . . x_l−1}，视觉-惯导优化问题表示为：

K^j是观测到3D点j的关键帧集合，这种优化可以概括为图2-a中所示的因子图。

图2：不同优化的因子图表示

注意，对于重投影误差，我们使用鲁棒的Huber核来减少虚假匹配的影响。由于惯性残差不存在虚假匹配，因此不需要Huber kernel函数。

IMU 初始化

为获得惯性变量的良好初始值：本体速度、重力方向及IMU偏置，我们提出了一种基于三个关键见解的快速准确的初始化方法。

在适当考虑传感器不确定性情况下，我们将IMU初始化描述为MAP估计问题：

1）只有视觉的 MAP估计

我们初始化纯单目SLAM并在2秒内运行它，以4Hz插入关键帧，得到一个由k=10个相机位姿和数百个点组成的一定规模的地图，仅使用视觉BA进行了优化（图2-b）。这些位姿被转换到body参考系下，获得轨迹T̄_0:k=[R，p̄]_0:k ；

2）只有惯性的MAP估计

仅使用T̄_0:k和这些关键帧之间的惯性测量来获得惯性变量的最佳估计。这些惯性变量可以叠加在仅有惯性的状态向量中：

s∈ R⁺是只有视觉的尺度因子，R_wg ∈ SO(3)是一个旋转矩阵，用于计算在世界参考系中的重力向量g = R_wgg_I，g_I = (0, 0, G) ^T,G是重力的大小；b = (b^a , b^g ) ∈ R⁶是加速度计和陀螺仪的偏置，在初始化阶段被认为是常量。v̄_0:k∈R³是从第一个关键帧到最后一个关键帧的本体速度，最初的估计来自于T̄_0:k。在这一点上，我们只考虑惯性测量集合I_0:k≐{I_0,1 . . . I_k−1,k }。

因此，我们可以陈述MAP估计问题，其中要最大化的后验分布是：

p(I_0:k |Y_k )表示似然估计，p(Y_k )表示先验值。

考虑到测量的独立性，仅惯性MAP估计问题可以写成：

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

当我们在流形中进行优化时，需要定义一个收缩，以在优化过程中更新R_wg。由于围绕重力方向的旋转不假设重力发生变化，此更新使用两个角度（δα_g，δβ_g）进行参数化：

为保证尺度因子在优化过程中保持正，将其更新定义为：

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

3）视觉-惯导MAP估计

一旦我们对惯性和视觉参数有了很好的估计，就可以进行联合视觉惯性优化（图2-a），以进一步完善结果。

在某些特定情况下，当慢动作不能提供惯性参数的良好可观察性时，初始化可能无法在15秒内收敛到精确的解。

为获得这种情况下的鲁棒性，我们使用从建图中估计的值，并对其进行修复。这种计算效率非常高的优化每10秒在局部建图线程中执行一次，直到地图具有100多个关键帧，或者自初始化以来已经过去了75秒以上。

最后，我们通过将比例因子固定为1，并将其从仅有惯性的优化变量中取出，从而很容易地将单目惯性初始化扩展到立体惯性，增强其收敛性。

跟踪和建图

跟踪方面，解决了简化的视觉惯性优化，其中只有最后两帧的状态得到优化，而地图点保持固定。建图方面，我们使用关键帧及其点的滑动窗口作为可优化变量，包括从共视关键帧中对这些点的观察，但保持它们的位姿固定。

跟踪丢失的鲁棒性

在纯视觉SLAM或VO系统中，临时的相机遮挡和快速运动会导致视觉元素的跟踪丢失，从而使系统丢失。

1）短期丢失：根据IMU读数估计当前本体状态，并以估计的相机姿势投影地图点，并在大图像窗口内搜索匹配，得到的匹配包含在视觉惯性优化中。多数情况下允许恢复视觉跟踪，否则5秒钟后进入下一阶段。

2）长期丢失：当新的视觉惯性地图被初始化，成为活跃地图时，系统如果在IMU初始化后15秒内丢失，则会丢弃地图，以防止积累不准确和无意义的地图。

地图合并和回环检测

位置识别

1）DBoW2 候选帧。查询具有活动关键帧K_a的Atlas DBoW2数据库，以检索出三个最相似的关键帧（与K_a共视的不算），将其称为K_m。

2）局部窗口。定义一个局部窗口，包括K_m、其最佳共视关键帧以及所有这些关键帧观察到的地图点。DBoW2直接索引，提供K_a中关键点与局部窗口关键帧之间的匹配集。

3）3D 对齐变换。我们使用RANSAC计算变换T_am，使K_m局部窗口中的地图点与K_a的地图点更好对齐。在单目视觉或者单目惯导中，地图还不成熟时计算T_am∈Sim(3)，否则是T_am∈SE(3)。这两种情况都使用Horn算法，让三个3D-3D匹配的最小集合来找到T_am的每个假设。

4）引导匹配优化。使用T_am对局部窗口中的所有地图点进行变换，以找到与K_a中关键点的更多匹配。然后在本地窗口的所有关键帧中，为K_a搜索匹配的地图点。所有匹配通过非线性优化来细化T_am，其中目标函数是双向重投影误差，使用Huber影响函数来提供对虚假匹配的鲁棒性。如果优化后的内点数量超过阈值，则使用较小的图像搜索窗口启动引导匹配和非线性细化的第二次迭代。

5）在三个可视关键帧中进行验证。我们在活跃地图中搜索与K_a共视的两个关键帧，其中与局部窗口中点的匹配数量超过阈值。如果没有找到，则对新传入关键帧进一步尝试验证，直到三个关键帧验证T_am，或者两个连续的新关键帧无法验证T_am为止。

6）VI重力方向验证。在视觉惯导的情况下，如果活跃地图是成熟的，已经估计了T_am ∈SE(3)。我们进一步检查俯仰角和滚转角是否低于阈值，以最终接受位置识别假设。

视觉地图合并

位置识别成功后，当活跃地图M_a中的关键帧K_a和Atlas M_m中的匹配关键帧K_m之间产生多地图数据关联时，通过对齐变换T_am，启动地图合并操作。为避免地图重复，可将M_a图纳入M_m参考。

图3：“连接”BA的因子图表示，重投影误差项（蓝色方块），IMU预积分项（黄色方块）和偏置随机游走（紫色方块）

1）连接窗口集合。连接窗口包括K_a及其共视关键帧、K_m及其共视关键帧，以及由它们所看到的所有地图点。在连接窗口之前，M_a中的关键帧和地图点将通过T_ma进行变换，使之相对于M_m对齐。

2）合并地图。M_a和M_m融合为新的活跃地图。要删除重复的点，将在M_m关键帧中主动搜索与M_a匹配的点。对于每个匹配，M_a中的点被移除，并且M_m中的点保持累积移除点的所有观测值。由于发现了新的中期点关联，通过添加连接M_m和M_a关键帧的边来更新共视性和本质图。

3）连接BA。执行局部BA，优化连接窗口中M_a和M_m的所有关键帧以及它们观察到的地图点（图3-a）。为了固定测量自由度，不属于连接窗口但观测到任何局部地图点的关键帧M_m在BA中被包含，其姿态固定。一旦优化完成，连接区域中的所有关键帧可以被用于相机跟踪，从而实现地图M_m的快速准确重用。

4）本质图优化。使用整个合并地图的本质图进行位姿图优化，保持连接区域中关键帧不变。这种优化将矫正从连接窗口传播到地图的其余部分。

视觉-惯导地图合并

视觉惯性合并算法遵循与纯视觉情况类似的步骤，修改步骤1）和3）以更好地利用惯性信息：

1）VI的连接窗口：如果活跃地图是成熟的，我们在将M_a包含在连接窗口中之前，应用T_ma∈SE(3)到地图M_a上。如果活跃地图不成熟，则使用可用的T_ma∈Sim(3)来对齐M_a。

2）VI连接BA：关键帧K_a和K_m及其最后五个时间关键帧的位姿、速度和偏移都是可优化的，这些变量与IMU预积分项相关（图3-b）。其中，M_m包括局部窗口之前的关键帧但位姿固定，M_a也包括类似的关键帧但其姿态可优化。上述关键帧看到的所有地图点以及K_m和K_a共视关键帧的位姿都得到了优化，所有关键帧和点都通过重投影误差进行关联。