ORB-SLAM3论文翻译

摘要

     ORB-SLAM3是第一个能够让单目、立体和RGB-D相机和针孔和鱼眼镜头模型解耦进行视觉、视觉惯性和多地图SLAM的系统。第一个主要的创新是一个基于特征的紧密集成视觉惯性SLAM系统，它完全依赖于最大后验概率（MAP）估计，甚至在IMU初始化阶段也是如此。第二个主要的创新是一个多地图系统，它依赖于一种新的位置识别方法和改进的召回。多亏了它，ORB-SLAM3能够在长时间的不良视觉信息下生存：当它丢失时，它会启动一个新的地图，当重新访问地图区域时，它将与以前的地图无缝地合并。与只使用最后几秒信息的视觉里程计系统相比，ORB-SLAM3也是第一个能够在所有算法阶段重用所有先前信息的系统。这允许在BA优化中使用历史的共视关键帧，即使这些关键帧来自很久以前的地图或者不同子图。该系统在小型和大型，室内和室外环境中都能稳定地运行，并且比以前的方法精确2到5倍。

引言

    当视觉帧跟踪丢失以后，orbslam3会重新记录一个新的地图，如果再次进入之前的地图区域，会与之前的地图合并。

ORB-SLAM3是基于ORB-SLAM和ORBSLAM Visual-Inertial构建，支持抑制轨迹精度发散的短期，中期和长期的数据关联。ORB-SLAM3在次基础上，更进一步，支持历史地图的复用。从而，真正达到一个全局的slam系统：建立一个地图实现精确的定位。

1. 单、双目的vo/vio slam系统，并支持鱼眼相机。
2. 地图复用，拥有合并地图的功能。
3. 高召回的地点识别（占用较低的计算资源换得高召回与精度）
4. 不限制相机模型，只需提供投影、反投影和Jacobian函数（常用的针孔与鱼眼模型）

相关工作

视觉SLAM

视觉惯性SLAM

多地图SLAM

系统概览

• Atlas（一个由一系列非连接的地图组成的多地图系统）
      其中包含一个活动的地图与许多非活动地图。活动地图与用于当前帧tracking线程的定位，同时会增加关键帧。该系统建立了一个独特的DBow2关键帧数据库，用于重定位 、回环 、合并地图等操作。

• 跟踪线程
      给活动的地图提供新的帧，最小化重投影误差（单帧），决定当前帧是否成为关键帧。VI模式中，通过IMU残差计算本体的速度与IMU bias。如果跟踪丢失，尝试在Atals所有地图中重定位，如果成功那个地图将成为活动的。如果几帧过后失败，则重新开始一个新的地图。

• 局部地图线程
      添加新的关键帧与MapPoint到活动的地图中，删除冗余，利用滑动窗口通过BA更新地图。VI模式中IMU的参数在这个线程初始化与更新，使用的是作者提出的最大后验估计技术。

• 回环与地图融合线程
      每添加一个关键帧，就探测活动的地图与其他地图的共有区域，如果检测到，执行回环矫正，如果不属于同一个地图，则将他们融合成一个。在矫正后另开一个线程进行整体的BA进一步更新地图且不影响实时性。检测回环的同时，完成有共视子图地图的合并等操作，这也是作者强调最终能生成一个全局一致地图的原因。

相机模型

    以往的ORB-SLAM算法假定为针孔像机模型，现在希望通过提取与摄像机模型相关的所有属性和投影函数来解耦相机模型和slam算法，从而使得系统可以使用任何摄像机模型。 在ORB-SLAM3中，除了针孔模型外，还提供了鱼眼模型。

重定位

    ORB-SLAM通过设置基于EPNP算法的透视n点求解器来解决重新定位问题，本文采用最大似然的PNP算法(MLPNP)，可以独立于所使用的摄像机模型。 相机模型只需要提供一个从像素的空间点的重投影函数，就可以使用重定位。

双目相机

    大多数双目视觉SLAM系统都是假设左右两帧已经进行过极线校正， 即这两幅图像都被转换为针孔投影，使用相同的焦距，图像平面共平面并与水平极线对齐，这样图像中的特征可以很容易地在另一个图像中的同一行附件来匹配，以降低特征搜索区域。 然而，校正立体图像的包含了很多限制，很多情况下是不合适的，如鱼眼相机中图像会被过度裁剪，失去大视场的优点，降低了鲁棒性。
    由于这个原因，我们的系统不依赖于图像恢复，考虑到立体声钻机作为两个单目摄像机，它们之间具有：
（1）恒定的相对$Se(3)$变换，
（2）可选地，一个观察场景相同部分的公共图像区域， 这些约束允许算法在对极约束和BA优化时有效地估计地图。

视觉惯性SLAM

基础

    ORBSLAM-VI假设的是针孔模型，且初始化太慢，在一些有挑战性的传感器上会失败。而orbslam3里面基于上面的工作提出了一个快速准确的IMU初始化技术，可用于针孔/鱼眼的单双目惯性视觉slam。
    ORBSLAM3需要计算优化的有body位姿$Ti$，速度$vi$，加速度计与陀螺仪的偏置，假设他们由布朗运动演化的（无规则），他们组成了状态向量$Si$：

    对于VIslam需要对两帧之间做IMU预积分，获得角度变化、速度变化与位置变化，还有一个关于全部测量向量的一个信息矩阵，定义惯性残差：

    重投影误差：

    其中$\Pi$表示相机的投影方程，$\oplus$表示李群$SE(3)$在三维空间上的转换运算。
    视觉惯性优化目标函数即最小化残差，同时也使用鲁棒的Huber核${\rho }_{Hub}$函数来减少虚假匹配的影响， 这种优化在跟踪和映射过程中会降低效率，

IMU初始化

    三个关键观点：
    纯单目slam可以提供非常准确的初始化地图，主要问题是没有尺度。解决这个问题的方法是添加IMU初始化。
    当尺度明确地表示为优化变量时，它收敛得更快，而不是使用BA的隐式表示。
     在IMU初始化阶段忽略传感器的不确定性会产生大量的不可预兆的误差。
    因此，考虑到传感器的不确定性，我们将IMU初始化描述为一个MAP（最大后验概率）估计问题，分为三个步骤：

• 视觉地图初始化
       用2秒钟来进行单目初始化，以4HZ的频率添加关键帧。此时已经有了10个相机位姿及上百个MapPoint，使用纯视觉BA。这些位姿转成以body坐标系下，上标表示变量尺度未知
                

• 惯导地图初始化
      这一步目的为了得到惯导参数的最佳估计，使用$T_{0:k}$与这些关键帧间的惯导测量数据，惯导参数如下。
  
      在只考虑惯性测量的情况下构建MAP估计问题表达如（6）。考虑到测量的独立性优化问题变为（7），再对（7）式中的IMU预积分和先验分布取负对数与假设高斯误差转换为（8），与上面不同的是没有加入视觉残差，但是先验残差$r_p$应该接近于0，其协方差由IMU特征给出。

    在流形中进行优化时会对尺度进行更新，以在优化过程中更新重力方向估计:（因为重力方向默认为向下的，绕着重力方向的旋转轴不管怎么转都不影响，这点在VIORB论文中也有介绍）
更新尺度方法如下：

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

           

• 视觉惯性联合初始化
      一旦获得一个好的IMU与视觉参数估计，可以执行视觉IMU联合优化进一步更新。对于所有关键帧有共同的偏置，包含与仅imu阶段共同的先验信息。
              

跟踪和建图

    跟踪阶段VI优化比较简单，只优化最近两帧的位姿，MapPoints固定。
     Mapping，执行整体优化对于大地图比较难处理。使用滑动窗口对关键帧和对应的MapPoints，包含与这些关键帧共视的关键帧，但它们是固定的。
    某些时候缓慢移动不能提供很好的惯导参数观测性，初始化在短短的15秒内可能不会收敛到准确结果。为了提高这种情况的鲁棒性，提出了一个新颖的尺度更新技术，基于一个改进的纯IMU优化，包含所有插入的关键帧，但只优化尺度与重力方向（如图2d）。注意，在这种情况下，假设恒定偏差是不正确的。相反，使用每个帧的估计值，并修复它们。这种优化在计算上非常高效，每10秒在LocalMapping线程中执行一次，直到地图有超过100个关键帧或自初始化以来已超过75秒。

                

对于跟踪丢失

     在纯VSLAM或者VO系统中，由于相机快速运动或遮挡容易导致丢失。ORBSLAM里面首创了基于词袋位置识别的快速重定位技术，但是在EUROC的困难数据集上还有些不足。当跟踪到少于15个MapPoints点时，这篇中VI系统会丢失。主要分成两个阶段提高鲁棒性：
    （1）短时丢失：目前的本体状态使用IMU的读取来估计，MapPoints在估计的相机位姿下被投影且在一个大的图像窗口中寻找匹配点。匹配结果被包含于VI优化中。在大多数情况下这样可以恢复视觉跟踪，否则5秒后进入下一阶段。
    （2）长时丢失：重新开一个地图，且作为活动的地图。

地图合并和回环

    为了防止假阳性结果导致地图混乱，DBoW2实施了时序与几何一致性检验，完成了百分百的精度，召回率30～40%。时序一致性检测需要至少三个关键帧。在ATLAS上使用了这种方法，这种延迟和低召回率经常导致相同或不同地图上的重复。
    无论何时在Mapping线程中创造了一个关键帧，位置识别都会尝试去探测与ATLAS中的关键帧的匹配。如果检测到匹配的关键帧属于活动的地图，执行回环，否则执行地图融合。
    作为这个方法的第二个新颖之处，一旦新的关键帧与地图确定了相对位姿，定义一个局部窗口，包含匹配的关键帧及其在covisibility graph中的临近帧。在这个窗口里密集地搜索中长度数据关联，提高回环与地图融合精度。与ORBSLAM2不同的细节如下：

位置识别（约等于重定位）

    为了达到较高的召回率，在使用DBow2进行回环检测时，每个候选帧都会进行好几步的几何验证，具体如下：
    DBoW2 candidate keyframes : 我们根据DBow2查询3个候选帧，但并不包括被查询帧的共视帧， 将查询到的候选帧，以及候选帧的最佳共视帧作为一个局部的小地图（包括3D地图点），因此可以建立2D-2D以及2D-3D的匹配关系
    3D aligning transformation : 使用Horn-algorithm结合RANSAC进行被查帧的位姿估计Guided matching refinement : 将Local window中构建的地图点全都投影到被查询帧上，寻找更多的匹配关系，从而进行重投影优化；本步骤可以多次进行，每次迭代收缩Local window的大小即可
    Verification in three covisible keyframes ： 连续三次检测到回环才可以认为Place Recognition成功。不过，一般认为Local window已经包含的全部的信息，因此不用每次重复进行DBow2检索。同时，不一定是新帧，被检索帧的相邻帧可以拿来检测，用于连续三次的计数。
VI Gravity direction verification：地图尺度成功恢复的情况下(VI模式)，pitch和roll角也可以设定阈值，进行Place Recognition

视觉地图合并

    Place Recognition成功以后就可以进行地图合并了，如果候选帧和被检测帧分别数据不同的地图，建议将被检测帧的地图合入候选帧所在的地图。由于地图合并一般较为耗时，因此地图合并被为两个阶段，详细步骤如下：

    1）Welding window assembly：将被检测帧以及其共视帧 和 候选帧以及其共视帧，再加上上述帧对应的地图点，组合一个局部的地图，成为Welding window。当前，由于属于不同的地图，因此需要根据Place Recognition的得到的旋转平移变换到候选帧所属的地图.
    2) Merging maps：将重复点进行删除，但保留被删除点的共视关系，这样很好理解。
    3) Welding bundle adjustment.：对Welding window中的关键帧和地图点进行BA，为了保证一致性，将有共视关系的帧进行fixed, 见下图
    4) Pose-graph optimization. ：fixed Welding window中的关键帧，对整个地图中剩余关键帧进行pose-graph优化

视觉惯性地图合并

    这里的合并和视觉地图合并基本相同，区别主要是第一步和第三步，改动如下：

• 两个待合并地图初始旋转和平移，根据地图是否优化成功决定是SE3还是SIM3
• 在Welding bundle中引入IMU预积分等约束信息，见下图，注意图中被框起来需要fixed的量
      注意： 这里和Visual Map Merging在fixed 共视帧的时候有一处不同，就是Visual Map Merging只fixed 合入地图关键帧的共视关系，Visual-Inertial Map Merging则全都要
  
      如果active的地图是mature的，一切照旧；如果不mature，说明尺度信息暂时不可靠，在缝合时，需要用相似变换Sim(3)变换而不是SE(3)将Ma中的点对齐到Mm。优化的变量和固定的变量如图所示，Easy。

回环

    闭环校正算法类似于地图合并，但位置识别时仅匹配active地图。 同样会根据匹配的关键帧构成Welding window，做去重，Welding window BA工作，然后是姿势图优化，将校正传播到地图的其余部分。
    最后一步是在考虑环路闭合 long-term和 mid-term 匹配后，进行全局BA。在视觉惯性的情况下，仅在关键帧的数量低于阈值时才执行全局BA，以避免巨大的计算成本。

实验结果

单目SLAM在EuRoc数据集上

视觉惯性SLAM在TUM-VI

总结

参考

• MLPnP思路
      提出了一种从图像观测到方向向量的方差传播的通用方法，以避免协方差矩阵的奇异性，该方法具有实时的迭代性能，迭代最小化投影到球面切空间上的重投影误差。
• MLPnP地图初始化可以分为三个步骤：
      1)仅视觉地图估计：使用BA求解短时间内的单目初始化ORB-SLAM(~2s)，以获得一个只有视觉的上尺度地图估计。 同时，计算关键帧之间的IMU预积分及其协方差。
      2)只惯性地图估计：只惯性光学化，使IMU轨迹和ORB-SLAM轨迹对齐，找出尺度、关键帧速度、重力方向和IMU偏差。
      3)视觉惯性图估计：使用上一步的解作为初始值，得到完整的视觉惯性BA求得最优解。

    根据贝叶斯公式，最大后验等于最大似然乘以先验概率，所以我们目的是求取观测值是最大似然。假设IMU预积分值分布和参数的先验分布概率密度函数为高斯分布的密度函数，就可以对整个似然函数求对数并加负号，最后变成最小二乘过程。

UrbanS LeitloffJ HinzS. MLPnP arealＧtime maximum likelihood solutiontothe erspective-n-point problem （ISPRS-2016）

• https://www.cnblogs.com/tweed/p/13384022.html