526互联

ORB_SLAM3源码阅读笔记（四）

发布时间 2023-04-17 11:56:52作者: Shang-Zhi

LoopClosing 线程

1 LoopClosing 线程的创建

    LoopClsing 线程的创建与启动和LocalMapping 线程一样，该线程的核心也在于Run（）函数，以下对LoopClosing 线程进行逐步的分析。

  • 创建LoopClosing 对象mpLoopCloser
 mpLoopCloser = new LoopClosing(mpAtlas, mpKeyFrameDatabase, mpVocabulary, mSensor!=MONOCULAR, activeLC);

LoopClosing构造函数中完成各种初始化，基本上就是对各种vector类型的存储容器进行初始化（清空），这一部分没有太多需要进行说明的。

  • 启动LoopClosing 线程
    同样的，LoopClosing 线程的关键在于其Run()函数，下面开始对Run()函数进行分析。

2 Run 函数

    进入Run()函数首先将完成标志位置 false,然后映入眼帘的就又是“死循环”了。重点也就是Run()函数中的“死循环”部分。现在进入循环中对代码的功能进行分析。

  • CheckNewKeyFrames()函数，回环检测所有的主要功能都是在这个函数返回为真的情况下进行的，这个函数的主要功能就是检查mlpLookKeyFrameQueue列表中是否存在待处理的关键帧，这里我们先将没有关键帧的情况进行分析，队列中没有关键帧那么就会进入下一条函数ResetIfRequested()，这个函数其实就是一个重置函数，将队列中的所有关键帧都清除，相当于重新开始建图和定位（当然前提是重置请求已经收到）如果是收到地图激活那么就会从当前开始利用已有的关键帧建立起局部的小地图，这也是ORB_SLAM3算法地图集的强大之处。最后通过检查全局标志位来跳出循环，完成回环检测。备注：关键帧的数据来自于定位建图线程
  • 在进入下一个函数前这里进行了一个判断：
if(mpLastCurrentKF)
{
	mpLastCurrentKF->mvpLoopCandKFs.clear();
	mpLastCurrentKF->mvpMergeCandKF.clear();
}

这一部分的功能暂时还没有完完全全的理解透彻（虽然知道是对存储的数据进行清空），所以暂时先不分析，后面在整体的细节分析环节在进行分析描述。直接进入下一个函数。

  • NewDetectCommonRegions()函数，进入函数开始进行回环检测，首先取出mlpLoopKeyFrameQueue队列中的第一个关键帧作为当前关键帧，接着将当前帧的mbNotErase标志位设置为真，用于防止此关键帧在别的线程中被删除，并将当前帧的回环检测标志位置为真，且获得当前关键帧的地图。后面的几个连续性的判断分别对应着不同情况下所执行的操作：
  1. IMU模式下：如果在IMU模式下还没有进行第二次初始化则不进行回环检测，并且添加关键帧到关键帧数据库。
  2. 双目模式下：如果当前地图的关键帧5，则不进行回环检测并添加当前关键帧到关键帧数据库。
  3. 若当前地图的关键帧数量小于12则不进行回环检测，并将当前关键帧添加至关键帧数据库。
    关键的部分即将开始，首先需要知道mnLoopNumCoincidences表示的是回环成功的次数，初始化时该变量初始化为0。
    • DetectAndReffineSim3FromLastKF()函数，该函数简单来说就是对当前的关键帧进行匹配判断（暂时不做更多深入的说明）。
    • 融合。
    • 若当前关键帧没有被检测到回环或融合，则寻找当前关键帧的三个回环候选帧和融合候选帧。
    • 若当前关键帧没有检测到回环且回环候选帧不为空，那么就对候选回环帧再次进行检测，判断是否发生回环。
    • 若当前帧没有被检测到融合，且融合选帧不为空，那么就对融合候选帧再次进行检测，并判断是否发生融合。
    • 最后，当检测到回环或者融合的时候返回true。
      当NewDetectCommonRegions()函数返回值为真，即检测到回环或者融合的时候开始进行后面的操作，后续的操作主要分为检测到融合和检测到回环两种情况，这里进行分开讨论。
  • mbMergeDetected == true，也就是检测到融合的情况。此时进入该部分的处理，首先对IMU是否已经进行了初始化进行判断，如果没有初始化则中断，否则开始继续进行处理。假设已经进行了初始化，此时分别获取到当前关键帧的位姿和融合匹配帧的位姿，最后经过一系列处理得到一个最终的四元数，这里需要注意的是MergeLocal2()函数和MergeLocal()函数。
  • mbLoopDetected==true，也就是检测到回环，此时就会进行回环矫正。这里的矫正其实就是通过阈值判断与变换矩阵参数的修改实现的。
    最终完成整个回环检测与优化的过程。