ORB_SLAM3 算法框架解析

论文地址：https://arxiv.org/abs/2007.11898v1
代码地址：https://github.com/UZ-SLAMLab/ORB_SLAM3
ORB_SLAM1算法框架解析地址: https://blog.csdn.net/weixin_43503355/article/details/121474574spm=1001.2014.3001.5501
ORB_SLAM2算法框架解析地址 : https://blog.csdn.net/weixin_43503355/article/details/121618694spm=1001.2014.3001.5501

一个典型的视觉SLAM系统主要包含数据处理、初始化、视觉里程计、地图维护、闭环检测等部分。

orb_slam3中上述框架主要完成的任务：

1. 数据处理：
   Frame：对获得的视觉图像进行特征提取，畸变矫正，
   imu：主要根据预测模型计算当前帧imu提供的位姿。IMU数据进行单目初始化、在视觉跟踪丢失的时候IMU可以提供短时间的位姿、在恒速跟踪模型中如果IMU完成初始化，当前帧位姿初始值可有IMU的预测模型得到。
2. 初始化：
   主要就是以第一帧图像为参考帧，对后续的视觉帧进行特征匹配、位姿求解（对极约束）、三角化完成3D地图点的生成。供后续pnp（3D-2D）求解。ORB初始化时并行计算单应矩阵和基础矩阵，根据模型得分自动选择选用何种模型进行求解。
3. 视觉里程计对应tracking线程：
   通过最小化重投影误差主要完成相邻帧之间视觉里程计的求解。首先通过imu或者恒速运动模型获得当前帧位姿的一个初始估计、然后通过帧间匹配对上述初始估计进行优化，最后再由当前帧的共视关键帧获得局部地图进行当前帧局部地图的匹配，进一步对位姿进行优化。此处的优化只优化当前帧的位姿，不对地图点进行优化。
4. 地图维护Localmaping：
   主要维护地图中的地图点，由于地图中保留的每个地图点都会被许多相机观测到，每个关键帧都有相对地图点的不同描述子（不同图像对同一个特征点提取的描述子不相同），因此在新关键帧插入后需要对每个受新关键帧影响的地图点法向量、描述子等进行更新。orb中描述子的更新采用的是距离中值法即最能代表该地图点的描述子应该与其他描述子具有最小的距离中值。(后续详解)
5. 闭环检测LoopClosing：
   主要通过视觉词典完成当前帧与历史关键帧之间相对位姿的求解。

---

1、概述

框架

ORB_SLAM3也满足上述几部分。与前两代相比主要是在单目、立体视觉、RBGD基础上加入了惯性传感器，引入了视觉惯性和多地图模式（atlas）以及地图融合(map merging).。系统框架如图所示与前一代相比新增的内容如红色框图所示：

A：Atlas地图集

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

B：跟踪线程

当定位跟踪失败时：首先会在所有的Atlas地图集中搜索当前位置的匹配，如果成功，则将当前的地图设置为non-active，而将在Atlas搜索到的地图设置为active，并继续在active的地图上进行局部跟踪。如果在全图搜索失败，则在一定时间后，当前的active地图会强行被设置为non-active，新的小地图将被构建，并设置为active。这样，就的地图的正确性不会被局部定位的失败影响，新旧地图之间的位姿变换关系不确定性也有希望被之后的共视减小或者消除

C：局部建图线程

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

D：回环和地图合并线程

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

2、创新点

• 一种单目和双目视觉惯性SLAM系统,即使在惯性测量单元初始化阶段,也完全依赖于最大后验估计.所提出的初始化方法先前已在[6]中介绍过.这里我们添加了它与ORB-SLAM视觉惯性[4]的集成,扩展至双目惯性SLAM,并在公共数据集进行彻底评估.我们的结果表明,单目和双目视觉惯性系统是极其鲁棒的,并且比其他视觉惯性方法更加精确,甚至在没有循环的序列中也是如此.
• High-recall place recognition。许多最近的视觉SLAM和VO系统使用DBoW2单词库包解决了位置识别问题。DBoW2需要时间一致性，在检查几何一致性之前，将三个连续的关键帧匹配到同一区域，以牺牲召回为代价来提高精度。结果导致系统在关闭循环和重用以前的地图方面太慢，ORB3提出了一种新的位置识别算法。首先检查候选关键帧的几何一致性，然后检查与三个可共视关键帧的局部一致性，这三个共视关键帧在大多数情况下已经在地图中。这种策略提高了查全率，增加了数据关联度，提高了地图精度，,但代价是计算成本稍高。
• 多地图系统Atlas，Atlas由很多的子地图组成，每个子地图都有自己的关键帧、地图点、公视觉图和生成树，并在其上平滑地应用所有的制图操作：位置识别、相机重新定位、闭环和精确的无缝地图合并。每个子地图的初始参考定义为建立这个地图的第一个相机中。atlas的使用使得系统自动使用和组合在不同时间构建的子地图（在子图之间检测到公视关系时将子图整合成更大的子图），执行增量多会话SLAM。每个传入的当前关键帧只更新atlas中的一个子地图称之为活跃地图，其他统称为非活跃地图。同时atlas包含所有地图的DBOW2识别数据库，用于识别地图中任意关键帧的所有信息。
• An abstract camera representation使SLAM与所使用的相机模型无关，并允许通过提供投影、反投影和雅可比函数来添加新模型。并提供了针孔和鱼眼模型的实现。

---

3、创新点详解

A：相机模型

ORB_SLAM3 提供针孔模型和鱼眼模型的实现。并优化了以下问题：

• Relocalization 重定位。 一个鲁棒的 SLAM 系统需要具有在跟踪失败时的重新定位能力。ORB-SLAM通过ePnP算法来解决这个问题，但是受限于标定的针孔相机模型（连接1）。因此为了达到相机模型独立于SLAM系统的目的，ORB_SLAM3使用MLPnP 算法，该方法以反投影光线作为输入。只要新的相机模型提供反投影函数，并可以完成重定位功能。
• Non-rectified Stereo SLAM 非立体矫正的双目 SLAM。大多数 SLAM 系统假设双目图像数据均已经完成了立体矫正（rectified）。但是这种假设有很大的局限性，在很多情况下不是很适合，比如发散的立体像对或者立体鱼眼镜头，需要有较大的图像裁剪，丢失大视场或大 FOV 的优势。因此，作者提出的系统不依赖于立体矫正，而是将双目看作具有如下性质的两个单目相机：两个相机有恒定的欧式变换SE(3) 视线范围具有重叠区域

当双目具有共同观测区域，则可以使用双目三角化真实的landmark scale。否则就当作多视图几何原理进行三角化。

总结 在SLAM系统中相机模型的主要功能有：

• 最小化重投影误差的时候计算3D-2D之间的投影关系（投影函数）；
• pnp里程计求解时计算2D-3D的反投影关系（反投影函数）；
• 优化上述两种误差模型的时候，需要用到非线性最小二乘进行迭代求解，因此需要知道上述两种函数的雅可比函数；
  因此，对于任意的一个相机模型只要给出投影函数、反投影函数以及对应的雅克比函数就可以完成对该相机模型的建模。

B：视觉惯性SLAM部分

1）基本原理

由于单目视觉SLAM没有尺度信息，可以通过增加惯性传感器IMU解决，imu的引入为目标优化函数带来额外的约束，最常见的约束即将一段时间内 IMU 构建的预积分量作为测量值，对两时刻之间的
状态量进行约束。获得惯性残差。在纯视觉SLAM中，估计状态仅包括当前相机姿态，而在视觉惯性SLAM中，需要计算其他变量。这些包含主体body姿态和在世界坐标系下的速度vi，以及陀螺仪的偏差big和加速度计的偏差bia——假定根据布朗运动而演化。这导致状态向量的表示方式为：

imu状态预测模型可以表示为：

即从第 i 时刻的PVQ 对 IMU 的测量值进行积分得到第 j 时刻的PVQ，预测模型存在：每次i处位姿被优化更新后，后续的状态量都需要重新进行积分，运算量较大，因此通过公式将预测模型转化为预积分模型：

转化后PVQ公式中的积分项则变成相对于第i 时刻的姿态，而不是相对于世界坐标系的姿态，

两侧相减即可获得预积分惯性残差：

等价于论文中：

连同重投影误差，VIO总的优化函数可以表示如下，对应的因子图如图（a）所示

2）IMU初始化

由于上述预积分模型会受到重力方向g和IMU偏差bias的影响因此在需要对IMU进行初始化，获得惯性变量获取良好的初始值：机体速度v，重力方向g和IMU偏差bias。针对ORB-SLAM-VI 系统存在的受限于针孔单目针孔相机、imu初始化很慢且在部分场景会失败等缺点，进行了以下改进：

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

新的IMU初始化主要分为以下三个步骤。

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

2. Inertial-only MAP Estimation
   在这一步中，我们的目标是在MAP估计的意义上，仅使用Vision-only中的T-和这些关键帧之间的惯性测量值(预积分量)来获得惯性变量的最佳估计。这些惯性变量可以堆叠在仅惯性状态向量中
   
   其中s是仅视觉解决方案的比例因子scale，Rwg是重力方向，用两个角度表示，因此世界参考中的重力矢量为g=Rwg*gi,gi=(0,0,G),G为的重力大小；b，是在初始化期间假设加速度计和陀螺仪恒定的偏置值，v是从第一个关键帧到最后一个关键帧的up-to-scale的body速度，最初是根据Vision-only中的T-估算的。
   在高斯误差分布且测量独立假设下，通过对最大后验取负对数将其转化成为最小二乘进行处理
   如图2c所示，此优化与VIO目标函数的不同之处在于：这里不包括视觉残差，但包含施加给IMU bias的、接近于零的先验残差rp，它的协方差由IMU的特性给出。（rp就是先验，后面那个就是针对每一次IMU预积分的求和。这个y函数的最小值就是我们所需要估计的参数的最大后验估计）。由于是在流形中进行优化时，需要定义一个回缩 retraction [62]用以在优化过程中更新重力方向估计（在优化上为了保证旋转矩阵还在流形上，我们整了指数变换来更新这一李群）：
   
   仅惯性优化完成后，帧的姿态和速度，3D地图点就会以估计的比例s缩放，并旋转以使world系z轴与估计的重力方向对齐。由于Bias被更新，因此需要重复计算IMU预积分，以减少将来的线性化误差。初始化过程先验选取中，IMU的bias设置为0，使之不会偏离0太远，且初始的协方差矩阵按照IMU的datasheet设定。当完成了IMU的MAP估计后，纯视觉的尺度被对齐，并且重力方向上也与Rwg旋转后的方向对齐，Bias值也得到了一个合理的估计。

3. Visual-Inertial MAP Estimation
   单目系统一旦系统对惯性和视觉参数有了良好的估计，就可以执行联合视觉惯性优化以进一步完善解决方案。该优化可以表示为图2a——对于所有关键帧，具有共同的偏差；与仅惯性步骤相比，包括相同的先验信息。但bias是不变的。且IMU方面的先验是和纯惯性估计的先验是一样的。
   对于双目通过将比例因子固定为1并将其从仅用于惯性的优化变量中删除，可以轻松地将monocular-inertial初始化扩展为stereo-inertial初始化，从而增强其收敛性。

C：跟踪和建图

Tracking解决了一个简化的视觉惯性优化问题，只优化最后两帧的状态，而地图点保持不变。
Mapping使用滑动窗口内的关键帧及其地图点作为可优化变量，包括优化关键帧的共视帧，但保持它们固定。（此处的滑动窗口与VINS中并不相同，此处主要滑窗主要包含当前关键帧，当前关键字的共视关键帧，以及当前关键帧共视关键帧的共视关键帧（仅作为约束不参与优化））
在某些特定情况下，当运动过慢不能提供良好的惯性观测时，初始化可能仅在15秒内无法收敛到精确解。为了在这种情况下获得鲁棒性，ORB3提出了一种新的尺度优化策略：基于改进的仅惯性优化,括所有插入的关键帧，尺度以及重力方向作为估计参数(子图的图d)，这种情况下，并不是将IMU的偏差Bias设定为恒定。而是使用每个帧的估计值，然后对其进行修复。这种优化的计算效率非常高，它每十秒钟在本地映射线程中执行一次，直到映射自初始化以来已超过100个关键帧或超过75秒为止。

D：跟踪丢失的稳健性

在纯视觉SLAM或VO系统中，暂时的相机遮挡和快速运动会导致失去视觉元素的跟踪，从而使系统迷路。 ORB-SLAM率先使用了基于词袋位置识别的快速重定位技术，但事实证明它们不足以解决EuRoC数据集中的困难序列。当跟踪少于15个点的地图时视觉惯性系统会进入视觉丢失状态，由于IMU的引入ORB将丢失状态又细分成短期丢失和长期丢失。
短期丢失：根据IMU读数估算当前的body状态，并在估算的摄像机姿态中投影地图点，并在Localmapping大滑动窗口中搜索匹配项。所得的匹配项包括在视觉惯性优化中。在大多数情况下，这可以恢复视觉跟踪。否则，在5秒钟后，我们进入下一个阶段。
长期丢失：如上所述短期丢失无法解决，一个新的视觉惯性地图图将被初始化，并成为活动图(active map)。

E：跟踪丢失的稳健性地图合并和回环检测

tracking和mapping线程通过将地图点投影到估计的相机位姿中，在附近几个像素的图像窗口中搜索匹配项，此来找到帧和活动地图之间的短期和中期的数据关联，短期数据关联能获得较为准确的里程计数据，但是对于跟踪丢失的时候就无能为力了。因此系统需要具备长数据关联能力。为了实现重定位和回环检测的长期数据关联，ORB-SLAM3使用DBoW2词袋位置识别系统，BoW2用它们的词袋向量建立一个关键帧数据库，且给定一个查询图像能够根据它们的词包高效地提供最相似的关键帧。
提及闭环检测必须提到两个衡量指标，召回率和准确率，召回率：系统所有的闭环中有多少闭环能通过检测算法被正确的检测出来。准确率：闭环检测算法检测到的闭环中有多少是真正正确的闭环。
本文提出了一种新的具有改进召回率的位置识别算法，用于长期和多地图数据关联。每当建图线程创建一个新的关键帧时，就会启动位置识别，尝试检测当前关键帧与地图集中关键帧的匹配。如果找到的匹配关键帧属于活动地图，则执行回环闭合。否则，它是一个多地图数据关联，然后合并活动地图和匹配地图。另外的创新点在于，一旦新的关键帧和匹配映射之间的相对姿态被估计出来，定义一个local window（局部窗口），其中包含匹配的关键帧及其在共视图中的邻域。在这个窗口中，进行中期数据关联，提高循环闭合和地图合并的精度。

1) Place Recognition（位置识别）

为了获得较高的召回率，对于每个新的活动关键帧，ORB3在DBoW2数据库中查询Atlas中的几个相似的关键帧。为了达到100%的精度，每个候选关键帧都要经过几个步骤的几何验证。所有几何验证步骤的基本操作是检查图像窗口内是否存在其描述符与映射点的ORB描述符匹配的ORB特征点，并使用它们之间的Hamming距离阈值。如果在搜索窗口中有多个候选项，为了丢弃不明确的匹配项，我们检查与第二个最近匹配项的距离比。位置识别算法的步骤是：

• 1、DBoW2候选关键帧
• 2、Local window（局部窗口）.
• 3、3D aligning transformation（三维对齐变换）
• 4、Guided matching refinement（引导匹配精细化）.
• 5、Verification in three covisible keyframes（在三个共视关键帧中验证）
• 6、VI Gravity direction verification（重力方向验证）

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

2) Visual Map Merging 纯视觉地图合并

如果位置识别成功，产生了多地图之间的数据关联，在活动地图中的关键帧和地图集中的不同地图中的匹配关键帧之间，使用对齐变换进行地图合并操作。需要确保当前活跃地图中的信息能被tracking线程及时调用，避免地图重复。合并过程分为两步：缝合和优化两部分

• wlding地图缝合
  当Ka和Km的匹配发生在不同小地图之间，就需要对他们进行缝合（wlding）操作。也就是当前帧Ka找到的闭环帧Km并不在当前活跃地图中，但是找到了两个地图之间的匹配关系，利用这个匹配关系将两个子地图合并成一个大地图。为此将Ka、Km两帧、Ka和Km所有共视帧、所有这些帧的特征点都放进缝合窗口（welding window）。在将地图Ma里面的特征点放进窗口前，将所有特征点均通过Tma变换到Mm地图（被召回的地图）下。在地图缝合时，需要将两图中重复的特征点进行剔除：对Ma中的每个特征点都在Mm中寻找其匹配点，如果找到则把Ma中的该点去除，并将这次观测放入Mm中去。同时，共视图也应当进行更新。
• 地图优化
  缝合完之后需要进行优化，调整合并后大地图的内部关系。优化分为两块：缝合优化和整体优化。
  • 缝合优化：将缝合窗口之外的部分进行固定，再在缝合窗口内部进行一个BA优化，缝合后的地图就可以用于追踪新来的帧了也就是合并后的地图成为当前活跃地图
  • 整体的位姿图优化：为了提升窗口内外的一致性，利用地图缝合减小整体的累计误差，在优化时，缝合窗口内被固定，窗口外采用essential graph进行优化。此时，回环矫正从缝合窗口传递向整个地图，缝合优化就完成了

3) 视觉惯性地图合并

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

1. IMU的引入是的地图具有尺度信息此时计算SE(3)，如果尺度信息不可用需要用相似变换Sim(3))将Ma中的点对齐到Mm。

2. 优化的变量和固定的变量如图所示
   测

参考：
epnp: https://blog.csdn.net/jessecw79/article/details/82945918
MLPNP https://blog.csdn.net/qq_39266065/article/details/115614421
https://zhuanlan.zhihu.com/p/181363155
https://blog.csdn.net/shyjhyp11/article/details/109540631
https://blog.csdn.net/shyjhyp11/article/details/110621998