视觉SLAM开源算法ORB-SLAM3原理与代码解析

ORB-SLAM最早的版本在2014年的RSS上发布，在2016年作者又发布了ORB-SLAM2,接着在2020年发布了ORB-SLAM 3。ORB-SLAM1只能针对单目相机数据进行处理；ORB-SLAM 2 增加了对于双目和RGB-D相机的处理，在回环检测模块增加了Full Global BA的处理；ORB-SLAM 3则增加了对于IMU融合的支持，兼容鱼眼相机模型，并且增加了Altas多地图的支持；同时，回环检测为了支持多地图的模式，提供了一种叫Welding BA的优化方式。ORB -SLAM的作者将上述提到的各版本项目都开源了，为学术研究还是工程落地都提供了很好的参考。

01

基础概念

首先来介绍一下ORB SLAM中涉及的一些基础概念。

帧&关键帧：视觉SLAM都是对一个图像序列进行处理，每一张图像被称为帧，而关键帧则是通过一定筛选机制得到的、具有一定代表性的图像帧。

地图点/路标点：将图像上被观察到的特征点通过三角化等方式进行深度恢复，我们就可以得到其对应的在三维空间的位置，同时包含帧的观测信息，这种点就被称为地图点或路标点。

共视：当一个地图点被多帧观察到时，我们就可以称这几帧有基于该地图点的共视关系。

共视图&本质图:我们可以把共视关系作用边表示，关键帧用节点表示，就可以建立共视图，而本质图在共视图基础上只保留具有较强共视关系的边。

Altas（地图集）：ORB-SLAM 3提供了多地图的存储和拼接功能，在跟踪丢失后可以尝试将现有地图和历史地图进行匹配、融合，并更新当前的活跃地图（Active Map）。

数据关联：在语义SLAM中，第k帧检测到物体Obj1、Obj2，第k+1帧检测到物体Obj3、Obj4，确定Obj1和Obj3、Obj4中的哪一个是对真实世界中同一个物体的多次观测，这是数据关联的一个直观例子。在间接法（特征法）SLAM中，表现为不同帧中的特征，哪些是对应于同一个空间路标点/地图点的。在ORB3中考虑到的数据关联包括短期内滑动窗口中关键帧观测到的路标点和图像特征的数据关联；中期的数据关联是指图像特征与局部地图点的关联；长期的数据关联包括利用场景识别技术和词袋模型，在回环检测、重定位等过程中的数据关联；而多地图的数据关联还可以实现地图之间地图点的匹配和融合。

ORB-SLAM 3的基本流程和此前的ORB版本没有显著的改变，只是也增加了部分新特性。基于词袋模型的关键帧数据和之前差不多，每一个关键帧均会被存入数据库用于回环检测。地图结构上进行了改进，ORB-SLAM3使用Altas地图集的结构，地图中包含一个Active Map和若干个Non-active Map，每个Map均包括地图点，关键帧，共视图，Spanning Tree等信息。跟踪线程添加了IMU的积分，并且和以前一样进行关键帧的判断和构造；在LocalMapping线程中中执行IMU的初始化，以及和之前相同的冗余关键帧和地图点剔除、新地图点的创建等工作；在回环检测部分主要添加了多地图的融合。

ORB-SLAM 3框架

总结一下，ORB-SLAM3的贡献包括：

1、提供了一个单双目VI-SLAM的系统；

2、改善召回率的场景识别技术；

3、多地图机制；

4、抽象的相机表示。

02

抽象相机模型介绍

为什么ORB-SLAM3需要一个抽象的相机模型呢？

相比于传统相机，鱼眼相机超过180度的广视角可以获取更多的信息，但是因为它不符合针孔模型数学建模的假设，导致uniform reprojection error的假设失效；如果对于图像直接进行裁剪，将会导致外围图像丢失，反而丧失了鱼眼相机大视角的优势。在ORB-SLAM3中，相机成像模型提供投影、反投影和相关的雅克比计算等函数，并且将此前系统中的EPNP更换为MAP-PNP, 从而实现了相机成像模型与SLAM部分的解耦，还可以扩展，能将相同的SLAM pipeline用于大部分类型的相机。

此外，针对双目相机模型，ORB-SLAM3也提供了一定的改善。ORB-SLAM2假设我们针对双目相机预先进行了极线矫正，但是很多时候由于无法保证左右目相机光轴的绝对平行，极线矫正的效果也往往不好；而有些时候，我们需要使用两个参数不同的相机进行观测，而ORB-SLAM2无法兼容这类双目相机，如类似RGB-D相机中焦距、分辨率相差巨大的彩色相机+近红外相机，如果将彩色图像和近红外图像组成双目图像，ORB-SLAM2无法综合利用这对图像估计相机位姿。在ORB-SLAM3中将左右两目相机视作为具有固定位姿变换的两台单目相机使用，并且也不再限制两台相机必须具有足够面积的、重叠的共视区域，解决了这个问题。

03

VISLAM实现和IMU初始化

ORB-SLAM3中VI-SLAM在ORB-SLAM-VI上进行了改进，包括：提供快速，准确的IMU初始化；支持单双目VI-SLAM；支持针孔/鱼眼相机模型。在视觉和IMU融合方面，ORB-SLAM3在位姿求解时所建立优化问题的残差项，包括所有关键帧和上一帧IMU估计的残差项，以及所有路标点观测的视觉误差项。其中针对视觉路标点的观测，为了避免错误匹配造成的极端值的影响，嵌套了鲁棒核函数。

IMU初始化的目的是为了得到Body系速度、重力方向和IMU偏置。ORB-SLAM3中初始化流程的设计建立在作者的几点思考上：

1、ORB-SLAM纯单目已经可以初始化得到精确的地图，尺度信息可以通过IMU得到；双目图像输入下则尺度客观，可以不考虑尺度信息的问题；

2、如果将尺度单独作为优化变量进行表示和优化，效果比在BA中的隐式表达收敛更快；

3、IMU初始化过程中必须考虑传感器的不确定性，否则会产生难以预测的巨大误差。

接下来的讨论IMU初始化问题时，均指单目输入时的初始化。ORB3中IMU初始化的步骤包含三步，第一步是纯视觉最大后验估计（MAP），第二步是纯惯性MAP，第三步是视觉+惯性MAP。针对纯视觉MAP，我们提取初始化后2s内10帧图像进行纯视觉BA，从而得到没有尺度信息的相机位姿和路标点位置。接下来我们进行只有IMU参与的初始化，最终得到的优化结果是：帧位姿、速度和地图点，并都具有正确的尺度；Body系Z轴将被旋转到和重力方向一致；IMU的偏置被更新。第三步是视觉IMU联合后验估计，ORB-SLAM3只需要2秒就可以完成尺度的初始化，误差在5%左右，此外，ORB-SLAM3还将进行只包含尺度因子和重力方向的优化，10秒一次，用于避免传感器运动缓慢时IMU激励不够的情况。

ORB-SLAM3中的跟踪和建图和ORB-SLAM-VI类似，在短期跟丢后，在满足一定条件时会尝试利用IMU积分得到的位姿信息进行重定位；当丢失持续一定时间后，将会重新初始化，创建新的Active map。

04

改进的回环检测与多地图融合

这一部分的内容很大程度上和ORB-SLAM2是相同的，我们首先来回顾一下基本概念。

准确率（PrecisionRate)：检测到的回环中正确的比率。

召回率（RecallRate）：检测到的回环占总真实回环数的比率。

在ORB-SLAM1/2中，仅通过DBoW词袋数据库就可实现50%~80%的准确率和召回率。在回环时，通过增加几何一致性和时间一致性检验，牺牲召回率来增加准确率。ORB-SLAM3改进了回环检测的速度，提高了召回率，并且增加了多地图的部分。

寻找闭环帧的过程可以分为六步：

1.针对每一个新关键帧，在数据库中查询到三个最相似的关键帧；

2.尝试对新关键帧及其共视关键帧，和候选关键帧及其共视关键帧进行数据关联；

3.利用匹配的特征点和地图点求解位姿转换；

4.利用位姿变换的初始估计，进行点云重投影寻找新的匹配，并且进行位姿的优化求精；

5.对时间一致性的检验，此前的步骤相对复杂，在ORB-SLAM3中局部地图里面已有关键帧的共视信息进行判断；

6.利用重力方向对于回环结果进行检查。

回环检测后是进行回环还是地图合并，取决于当前关键帧检测到的回环关键帧是在当前的active map还是在其他map。当对non-active map和active map进行融合时，共视图和本质图同步更新,active map中的信息被追加到历史地图中，匹配到的non-active map变成新的map。

05

总结

总体来说，ORB-SLAM3的流程和ORB-SLAM1/2非常相似，对于ORB-SLAM系列熟悉的同学应该很容易上手；相机模型的抽象处理，使得SLAM位姿求解过程和相机成像模型解耦，理论上支持绝大多数成像模型的相机；通过对于IMU的支持，ORB-SLAM系列加入了VI-SLAM的大家庭，也表明多传感器融合的SLAM是目前一大发展趋势；多地图的机制有利于跟丢后保留尽可能多的信息用于后续补救，也为后续实现多机器协同的SLAM提供了工作基础。