闭环探测的总体过程

闭环探测的主要作用是，降低前端里程计的累计误差。通过判定机器人是否回到了曾经到过的地方，在共视图中建立闭环约束。再对整个地图进行 BA 优化，将较大的累计误差均摊到闭环的各个关键帧上。ORB-SLAM2 在一个独立的线程 LOOP CLOSING 中对每个新生成的关键帧进行闭环检测，并根据 Essential Graph 进行闭环修正。该线程还会开启第四个线程 FULL BA，对整个地图进行优化，获得数据意义上最优的地图点和相机运动轨迹的估计。

我们在第一部分的介绍中，了解到 ORB_SLAM2 通过类 LoopClosing 将上述过程封装到一个闭环探测器中完成。本文中，我们将结合2015年和2017年的论文以及代码，从总体上了解闭环检测和FULL BA具体过程。

1. 总体过程浅析

右图是从2017年论文的系统框图中截取的关于闭环检测的过程。 2015年论文的第 VII 部分中，作者从检测闭环候选帧到优化基图详细地介绍了框图中右侧的LOOP CLOSING标记的四个任务。在2017年的论文中，介绍了 Full BA。总体上来说，闭环探测过程可以分为检测闭环、闭环修正、全局优化三个部分：

检测闭环: 对应框图中的 “Query Database” 和 “ComputeSE3” 两个环节。其中“Query Database”主要是在历史的关键帧中，根据词袋模型的相似度，从共视图宗筛选出可能的闭环候选帧。 “ComputeSE3”是在估计候选帧与当前关键帧的相似变换关系，一共涉及到7个自由度：三个平移、三个旋转、和一个尺度。
闭环修正: 对应框图中的 “Loop Fusion” 和 “Optimize Essential Graph” 两个环节。首先需要融合空间上重复的地图点，同时在共视关系图中插入一条新的闭环边。这就是 Loop Fusion 环节的任务。然后会根据基图, Essential Graph 进行位姿图优化，优化相机位姿和尺度以及地图点坐标。
全局优化: 对应框图中的 “Full BA” 和 “Update Map” 两个环节。可能是觉得根据基图的优化还不够，作者在 2017 年发布的工作中，额外增加了一个 FULL BA 的线程，对地图中所有的关键帧和地图点进行优化。

在闭环探测器——LoopClosing一文中，我们查看了类 LocalMapping 的成员变量和成员函数。了解到它的构造函数只有一个初始化列表完成了各个成员变量的初始化工作，其函数体是空的。系统对象通过如下的语句，先后构造了闭环探测器对象 mpLoopCloser 和线程 mptLoopClosing。

mpLoopCloser = new LoopClosing(mpMap, mpKeyFrameDatabase, mpVocabulary, mSensor!=MONOCULAR);
mptLoopClosing = new thread(&ORB_SLAM2::LoopClosing::Run, mpLoopCloser);

类 LoopClosing 的成员函数 Run() 是闭环探测的运行函数。下面是该函数的实现，代码十分简洁，整个就是一个 while 超级循环。它先后通过函数 DetectLoop，ComputeSE3 完成了检测闭环的两项工作，筛选了闭环候选帧，计算了相似变换矩阵。然后在函数 CorrectLoop 中完成了闭环修正中提到的地图点融合以及位姿图优化两项工作。

展开 LoopClosing::Run >>>

        void LoopClosing::Run()
        {
            mbFinished =false;
            while(1) {
                // Check if there are keyframes in the queue
                if(CheckNewKeyFrames()) {
                    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);
            }
            SetFinish();
        }

上面的这个函数中并没有体现全局优化的工作。实际上函数 CorrectLoop 在完成位姿图优化之后，会专门创建了一个线程，如下面的代码片段所示，在线程函数 RunGlobalBundleAdjustment 中完成全局BA优化并更新地图。

        // 在函数 void LoopClosing::CorrectLoop() 中通过如下语句创建线程，完成全局优化
        // mpThreadGBA = new thread(&LoopClosing::RunGlobalBundleAdjustment,this,mpCurrentKF->mnId);
        void LoopClosing::RunGlobalBundleAdjustment(unsigned long nLoopKF)
        {
            int idx =  mnFullBAIdx;
            Optimizer::GlobalBundleAdjustemnt(mpMap,10, &mbStopGBA, nLoopKF,false);
        
            {
                // 更新所有的地图点和关键帧
                // Local Mapping was active during BA, that means that there might be new keyframes
                // not included in the Global BA and they are not consistent with the updated map.
                // We need to propagate the correction through the spanning tree
            }
        }

2. 检测闭环

2.1 计算候选闭环关键帧

在理解论文中描述的筛选候选闭环关键帧的过程之前，先回顾一下闭环探测器中专门设计的类 ConsistentGroup 及其概念。在 LoopClosing 中，把一个关键帧\(K_i\)及其在共视图中的邻接关键帧一起称为一个Group（包含关键帧本身），记为\(G_{K_i}\)。如果两个组 \(G_{K_i}\) 和 \(G_{K_j}\) 的交集非空，意味着存在至少一个关键帧同时出现在两个 Group 中，那么我们称\(G_{K_i}\) 和 \(G_{K_j}\)是一致的(Consistent)。

ORB_SLAM2 会根据关键帧的词袋向量，计算当前帧\(K_c\)与\(G_{K_c}\)中其它关键帧的相似度，并记录下最低的相似度\(s_{min}\)。然后在\(G_{K_c}\)之外的所有关键帧中，筛选出相似度超过 \(s_{min}\) 的关键帧，用一个容器记录下这些关键帧的组。然后判定新容器中的组与上一个容器中的组的一致性关系，并累加计数。如果一个组计数超过了三次，那么我们就认为成功检测到一次闭环，该组对应的关键帧以后称该关键帧为闭环关键帧，记为\(K_l\)。通过筛选的关键帧可能有多个，所以最后得到的是一个关键帧的集合。

2.2 计算相似变换(SE3)

一般来说，对于机器人的位置估计，有 6 个自由度的误差，三轴平移和三轴旋转。在单目的 SLAM 系统中，还增加了第七个误差项——尺度。因此闭环发生之后，我们需要估计当前帧\(K_c\)和闭环帧\(K_l\)之间的相似变换关系\(S = [s\boldsymbol{R}|\boldsymbol{t}]\)，其中\(s\)表示尺度， \(\boldsymbol{R}\)和\(\boldsymbol{t}\)分别是旋转矩阵和平移向量。进一步的根据相似变换关系，对闭环进行几何验证。

首先根据词袋关系，对关键帧\(K_c\)与\(K_l\)中地图点进行ORB特征匹配，建立3D-3D的特征点映射关系。然后以 RANSAC 的形式通过Horn 算法求得一个粗解，最后通过迭代的方式对该解进行优化，统计内点数量。只有内点足够多，我们才认为两个关键帧的相似变换是合理的，确实发生了闭环，将该变换记为\(S_{cl}\)。

3. 闭环修正

3.1 闭环融合

闭环融合主要是在做两件事情，其一在共视图中插入一条边来描述当前帧与闭环帧的共视关系，其二融合重复的地图点。

首先，根据刚刚计算的相似变换关系\(S_{cl}\)修正当前帧的位姿，并通过concatenating transform把修正量传播给当前帧的所有邻接关键帧，达到对齐闭环连边两侧的关键帧的位姿关系的目的。然后将闭环帧的group \(G_{K_l}\) 中出现的所有地图点都投影到当前帧即其邻接关键帧上。对于每个映射后的地图点，都在一个很小的空间中搜索匹配点。如果成功找到了匹配关系，并且相似变换\(S_{sl}\)中判定是内点，则认为该地图点和匹配点实际是一个点，融合之。

融合地图点的过程，将同时更新关键帧中地图点的观测信息，完成共视关系的更新。

3.2 优化基图(Essential Graph)

在 ORB-SLAM2 中存在三个关于描述关键帧连接关系的图：共视图(covisibility graph)、生成树(spanning tree)、基图(essential graph)。有些资料里面把 Essential Graph 翻译为本征图，感觉更好一点。它们只是稀疏程度不同。在共视图中，当两帧之间共视的地图点数量超过 15 个的时候，就会在两帧之间建立连边。对于一个关键帧，生成树中只记录了与之共视地图点数量最多的那个关键帧。基图包含了生成树，记录了最优的共视关系之外，还记录了那些共视地图点数量超过 100 个的关键帧。

闭环增加了新的约束，LoopCloser 会在基图的上进行位姿图优化。优化过程中，闭环约束可以有效地将前端里程计的累计误差和相似变换的估计误差，均摊到位姿图中的各个节点上，达到降低系统整体误差的目的。估计也这是人们将 BA 算法翻译成光束平差法的原因吧。优化的方法和套路与局部BA优化没什么本质区别，只是参与优化的节点和约束增多了。

4. 全局优化

本来按照2015年的套路，在基图上完成图优化之后故事就应该结束了。2017 发布的在 ORB-SLAM2 增加了双目和深度相机的支持，这两个传感器可以直接观测到尺度，所以在闭环检测时不需要像单目相机那样进行相似变换，处理尺度漂移。所以 ORB-SLAM2 又额外增加了一个 Full BA 的操作，对所有的关键帧和地图点进行 BA 优化。这是一个非常耗时的过程，ORB-SLAM2 将单独开启一个线程来完成这项任务。

虽然多线程并行在一定程度上提高了系统的运行效率，但是它带来了一个数据不一致的问题。在 Full BA 进行全局优化的过程中，地图仍然在更新，而且可能再次检测到新的闭环。如何将在旧地图上的优化结果同步到新的地图上是 Updata Map 环节主要考虑的问题。

如果在全局优化的过程中，检测到了新的闭环，将放弃当前正在进行的优化，完成闭环修正之后，重新开启 Full BA 线程。全局优化结束之后，需要根据生成树的连接关系，将参与优化的关键帧和地图点的状态，传播给新增的关键帧和地图点。新增的关键帧通过生成树找到旧图中与之共视关系最紧密的关键帧，根据它们的帧间位姿关系，修正新增的关键帧位姿。新增的地图点则根据它们的参考关键帧修正后的位姿，更新空间坐标。

5. 完

本文中，我们结合代码从总体上了解了闭环探测的总体过程。在 2015 年的论文中，只有闭环检测和闭环修正两个部分，先后依次完成了候选闭环关键帧的筛选、当前帧与闭环帧之间的相似变换估计、闭环融合更新共视关系、基于基图的图优化。在2017年的论文中，又新增了一个 Full BA 的线程对所有的关键帧和地图点进行全局BA优化，然后根据生成树将优化后的结果传播给新增的关键帧和地图点。