ORB-SLAM3 双目 Stereo + 论文

Stereo 双目流程

总结

• 只使用左右目共视特征
• 双目主要是通过左右视图得出 3D地图匹配点，并进行跟踪
• 初始化较单目比较简单，因为不需要计算 前后帧中 2D-2D的匹配，只需 3D-2D即可完成初始化

System

• 构造等函数见：system
• 具体如何调用：
  • ros 跟单目类似，直接读取左右图像，并与imu数据 同步发布
  • 非ros端，读取 左右图像的目录，然后在依次读取，例如：Example/Stereo/ stereo_euroc.cc

TrackStereo

• Param: imLeft，imRight，timestamp，vImuMeas，filename

  • vImuMeas，filename 二者有缺省构造

• return Tcw

• 流程：

  • 检测 模式 改变

    • 两种模式： 定位模式 和 非定位模式

    • 若要切定位模式，则 停止局部地图构建，完成后更新最新状态

    • 若要停止定位模式，则 局部地图Release，并更新状态

    • 检测 reset

      • 若有 重置标志时，tracker进行重置

  • 若使用 imu，则 mpTracker->GrabImuData

  • 更新 立体图像，返回Tcw mpTracker->GrabImageStereo

Tracking

• 读取参数部分不一致，读取了 右图的一些数据

ParseCamParamFile

• 若 立体相机时，读取 Camera2 的一些数据
  • “Camera2.fx”、“Camera2.fy”、“Camera2.cx”、“Camera2.cy”
  • “Camera2.k1”、“Camera2.k2”、“Camera2.k3”、“Camera2.k4”

GrabImageStereo

• Param：imLeft，imRight，timestamp，filename
• 流程：
  • 图像转换为灰度图：
    • 三通道、四通道 都可用 cvColor 函数直接转换
  • 构造 frame， Frame
    • 是否使用IMU
      • 多了 mTlr、&mLastFrame 和 mpImuCalib
    • 是否初始化 mpCamera2 ?
      • 若初始化时， 引入了 mpCamera2 参数
  • Track 该track 跟单目啥的没什么区别
  • 返回当前帧的 mCurrentFrame.mTcw.clone()

StereoInitialization

双目有深度信息，所以第一帧(左右目)可以直接产生3D地图点，然后下一帧图像可以直接进行3D-2D匹配进行跟踪。所以双目初始化过程相对单目比较简单。

• 初始化，在 Track() 函数中

流程：

• 若 当前帧特征点 小于 500时，直接返回
• 若 使用 IMU，则 重新跟新预积分 mpImuPreintegratedFromLastKF
  • 检查一下是否有预积分，如果没有说明没有Imu测量
  • 检查一下当前帧加速度和上一帧加速度的差值是否大于0.5，如果小于说明加速度变化不够，也不行
• 将 Frame 位姿设置为原点
  • 确定当前帧位姿：
    • 若使用imu，则为imu 的位姿
    • 直接设置初始化帧(当前帧)的位姿为单位阵
  • 创建关键帧 ，并插入地图集
• 创建地图点并关联关键帧
  • 若不使用 mpCamera2，则：
    • 遍历特征点，并若该特征点深度大于0，则 添加地图点
      • 将当前帧构造成关键帧，加入地图集Atlas中，然后生成初始地图点，添加地图点和关键帧之间的连接关系
      • 关键帧中添加地图点
  • 使用 mCamera2，则：
    • 遍历特征点，若该特征点 在右图中也存在，则 添加地图点
      • 将当前帧构造成关键帧，加入地图集Atlas中，然后生成初始地图点，添加地图点和关键帧之间的连接关系
      • 计算地图点最好的描述子 ComputeDistinctiveDescriptors()
      • 更新平均观测方向和深度范围 UpdateNormalAndDepth()
• 局部地图 插入该关键帧，并更新lastFrame
• 地图集中添加该关键帧

Frame

Frame 构造流程：

• Frame ID 默认加加：mnId=nNextId++;
• Scale Level Info
• orb extraction ExtractORB
  • 左右目分别两个线程进行 提取
  • threadLeft 、threadRight
• mbInitialComputations 如果是第一帧，计算去畸变的边界：
  • 计算图像边界 ComputeImageBounds(imLeft)
    • 若 失真系数不为0时，
      • 将图像边缘四个点 按照失真系数投影，得到新的四个点
      • 得到 x\y的最大最小四个参数
    • 非 失真，即无畸变，则直接赋值四个点
  • 计算图像 宽、高 的逆
  • 计算 invfx、invfy
• 计算参数 b = bf / fx
• 获取左右特征的匹配关系 ComputeStereoFishEyeMatches
• 把特征点放到对应的格子里 AssignFeaturesToGrid
• 获得去畸变的坐标 UndistortKeyPoints

ExtractORB

调用 ORBextractor 中的： ()操作符

在当前图片上，提取尽可能平均分布的FAST角点，同时计算他们ORB描述子。

• 计算高斯图像金字塔 ：ComputePyramid

• 提取特征点并生成四叉树： ComputeKeyPointsOctTree

• 为每个关键帧描述子分配32位内存，

• 为加快立体鱼眼匹配而修改，遍历金字塔逐层计算特征点描述子

  • 对图像进行高斯模糊 GaussianBlur cv函数
  • 计算特征点描述子 computeDescriptors
  • 对特征点进行尺度缩放 --金字塔层数对应
  • 根据它与LappingArea的关系，确定当前特征点描述子在总描述子容器中的位置
    • 若在 该区域内，则 放到后面，否则放到前面

• 返回区域内的下标： monoIndex

有效数据：

• 左图：mvKeys + mDescriptors
• 右图：mvKeysRight + mDescriptorRight
• 返回： monoIndex

ComputeStereoFishEyeMatches

• 通过在lapping区域 定位 关键帧 和描述子
  • 左图：stereoLeft + stereoDescLeft
  • 右图：stereoRight + stereoDescRight
• BFmatcher 描述子匹配，返回匹配结果 knnMatch openv函数
  • 参数，左右描述子，返回匹配结果
  • cv::BFmatcher 简介
    • 描述cv::BFMatcher(cv::NORM_HAMMING);
    • BFMatcher对象有两个方法BFMatcher.match()和BFMatcher.knnMatch()。
    • 第一个方法会返回最佳匹配。第二个方法为每个关键点返回k个最佳匹配，其中k是由用户设定的。
• 通过Lowe's ratio检测匹配效果
  • 对于每一个好的匹配，检查视差和重投影误差以丢弃虚假匹配
  • 每个好的匹配点：(*it).size() >= 2 && (*it)[0].distance < (*it)[1].distance * 0.7

AssignFeaturesToGrid

• 将整张图片分为64×48的网格
  • const int nCells = FRAME_GRID_COLS*FRAME_GRID_ROWS;
  • 为每个网格预留一定的内存 ，大小为： 0.5f*N/(nCells)
• 遍历特征，将特征放入 网格中
  • 判断特征在哪个网格中 PosInGrid
  • 将该特征的 id push 到相应的网格中

UndistortKeyPoints

• 评判是否需要去畸变
  • 基于提供的去畸变参数mDistCoef 确定
  • 如果第一个畸变参数为0，不需要矫正
• 去畸变完成后再恢复单通道
  • 去畸变数据 赋值
    • 定义 特征点保存矩阵，将N个特征点保存在N*2的矩阵中
      • N为提取的特征点数量，为满足OpenCV函数输入要求
    • 遍历每个特征点，并将它们的坐标保存到 上述矩阵中
      • 然后将这个特征点的横纵坐标分别保存
    • 为了能够直接调用opencv的函数来去畸变，需要先将矩阵调整为2通道（对应坐标x,y）
  • 调用 cv 函数去畸变
    • 输入的特征点坐标,输出的校正后的特征点坐标覆盖原矩阵,机的内参数矩阵,相机畸变参数矩阵,一个空矩阵，对应为函数原型中的R,新内参数矩阵，对应为函数原型中的P
    • cv::undistortPoints(mat,mat, static_cast(mpCamera)->toK(),mDistCoef,cv::Mat(),mK);
• 去畸变完成后,将特征点赋值 去畸变坐标 后存储到新的容器mvKeysUn中

论文阅读

ORB-SLAM3: An Accurate Open-Source Library for Visual, Visual-Inertial and Multi-Map SLAM

摘要：Abstract

• 本文提出的ORBSLAM3，采用针孔相机和鱼眼镜头模型，第一个在单目、双目以及RGBD相机上都能执行纯视觉、视觉-惯性以及多地图的SLAM系统。
• 两个创新点：
  • 基于特征的紧耦合视觉惯性SLAM系统，完全依赖于最大后验估计MAP算法，甚至在IMU初始化阶段期间。这个SLAM系统，在实时的、小型或大型、室内或室外环境中，都能鲁棒地运行系统，并且要比之前的方法精确2到5倍。
  • 多地图系统，依赖于一个提升召回率的新的位置识别算法。基于此，ORB-SLAM3在长期缺乏视觉信息的情况下依旧能够运行：当特征跟踪丢失时，它将会开启一个新地图，遇到建图过的区域将会无缝合并之前的地图。与那些只是用前几秒信息的视觉里程计系统相比，ORBSLAM3系统是第一个，能在每个算法阶段，复用之前的所有信息。这个允许包括在光束平差法共视关键帧阶段，其提供提升精度的高视差观测，即使在时间序列上是分隔的，或者他们来自之前的建图会话Session。
• 我们的实验显示，在所有的传感器配置中，ORBSLAM3和已有文献中最好的系统一样鲁棒，并且要更加精确。注意的是，我们的双目-惯性SLAM在EuRoC飞行器数据集上达到了平均3.6cm的精度，在TUM-VI（AR/VR场景代表）快速手持运动环境的室内数据集中达到了平均9mm的精度。为了社区的明天，本文开源了代码。

引言

• 许多系统依赖于最大后验估计，其在视觉传感器的情况下对应于BA光束平差法，要么在基于特征的几何BA方法中，最小化特征的重投影误差，要么在直接法的光度BA中，最小化选择像素光度误差。
• 视觉里程计VO和SLAM之间的边界更加模糊。
  • 视觉SLAM的目标是使用机载传感器去构建环境地图并计算这张地图中机器人的实时位姿。
  • 视觉里程计VO则是聚焦于计算机器人的自我运动上，而不是去构建地图。
• SLAM地图的最大优势是允许在BA捆机调整中匹配和使用先前的观测数据，去执行三种数据关联：
  • 短期数据关联 Short-term data association，匹配前几秒得到的地图元素。这是大多数视觉里程计VO系统所使用的唯一的数据关联类型，一旦它们丢失了视野就会忘记环境中的元素，即使系统在同一个区域内移动，也会导致连续的估计漂移。
  • 中期数据关联 Mid-term data association，匹配相机累积漂移仍然较小的地图元素。这些可以在BA中与短期观测数据以同一种方式进行匹配和使用，并允许在系统移动到建图过的区域中时达到零漂移。与带回环检测的VO系统相比，中期数据关联是ORBSLAM3系统得到更高精度的关键。
  • 长期数据关联 Long-term data association，使用位置识别技术匹配之前访问过区域中的观测数据，而忽略累积的漂移（回环检测）或者即使跟踪丢失（重定位）。长期匹配允许去重置漂移并使用位姿图优化纠正回环，或者更准确点，是使用BA光束平差法。这是在中大型循环环境中SLAM精度的关键。
• 本文工作基于先前的ORBSLAM2以及ORBSLAM 视觉惯性系统，我们的系统是第一个视觉和视觉-惯性系统，能够完全利用短期、中期和长期的数据关联，在建图过的区域能达到零漂移。因此，我们进一步提出一种多地图数据关联Multi-map data association，它可以使得我们可以在BA阶段匹配和使用之前建图模块得到的地图元素，达到一个SLAM系统的真正目的：构建一个地图，然后之后可以用于提供精确定位信息。
• 本文本质上是一篇系统性论文，最重要的贡献是ORB-SLAM3库本身，目前为止最完备和精确的视觉，视觉-惯性和多地图系统。ORBSLAM3的主要创新点如下：
  • 一个单目和双目视觉-惯性SLAM系统，单目和双目视觉-很关心系统是非常地鲁棒，并且要比其它视觉惯性方法要更加精确，即使在没有回环的场景中
  • 高召回率的位置识别技术 High Recall Place Recognition
    • DBoW2需要时间一致性，在检查几何一致性之前会匹配三个连续关键帧到同一个区域，以牺牲召回率为代价来提升精度。导致的结果是，这个系统在闭环和复用之前地图时太慢了。
    • 其中候选关键帧首先检查其几何一致性，然后对于三个共视关键帧保持局部一致性。这个策略提高了召回率并使得数据关联更加紧密，提升了地图的精度，这是以稍微较高的计算代价消耗为代价。
  • ORB-SLAM Atlas地图集，一个相互无关联的地图集合，并将多地图平滑应用到所有建图操作中：位置识别，相机重定位，闭环检测以及精确的无缝地图拼接。这允许自动使用和组合在不同时刻构建的地图，执行增量的多会话SLAM系统。
  • 一种抽象的相机表示，使得SLAM代码与所使用的相机模型无关，并允许通过提供投影、反投影以及雅各比函数，来增加新的相机模型。

相关工作 Related Work：

• 视觉slam。
  • 在所有案例中**，缺乏短期、中期与长期数据关联的集成会导致比我们提出的ORBSLAM3系统更低的精度**。
• 视觉-惯性SLAM
  • 视觉和惯性传感器的组合给弱纹理，运动模糊以及遮挡场景提供了鲁棒性，并在单目系统情况下，使得尺度可观。
  • ORB-SLAM-VI第一次提出一种视觉惯性SLAM系统，能重复使用具有短期、中期以及长期数据关联的地图，在一个基于IMU预积分的精确局部视觉惯性BA中进行使用。然而，它的IMU初始化技术太慢了，需要15秒，这破坏了系统的鲁棒性和精确度。
  • VINS-Mono是非常精确和鲁棒的单目视觉里程计系统，使用DBoW2进行回环检测，以及4自由度的姿态图优化和地图合并。特征跟踪使用的是LK跟踪器，要比描述子匹配来的稍微更加鲁棒一点。在VINS-Fusion中，将VINS扩展到了双目和双目-惯性版本。Kimera是一个新的优秀的度量予以的建图系统，但是它的度量部分包含双目-惯性里程计加上DBoW2的回环以及姿态图优化，达到了与VINS-Fusion类似的效果。最近提出的BASALT是一个双目视觉惯性里程计系统，从视觉惯性里程计中抽取出非线性因子，然后在BA中使用它们，并且闭环匹配ORB特征，达到非常优秀的精度。
  • VI-DSO将DSO扩展成了视觉-惯性里程计。它们提出了一种组合了惯性观测数据和在选取的高梯度像素中的光度误差的BA方法，达到了很好的精度。由于高梯度值像素中的信息被成功利用，带有弱纹理的场景区域中的鲁棒性也被增强了。它们的初始化方法依赖于视觉-惯性BA并耗费了20-30秒收敛到1%的尺度误差。
• 多地图SLAM
  • 在探索过程中跟踪丢失，利用地图构建和融合来增加鲁棒性的思想第一次是在滤波方法中提出。第一个基于关键帧的多地图系统中是文献【68】，但是地图是手动初始化的，并且系统不能合并或者关联到不同的子地图。
  • CCM-SLAM提出一种分布式多地图系统，用于带有双向信息流的多个无人机群中，基于ORB-SLAM进行构建。它们着重于克服限制带宽和分布式处理带来的挑战，而我们的系统是关注于SLAM算法的精度和鲁棒性，在EuRoC数据集上达到非常好的效果。ORBSLAMM也提出一种ORBSLAM2的多地图扩展版本，但是将子地图作为分离的实体，而我们的方法能够执行无缝的地图合并，构建一个精确的全局地图。
  • VINS-Mono是一个带有闭环和多地图能力，依赖于位置识别词袋模型DBoW2，的视觉里程计系统。ORBSLAM3要比VINS-Mono准2.6倍，这是由于我们系统具有使用中期数据关联的能力。我们的Atlas多地图系统也是构建在DBoW2词袋模型上的，但提出了一种新的高召回率的位置识别技术，并使用局部BA合并成更细节、更精确的全局地图，在EuRoC数据集上的多会话操作中，ORBSLAM3要比VINS-Mono精确3.2倍。

系统概览 System Overview

• ORB-SLAM3是构建在ORB-SLAM2和ORB-SLAM-VI上的。它是一个完全多地图和多会话系统，能够在纯视觉或视觉-惯性模式中工作，带有单目、双目或者RGBD传感器，使用针孔相机和鱼眼相机模型。
• 与ORB-SLAM2相似，有自己的主要创新点，总结如下：
  • Atlas地图集 是由不关联地图集合构成的多地图表示形式。有一个active map活动地图，其中跟踪的现场定位输入的帧图像，并通过局部建图线程进行持续优化和增加新的关键帧。我们将Atlas中的其它地图当作non-active map非激活地图。系统构建了一个独一无二的DBoW2关键帧数据库，用于重定位、闭环检测和地图合并。
  • Tracking thread 跟踪线程 用来处理传感器信息，并通过最小化匹配地图点的重投影误差，实时计算与活动地图相关的当前帧图像的姿态。跟踪线程也决定了当前帧能否成为关键帧Keyframe。在视觉-惯性模式中，载体速度和IMU偏置是通过包括优化中惯性残差来进行估计的。当跟踪丢失时，跟踪线程会尝试在所有Atlas的子地图中重定位出当前帧所在位置。如果重定位完成，跟踪会恢复，如果需要的话，会切换到活动地图active map中。否则，在一段时间后，active map活动地图会被标记为非活动non active状态，并从头开始初始化一个新的活动地图。
  • Local mapping thread 局部建图线程 增加关键帧和特征点到活动地图active map中，剔除掉冗余的关键帧和点，并使用视觉或视觉惯性BA光束平差法优化地图，在离当前帧最近的关键帧的局部窗口中来执行优化。此外，在惯性情况下，IMU参数通过线程，使用我们提出的新MAP最大后验估计技术来进行初始化和优化。
  • Loop and map merging thread 回环和地图合并线程 以关键帧的频率检测active map活动地图和整个Atlas地图集之间的共同区域。如果共同区域是属于active map活动地图的，那么线程将执行回环矫正；如果共同区域是属于不同的地图，那么两张子地图会被无缝合并成一张地图，然后此时令其成为active map活动地图。在回环矫正过后，完整的BA会在一个独立的线程中启动，进一步优化整张地图，而不会影响系统的实时性能。

相机模型 Camera Model：

• ORBSLAM在所有系统模块中，假设是一种针孔相机模型。我们的目标是通过抽取出跟相机模型相关的所有特性（投影、反投影以及Jacobian等）和功能从整个SLAM管线抽象出相机模型，去分离不同模块。这使得我们的系统可以使用任何相机模型，只需要提供对应的相机模型即可。在ORBSLAM3库中，除了针孔相机模型外，我们还提供了Kanala-Brandt鱼眼相机模型。然而，相机模型的抽象会造成一些困难，需要去解决，讨论如下：
• 重定位 Relocalization
  • 一个鲁棒SLAM系统需要具备在跟踪失败时重新定位相机的能力。ORB-SLAM基于ePnP算法通过设置一个PnP求解器来解决重定位问题，ePnP算法假设标定后的针孔相机模型具备所有的形式。为了跟进我们的方法，我们需要一个可以独立于使用的相机模型进行工作的PnP算法。对于这个原因，我们采用最大似然PnP算法，其与相机模型完全解耦合，因为其使用投影射线作为输入。相机模型仅仅需要提供从像素到投影射线的反投影函数即可，就能够使用重定位功能了。
• Non-reactified Stereo SLAM
  • 大多双目SLAM系统假设双目帧图像时被矫正过的，比如，使用相同的焦距，图像共平面，两幅图像被转换到针孔投影模型中，并且与水平极线Horizontal epipolar lines对齐，比如一幅图像中的一个特征能轻易被匹配匹配到，通过在其它图像中查看相同行。然而，矫正双目图像的假设是非常受限制的，并且在很多应用中，是既不合适也不可用的。例如，矫正一个分散的双目图像对，或者一个双目鱼眼相机需要严重的图像裁剪，失去一个大视场的优势：更快速的环境建图或者对于遮挡情况具有更好的鲁棒性。
  • 对于上述这个原因，我们的系统不依赖于图像矫正，考虑将双目设备当作两个单目相机，具备：
    • 两个单目相机之间的一个恒定的相对特殊欧式群SE(3)
    • 可选的，观察场景中相同部分的公共图像区域
  • 这些约束使得我们能够在三角化新地标点或者在BA优化中，通过引入这些信息有效的估计地图的尺度。跟着这种想法，我们的SLAM管线估计6自由度的刚体姿态，参考坐标系统可以定位在其中一个相机或者IMU传感器中，并且表示这两个相机关于这个刚体位姿的变换关系。
  • 如果这两个相机有着重叠区域，在其中我们有双目的观测数据，那么我们三角化第一次看到的真实尺度的地标点。两幅图像的剩下部分仍然有许多相关的信息用于SLAM管线，并且用作单目的信息。在这些区域被看到的特征会首先从多个视角被三角化，比如在单目情况下。

视觉-惯导 SLAM

• ORB-SLAM-VI是第一个真正的能够重用地图的视觉-惯性SLAM系统。然而，其受限于针孔单目相机模型，并且它的初始化太慢了，在一些具有挑战性场景中是失效的。在本文工作中，我们基于ORB-SLAM-VI进行构建，提供一种快速精确的IMU初始化技术，以及一个开源SLAM库，使用针孔相机以及鱼眼相机完成单目-惯性SLAM以及双目惯性SLAM。

• 基本原理 Fundamentals

  • 状态量：世界坐标系 位姿+速度+陀螺仪和加速度计偏差 $S_i=\{Ti,v_i,b_i^g,b_i^a\}$
  • 文献【62】提出的 manifolds流形 上进行表述。得到IMU预积分计算后的旋转、速度以及位置PVQ观测数据
  • 结合惯性残差，可使用第$i$帧图像和在位置$x_j$处的三维空间点$j$之间的重投影误差$r_{ij}$ 。
  • 因子图 :[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-s3TN5Vmy-1657458829102)(orb_imu_visio_graph.png)]

• IMU 初始化

  • 初始化步骤的目的是去得到惯性变量：载体速度、重力方向和IMU偏置的良好初始化值。一些系统比如VI-DSO尝试从零开始解决视觉-惯性BA问题，避开特定的初始化过程，获得初始化参数的缓慢收敛（达到30秒）。
  • 这项工作中，我们提出一个快速精确的初始化方法，基于三个主要的两点：
    • 纯单目SLAM能够提供非常精确的初始化地图，它的主要问题是尺度未知。首先解决纯视觉问题将会增强IMU初始化过程。
    • 当尺度表示为优化变量时尺度收敛会越快，替换使用BA的隐式表示。
    • 在IMU初始化过程中，忽略传感器不确定性会产生不可预测的误差。
  • 因此，合理考虑传感器的不确定性，我们可以将IMU初始化看作是一个最大后验概率估计问题，分为下面三步：
    • 纯视觉的最大后验概率估计
    • 纯惯性的最大后验概率估计
      • 一旦纯惯性优化结束，帧图像的位姿和速度，以及三维地图点，根据估计出的尺度因子进行缩放，并且根据估计出的重力方向进行旋转到与Z轴对齐。IMU偏置会被更新，IMU预积分会重复计算，旨在减少未来的线性化误差linearization errors。
    • 视觉-惯性 最大后验概率估计

• 跟踪和建图 Tracking and Mapping

  • 对于跟踪和建图，我们采用文献【4】中的方案。跟踪解决的是一个简化的视觉-惯性优化问题，其中只有最近两帧的状态需要被优化，而整个地图点保持不变。
  • 对于建图，尝试从公式4中解决整个优化问题对于较大地图来说是无法解出的。我们将关键帧的滑动窗口以及他们对应的地图点作为优化变量，包括共视关键帧（covisible keyframe）但需要保持固定。
  • 在一些具体的情况中，当缓慢运动不能提供良好的惯性参数观测，初始化可能在仅仅15秒内收敛不到精确解。为了得到这种情况下的鲁棒性，我们提出一种尺度细化技术scale refinement technique，基于改进的纯惯性优化，其中包括所有输入的关键帧，但是尺度因子和重力方向是唯二需要被估计的参数（如图2d所示）。注意，在那种情况中，IMU偏置的恒定假设是不正确的。相反，我们给每帧图像使用IMU偏置的估计值，然后再固定其不变。这个优化计算上很高效，再局部建图线程中每10秒执行一次，直到地图有了超过100个关键帧，或者初始化过去超过75秒后停止。

• 跟踪损失的鲁棒性 Robustness to tracking loss

  • 在纯视觉SLAM或视觉里程计VO系统中，临时的相机遮挡以及快速运动会导致视觉对象的跟踪丢失，系统就挂掉了。ORB-SLAM率先使用了基于词袋模型位置识别的快速重定位技术，但是它被证明不足以解决EuRoC数据集中较难序列子集。当少于15个特征点被跟踪时，我们的视觉-惯性系统进入视觉丢失visually lost状态，并在两个阶段中来实现系统鲁棒性：
    • 短期丢失Short-term lost：当前载体状态从IMU读数进行估计，并且地图点以估计的相机位姿进行投影，在一个大的图像窗口中搜索匹配点。匹配结果包括在视觉-惯性优化过程中。在大多数情况中，这允许恢复视觉跟踪。否则在5秒后，跳过这个阶段。
    • 长期丢失Long-term lost：一个新的视觉惯性地图被初始化，并该地图是active map活动地图。

• 地图合并以及闭环检测 Map Merging and Loop Closing

  • 通过跟踪和建图线程中将地图点投影到估计出的相机位姿中并在少数几个像素的图像窗口中搜索匹配点，帧图像之间短期和中期数据关联，以及active map活动地图通常会被找到。为了在重定位以及闭环检测步骤中实现长期数据关联，ORB-SLAM采用DBoW2词袋模型位置识别系统。该方法被大多数最近的视觉里程计VO以及SLAM系统采纳来实现闭环，如表1所示。

  • 不像跟踪，位置识别不会从相机位姿的初始估计开始。相反，DBoW2构建了一个关键帧的数据库，存储了关键帧的词袋向量，并给定一个查询图像能够有效的根据词包提供最相似的关键帧。

    • 只使用第一个候选帧，原先的DBoW2查询实现precision准确率和recall召回率达到了50-80%。
    • 为了避免可能会破坏地图的false positives假阳性，DBoW2实现了时序上以及几何上的一致性检查，使达到100%的准确率以及30-40%的召回率。
    • 关键的是，时序一致性检查至少延迟了3个关键帧上的位置识别。当尝试在我们的Atlas多地图系统中使用DBoW时，我们发现在相同或者不同地图中的重复区域内，这个延迟以及低召回率的结果的现象太频繁。

  • 本文工作中，我们提出一种新的位置识别算法，提升了long-term长期数据关联和multi-map多地图数据关联的召回率recall。

    • 无论什么时候，建图线程创建一个新的关键帧，位置识别模块会启动，尝试与已经在Atlas地图集中的其他关键帧进行检测匹配。如果发现匹配关键帧是属于active map活动地图，那么就会执行闭环检测。否则，它就是一个多地图数据关联，然后，这个活动地图以及匹配地图会合并到一起。
    • 作为我们方法的第二个创新点，一旦新关键帧和匹配地图之间的相对位姿被估计出来，那么我们会用匹配关键帧以及共视图中的相邻关键帧，定义一个local window局部窗口。在这个窗口中，我们集中搜索mid-term中期数据关联，提升闭环检测以及地图合并的准确度。
    • 这两个创新点解释了，在EuRoC实验中与ORB-SLAM2相比，ORB-SLAM3为什么获得更好的精度。不同操作的细节解释如下。

  • 位置识别 Place Recognition

    • 为了获得高召回率，对于每个新的活动关键帧，我们 查询DBoW2数据库Atlas地图集中几个相似的关键帧。为了达到100%的准确率，这些候选关键帧通过集合验证的几个步骤。所有集合验证步骤的基础操作包括检查图像窗口内是否存在ORB关键点，该图像窗口的描述子匹配了地图点的ORB描述子，采用两者之间的汉明距离作为阈值。如果在搜索窗口内，存在几个候选关键帧，丢弃含有歧义的匹配对，我们检查第二近的匹配对的距离比。我们的位置识别算法步骤如下：

      • DBoW2 候选关键帧。我们用活动的关键帧$K_a$去查询Atlas DBoW2数据库，检索最相似的三个关键帧，排除与关键帧$K_a$ 共视的关键帧。 我们将位置识别中的每个候选匹配关键帧记为$K_m$。
      • Local Window 局部窗口。对于每个候选匹配关键帧$K_m$，我们定义一个包含该候选关键帧的局部窗口，它的最佳共视关键帧，以及被这些关键帧观测的地图点。DBoW2直接索引提供了一组$K_a$中以及局部窗口关键帧中，关键特征点之间的假设匹配。对于每一个这样的2D-2D匹配，我们同样有这他们对应地图点之间的3D-3D匹配对。
      • 3D aligning transformation 对齐变换。我们使用RANSAC算法计算用$K_a$中的活跃关键帧较好对齐到$K_m$局部窗口中地图点的变换矩阵$T_{am}$，在纯单目、或者单目-惯性中，当地图仍然不完备mature(成熟)时，我们计算相似变换群 $T_{am}\in Sim(3)$ 替代特殊欧式群$T_{am}\in SE(3)$。在这两种情况下，我们使用Horn 算法，其采用三组3D-3D匹配对的一个最小几何去找到$T_{am}$的每一个假设。在适用相似变换群$T_{am}\in Sim(3)$转换$K_a$中地图点后，那些活动关键帧集合$K_a$中重投影误差小于某一个阈值的假定匹配putative matches，对这个假设给与正面的投票（give a positive vote to the hypothesis）。如果数字超过阈值，则选择更多投票的假设。
      • **Guided matching refinement 引导匹配优化。**所有局部窗口中的地图点会通过变换矩阵$T_{am}$找打$K_a$中更多的关键帧匹配对，搜索也是逆向的，在所有局部窗口关键帧中找到$K_a$个地图点的匹配。搜索也是逆向的，在所有局部窗口关键帧中找到$K_a$个地图点的匹配。使用找到的所有匹配，变换矩阵$T_{am}$通过非线性优化算法进行优化，其中目标函数是双向重投影误差，采用Huber核函数去提供假定匹配的鲁棒性。如果在优化后inlier的数目超过一个阈值，那么引导匹配的第二次迭代以及非线性优化会启动，使用一个更小的图像搜索窗口。
      • Verification in three covisible keyframes 在三个共视关键帧中进行验证。为了避免假阳性，DBoW2在三个来纳许关键帧中等待位置识别功能触发，延迟或者丢失位置识别。我们的关键亮点是大多数时间，验证需要的信息已经在地图中了。为了验证位置识别，我们在地图的活动部分搜索两个共视的关键帧，其中局部窗口中特征点匹配数目是超过阈值的。如果没有找到匹配点，之后会尝试与新的输出关键帧进行验证，不需要再次启动BoW词包模型。直到三个关键帧验证了变换矩阵$T_{am}$，验证validation才会结束。或者两个连续新的关键帧不能验证它。
      • VI Gravity direction verification 视觉惯性重力方向验证。在视觉惯性的情况中，如果活动地图是成熟的，那么我们估计 $T_{am}\in Sim(3)$ 。我们进一步检查pitch俯仰角和roll横滚角是否小于阈值，去明确的接受位置识别的假设。

  • 视觉地图合并 Visual Map Merging

    • 当一个位置识别成功生成后，一个活跃地图$M_a$中关键帧集合$K_a$之间的多地图数据关联，以及使用对其变换$T_{am}$在Atlas地图集$M_m$中存储的一个不同地图的匹配关键帧$K_m$时，我们启动地图合并map merging的操作，在这个过程中，特别需要关心确保 $M_m$ 中的信息可以被跟踪线程及时的重复使用避免地图的重复。为此，我们提出将$M_a$地图引入$M_m$参考。由于$M_a$中可能包含许多元素，并且合并它们会花很长时间，合并时分成两步完成的。第一，合并是在一个由$K_a$和$K_m$的相邻关键帧构成的weding window 窗口中进行的。在第二阶段，修正是通过姿态图优化，将修正传播到合并地图的其余部分，地图合并算法详细步骤如下：
      • Welding window assembly 合并窗口组件。焊接窗口中包括活动地图中关键帧集合$K_a$和他们的共视关键帧$K_m$，和他的共视关键帧，以及在这些关键帧中观测到的所有地图点。在将他们纳入焊接窗口之前，关键帧和属于$M_a$的通过变换矩阵$T_{ma}$转换到$M_m$。
      • Merging maps 合并地图。地图$M_a$和$M_m$融合在一起变成新的活跃地图，为消除重复点，在地图$M_m$关键帧中搜索匹配的活动地图点$M_a$，对于每次匹配，来自活跃地图的$M_a$的点会被移除，并在$M_m$中会保持累计所有移除掉点的观测。由于新的中期点数据关联被找到，通过添加从匹配地图$m_m$到活跃地图$M_a$之间的关联关键帧边来更新共视关系以及基本图。
      • Welding bundle adjustment. 合并捆机调整BA。在一个合并窗口weding window中，从活跃地图$M_a$到匹配地图$M_m$之间所有关键帧执行局部BA优化。为了固定测量的自由度，那些在$M_m$之间的所有关键帧保持固定不变。一旦优化结束，所有包含在焊接区域的关键帧可以用于相机的跟踪，使地图$M_m$快速和准确的重复使用。
      • Pose-graph Optimization 姿态图优化。一个姿态图优化是采用整个合并后的地图进行的，在合并区域中的关键帧保持固定不变。优化算法从合并窗口传播修正到地图的其余部分。

  • Visual-Inertial Map Merging 视觉-惯性地图合并

    • 视觉惯性合并算法相比纯视觉的情况遵循相似的步骤。步骤1）和3）修改成更好的利用惯性信息。
    • 1》VI welding window assembly 视觉惯性合并窗口组件：如果活动地图是mature成熟的，在插入到合并窗口前，我们采用可用的 $T_{am}\in SE(3)$ 到活跃地图$M_a$ 上，如果活跃地图不是成熟的，我们使用$T_{am}\in SE(3)$ 对齐活跃地图。
    • 2》VI welding bundle adjustment 视觉惯性合并捆机调整BA：活跃关键帧$K_a$的姿态，速度和偏差以及其5个最新关键帧被包括进来进行优化。这些变量与IMU预积分项相关。对于匹配地图$M_m$，我们同样地进行处理，包括$K_m$的姿态、姿态和偏差及5个时序上相邻的关键帧。对于匹配地图$M_m$，局部窗口之前的关键帧立即会被包含进来并且固定不变，而对于活跃地图$M_a$，相似的关键帧会被包含进来，但它的姿态会保持可优化状态，被这些所有关键帧观测到的地标点以及观测这些点的关键帧位姿也会被优化，所有关键帧和地标点通过重投影误差进行关联。

  • Loop Closing 闭环

    • 闭环校正算法类似于地图合并，但是在一种情况中，其中关键帧是通过属于活动地图的位置识别来进行匹配的。一个welding合并窗口由匹配的关键帧组装而成的，检测出重复的地标点并在共视基本图中融合创建新的链接。下一个步骤是位姿图优化去传播回环校正给地图的其余部分。在考虑闭环中期和长期匹配后，最后一步是全局BA去找到最大后验估计。在视觉惯性情况中，只有在关键帧数目小于阈值时全局BA才会执行，来避免巨大的计算成本。