时隔多年，ORB-SLAM3终于来了

重磅干货，第一时间送达

首先回顾一下历史：ORB-SLAM首次在2015年被提出，它的改进版ORB-SLAM2在2017年被提出，同年提出了ORB-SLAM-VI，时隔3年，ORB-SLAM3横空出世，朋友圈、学术群里到处都在热议这个挂在Arxiv才不到3天的论文。好奇心的驱使下，本人偷瞄了一下论文，就在这里总结一下吧。

开始之前，先放两条ORB-SLAM3的展示视频撑撑场面。

ORB-SLAM3是一个基于视觉惯导紧耦合的系统，能够对多地图进行复用；另外支持单目/双目/RGB-D作为输入，支持针孔以及鱼眼相机模型。

本文第一个创新点是提出了一种基于特征点法的视觉惯导紧耦合SLAM系统，这套系统在很大使用了最大后验估计对系统状态量进行求解，即使是在系统初始化阶段亦是如此。这套系统可在大/小/室内/室外各种环境下鲁棒实时运行，相较于之前的算法有2~5倍的精度提升。

本文第二个主要创新点在于提出了多地图复用技术，该技术使用了一种高召回率的场景识别算法。正是依赖于这一点，ORB-SLAM3能够有效应对长时弱纹理的环境：若系统丢失，它重新开始建图并在当经过之前走过的地点时与原来的地图无缝融合。另外，相比于其它仅用到之前一段时间信息的VO方案，本文提出的方案用到之前所有时刻的信息。这样可以利用到很久之前或者来自不同地图的信息。

实验表明，本文提出的算法在学术领域已有的SLAM系统中表现最优（这句话说的很严谨，没有提到工业界，因为没开源）。

ORB-SLAM3基于ORB-SLAM2以及ORB-SLAM-VI进行改进，上图是其系统框图。这基本上保持了与ORB-SLAM2类似的框架，同时增加了前述的几个创新点，接下来对其中主要步骤需要进行简要说明。

Atlas(地图集)

所谓Atlas就是一种多地图的表示形式，多地图由不相连的地图组成。可以看到，它由active map以及non-active map组成。其中active map被Tracking thread（追踪线程）用作定位，它被连续优化同时会加入新的关键帧。系统会构建基于DBoW2的关键帧数据库，可以用来做重定位/闭环以及地图融合。

追踪线程

与ORB-SLAM2类似，该线程用来处理传感器信息，计算当前帧相对于active map的位姿以及最小化匹配到的地图点的重投影误差。该线程决定何时当前帧被判定为关键帧。在VI模式下，机体的速度以及IMU的bias通过优化惯导残差被估计。当系统追踪丢失后，会触发重定位模式，即当前帧在所有的Altas进行重定位；若重定位成功，当前帧恢复追踪状态；否则，经过一段时间（超过5秒），当前的active map会被存储为non-active map，同时开启新的建图过程。

局部建图线程

向active map中新增/删减/优化关键帧以及地图点，上述的操作是通过维护一个靠近当前帧的局部窗口的关键帧进行实现。与此同时，IMU的参数被初始化然后被该线程通过本文提出的最大后验估计技术进行求解。

闭环和地图融合线程

该线程检测active map与整个Atlas是否有共同的区域，若共同的区域同属于active map，此时进行闭环矫正；若共同的区域属于不同的map，那么这些map就会被融合（融合为1个）,并变为active map。BA被一个独立的线程执行（不损害实时性），用来修正地图。

ORB-SLAM假设所有系统组件都符合针孔模型。对于重定位这项任务，ORB-SLAM基于EPnP算法建立PnP求解器进行解算位姿，其中所有的公式都假设相机为标定好的针孔相机。为适配本文的方法，作者使用了一种独立于相机模型的求解器：Maximum Likelihood Perspective-n-Point algorithm (MLPnP)（最大后验PnP算法）。该算法实现了相机模型与求解算法的解耦，相机模型只是用来提供投影射线作为输入。

非矫正双目SLAM

几乎所有的SLAM系统都假设双目图像是已经被矫正的，这里的矫正是指，使用相同的焦距将两个图像转换为针孔投影，且像平面共面，同时与水平对极线对齐，从而可以通过查看图像中的同一行进行特征匹配。然而，这个假设是及其严格的，在很多应用中，这个假设通常难以满足或则不合适使用。例如，矫正（一对基线较大的相机）双目相机或双目鱼眼镜头将需要对其拍摄的图像进行严格的图像裁剪，从而失去了大视场角的优势，所以我们需要更快的环境映射方式以应对遮挡等情况。

本文设计的系统并不依赖于图像的矫正，而是将双目设备看作两个单目相机，于是有如下约束：

1. 两个单目相机之间存在一个固有的SE(3)变换（即双目外参）；
2. 两个单目相机之间存在共视；

上述两个约束可以用来有效地估计尺度。按照这种思路，ORB-SLAM3可估计6DOF刚体机体位姿，注意，机体位于某一个相机或者IMU上。另外，若两个相机之间存在共视，我们可以在通过三角化恢复路标点的尺度。注意：仅在首次看到该其区域的路标点时进行三角化，其它时刻只使用单目信息。

之前的ORB-SLAM-VI受限于针孔相机/初始化时间太长以及无法应对某些挑战场景。本文设计了一种快速准确IMU初始化技术，以及开发了一种单目/双目惯导SLAM，支持针孔以及鱼眼相机图像作为输入。

原理

纯视觉SLAM的状态量仅有当前帧的位姿，而在VI-SLAM中，增加了一些需要被估计的状态量。除了机体在世界系下的位姿（这里是指）还有速度，以及陀螺仪和加速度计的bias: 。我们将这些状态量合在一起构成了如下状态向量：

对于视觉惯导SLAM而言，我们通过对IMU的测量进行预积分，可以获得连续两帧（如第到第帧）之间的相对位姿测量：，，以及整个测量向量的信息矩阵。于是给定上述预积分项以及状态向量以及，我们就可以得到如下IMU残差：

此外，除了IMU残差还有帧与3D点之间的视觉残差，即重投影误差：

其中是3D路标点在当前帧上的投影，信息矩阵为，表示机体系到相机的刚体变换。给定个关键帧及其状态量，同时给定个3D点集，它的状态量为，于是该优化问题可表示为IMU残差以及重投影误差的组合，具体如下形式：

其中表示观测到第个3D点的关键帧集合。这个优化可以用因子图进行表示，如下图。

IMU

这一步的目的是获取IMU参数较好的初始值：速度/重力向以及Bias。本文的工作重点是设计一种快速准确的IMU初始化算法。文中提到如下3个见解：

1. 纯单目SLAM可以提供非常准确的初始地图，问题是尺度未知。首先求解vision-only问题可以提升IMU初始化的性能。
2. 如果将尺度scale显式表示为优化变量，而不是使用BA的隐式表示，则scale收敛得更快。
3. 若在IMU初始化过程中忽略传感器不确定性会产生大量不可预测的错误。

根据以上三点，本文设计了如下基于最大后验估计的IMU初始化算法，主要分为如下三个步骤：

Step1: Vision-only MAP Estimation

进行单目SLAM，按照关键帧速率4Hz持续运行2s，然后我们可以得到按比例缩放的地图，包括个关键帧以及上百个地图点，然后通过Visual-Only BA进行优化，因子图如下图所示。于是可以得到优化后的轨迹，其中上划线表示按比例缩放的变量（即尺度未定）。

Step2: Inertial-only MAP Estimation

这一步的目的是获得IMU参数最优估计。利用以及这些关键帧之间的IMU测量，IMU的参数可以放在一起构成状态向量：

其中的表示尺度因子，表示重力向，由两个角度表示，重力向量在世界系下的表示为，其中，是重力大小模值；表示加速度计以及陀螺仪的bias，在初始化阶段可以假设该值为常数；表示从首帧到末尾帧的无尺度机体速度。此时，我们有IMU的测量，于是MAP估计问题中的需要被最大化的后验分布为：

具体地，表示似然，表示先验，考虑到IMU测量之间iid，于是最大后验概率估计问题可以表示为如下形式：

对上式取负对数并且假设IMU预积分以及先验分布服从高斯分布，于是最终的优化问题可以表示为：

这个优化问题的因子图表示为下图，可以看到上式中不包含视觉残差，而是多了一个先验的残差项用来约束IMU的bias需要接近于0。

由于上述优化在流形上进行，重力向的在优化过程中的更新需要构建一个"retraction"：

其中表示从到指数映射。为了保证优化过程中尺度因子保持正数，尺度因子的更新形式为如下形式：

Inertial-only MAP Estimation完成之后，帧位姿/速度以及3D点根据估计的尺度进行调整，同时将轴对齐重力向。

Step3: Visual-Inertial MAP Estimation

一旦视觉以及IMU有了较好的估计后，进行一个VI联合优化进一步对这些参数进行精化，优化因子图见下图。作者在EuRoC数据集上进行测试发现上述初始化方式非常有效，可达到2秒内仅5%的误差。为了进一步提升初始估计精度，初始化后会进行5~15秒的VI BA优化，这样可以收敛到仅1%的尺度误差。相较于ORB-SLAM-VI需要15秒才获得首个尺度因子更加快速。经过了这些BA操作之后，我们就认为这个map是mature（成熟的），意思就是尺度/IMU参数/重力向已经被较好地估计完成。

追踪与建图

追踪过程借鉴了VI-SLAM-VI的思路：追踪过程解决了简化版的VI优化问题，只优化最后两帧位姿，同时保持地图点固定。

建图过程是为了解决全图优化问题，若图的规模比较大，这个问题会变得很棘手。本文采用了滑动窗口的思想，即维护了关键帧与地图点的滑动窗口，同时包括它们的共视关键帧，只是在优化时需要保持这些关键帧固定状态。

慢速运动不能为IMU参数的提供好的可观性，会使初始化无法在仅15秒内收敛到精确的结果。为了应对这种情况，本文基于修改版“Inertial-only MAP Estimation”的提出了一种尺度精化技术：所有插入的关键帧都参与优化，但优化量只有重力向以及尺度因子，优化因子图见下图，在这种情况下bias为常数的假设就不再成立。我们使用了每帧的估计量并固定它们。上述的这种操作非常高效，在Local Mapping线程每隔10秒执行一次，直到Map中有100个关键帧或者从初始化起已经过了75秒。

鲁棒性与追踪丢失

快速运动/无纹理/遮挡等会导致系统丢失，本文借鉴的ORB-SLAM采用了基于词袋的场景识别进行重定位。本文的VI系统在追踪到少于15个特征点时就会视觉丢失（visually lost），鲁棒性体现在如下两点：

1. 短期丢失：通过IMU读数对当前帧状态进行估计，地图点根据估计的位姿投影到当前帧后设置较大搜索窗口进行匹配。大多数情况下，通过这种方式能够恢复系统位姿。若超过5秒仍未重定位成功，则进入下一个状态；
2. 长期丢失：新的VI地图会被重新初始化，并成为active map。

前文的介绍可知，短期以及中期数据关联可以通过Tracking以及Local Mapping进行完成。而对于长期的数据关联，可通过重定位以及闭环实现。

ORB-SLAM采用的基于视觉词袋场景识别的重定位，若候选关键帧只有1个，召回率为50~80%。为了应对假阳性的干扰，算法使用了时域校验以及几何校验，这两种手段能够使精确率达到100%的同时召回率达到30~40%。至关重要的是，时域连续性检测将使场景识别滞后至少3个关键帧，同时召回率较低，这都是目前存在的问题。

为了应对这个问题，本文提出一种新的场景识别（召回率得到改善）以及多地图数据关联算法。一旦Local Mapping线程创建了关键帧，场景识别算法就会被激活并且寻找该帧在Atlas中的数据关联。若匹配的关键帧在active map中，则进行闭环；否则，则进行多地图间的数据关联，即将active map与匹配的map进行融合。一旦这个新的关键帧与匹配地图间的相对位姿被计算出，就定义一个在局部窗口，这个局部窗口包括匹配的关键帧以及这个关键帧的共视关键帧。在这个局部窗口中，我们会寻找中期数据关联，以提高闭环以及地图融合的精度。这个改进使得ORB-SLAM3比ORB-SLAM2具有更高的精度。

场景识别

此处较为简单，与ORB-SLAM2基本类似，只是增加了重力向校验的步骤。具体的，计算出了当前关键帧在matched map（可能是active map或者其它地图）中的位姿，检验pitch(俯仰角)和roll(横滚角)是否小于一定阈值来对场景识别结果进行校验。

视觉地图融合

当场景识别成功后，位于active map 中的当前关键帧与位于active map 中的当前关键帧产生了多地图的数据关联，此时会进行地图融合。在此过程中，必须格外小心，以确保跟踪线程可以迅速重用中的信息，以避免地图重复。

由于可能包含许多元素，融合它们可能需要很长时间，因此融合分为两个步骤。首先，在由和的邻域定义的welding window（焊接窗口）中执行融合，随后在第二阶段，通过位姿图优化将校正传播到融合图的其余部分。具体过程如下：

step1: Welding window assembly

Welding window包括当前关键帧及其共视帧，以及其共视帧，以及被这些关键帧观测的地图点。在将它们包含在Welding window中之前，属于的关键帧和地图点通过进行变换，以使其与对齐。

step2: 融合

与融合为一个新的active map。为了要删除重复的点，关键帧主动搜索匹配中的点。对于每个匹配点，都会删除中的点，并保留中的点，同时更新共视图以及本质图。

step3: Welding BA

Welding window范围内的所有关键帧进行局部BA优化（见下图）。为了确定尺度自由度，观测到的那些关键帧需要保持固定。优化完成后，Welding window区域中包含的所有关键帧都可以用于跟踪，实现地图的快速/准确复用。

位姿图利用本质图在融合后的地图范围进行优化，Welding window范围内的关键帧保持固定。这一步的意义在于分摊误差。

视觉惯导地图融合

与上述纯视觉地图融合步骤类似，只是修改了步骤1和3，具体如下：

step1: VI welding window assembly

若active map是mature的，与纯视觉类似，将对地图进行变换，与对齐。若active map不是mature的，将对地图进行变换，与对齐。

step2: 同上

step3: VI welding BA

active关键帧及其前5个时间连续的关键帧的位姿，速度和bias参与优化，对于中的及其前5个时间连续的关键帧的位姿，速度和bias也参与优化，详细见下图。对于，优化中包括但不固定在紧接局部窗口之前的那一关键帧位姿，而对于，局部窗口之前的那一关键帧，但其姿势仍然参与优化。所有这些关键帧看到的所有地图点以及观测这些地图点的关键帧姿势也都进行优化。所有关键帧和地图点都通过重投影误差项（作为约束因子，下图中的蓝色小方块）进行关联。

闭环

闭环校正算法类似于地图融合，但是在这种情况下，当前关键帧以及匹配关键帧都同属于active map。welding window是由匹配的关键帧组合而成/重复的3D点被融合/更新共视图以及本质图的连接关系。下一步是姿势图优化（PG），以均分误差。由于闭环增加了中期/长期数据关联，此时进行全局BA。在VI情况下，仅在关键帧的数量低于阈值时才执行全局BA，以避免巨大的计算量。

实验主要分为如下部分：

• EuRoC单一会话（地图）：11个场景中的每个序列产生一个地图；传感器配置：单目，单目+IMU，双目以及双目+IMU；
• TUM-VI数据：比较单目/双目鱼眼VI配置下的表现；
• 多次会话，上述所有数据；
  EuRoC单次会话

VI SLAM以及TUM-VI数据集

基于ORB-SLAM/ORB-SLAM2以及ORB-SLAM-VI，本文提出了ORB-SLAM3，它是一个功能更加完整，性能更加出众的SLAM系统。这使得该系统更加适合长时/大规模SLAM实际应用。

实验结果表明，ORB-SLAM3是第一个能够有效利用短期，中期，长期和多地图数据关联的视觉和视觉惯性系统，其精度水平已经超出了现有系统。实验结果还表明，关于精度，使用所有这些类型的数据关联的能力会超过其他选择，如使用直接方法代替特征点法或对局部BA执行关键帧边缘化，而不是像我们那样假设一组外部静态关键帧。

关于鲁棒性，直接法在低纹理环境中可能更鲁棒，但仅限于短期和中期数据关联。另一方面，匹配特征描述符可以成功解决长期和多地图数据关联问题，但与使用光度信息的Lucas-Kanade相比，跟踪功能似乎不那么可靠。在直接法中使用这四种数据关联方式是一个非常有趣的研究领域，我们目前正在根据这个想法探索从人体内部的内窥镜图像构建地图。

在四种不同的传感器配置中，双目惯导SLAM提供了最可靠，最准确的解决方案。此外，惯性传感器允许以IMU速率估算姿势，IMU速率比帧速率高几个数量级，这可能也会在某些领域发挥优势（如AR/MR等领域）。对于设备体积/成本受限等应用，可以选择使用单目惯导方案，精度与鲁棒性并不会下降多少。

在慢速运动或没有旋转和俯仰旋转的应用中，例如平坦区域中的汽车，IMU传感器可能难以初始化。在这些情况下，推荐使用双目SLAM。或者，使用CNN进行单目深度恢复的最新研究成果为单目SLAM恢复尺度提供了良好的前景，但是需要保证在同样的环境中对网络进行了训练（泛化性问题）。