(01)ORB-SLAM2源码无死角解析-(53) 闭环线程→了解闭环检测、主体框架讲解:LoopClosing::Run

本人讲解关于slam一系列文章汇总链接:史上最全slam从零开始，针对于本栏目讲解的(01)ORB-SLAM2源码无死角解析-接如下:
(01)ORB-SLAM2源码无死角解析-(00)目录_最新无死角讲解：https://blog.csdn.net/weixin_43013761/article/details/123092196

${\color{blue}{文末正下方中心}提供了本人 \color{red} 联系方式，\color{blue}点击本人照片即可显示WX→官方认证}$

一、前言

1、回环检测的意义

        该部分的内容摘抄《视觉 SLAM 十四讲:从理论到实践》：通过前面的博客，可以知道，ORB-SLAM2在追踪的过程中，使用最多的跟踪方式为恒速模型跟踪当前普通帧TrackWithMotionModel()，其是视觉里程计追踪，也就是利用连续的图像序列来估计机器人移动距离的方法。然而，如果像视觉里程计那样仅考虑相邻时间上的关键帧，那么，之前产生的误差将不可避免地累积到下一个时刻，使得整个 SLAM 出现票积误差，长期估计的结果将不可靠，或者说，我们无法构建 $\color{red} 全局一致$ 的轨迹和地图。
        举个简单的例子:在自动驾驶的建图阶段，我们通常会指定采集车在某个给定区域绕若干圈以覆盖所有采集范围。假设我们在前端提取了特征，然后忽略特征点，在后端使用位姿图优化整个轨迹，如下图 (a) 所示。前端给出的只是局部的位姿间约束，例如，可能是 $x_1 -x_2$ ， $x_1-x_2$ 等等。但是，由于 $x_1$ 的估计存在误差， $x_2$ 是根据 $x_1$ 决定的， $x_3$ 又是由 $x_2$ 决定的。依此类推，误差就会被累积起来，使得后端优化的结果如下图 (b) 所示，慢慢地趋向不准确。在这种应用场景下，我们应该保证，优化的轨迹和实际地点一致。当我们实际经过同一个地点时，估计轨迹也必定经过同一点。在这里插入图片描述         虽然后端能够估计最大后验误差，但所谓"好模型架不住烂数据"，只有相邻关键帧数据时，我们能做的事情并不多，也无从消除累积误差。但是，回环检测模块能够给出除了相邻帧的一些
时隔更加久远的约束：例如 $x_l -x_{100}$ 之间的位姿变换。为什么它们之间会有约束呢?这是因为我们察觉到相机经过了同一个地方，采集到了相似的数据。而回环检测的关键，就是如何有效地检测出相机经过同一个地方这件事。如果我们能够成功地检测到这件事，就可以为后端的位姿图提供更多的有效数据，使之得到更好的估计，特别是得到一个全局一致的估计。为什么它们之间会有约束呢?这是因为我们察觉到相机经过了同一个地方，采集到了相似的数据。而回环检测的关键，就是如何有效地检测出相机经过同一个地方这件事。如果我们能够成功地检测到这件事，就可以为后端的位姿图提供更多的有效数据，使之得到更好的估计，特别是得到一个全局一致的估计。
        回环检测对于 SLAM 系统意义重大。一方面，它关系到我们估计的轨迹和地图在长时间下的正确性。另一方面，由于国环检测提供了当前数据与所有历史数据的关联，我们还可以利用回
环检测进行重足位。重定位的用处就更多→些。例如，如果我们事先对某个场景录制了一条轨迹并建立了地图，那么之后在该场景中就可以一直跟随这条轨迹进行导航，而重定位可以帮助我们确定自身在这条轨迹上的位置。因此，回环检测对整个 SLAM 系统精度与稳健性的提升是非常明显的。甚至在某些时候，我们把仅有前端和局部后端的系统称为视觉里程计，而把带有回环检测和全局后端的系统称为 SLAM。

2、回环检测的方法

        下面我们来考虑回环检测如何实现的问题。事实上存在若干种不同的思路来看待这个问题，包括理论上的和工程上的。最简单的方式就是对任意两幅图像都做一遍特征匹配，根据正确匹配的数量确定哪两幅图像存在关联一一这确实是一种朴素且有效的思想。缺点在于，我们盲目地假设了"任意两幅图像都可能存在国环"，使得要检测的数量实在太大:对于 $N$ 个可能得回环，我们要检测 $C_N^2$ 那么多次，这是 $O(N^2)$ 的复杂度，，随着轨迹变长增长太快，在大多数实时系统中是不实用的。另一种朴素的方式是，随机抽取历史数据并进行回环检测，例如在 $n$ 帧中随机抽取 5 帧与当前帧比较。这种做法能够维持常数时间的运算量，但是这种盲目试探方法在帧数增长时，抽到回环的概率又大幅下降，使得检测效率不高。
        上面说的朴素思路都过于粗糙。尽管随机检测在有些实现中确实有用，但我们至少希望有一个"哪处可能出现回环"的预计，才好不那么盲目地去检测。这样的方式大体有两种思路:基于里程计( Odometry based )的几何关系，或基于外观( Appearance based )的几何关系。基于里程计的几何关系是说，当我们发现当前相机运动到了之前的某个位置附近时，检测它们有没有回环关系一一这自然是一种直观的想法，但是由于累积误差的存在，我们往往没法正确地发现"运动到了之前的某个位置附近"这件事实，回环检测也无从谈起。因此，这种做法在逻辑上存在一点问题，因为回环检测的目标在于发现"相机回到之前位置"的事实，从而消除累积误差而基于里程计的几何关系的做故法假设了"相机回到之前位置附近’'，这样才能检测回环，这是有倒果为因的嫌疑的，因而也无法在累积误差较大时工作。
        另一种方法是基于外观的。它和前端、后端的估计都无关，仅根据两幅图像的相似性确定回环检测关系。这种做法摆脱了累积误差，使回环检测模块成为 SLAM 系统中一个相对独立的模块(当然前端可以为它提供特征点)。自 21 世纪初被提出以来，基于外观的回环检测方式能够有效地在不同场景下工作，成了视觉 SLAM 中主流的做法，并被应用于实际的系统中。
        除此之外，从工程角度我们也能提出一些解决围环检测的办法。例如，室外的元人车通常会配备 GPS ，可以提供全局的位置信息。利用 GPS 信息可以很轻松地判断汽车是否回到某个经过的点，但这类方法在室内就不怎么好用。
        在基于外观的回环检测算法中，核心问题是如何计算图像间的相似性。例如，对于图像 $A$ 和图像 $B$ ，我们要设计一种方法，计算它们之间的相似性评分 $s (A, B)$ ，。当然，这个评分会在某个区间内取值，当它大于一定量后我们认为出现了一个回环。读者可能会有疑问:计算两幅图像之间的相似性很困难吗?例如直观上看，图像能够表示成矩阵，那么直接让两幅图像相喊，然后取某种范数行不行呢? $s (A, B) = ∣∣ A - B ∣∣$ 为什么我们不这样做?
1.前面也说过，像素灰度是一种不稳定的测量值，它严重地受环境光照和相机曝光的影响。假设相机未动，我们打开了一支电灯，那么图像会整体变亮。这样，即使对于同样的数据，我们也会得到一个很大的差异值。
2.当相机视角发生少量变化时，即使每个物体的光度不变，它们的像素也会在图像中发生位移，造成一个很大的差异值。

由于这两种情况的存在，实际中，即使对于非常相似的图像 $A - B$ 也会经常得到一个(不符合实际的)很大的值。所以我们说，这个函数不能很好地反映图像间的相似关系。

$\color{blue} 核心：$ 也就是说，闭环检测其核心要点在于比较两张图像的相似度。后面主要也是围绕这个核心进行讲解。

二、闭环线程

通过前面的博客，已经对追踪线程以及局部建图线程进行了详细的讲解，接下来就是对闭环线程的讲解了。闭环线程在在 src/System.cc 文件中 System::System() 构造函数里创建与启动，代码如下：

    //Initialize the Loop Closing thread and launchiomanip
    mpLoopCloser = new LoopClosing(mpMap, 						//地图
    							   mpKeyFrameDatabase, 			//关键帧数据库
    							   mpVocabulary, 				//ORB字典
    							   mSensor!=MONOCULAR);			//当前的传感器是否是单目
    //创建回环检测线程
    mptLoopClosing = new thread(&ORB_SLAM2::LoopClosing::Run,	//线程的主函数
    							mpLoopCloser);

其核心函数循环函数为 LoopClosing::Run，在进行讲解之前，先来回顾整体结构图：
在这里插入图片描述
其上的红色框部分就是就是我们接下来主要讲解的内容，可见其主要包含了查询数据库、计算Sim3、闭环融合、本质图优化。回到代码 src/LoopClosing.cc 文件中的 void LoopClosing::Run() 函数。其注释如下：

// 回环线程主函数
void LoopClosing::Run()
{
    
    
    mbFinished =false;

    // 线程主循环
    while(1)
    {
    
    
        // Check if there are keyframes in the queue
        // Loopclosing中的关键帧是LocalMapping发送过来的，LocalMapping是Tracking中发过来的
        // 在LocalMapping中通过 InsertKeyFrame 将关键帧插入闭环检测队列mlpLoopKeyFrameQueue
        // Step 1 查看闭环检测队列mlpLoopKeyFrameQueue中有没有关键帧进来
        if(CheckNewKeyFrames())
        {
    
    
            // Detect loop candidates and check covisibility consistency
            if(DetectLoop())
            {
    
    
               // Compute similarity transformation [sR|t]
               // In the stereo/RGBD case s=1
               if(ComputeSim3())
               {
    
    
                   // Perform loop fusion and pose graph optimization
                   CorrectLoop();
               }
            }
        }

        // 查看是否有外部线程请求复位当前线程
        ResetIfRequested();

        // 查看外部线程是否有终止当前线程的请求,如果有的话就跳出这个线程的主函数的主循环
        if(CheckFinish())
            break;

        //usleep(5000);
		std::this_thread::sleep_for(std::chrono::milliseconds(5));

	}

    // 运行到这里说明有外部线程请求终止当前线程,在这个函数中执行终止当前线程的一些操作
    SetFinish();
}

三、结语

与追踪线程与局部建图线程相比，很明显闭环线程看起来简单了很多，其实并没有，不要被他的外表迷惑了，其内部还是十分复杂的。其主体架构主要为以下几个部分：
$\color{blue}(1)：$ 查看闭环检测队列mlpLoopKeyFrameQueue中有没有关键帧进来。在上一篇博客中进行了讲解，mlpLoopKeyFrameQueue 的关键帧插入，在局部建图线程 LocalMapping::Run() 函数中的 mpLoopCloser->InsertKeyFrame(mpCurrentKeyFrame); 代码段实现。

$\color{blue}(2)：$ 如果有关键帧进行，则进行闭环检测，DetectLoop()。

$\color{blue}(3)：$ 如果闭环检测成功，则调用 ComputeSim3() 计算 Sim3

$\color{blue}(4)：$ 执行循环融合和位姿图优化

$\color{blue}(5)：$ 查看是否有外部线程请求复位当前线程，查看外部线程是否有终止当前线程的请求,如果有的话就跳出这个线程的主函数的主循环.

总的来说，结构还是很简单的，下面就是去每个函数进行细节的分析了。