ORB-SLAM2：LoopClosing线程学习随笔【李哈哈：看看总有收获篇】

前言

闭环检测线程处理的是从局部建图线程传过来的关键帧，然后我们从历史关键帧里找能够匹配的，形成闭环，优化地图

整体流程梳理

首先看一下ORB-SLAM2该部分的整体流程图：
从局部建图线程获得关键帧，该关键帧作为当前帧，进行回环检测（BoW二进制词典匹配检测，通过Sim3算法计算相似变换），如果发现回环，就进行回环矫正（回环融合和图优化），最终对完整的地图进行全局BA
看一下代码的整体结构：
首先在主函数，以mono_kitti.cc为例，其中初始化了system类SLAM：

ORB_SLAM2::System SLAM(argv[1], argv[2], ORB_SLAM2::System::MONOCULAR, true);

在System类的构造函数中，完成了对loopclosing的初始化及线程的开启：

//Initialize the Loop Closing thread and launchiomanip
//!初始化LoopClosing对象，并开启LoopClosing线程
mpLoopCloser = new LoopClosing(mpMap,                 //地图
                               mpKeyFrameDatabase,    //关键帧数据库
                               mpVocabulary,          //ORB字典
                               mSensor != MONOCULAR); //当前的传感器是否是单目，这里经过判断，会输入True或False
//创建回环检测线程
mptLoopClosing = new thread(&ORB_SLAM2::LoopClosing::Run, //线程的主函数
                            mpLoopCloser);                //该函数的参数

打开函数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()) ///计算Sim3相似变换
                {
                    // Perform loop fusion and pose graph optimization
                    CorrectLoop(); ///矫正位姿图
                }
            }
        }

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

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

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

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

其中比较重要的就是利用CheckNewKeyFrames()检测是否有待检测的关键帧
若有，使用DetectLoop()检测是否有可能的回环候选关键帧（从历史的关键帧中找到可能成为回环的关键帧）
若找到了，使用ComputeSim3()计算Sim3相似变换，在这里也在筛选候选关键帧，与当前帧进行匹配
最终使用CorrectLoop()来矫正位姿图，也就是矫正关键帧和地图点的位姿

闭环检测：DetectLoop()

Step 1： 从队列中取出一个关键帧,作为当前检测闭环关键帧
将取出的关键帧放入mpCurrentKF
Step 2： 如果距离上次闭环没多久（小于10帧），或者map中关键帧总共还没有10帧，则不进行闭环检测
这里如果在10帧以内，就把当前帧加入关键帧数据库mpKeyFrameDB，不知道是什么作用
Step 3： 遍历与当前关键帧所有连接（>15个共视地图点）关键帧，计算当前关键帧与每个共视关键的bow相似度得分，并得到最低得分minScore
我们不希望找的回环候选帧在当前帧附近，所以这里取出当前帧的共视图，有两个目的：

• 在寻找候选关键帧的时候，这些帧不作考虑
• 计算当前帧与共视帧的BoW评分，作为后边寻找候选关键帧的一个阈值

这里采用如下函数找到共视帧：

const vector<KeyFrame *> vpConnectedKeyFrames = mpCurrentKF->GetVectorCovisibleKeyFrames();

函数返回的是已经排好序的，共视地图点大于15个的关键帧序列
特别的：只有这里用的是共视帧，后边用的都是相连帧
这里的共视帧存入了const vector<KeyFrame *> vpConnectedKeyFrames中
BoW评分最低值存入float minScore中（目测该值应该是0-1之间）
Step 4： 在所有关键帧中找出闭环候选帧（注意不和当前帧连接）
调用DetectLoopCandidates()函数来找到有可能的候选关键帧，存入vpCandidateKFs之中：

vector<KeyFrame *> vpCandidateKFs = mpKeyFrameDB->DetectLoopCandidates(mpCurrentKF, minScore);

进入该函数，依次完成了以下几步：
①找出和当前帧具有公共单词的所有关键帧，不包括与当前帧连接（也就是共视）的关键帧
这里有两层循环，外层遍历当前帧所有的单词，内层遍历包含某个单词的所有关键帧，与当前帧进行对比
执行结束后，所有候选帧放入lKFsSharingWords，候选帧的mnLoopQuery保存了当前帧的ID，同时候选帧的mnLoopWords保存了观测到与当前帧相同单词的个数
②统计上述所有闭环候选帧中与当前帧具有共同单词最多的单词数，用来决定相对阈值
一个循环找到共同单词最多的数量，存入maxCommonWords中
最大相同单词数的0.8倍作为一个筛选的相对阈值，存入minCommonWords中
③遍历上述所有闭环候选帧，挑选出共有单词数大于minCommonWords且单词匹配度BoW大于minScore的候选帧以及评分存入lScoreAndMatch

注意：以上是对单帧进行的筛选，之后开始用组来筛选了哦，就玄学

④计算上述候选帧对应的共视关键帧组的总得分，得到最高组得分bestAccScore，并以此决定阈值minScoreToRetain
使用如下语句得到候选关键帧的10张最佳共视关键帧：

vector<KeyFrame *> vpNeighs = pKFi->GetBestCovisibilityKeyFrames(10); ///这里找了10个最佳的共视关键帧

特别的：在计算组得分的时候，只有组中的帧也是候选关键帧的时候，才能够为组得分做贡献
小问题❓：遍历组的时候，本身不遍历了是吗？
经过这一步，将各组的组得分以及组中BoW得分最高的关键帧放入lAccScoreAndMatch中，并且记录了所有组中最高得分进入bestAccScore
将最高组得分的0.75倍存入minScoreToRetain中，作为又一个相对阈值
⑤只取组得分大于阈值的组，得到组中分数最高的关键帧作为闭环候选关键帧
经过上述条件，候选关键帧被存入vpLoopCandidates，进而返回
至此，Step 4中的主要函数 DetectLoopCandidates() 执行完毕，初步找到了候选关键帧
Step 5： 在候选帧中检测具有连续性的候选帧（相当不好理解）
这一步的目的在于，我们不能仅通过一次判断，就认为是回环了，而是要连续3次才能认为是回环
所以程序会记录之前找到的候选关键帧的组
步骤如下：
①遍历刚才得到的每一个候选关键帧
②将候选帧以及与候选帧相连的关键帧构成一个“子候选组”（所有共视帧大于15个地图点共视 ）
③遍历前一次闭环检测到的“子连续组”（注意是前一次！）
④遍历每个“子候选组”，检测子候选组中每一个关键帧在“子连续组”中是否存在
这里发现了一个bug，子候选组可能并没有经过15这个阈值筛选❗️❗️❗️
梳理一下这里的逻辑，其实这里是三层循环

• 最外层循环：遍历每一个候选关键帧，对自己以及相连关键帧构成“子候选组”
• 中间层循环：遍历上一次闭环的连续组，准备寻找是否可以继续生成连续组
• 最内层循环：遍历当前子候选组中的关键帧

这样就将找到是否有连续组
⑤如果判定为连续，接下来判断是否达到连续的条件
⑥如果该“子候选组”的所有关键帧都和上次闭环无关（不连续），vCurrentConsistentGroups 没有新添加连续关系，归零
🌟🌟🌟这张图生动的表现了遍历过程，值得注意的是，上一次子连续组的关键帧是以集合形式表现的，并没有“显式的”遍历
最终，程序将层层筛选出来的候选关键帧放入mvpEnoughConsistentCandidates中
文中对该vevtor的解释为：

从上面的关键帧中进行筛选之后得到的具有足够的"连续性"的关键帧 – 这个其实也是相当于更高层级的、更加优质的闭环候选帧

到这里，函数DetectLoop()就执行结束了，找到了满足条件的候选关键帧，就会返回True，进入接下来的程序

计算Sim3相似变换：ComputeSim3()

经过上述的回环检测，我们得到了一些条件很棒的关键帧，放入了mvpEnoughConsistentCandidates内
在这个函数，我们将使用相似变换Sim3来锁定闭环匹配关键帧，同时得到更好的相似变换关系
Step 1： 遍历闭环候选帧集，初步筛选出与当前关键帧的匹配特征点数大于20的候选帧集合，并为每一个候选帧构造一个Sim3Solver
①将取出的闭环关键帧设置为不可剔除，防止在LocalMapping线程中KeyFrameCulling()函数处理掉

// Step 1.1 从筛选的闭环候选帧中取出一帧有效关键帧pKF
KeyFrame *pKF = mvpEnoughConsistentCandidates[i];
// 避免在LocalMapping中KeyFrameCulling函数将此关键帧作为冗余帧剔除
pKF->SetNotErase();

②利用BoW词袋向量，匹配当前帧与候选帧，将第i帧匹配到的地图点存入vvpMapPointMatches[i]

int nmatches = matcher.SearchByBoW(mpCurrentKF, pKF, vvpMapPointMatches[i]);

③如果该帧匹配数小于20就跳过，超过20则通过匹配点构造Sim3求解器Sim3Solver

// 粗筛：匹配的特征点数太少，该候选帧剔除
if (nmatches < 20)
{
    vbDiscarded[i] = true;
    continue;
}
else
{
    // Step 1.3 为保留的候选帧构造Sim3求解器
    // 如果 mbFixScale（是否固定尺度） 为 true，则是6 自由度优化（双目 RGBD）
    // 如果是false，则是7 自由度优化（单目）
    Sim3Solver *pSolver = new Sim3Solver(mpCurrentKF, pKF, vvpMapPointMatches[i], mbFixScale);
    ///这是一个新的类，叫做Sim3Solver

    // Sim3Solver Ransac 过程置信度0.99，至少20个inliers 最多300次迭代
    pSolver->SetRansacParameters(0.99, 20, 300);
    vpSim3Solvers[i] = pSolver;
}

注意这里只是初始化了Sim3求解器，设置了RANSAC基本参数，还没开始迭代
此时第一次对候选帧的遍历结束，构造的Sim3求解器放入了vpSim3Solvers中，后边还会拿出来
Step 2： 对每一个候选帧用Sim3Solver 迭代匹配，直到有一个候选帧匹配成功，或者全部失败
这里迭代了每一个候选帧，对通过Sim3变换的候选帧进行了进一步的地图点匹配和优化
①取出之前构造的的Sim3求解器，并使用iterate()迭代
取出迭代器操作如下：

Sim3Solver *pSolver = vpSim3Solvers[i];

调用iterate()函数，进行Sim3求解，若解得，返回Scm是候选帧pKF到当前帧mpCurrentKF的Sim3变换：

// 最多迭代5次，返回的Scm是候选帧pKF到当前帧mpCurrentKF的Sim3变换（T12）
cv::Mat Scm = pSolver->iterate(5, bNoMore, vbInliers, nInliers);

当前函数最多迭代5次，总的迭代次数不能超过300（不是很清楚这个总的迭代次数做什么❓）
这个迭代函数内部过程为：

a.随机取三组点，取完后从候选索引中删掉
b.根据随机取的两组匹配的3D点，计算P3Dc2i 到 P3Dc1i 的Sim3变换
c.对计算的Sim3变换，通过投影误差进行inlier检测
d.记录并更新最多的内点数目及对应的参数
e.如果已经达到了最大迭代次数了还没得到满足条件的Sim3，说明失败了，放弃，返回空矩阵

②通过上面求取的Sim3变换引导关键帧匹配，弥补Step 1中的漏匹配，调用函数SearchBySim3()
该部分利用之前求得的Sim3变换，优化地图点匹配
可参考下图进行理解：
通过Sim3变换，投影搜索pKF1的特征点在pKF2中的匹配，同理，投影搜索pKF2的特征点在pKF1中的匹配
函数内还完成了一致性匹配，也就是都匹配上才认为是匹配的地图点
③用刚刚得到的新的地图点匹配来优化 Sim3，只要有一个候选帧通过Sim3的求解与优化，就跳出停止对其它候选帧的判断
调用函数OptimizeSim3()完成优化：

// 优化mpCurrentKF与pKF对应的MapPoints间的Sim3，得到优化后的量gScm
const int nInliers = Optimizer::OptimizeSim3(mpCurrentKF, pKF, vpMapPointMatches, gScm, 10, mbFixScale);

返回的是匹配上的内点数量
如果优化后，通过卡方检测的内点数大于20的话，将该候选帧存入mpMatchedKF中，这就是最终得到的闭环匹配帧
得到从世界坐标系到候选帧的Sim3变换gSnw，都在一个坐标系下，所以尺度均为1：❓

// gSmw：从世界坐标系 w 到该候选帧 m 的Sim3变换，都在一个坐标系下，所以尺度 Scale=1
g2o::Sim3 gSmw(Converter::toMatrix3d(pKF->GetRotation()), Converter::toVector3d(pKF->GetTranslation()), 1.0);

注意：只要有一个候选帧通过Sim3的求解与优化，就跳出停止对其它候选帧的判断
Step 3： 取出闭环匹配帧及其共视关键帧，以及这些共视关键帧的地图点
闭环候选帧组存入vpLoopConnectedKFs，组中的所有地图点放入mvpLoopMapPoints
Step 4： 将闭环关键帧及其共视关键帧的所有地图点投影到当前关键帧进行投影匹配
调用SearchByProjection()函数实现投影匹配，当前帧地图点是否匹配上的信息存入mvpCurrentMatchedPoints
Step 5： 统计当前帧与检测出的所有闭环关键帧的匹配地图点数目，超过40个说明成功闭环，否则失败
利用刚刚保存的匹配信息，统计有多少个匹配地图点，超过40个就认为匹配成功
🌟🌟🌟最终的闭环匹配帧放在了mpMatchedKF哦

闭环矫正与全局BA：CorrectLoop()

Step 0： 结束局部建图线程、全局BA，为闭环矫正做准备
调用RequestStop()函数，暂停局部建图线程：

// 请求局部地图停止，防止在回环矫正时局部地图线程中InsertKeyFrame函数插入新的关键帧
mpLocalMapper->RequestStop();

Step 1： 根据共视关系更新当前帧与其它关键帧之间的连接
注意是更新当前帧的连接关系：

// 因为之前闭环检测、计算Sim3中改变了该关键帧的地图点，所以需要更新
mpCurrentKF->UpdateConnections();

因为之前改变了地图点，而连接关系是根据地图点判断的，所以这里重新更新连接关系
Step 2： 通过位姿传播，得到Sim3优化后，与当前帧相连的关键帧的位姿，以及它们的MapPoints（位姿传播矫正）
该步完成如下几个步骤，个人认为是比较重要的优化过程：
①通过mg2oScw（认为是准的）来进行位姿传播，得到当前关键帧的共视关键帧的世界坐标系下Sim3 位姿（还没有修正）
这里仅仅是得到了这些共视关键帧位姿传播优化之后的关键帧，并没有修正这些共视关键帧的位姿
②得到矫正的当前关键帧的共视关键帧位姿后，修正这些关键帧的地图点
注意：这里遍历的是当前帧的共视关键帧，是不包括当前帧的
修正的策略： 取得地图点世界坐标系下的坐标，然后根据这些共视关键帧矫正之前的位姿求出地图点在相机坐标系下的位姿，然后用共视关键帧矫正之后的位姿乘上刚刚得到的地图点在相机坐标系下的位姿，就是矫正之后地图点在世界坐标系下的坐标
③将共视关键帧的Sim3转换为SE3，根据更新的Sim3，更新关键帧的位姿
刚刚只是得到了共视关键帧矫正之后的位姿，并没有更新，这里完成了更新
④根据共视关系更新这些矫正位姿后的当前关键帧的共视关键帧与其它关键帧之间的连接
因为上一步刚刚对这些共视关键帧进行了矫正，所以紧接着对他们进行更新连接关系：

// 地图点的位置改变了,可能会引起共视关系\权值的改变
pKFi->UpdateConnections(); ///注意，不是当前帧，而是当前帧的共视帧

到这里，对当前帧的共视关键帧的遍历就结束了，总而言之就是利用位姿传播矫正，更新关键帧位姿和地图点位姿
Step 3： 检查当前帧的MapPoints与闭环匹配帧的MapPoints是否存在冲突，对冲突的MapPoints进行替换或填补
这里取出了相同索引下，利用闭环候选帧得到的地图点和当前帧的地图点，对比是否重复：

if (pCurMP)
    // 如果有重复的MapPoint，则用匹配的地图点代替现有的
    // 因为匹配的地图点是经过一系列操作后比较精确的，现有的地图点很可能有累计误差
    pCurMP->Replace(pLoopMP);
else
{
    // 如果当前帧没有该MapPoint，则直接添加
    mpCurrentKF->AddMapPoint(pLoopMP, i);
    pLoopMP->AddObservation(mpCurrentKF, i);
    pLoopMP->ComputeDistinctiveDescriptors();
}

可是为什么二者会不同呢❓
如果发现重复，优先使用闭环匹配下的地图点，因为认为其误差更小
如果发现没有匹配地图点，就添加上闭环匹配的地图点
Step 4： 通过将闭环时相连关键帧的mvpLoopMapPoints投影到这些关键帧中，进行MapPoints检查与替换
使用SearchAndFuse()函数完成这一步：

// 因为 闭环相连关键帧组mvpLoopMapPoints 在地图中时间比较久经历了多次优化，认为是准确的
// 而当前关键帧组中的关键帧的地图点是最近新计算的，可能有累积误差
// CorrectedSim3：存放矫正后当前关键帧的共视关键帧，及其世界坐标系下Sim3 变换
SearchAndFuse(CorrectedSim3);

该函数的作用是对于每个经过位姿传播矫正过的当前帧的相连关键帧，将闭环地图点按照矫正后的位姿投影到这些帧中：

1. 若该地图点已经可以被该帧观测到则跳过，否则利用区域块内的描述子匹配找到该地图点所匹配的最佳特征点
2. 如果该特征点还没有被匹配，则添加该特征点和地图点的匹配关系
3. 如果该特征点已经有匹配的地图点，则用闭环特征点替换掉原来的地图点作为新的匹配

这个新的匹配的地图点暂时放在MapPoint的mpReplaced变量里。而同时也需要更新所有能观测到该地图点（需要更新的原来地图点）为新的闭环地图点（这段是别人写的，不知道啥意思，表达有点奇怪❓）

Step 5： 更新当前关键帧之间的共视相连关系，得到因闭环时MapPoints融合而新得到的连接关系
这里其实弄了不少操作，奈何现在还没看那么深入，先放到这里❓：

// Step 5：更新当前关键帧之间的共视相连关系，得到因闭环时MapPoints融合而新得到的连接关系
// LoopConnections：存储因为闭环地图点调整而新生成的连接关系
map<KeyFrame *, set<KeyFrame *>> LoopConnections;

// Step 5.1：遍历当前帧相连关键帧组（一级相连）
for (vector<KeyFrame *>::iterator vit = mvpCurrentConnectedKFs.begin(), vend = mvpCurrentConnectedKFs.end(); vit != vend; vit++)
{
    KeyFrame *pKFi = *vit;
    // Step 5.2：得到与当前帧相连关键帧的相连关键帧（二级相连）
    vector<KeyFrame *> vpPreviousNeighbors = pKFi->GetVectorCovisibleKeyFrames();

    // Update connections. Detect new links.
    // Step 5.3：更新一级相连关键帧的连接关系(会把当前关键帧添加进去,因为地图点已经更新和替换了)
    pKFi->UpdateConnections();
    // Step 5.4：取出该帧更新后的连接关系
    LoopConnections[pKFi] = pKFi->GetConnectedKeyFrames();
    // Step 5.5：从连接关系中去除闭环之前的二级连接关系，剩下的连接就是由闭环得到的连接关系
    for (vector<KeyFrame *>::iterator vit_prev = vpPreviousNeighbors.begin(), vend_prev = vpPreviousNeighbors.end(); vit_prev != vend_prev; vit_prev++)
    {
        LoopConnections[pKFi].erase(*vit_prev);
    }
    // Step 5.6：从连接关系中去除闭环之前的一级连接关系，剩下的连接就是由闭环得到的连接关系
    for (vector<KeyFrame *>::iterator vit2 = mvpCurrentConnectedKFs.begin(), vend2 = mvpCurrentConnectedKFs.end(); vit2 != vend2; vit2++)
    {
        LoopConnections[pKFi].erase(*vit2);
    }
}

Step 6： 进行EssentialGraph优化，LoopConnections是形成闭环后新生成的连接关系，不包括步骤7中当前帧与闭环匹配帧之间的连接关系
这里用了一个很长的函数：

Optimizer::OptimizeEssentialGraph(mpMap, mpMatchedKF, mpCurrentKF, NonCorrectedSim3, CorrectedSim3, LoopConnections, mbFixScale);

优化的连接关系，大部分是当前帧连接组和闭环匹配帧连接组之间的连接关系，当然夜包括一些当前帧连接组内之间的新增加的关系
💫💫💫这个函数就是所谓的根据闭环的关系，将累积的误差分配到各处
OptimizeEssentialGraph()函数完成了以下一系列操作：
①设置g2o优化器
②将目前为止地图中的所有关键帧的位姿，添加为图优化的顶点。如果当前帧是闭环帧，则该帧的位姿固定，不优化。并且，如果该帧的位姿经过闭环传播调整过，那么使用经过调整后的位姿。
③添加由于闭环时地图点的更新而新出现的关键帧之间的联系作为图优化的边。当然这部分边主要是当前帧连接组和闭环帧连接组之间新建立的边。
④对所有关键帧添加跟踪时得到的边，也就是添加该关键帧和其父关键的联系作为边，并且使用未经过闭环位姿传播调整的原始位姿求相对位姿作为边的观测值；对所有关键帧添加该关键帧的所有闭环帧连接得到的边，并且使用未经过位姿传播调整的原始位姿求相对位姿作为边的观测值；对所有关键帧中的每个关键帧，添加与该关键帧共视地图点个数大于100的关键帧组成的边，且使用未经过位姿传播调整的原始位姿求解相对位姿作为边的观测值。
⑤开始执行优化，迭代20次
⑥根据优化，更新所有关键帧的位姿。
⑦根据优化，更新所有地图点的三维位置。这里有个细节就是需要找到该地图点的参考关键帧，然后利用该参考关键帧的位姿，求解地图点的在相机坐标系下的坐标。然后利用优化后的位姿，再将相机位姿再映射到世界坐标系下。

Step 7： 添加当前帧与闭环匹配帧之间的边（这个连接关系不优化）（备注作者认为应该放到第六步之前，因为上一步的函数内优化了二者的关系）
Step 8： 新建一个线程用于全局BA优化【心脏骤停】💔

关于共视图和闭环候选帧的选择原则小问题

共视图的建立是在局部建图线程，基本方法是寻找观测到当前帧地图点的先前的关键帧，建立的线索在于共同的地图点（值得一提的是，共视关键帧是共同看到了大于等于15个地图点的关键帧）
而闭环候选帧的选择使用的是BoW向量，先通过BoW向量的相似关系，找匹配的地图点，还是有一定区别的
那么问题来了，希望的候选关键帧会不会已经是共视关键帧了？

小问题

什么是内点和外点

在十四讲里没有找到内外点的定义
网上有人这样解释：
内外点之分最简单的说法就是是否符合当前位姿的判断：如果根据当前位姿，之前帧二维特征点所恢复出的地图点重投影到当前帧与实际的二维特征点匹配不上了，那么认为这个是质量差的点是outlier，抛弃掉，如果能匹配上，那就是inlier，保留。
还有一种情况，根据地图点3D位置，当前帧位姿理论上看不到这个地图点，在相机视野之外，也认为这个地图点是当前帧的outlier。