ORB-SLAM3论文（三）

ORB-SLAM3（三）

一、系统概述

上图为ORB-SLAM3的系统框架图，可以看出新增部分如下：

（一）Atlas（地图集）

如图中所示，Atlas包括一系列分离的地图组成的多地图表示（包括active map 和 non-active-map）。其中一个active map（活动地图）表示当前位于的map，tracking线程向其中传入帧，并由local mapping不断优化以及增加新的关键帧扩大规模。其他地图为non-active-map（非活动地图）。该系统建立了一个独特的DoBW2关键帧数据库，用于重定位、回环和地图融合。

（二） Tracking thread（跟踪线程）

判断当前帧是否为关键帧；实时处理传感器信息并计算当前姿态；最大限度地减少匹配地图特征的（reprojection error ）重投影误差；在VI模式中，通过加入（bias）惯性残差来估计物体速度以及惯性测量单元偏差。当地图跟丢时，tracking 线程利用Atlas进行重定位，若成功则继续跟踪，并在必要的时候切换active map（活动地图）；若失败，则短暂延时后将当前地图存储为non-active-map，并新建一个active map（活动地图）进行建图。

（三）Local mapping thread（局部建图线程）

将关键帧和地图点添加到活动地图，删除redundant（多余关键帧），并使用视觉或视觉惯性BA优化地图。另外此线程也包括惯性情况下，利用MAP估计初始化和优化IMU参数。

（四）Loop and map merging thread（回环和地图融合线程）

基于关键帧的速度，对地图集中的active map （活动地图）和 non-active-map（非活动地图）进行相似性度量。如果相似区域处于活动地图，则进行回环校正；若属于不同的地图，则将两个地图进行无缝拼接，并作为活动地图。回环校正时，另开一个线程进行全局BA。

二、相机模型

ORB-SLAM3默认所有系统组件都是针孔摄像机模型，这样通过提取与摄像机模型相关的参数（projection and unprojection functions, Jacobian等）抽象出摄像机模型。同时ORB-SLAM3还提供了鱼眼模型。下面是为了抽象相机模型做出的调整：

（一）Relocalization（重定位部分）

之前ORB-SLAM当跟踪失败的时候，系统会启动重定位。 找出一些候选关键帧，对每个候选关键帧，用ransac和EPNP估计位姿，然后更新当前帧的地图点匹配，然后优化位姿，如果内点较少，则通过投影的方式对之前未匹配的点进行匹配，再进行优化求解，直到有足够的内点支持，重定位完成。

但是ePnP算法前提是calibrated pinhole camera（针孔相机），为了兼容ORB-SLAM3对相机模型的抽象，采用了MLPnP（极大似然透视n点算法）。该算法使用投影光线作为输入，相机只需要提供一个从像素到投影光线的非投影函数即可重定位。

（二）Non-rectified Stereo SLAM（非修正双目SLAM）

目前的双目SLAM都将双目相机理想化：即两图像都被转换成针孔投影，拥有相同的焦距、图像平面共面、与水平极线对齐。这种假设并不适用，比如鱼眼相机会因此丧失大视角的优势。因此可以考虑以下两点：

1、左右两相机之间的恒定相对SE（3）变换，

2、两相机观察场景的公共图像区域。

这样有利于我们进行三角化以及计算BA，进而有效地估计地图比例。因此，ORB-SLAM中估计了一个6-DoF（自由度）的刚体姿态，

如果两个相机都有一个重叠区域，我们可以在第一次看到真实比例的地标时对其进行三角测量。

如果无重叠区域，则作为单目使用：从多视图进行三角化的。

三、视觉惯性融合SLAM

ORB-SLAM-VI是第一个真正能够重复使用地图的、视觉惯性SLAM，但是仅限与单目且初始化较慢。ORB-SLAM3建立在ORB-SLAM-VI上，提供了一种快速精准IMU初始化策略。

上图为各种情况下的因子图

（一）基本原理

1、待优化变量–>同之前：

2、连续视觉帧i和i+1之间IMU预积分：

3、xj处相机与3D landmark（地图点）的视觉重投影误差：

4、结合惯性项和视觉残差项，可视惯性SLAM可以作为基于关键帧的最小化问题。优化变量为：

前者为惯性残差，后者为附加鲁棒核函数的视觉残差（目的减少误匹配带来的影响）。此处可由上图（a）表示。

（二）IMU Initialization(IMU初始化)

IMU初始化的目的为获得惯导的良好初始值：速度、重力方向和IMU的bias。

初始化要求：1、先解决视觉问题有利于IMU初始化；2、尺度显示表示为优化变量将加快收敛速度；3、不能忽略IMU传感器的不确定性。

基于MAP估计的IMU初始化：

1、纯视觉地图估计：初始化纯单目SLAM，并设定在2秒内运行，在4Hz处插入关键帧。在这段时间之后，我们得到了一张由k=10个相机姿势和数百个点组成的高比例地图，该地图使用纯视觉BA进行了优化（因子图中的图b）。这些姿势转换为body坐标系，得到轨迹
，其中横杠表示最大比例变量。

2、纯惯性地图估计：这一步目的是获得惯性变量的最佳估计，在MAP估计中，仅使用关键帧之间的
和惯性测量。将其与纯惯性状态向量叠加：

上式中，s为视觉求解的比例因子，Rwg为重力方向，b为加速度计、陀螺仪偏差，
为从第一个关键帧到最后一个关键帧的最大速度。

若只考虑惯性测量值

，文中描述了一个MAP估计问题，其中需要最大化的后验分布是：

假设IMU预积分和先验分布的高斯误差，则最终优化问题为：

当在流行空间中优化时，本系统定义了一个retraction，以便更新重力方向估计：

为了保证在优化过程中比例因子保持正值，我们将其更新定义为：

惯性优化完成后，帧的姿态和速度以及三维地图点将按估计的比例缩放并旋转以使z轴与估计的重力方向对齐。重复的修正偏差和IMU预积分，以减少未来的线性化误差。

3、视觉惯性地图估计：一旦完成惯性参数和视觉参数的估计，接下来就是视觉惯性融合进行优化（因子图的图a），但是对于所有关键帧都有共同的偏差，并且包括与仅惯性步骤相同的先验信息。

（三）Tracking and Mapping（跟踪和建图）

Tracking解决了一个简化的视觉惯性优化问题，只优化最后两帧的状态，而地图点保持不变。

Mapping使用关键帧及其地图点的滑动窗口作为可优化变量，包括其共视帧的关键帧，但保持它们固定。

对于因为运动过慢无法正确初始化的情况，本文提出了一种新的尺度优化策略：基于改进的仅惯性优化，包括所有插入的关键帧，将尺度以及重力方向作为估计参数（因子图的图d）。

（四）Robustness to tracking loss（对tracking丢失的鲁棒性）

ORB-SLAM开创了基于词袋位置识别的快速重定位技术，但仍然不够鲁棒。ORB-SLAM3跟丢目标时，将在两个阶段实现鲁棒性：

1、Short-term lost（短期丢失）：当前位姿根据IMU进行估计，地图点被投影到估计的相机位姿中，并在一个大的图像窗口中搜索匹配。结果匹配包含在视觉惯性优化中。若失败则进入下一阶段；

2、Long-term lost（长期丢失）：重新初始化一个视觉惯性地图作为active map。