(01)ORB-SLAM2源码无死角解析-(55) 闭环线程→闭环检测:寻找闭环候选关键帧 LoopClosing::DetectLoop()

讲解关于slam一系列文章汇总链接:史上最全slam从零开始，针对于本栏目讲解的(01)ORB-SLAM2源码无死角解析链接如下(本文内容来自计算机视觉life ORB-SLAM2 课程课件):
(01)ORB-SLAM2源码无死角解析-(00)目录_最新无死角讲解：https://blog.csdn.net/weixin_43013761/article/details/123092196
 
$文末正下方中心提供了本人联系方式，点击本人照片即可显示WX\to 官方认证$
 

一、前言

通过上一篇博客，对闭环检测进行了详细的讲解，通过闭环检测 LoopClosing::DetectLoop() 找到闭环候选关键帧，然后存储于 mvpEnoughConsistentCandidates 之中，根据 LoopClosing::Run() 函数的流程，执行完 DetectLoop() 之后，则会调用 ComputeSim3() 函数。该函数位于 src/LoopClosing.cc 文件中。

$问题：$ 这里首先出现的一个疑问就是，为什么我们要使用sim3？sim也叫相似变换，相似变换和欧式变换他们只相差了一个尺度因子(缩放系数)，欧式变换和相似变换表述如下：
T = [ R t 0 1 ]                    T s = [ s R t 0 1 ] (01) \color{Green} \tag{01} \mathbf T=\left[Rt01

\right]~~~~~~~~~~~~~~~~~~Ts=\left[sRt01\right] T=⎣ ⎡​R0​t1​⎦ ⎤​                  Ts=⎣ ⎡​sR0​t1​⎦ ⎤​(01)左边为欧式变换矩阵，右边的是相似变换矩阵，可以很明显的知道仅仅相差一个尺度因子 $s$，当 $s=1$的时候，相似变换就成了欧式变换。在 ComputeSim3() 函数，如果使用双目或者深度相机，那么其 $s=1$。也就是说 ComputeSim3() 函数中，只有是单目的情况下才使用欧式相似变换，否则可以看作欧式变换。

那么自然就引入了一个问题，为什么单目的情况下就需要使用相似变换呢？这里主要涉及到一个为尺度不确定性( Scale Ambiguity )的问题。下面两篇博客对单目相机的尺度不确定性(尺度漂移)做了比较详细的介绍与解答：
(01)ORB-SLAM2源码无死角解析-(14) 地图初始化→单目初始化MonocularInitialization():尺度不确定性
(01)ORB-SLAM2源码无死角解析-(24) 单目SFM地图初始化→CreateInitialMapMonocular()-细节分析:尺度不确定性

由于单目尺度不确定性(尺度漂移)的原因，所以需要把尺度因子考虑进去，具体的细节来看 ComputeSim3() 函数，该函数涉及到的东西有点多，该篇博客主要对其总体流程进行讲解，其中调用的核心函数主要有如下几个

int nmatches = matcher.SearchByBoW(mpCurrentKF,pKF,vvpMapPointMatches[i]);
cv::Mat Scm  = pSolver->iterate(5,bNoMore,vbInliers,nInliers);
matcher.SearchBySim3(mpCurrentKF,pKF,vpMapPointMatches,s,R,t,7.5);
const int nInliers = Optimizer::OptimizeSim3(mpCurrentKF, pKF, vpMapPointMatches, gScm, 10, mbFixScale);
matcher.SearchByProjection(mpCurrentKF, mScw, mvpLoopMapPoints, mvpCurrentMatchedPoints,10);
1
2
3
4
5

总的来说，东西还是很多的，不过没有关系。后续会对每一个函数进行详细的讲解(原理性的东西后面再进行详细的讲解，先来看看总体流程)。先看下图：

 
$核心$ 首先当前关键帧(红色点)与所有的候选关键帧进行匹配，确定一个合格的候选关键帧(绿色点)，该候选关键帧结合他的共视关键帧(黄色点)共同组成一个闭环帧小组(绿色虚线圈)，然后获得闭环帧小组的所有地图点，把这些地图点通过 sim3 映射到当前关键帧，然后进行搜寻匹配。

 

二、准备工作

ComputeSim3()函数在正式处理之前，首先做了一些准备工作，先获得上一篇博客 DetectLoop() 函数得到的闭环候选关键帧 mvpEnoughConsistentCandidates 的数量，后续用于循环。创建 ORB 匹配器， ORBmatcher matcher(0.75,true)，这里的0.75 是最优评分和次优评分比例的阈值，true 表示对特征点的方向进行检查，也就是进行角度差筛选。创建变量 vpSim3Solvers，用于存储每一个候选帧的 Sim3Solver 求解器。以及其他的一些变量，如 vvpMapPointMatches→存储每个候选帧的匹配地图点信息等，
 

三、BoW匹配

对候选关键帧进行遍历，首先执行 pKF->SetNotErase(); 避免在LocalMapping中KeyFrameCulling函数将此关键帧作为冗余帧剔除。然后判断一下该候选关键帧是否为坏帧，如果候选帧质量不高，直接PASS。然后通过BoW加速匹配，得到 mpCurrentKF 与 pKF 之间的匹配特征点。但是注意，这里是一个初匹配，比如说词袋中没有特征点，就很可能匹配不上。vvpMapPointMatches 是匹配特征点对应的地图点,本质上来自于候选闭环帧。如果匹配的特征点数太少，该候选帧剔除。否则为该候选帧创建一个
Sim3Solver求解器。主要核心代码如下：

	int nmatches = matcher.SearchByBoW(mpCurrentKF,pKF,vvpMapPointMatches[i]);
	Sim3Solver* pSolver = new Sim3Solver(mpCurrentKF,pKF,vvpMapPointMatches[i],mbFixScale);
	pSolver->SetRansacParameters(0.99,20,300);
1
2
3

Sim3 也是通过迭代进行求解的，SetRansacParameters 参数20表示至少20个内点，才停止迭代。最多迭代300次，0.99 是置信度。所有的代码后续都会进行详细讲解，大家不要着急。
 

四、Sim3 计算匹配

$(1)：$对所有通过上一步(粗匹配)的候选关键帧进行遍历，每一个候选帧用Sim3Solver 迭代匹配，直到有一个候选帧匹配成功，或者全部失败。
 
$(2)：$利用初匹配的特征点对，通过迭代的方式进行 Sim3 变换的求解，核心函数如下：

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

 
$(3)：$ 利用计算出来的Sim3，通过Sim3变换，投影搜索pKF1的特征点在pKF2中的匹配，同理，投影搜索pKF2的特征点在pKF1中的匹配。只有互相都成功匹配的才认为是可靠的匹配。核心代码如下：

	matcher.SearchBySim3(mpCurrentKF,pKF,vpMapPointMatches,s,R,t,7.5);
1

 

五、图优化

优化mpCurrentKF与pKF对应的MapPoints间的Sim3，得到优化后的量gScm，核心代码如下：

	// 如果mbFixScale为true，则是6 自由度优化（双目 RGBD），如果是false，则是7 自由度优化（单目）
	// 优化mpCurrentKF与pKF对应的MapPoints间的Sim3，得到优化后的量gScm
	const int nInliers = Optimizer::OptimizeSim3(mpCurrentKF, pKF, vpMapPointMatches, gScm, 10, mbFixScale);
1
2
3

这里又遇到了熟悉的东西，那就是 Optimizer。与前面的 Optimizer::PoseOptimization()与Optimizer::LocalBundleAdjustment() 函数一样，后面会有专门的篇章进行详细讲解。这里我们先跳过细节讲解，总的来说，大家暂时只要直到是对 Sim3 变换矩阵进行优化就可以了。
 

六、闭环关键帧小组

当然，如果最终没有一个闭环匹配候选帧通过Sim3的求解与优化，则结束 while(nCandidates>0 && !bMatch) 循环，清空mvpEnoughConsistentCandidates，这些候选关键帧以后都不会在再参加回环检测过程了，前关键帧也将不会再参加回环检测了。sim3 计算失败，退出了。

当然找到一个合格的候选关键帧(内点数大于20个)，即其对应的 sim3 变换矩阵(经过优化的)合格。也可跳出 while(nCandidates>0 && !bMatch) 循环。此时会为该候选关键帧 mpMatchedKF 创建一个闭环关键帧小组。这个小组的成员为该候选关键帧的共视关键帧，以及其本身。

 

七、Sim3重映射匹配

对闭环关键帧小组进行遍历，获得所有的地图点，存储于 mvpLoopMapPoints 变量之中。然后把这些地图点全部映射到当前关键帧，进行匹配。核心函数为：

    // Find more matches projecting with the computed Sim3
    // Step 4：将闭环关键帧及其连接关键帧的所有地图点投影到当前关键帧进行投影匹配
    // 根据投影查找更多的匹配（成功的闭环匹配需要满足足够多的匹配特征点数）
    // 根据Sim3变换，将每个mvpLoopMapPoints投影到mpCurrentKF上，搜索新的匹配对
    // mvpCurrentMatchedPoints是前面经过SearchBySim3得到的已经匹配的点对，这里就忽略不再匹配了
    // 搜索范围系数为10
    matcher.SearchByProjection(mpCurrentKF, mScw, mvpLoopMapPoints, mvpCurrentMatchedPoints,10);
1
2
3
4
5
6
7

统计当前帧与闭环关键帧的匹配地图点数目，超过40个说明成功闭环，否则失败。如果当前回环可靠,保留当前待闭环关键帧，其他闭环候选全部删掉以后不用了。也就是说，所有的闭环候选关键帧中，到最后，只留下了一个。

 

八、代码注释

/**
 * @brief 计算当前关键帧和上一步闭环候选帧的Sim3变换
 * 1. 遍历闭环候选帧集，筛选出与当前帧的匹配特征点数大于20的候选帧集合，并为每一个候选帧构造一个Sim3Solver
 * 2. 对每一个候选帧进行 Sim3Solver 迭代匹配，直到有一个候选帧匹配成功，或者全部失败
 * 3. 取出闭环匹配上关键帧的相连关键帧，得到它们的地图点放入 mvpLoopMapPoints
 * 4. 将闭环匹配上关键帧以及相连关键帧的地图点投影到当前关键帧进行投影匹配
 * 5. 判断当前帧与检测出的所有闭环关键帧是否有足够多的地图点匹配
 * 6. 清空mvpEnoughConsistentCandidates
 * @return true         只要有一个候选关键帧通过Sim3的求解与优化，就返回true
 * @return false        所有候选关键帧与当前关键帧都没有有效Sim3变换
 */
bool LoopClosing::ComputeSim3()
{
    // Sim3 计算流程说明：
    // 1. 通过Bow加速描述子的匹配，利用RANSAC粗略地计算出当前帧与闭环帧的Sim3（当前帧---闭环帧）          
    // 2. 根据估计的Sim3，对3D点进行投影找到更多匹配，通过优化的方法计算更精确的Sim3（当前帧---闭环帧）   
    // 3. 将闭环帧以及闭环帧相连的关键帧的地图点与当前帧的点进行匹配（当前帧---闭环帧+相连关键帧）     
    // 注意以上匹配的结果均都存在成员变量mvpCurrentMatchedPoints中，实际的更新步骤见CorrectLoop()步骤3
    // 对于双目或者是RGBD输入的情况,计算得到的尺度=1


    //  准备工作
    // For each consistent loop candidate we try to compute a Sim3
    // 对每个（上一步得到的具有足够连续关系的）闭环候选帧都准备算一个Sim3
    const int nInitialCandidates = mvpEnoughConsistentCandidates.size();

    // We compute first ORB matches for each candidate
    // If enough matches are found, we setup a Sim3Solver
    ORBmatcher matcher(0.75,true);

    // 存储每一个候选帧的Sim3Solver求解器
    vector<Sim3Solver*> vpSim3Solvers;
    vpSim3Solvers.resize(nInitialCandidates);

    // 存储每个候选帧的匹配地图点信息
    vector<vector<MapPoint*> > vvpMapPointMatches;
    vvpMapPointMatches.resize(nInitialCandidates);

    // 存储每个候选帧应该被放弃(True）或者 保留(False)
    vector<bool> vbDiscarded;
    vbDiscarded.resize(nInitialCandidates);

    // 完成 Step 1 的匹配后，被保留的候选帧数量
    int nCandidates=0;

    // Step 1. 遍历闭环候选帧集，初步筛选出与当前关键帧的匹配特征点数大于20的候选帧集合，并为每一个候选帧构造一个Sim3Solver
    for(int i=0; i<nInitialCandidates; i++)
    {
        // Step 1.1 从筛选的闭环候选帧中取出一帧有效关键帧pKF
        KeyFrame* pKF = mvpEnoughConsistentCandidates[i];

        // 避免在LocalMapping中KeyFrameCulling函数将此关键帧作为冗余帧剔除
        pKF->SetNotErase();

        // 如果候选帧质量不高，直接PASS
        if(pKF->isBad())
        {
            vbDiscarded[i] = true;
            continue;
        }

        // Step 1.2 将当前帧 mpCurrentKF 与闭环候选关键帧pKF匹配
        // 通过bow加速得到 mpCurrentKF 与 pKF 之间的匹配特征点
        // vvpMapPointMatches 是匹配特征点对应的地图点,本质上来自于候选闭环帧
        int nmatches = matcher.SearchByBoW(mpCurrentKF,pKF,vvpMapPointMatches[i]);

        // 粗筛：匹配的特征点数太少，该候选帧剔除
        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 Ransac 过程置信度0.99，至少20个inliers 最多300次迭代
            pSolver->SetRansacParameters(0.99,20,300);
            vpSim3Solvers[i] = pSolver;
        }

        // 保留的候选帧数量
        nCandidates++;
    }

    // 用于标记是否有一个候选帧通过Sim3Solver的求解与优化
    bool bMatch = false;

    // Step 2 对每一个候选帧用Sim3Solver 迭代匹配，直到有一个候选帧匹配成功，或者全部失败
    while(nCandidates>0 && !bMatch)
    {
        // 遍历每一个候选帧
        for(int i=0; i<nInitialCandidates; i++)
        {
            if(vbDiscarded[i])
                continue;

            KeyFrame* pKF = mvpEnoughConsistentCandidates[i];

            // 内点（Inliers）标志
            // 即标记经过RANSAC sim3 求解后,vvpMapPointMatches中的哪些作为内点
            vector<bool> vbInliers; 
        
            // 内点（Inliers）数量
            int nInliers;

            // 是否到达了最优解
            bool bNoMore;

            // Step 2.1 取出从 Step 1.3 中为当前候选帧构建的 Sim3Solver 并开始迭代
            Sim3Solver* pSolver = vpSim3Solvers[i];

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

            // If Ransac reachs max. iterations discard keyframe
            // 总迭代次数达到最大限制还没有求出合格的Sim3变换，该候选帧剔除
            if(bNoMore)
            {
                vbDiscarded[i]=true;
                nCandidates--;
            }

            // If RANSAC returns a Sim3, perform a guided matching and optimize with all correspondences
            // 如果计算出了Sim3变换，继续匹配出更多点并优化。因为之前 SearchByBoW 匹配可能会有遗漏
            if(!Scm.empty())
            {
                // 取出经过Sim3Solver 后匹配点中的内点集合
                vector<MapPoint*> vpMapPointMatches(vvpMapPointMatches[i].size(), static_cast<MapPoint*>(NULL));
                for(size_t j=0, jend=vbInliers.size(); j<jend; j++)
                {
                    // 保存内点
                    if(vbInliers[j])
                       vpMapPointMatches[j]=vvpMapPointMatches[i][j];
                }

                // Step 2.2 通过上面求取的Sim3变换引导关键帧匹配，弥补Step 1中的漏匹配
                // 候选帧pKF到当前帧mpCurrentKF的R（R12），t（t12），变换尺度s（s12）
                cv::Mat R = pSolver->GetEstimatedRotation();
                cv::Mat t = pSolver->GetEstimatedTranslation();
                const float s = pSolver->GetEstimatedScale();

                // 查找更多的匹配（成功的闭环匹配需要满足足够多的匹配特征点数，之前使用SearchByBoW进行特征点匹配时会有漏匹配）
                // 通过Sim3变换，投影搜索pKF1的特征点在pKF2中的匹配，同理，投影搜索pKF2的特征点在pKF1中的匹配
                // 只有互相都成功匹配的才认为是可靠的匹配
                matcher.SearchBySim3(mpCurrentKF,pKF,vpMapPointMatches,s,R,t,7.5);

                // Step 2.3 用新的匹配来优化 Sim3，只要有一个候选帧通过Sim3的求解与优化，就跳出停止对其它候选帧的判断
                // OpenCV的Mat矩阵转成Eigen的Matrix类型
                // gScm：候选关键帧到当前帧的Sim3变换
                g2o::Sim3 gScm(Converter::toMatrix3d(R),Converter::toVector3d(t),s);
            
                // 如果mbFixScale为true，则是6 自由度优化（双目 RGBD），如果是false，则是7 自由度优化（单目）
                // 优化mpCurrentKF与pKF对应的MapPoints间的Sim3，得到优化后的量gScm
                const int nInliers = Optimizer::OptimizeSim3(mpCurrentKF, pKF, vpMapPointMatches, gScm, 10, mbFixScale);

                // 如果优化成功，则停止while循环遍历闭环候选
                if(nInliers>=20)
                {
                    // 为True时将不再进入 while循环
                    bMatch = true;
                    // mpMatchedKF就是最终闭环检测出来与当前帧形成闭环的关键帧
                    mpMatchedKF = pKF;

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

                    // 得到g2o优化后从世界坐标系到当前帧的Sim3变换
                    mg2oScw = gScm*gSmw;
                    mScw = Converter::toCvMat(mg2oScw);
                    mvpCurrentMatchedPoints = vpMapPointMatches;

                    // 只要有一个候选帧通过Sim3的求解与优化，就跳出停止对其它候选帧的判断
                    break;
                }
            }
        }
    }

    // 退出上面while循环的原因有两种,一种是求解到了bMatch置位后出的,另外一种是nCandidates耗尽为0
    if(!bMatch)
    {
        // 如果没有一个闭环匹配候选帧通过Sim3的求解与优化
        // 清空mvpEnoughConsistentCandidates，这些候选关键帧以后都不会在再参加回环检测过程了
        for(int i=0; i<nInitialCandidates; i++)
            mvpEnoughConsistentCandidates[i]->SetErase();
        // 当前关键帧也将不会再参加回环检测了
        mpCurrentKF->SetErase();
        // Sim3 计算失败，退出了
        return false;
    }

    // Step 3：取出与当前帧闭环匹配上的关键帧及其共视关键帧，以及这些共视关键帧的地图点
    // 注意是闭环检测出来与当前帧形成闭环的关键帧 mpMatchedKF
    // 将mpMatchedKF共视的关键帧全部取出来放入 vpLoopConnectedKFs
    // 将vpLoopConnectedKFs的地图点取出来放入mvpLoopMapPoints
    vector<KeyFrame*> vpLoopConnectedKFs = mpMatchedKF->GetVectorCovisibleKeyFrames();

    // 包含闭环匹配关键帧本身,形成一个“闭环关键帧小组“
    vpLoopConnectedKFs.push_back(mpMatchedKF);
    mvpLoopMapPoints.clear();

    // 遍历这个组中的每一个关键帧
    for(vector<KeyFrame*>::iterator vit=vpLoopConnectedKFs.begin(); vit!=vpLoopConnectedKFs.end(); vit++)
    {
        KeyFrame* pKF = *vit;
        vector<MapPoint*> vpMapPoints = pKF->GetMapPointMatches();

        // 遍历其中一个关键帧的所有有效地图点
        for(size_t i=0, iend=vpMapPoints.size(); i<iend; i++)
        {
            MapPoint* pMP = vpMapPoints[i];
            if(pMP)
            {
                // mnLoopPointForKF 用于标记，避免重复添加
                if(!pMP->isBad() && pMP->mnLoopPointForKF!=mpCurrentKF->mnId)
                {
                    mvpLoopMapPoints.push_back(pMP);
                    // 标记一下
                    pMP->mnLoopPointForKF=mpCurrentKF->mnId;
                }
            }
        }
    }

    // Find more matches projecting with the computed Sim3
    // Step 4：将闭环关键帧及其连接关键帧的所有地图点投影到当前关键帧进行投影匹配
    // 根据投影查找更多的匹配（成功的闭环匹配需要满足足够多的匹配特征点数）
    // 根据Sim3变换，将每个mvpLoopMapPoints投影到mpCurrentKF上，搜索新的匹配对
    // mvpCurrentMatchedPoints是前面经过SearchBySim3得到的已经匹配的点对，这里就忽略不再匹配了
    // 搜索范围系数为10
    matcher.SearchByProjection(mpCurrentKF, mScw, mvpLoopMapPoints, mvpCurrentMatchedPoints,10);

    // If enough matches accept Loop
    // Step 5: 统计当前帧与闭环关键帧的匹配地图点数目，超过40个说明成功闭环，否则失败
    int nTotalMatches = 0;
    for(size_t i=0; i<mvpCurrentMatchedPoints.size(); i++)
    {
        if(mvpCurrentMatchedPoints[i])
            nTotalMatches++;
    }

    if(nTotalMatches>=40)
    {
        // 如果当前回环可靠,保留当前待闭环关键帧，其他闭环候选全部删掉以后不用了
        for(int i=0; i<nInitialCandidates; i++)
            if(mvpEnoughConsistentCandidates[i]!=mpMatchedKF)
                mvpEnoughConsistentCandidates[i]->SetErase();
        return true;
    }
    else   
    {
        // 闭环不可靠，闭环候选及当前待闭环帧全部删除
        for(int i=0; i<nInitialCandidates; i++)
            mvpEnoughConsistentCandidates[i]->SetErase();
        mpCurrentKF->SetErase();
        return false;
    }
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262

 
 
本文内容来自计算机视觉life ORB-SLAM2 课程课件