ORBSLAM2总结之Loop_Closing

下列函数代码都在Loop_Closing.cpp中

参考博客

一、回环检测流程

代码对应函数为：void LoopClosing::Run()

// 回环线程主函数
//从tracking处理的关键帧，在经过Local Mapping处理后的关键帧放入回环处理
//进入主循环
    //1、检测是否有闭环
    //2、利用sim3计算出，闭环的位姿变换和尺度变换(尺度也会存在漂移)
    //3、纠正闭环
    // 查看外部线程是否有终止当前线程的请求,如果有的话就跳出这个线程的主函数的主循环
//跳出主循环 ----说明有外部线程请求终止当前线程执行终止当前线程的一些操作
void LoopClosing::Run()
{
    mbFinished =false;//判断当前线程是否已经结束工作

    // 线程主循环
    //线程主循环比较简单 
    //从tracking处理的关键帧，在经过Local Mapping处理后的关键帧放入回环处理
    //1、检测是否有闭环
    //2、利用sim3计算出，闭环的位姿变换和尺度变换(尺度也会存在漂移)
    //3、纠正闭环
    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();
}

二、闭环帧的检测

代码中对应函数为：DetectLoop()
首先通过函数CheckNewKeyFrames()检测mlpLoopKeyFrameQueue里的关键帧是否为空，如果不空则进行闭环检测的处理：也就是DetectLoop()。（Loopclosing中的关键帧是LocalMapping发送过来的，LocalMapping是Tracking中发过来的，在LocalMapping中通过InsertKeyFrame将关键帧插入闭环检测队列）
闭环帧的检测流程如下：
1、将相似性比较高的一些帧作为候选帧。
2、得到与当前帧共同单词数量最高为N，删除那些与当前帧共同单词数量<0.8N的那些帧。
3、处理剩下的候选帧，找出每一个候选帧共视最好的10帧，将10（与候选帧共视前10的帧）+1（候选帧）组成1组，计算该组与当前帧相似性分数之和，记录相似性分数之和最高的那个候选关键帧。最高的相似性分数之和，记为M，0.75M为阈值。
4、将相似性分数之和大于0.75*M的组里与当前帧匹配最高的关键帧加入最终的候选帧，至此一个关键帧的所有候选闭环帧寻找完毕。
5、最后如何从最终的候选帧找到闭环帧，如下图所示。
从k0 - k3为从队列里依次取出来的4个关键帧。同一列的绿点属于该关键帧的候选关键帧，与绿点相连接的蓝点为该绿点代表关键帧的共视关键帧，所以按照图片所示四个蓝点和一个绿点代表了一个子候选关键帧组，所以没一列的三个星状图标表示该列最下方关键帧的候选关键帧组。列与列之间的虚线表示连线的两个帧是同一个帧，这也表示了这两个子候选关键帧组有联系。红色椭圆是将会传递到下一次检测的连续组。

接下来开始讲下筛选过程。整体利用的思路就是，如果真的在某个地方发生了闭环，那么在该闭环帧的后面几帧里，每前后两帧的候选关键帧组都会有或多或少的联系。假设k0为取出的当前关键帧，该列上的三个绿点为上一步筛选过后剩下的候选闭环关键帧，然后找到每个关键帧的前几个共视关键帧（代码中取了10个，图里中只画了四个）。如果当前的变量mvConsistentGroups，也就是上一次的连续组为空，那么就把现在的包含四个关键帧组的连续组赋值给mvConsistentGroups，并且标记该组的三个成员连续的计数为0。进入下一次循环，这次取出关键帧k1作为当前关键帧，同样构建k1的候选关键帧的候选关键帧组，然后在上一次的候选关键帧连续组中找是否和当前的候选关键帧连续组有联系，这里的联系在代码里指的就是至少包含一个相同的关键帧，如果有，将当前某个组元素和之前的某个组元素相连接，并且连续计数加1。然后继续从队列里取下一个关键帧，按照上面的办法寻找连续，并更新计数。直到当前取出当前关键帧为k3，此时k3的红点所在的候选组的连续计数已经为3，那么该组所代表的候选关键帧，也就是红点所代表的候选关键帧将入选，成为真正的闭环关键帧。

/**
 * @brief 闭环检测---Loop_Closing中主函数中的检测闭环函数
 *        Step 1 从队列中取出一个关键帧,作为当前检测闭环关键帧
 *        Step 2：如果距离上次闭环没多久（小于10帧），或者map中关键帧总共还没有10帧，则不进行闭环检测
 *        Step 3：遍历当前回环关键帧所有连接（>15个共视地图点）关键帧，计算当前关键帧与每个共视关键的bow相似度得分，并得到最低得分minScore
 *        Step 4：在所有关键帧中找出闭环候选帧（注意不和当前帧连接)
 *                          这个第四步就是找到闭环候选帧
 *        Step 5：在候选帧中检测具有连续性的候选帧
 *                 for--Step 5.1：遍历刚才得到的每一个候选关键帧
 *                         Step 5.2：将自己以及与自己相连的关键帧构成一个“子候选组”
 *                          Step 5.3：遍历前一次闭环检测到的“子连续组”---进入for循环
                                                上一次闭环的连续组 std::vector<ConsistentGroup> mvConsistentGroups
                                                其中ConsistentGroup的定义：typedef pair<set<KeyFrame*>,int> ConsistentGroup
                                                其中 ConsistentGroup.first对应每个“连续组”中的关键帧，ConsistentGroup.second为每个“连续组”的已连续次数
                             Step 5.4：遍历每个“子候选组”，检测子候选组中每一个关键帧在“子连续组”中是否存在
                                                  如果有一帧共同存在于“子候选组”与之前的“子连续组”，那么“子候选组”与该“子连续组”连续
                             Step 5.5：如果判定为连续，接下来判断是否达到连续的条件
                                                  将当前候选组连续程度+1，并将其加入当前连续组
                                                  如果已经连续得足够多了,那么当前的这个候选关键帧是足够靠谱的
                                                  连续性阈值 mnCovisibilityConsistencyTh=3
                                                  足够连续的标记 bEnoughConsistent
                                                  这里可以break掉结束当前for循环吗？
                                                  回答：不行。因为虽然pCandidateKF达到了连续性要求
                                                  但spCandidateGroup 还可以和mvConsistentGroups 中其他的子连续组进行连接
                                                  我们使用循环结束后的连续组，换而言之连续组mvConsistentGroups中只保存连续组链中末尾的组---结束for循环
                            Step 5.6：如果该“子候选组”的所有关键帧都和上次闭环无关（不连续），vCurrentConsistentGroups 没有新添加连续关系
                                                 于是就把“子候选组”全部拷贝到 vCurrentConsistentGroups， 用于更新mvConsistentGroups，连续性计数器设为0
                     for--       
                     当前闭环检测的关键帧添加到关键帧数据库中
                     判断是否存在闭环----mvpEnoughConsistentCandidates.empty()                                                                        
 * @return true             成功检测到闭环
 * @return false            未检测到闭环
 */
bool LoopClosing::DetectLoop()
{
    // Step 1 从队列中取出一个关键帧,作为当前检测闭环关键帧
    {
        
        unique_lock<mutex> lock(mMutexLoopQueue);
        // 从队列头开始取，也就是先取早进来的关键帧
        mpCurrentKF = mlpLoopKeyFrameQueue.front();
        // 取出关键帧后从队列里弹出该关键帧
        mlpLoopKeyFrameQueue.pop_front();
        // Avoid that a keyframe can be erased while it is being process by this thread
        // 设置当前关键帧不要在优化的过程中被删除
        mpCurrentKF->SetNotErase();
    }

    //If the map contains less than 10 KF or less than 10 KF have passed from last loop detection
    // Step 2：如果距离上次闭环没多久（小于10帧），或者map中关键帧总共还没有10帧，则不进行闭环检测
    // 后者的体现是当mLastLoopKFid为0的时候
    if(mpCurrentKF->mnId<mLastLoopKFid+10)
    {
        mpKeyFrameDB->add(mpCurrentKF);
        mpCurrentKF->SetErase();
        return false;
    }

    // Compute reference BoW similarity score
    // This is the lowest score to a connected keyframe in the covisibility graph
    // We will impose loop candidates to have a higher similarity than this
    // Step 3：遍历当前回环关键帧所有连接（>15个共视地图点）关键帧，计算当前关键帧与每个共视关键的bow相似度得分，并得到最低得分minScore
    const vector<KeyFrame*> vpConnectedKeyFrames = mpCurrentKF->GetVectorCovisibleKeyFrames();
    const DBoW2::BowVector &CurrentBowVec = mpCurrentKF->mBowVec;
    float minScore = 1;
    for(size_t i=0; i<vpConnectedKeyFrames.size(); i++)
    {
        KeyFrame* pKF = vpConnectedKeyFrames[i];
        if(pKF->isBad())
            continue;
        const DBoW2::BowVector &BowVec = pKF->mBowVec;
        // 计算两个关键帧的相似度得分；得分越低,相似度越低
        float score = mpORBVocabulary->score(CurrentBowVec, BowVec);
        // 更新最低得分
        if(score<minScore)
            minScore = score;
    }

    // Query the database imposing the minimum score
    // Step 4：在所有关键帧中找出闭环候选帧（注意不和当前帧连接）
    // minScore的作用：认为和当前关键帧具有回环关系的关键帧,不应该低于当前关键帧的相邻关键帧的最低的相似度minScore
    // 得到的这些关键帧,和当前关键帧具有较多的共视单词,并且相似度评分都挺高
    vector<KeyFrame*> vpCandidateKFs = mpKeyFrameDB->DetectLoopCandidates(mpCurrentKF, minScore);

    // If there are no loop candidates, just add new keyframe and return false
    // 如果没有闭环候选帧，返回false
    if(vpCandidateKFs.empty())
    {
        mpKeyFrameDB->add(mpCurrentKF);
        mvConsistentGroups.clear();
        mpCurrentKF->SetErase();
        return false;           
    }

    // For each loop candidate check consistency with previous loop candidates
    // Each candidate expands a covisibility group (keyframes connected to the loop candidate in the covisibility graph)
    // A group is consistent with a previous group if they share at least a keyframe
    // We must detect a consistent loop in several consecutive keyframes to accept it
    // Step 5：在候选帧中检测具有连续性的候选帧
    // 1、每个候选帧将与自己相连的关键帧构成一个“子候选组spCandidateGroup”， vpCandidateKFs-->spCandidateGroup
    // 2、检测“子候选组”中每一个关键帧是否存在于“连续组”，如果存在 nCurrentConsistency++，则将该“子候选组”放入“当前连续组vCurrentConsistentGroups”
    // 3、如果nCurrentConsistency大于等于3，那么该”子候选组“代表的候选帧过关，进入mvpEnoughConsistentCandidates
    
    // 相关的概念说明:（为方便理解，见视频里的图示）
    // 组(group): 对于某个关键帧, 其和其具有共视关系的关键帧组成了一个"组";
    // 子候选组(CandidateGroup): 对于某个候选的回环关键帧, 其和其具有共视关系的关键帧组成的一个"组";
    // 连续(Consistent):  不同的组之间如果共同拥有一个及以上的关键帧,那么称这两个组之间具有连续关系
    // 连续性(Consistency):称之为连续长度可能更合适,表示累计的连续的链的长度:A--B 为1, A--B--C--D 为3等;具体反映在数据类型 ConsistentGroup.second上
    // 连续组(Consistent group): mvConsistentGroups存储了上次执行回环检测时, 新的被检测出来的具有连续性的多个组的集合.由于组之间的连续关系是个网状结构,因此可能存在
    //                          一个组因为和不同的连续组链都具有连续关系,而被添加两次的情况(当然连续性度量是不相同的)
    // 连续组链:自造的称呼,类似于菊花链A--B--C--D这样形成了一条连续组链.对于这个例子中,由于可能E,F都和D有连续关系,因此连续组链会产生分叉;为了简化计算,连续组中将只会保存
    //         最后形成连续关系的连续组们(见下面的连续组的更新)
    // 子连续组: 上面的连续组中的一个组
    // 连续组的初始值: 在遍历某个候选帧的过程中,如果该子候选组没有能够和任何一个上次的子连续组产生连续关系,那么就将添加自己组为连续组,并且连续性为0(相当于新开了一个连续链)
    // 连续组的更新: 当前次回环检测过程中,所有被检测到和之前的连续组链有连续的关系的组,都将在对应的连续组链后面+1,这些子候选组(可能有重复,见上)都将会成为新的连续组;
    //              换而言之连续组mvConsistentGroups中只保存连续组链中末尾的组

    // 最终筛选后得到的闭环帧
    mvpEnoughConsistentCandidates.clear();

    // ConsistentGroup数据类型为pair<set<KeyFrame*>,int>
    // ConsistentGroup.first对应每个“连续组”中的关键帧，ConsistentGroup.second为每个“连续组”的已连续几个的序号

    vector<ConsistentGroup> vCurrentConsistentGroups;

    // 这个下标是每个"子连续组"的下标,bool表示当前的候选组中是否有和该组相同的一个关键帧
    vector<bool> vbConsistentGroup(mvConsistentGroups.size(),false);

    // Step 5.1：遍历刚才得到的每一个候选关键帧
    for(size_t i=0, iend=vpCandidateKFs.size(); i<iend; i++)
    {
        KeyFrame* pCandidateKF = vpCandidateKFs[i];

        // Step 5.2：将自己以及与自己相连的关键帧构成一个“子候选组”
        set<KeyFrame*> spCandidateGroup = pCandidateKF->GetConnectedKeyFrames();
        // 把自己也加进去
        spCandidateGroup.insert(pCandidateKF);

        // pCandidateKF 连续性达标的标志
        bool bEnoughConsistent = false;
        bool bConsistentForSomeGroup = false;
        // Step 5.3：遍历前一次闭环检测到的“子连续组”
        // 上一次闭环的连续组 std::vector<ConsistentGroup> mvConsistentGroups
        // 其中ConsistentGroup的定义：typedef pair<set<KeyFrame*>,int> ConsistentGroup
        // 其中 ConsistentGroup.first对应每个“连续组”中的关键帧，ConsistentGroup.second为每个“连续组”的已连续次数
        for(size_t iG=0, iendG=mvConsistentGroups.size(); iG<iendG; iG++)
        {
            // 取出之前的一个子连续组中的关键帧集合
            set<KeyFrame*> sPreviousGroup = mvConsistentGroups[iG].first;

            // Step 5.4：遍历每个“子候选组”，检测子候选组中每一个关键帧在“子连续组”中是否存在
            // 如果有一帧共同存在于“子候选组”与之前的“子连续组”，那么“子候选组”与该“子连续组”连续
            bool bConsistent = false;
            for(set<KeyFrame*>::iterator sit=spCandidateGroup.begin(), send=spCandidateGroup.end(); sit!=send;sit++)
            {
                if(sPreviousGroup.count(*sit))
                {
                    // 如果存在，该“子候选组”与该“子连续组”相连
                    bConsistent=true;
                    // 该“子候选组”至少与一个”子连续组“相连，跳出循环
                    bConsistentForSomeGroup=true;
                    break;
                }
            }

            if(bConsistent)
            {
                // Step 5.5：如果判定为连续，接下来判断是否达到连续的条件
                // 取出和当前的候选组发生"连续"关系的子连续组的"已连续次数"
                int nPreviousConsistency = mvConsistentGroups[iG].second;
                // 将当前候选组连续程度 +1，
                int nCurrentConsistency = nPreviousConsistency + 1;
                // 如果上述连续关系还未记录到 vCurrentConsistentGroups，那么记录一下
                // 注意这里spCandidateGroup 可能放置在vbConsistentGroup中其他索引(iG)下
                if(!vbConsistentGroup[iG])
                {
                    // 将该“子候选组”的该关键帧打上连续编号加入到“当前连续组”
                    ConsistentGroup cg = make_pair(spCandidateGroup,nCurrentConsistency);
                    // 放入本次闭环检测的连续组vCurrentConsistentGroups里
                    vCurrentConsistentGroups.push_back(cg);
                    //this avoid to include the same group more than once
                    // 标记一下，防止重复添加到同一个索引iG
                    // 但是spCandidateGroup可能重复添加到不同的索引iG对应的vbConsistentGroup 中
                    vbConsistentGroup[iG]=true; 
                }
                // 如果已经连续得足够多了,那么当前的这个候选关键帧是足够靠谱的
                // 连续性阈值 mnCovisibilityConsistencyTh=3
                // 足够连续的标记 bEnoughConsistent
                if(nCurrentConsistency>=mnCovisibilityConsistencyTh && !bEnoughConsistent)
                {
                    // 记录为达到连续条件了
                    mvpEnoughConsistentCandidates.push_back(pCandidateKF);
                    //this avoid to insert the same candidate more than once
                    // 标记一下，防止重复添加
                    bEnoughConsistent=true; 

                    // ? 这里可以break掉结束当前for循环吗？
                    // 回答：不行。因为虽然pCandidateKF达到了连续性要求
                    // 但spCandidateGroup 还可以和mvConsistentGroups 中其他的子连续组进行连接
                }
            }
        }

        // If the group is not consistent with any previous group insert with consistency counter set to zero
        // Step 5.6：如果该“子候选组”的所有关键帧都和上次闭环无关（不连续），vCurrentConsistentGroups 没有新添加连续关系
        // 于是就把“子候选组”全部拷贝到 vCurrentConsistentGroups， 用于更新mvConsistentGroups，连续性计数器设为0
        if(!bConsistentForSomeGroup)
        {
            ConsistentGroup cg = make_pair(spCandidateGroup,0);
            vCurrentConsistentGroups.push_back(cg);
        }
    }// 遍历得到的初级的候选关键帧

    // Update Covisibility Consistent Groups
    // 更新连续组
    mvConsistentGroups = vCurrentConsistentGroups;


    // Add Current Keyframe to database
    // 当前闭环检测的关键帧添加到关键帧数据库中
    mpKeyFrameDB->add(mpCurrentKF);

    if(mvpEnoughConsistentCandidates.empty())
    {
        // 未检测到闭环，返回false
        mpCurrentKF->SetErase();
        return false;
    }
    else 
    {
        // 成功检测到闭环，返回true
        return true;
    }

    // 多余的代码,执行不到
    mpCurrentKF->SetErase();
    return false;
}

其中第四部=步的mpKeyFrameDB->DetectLoopCandidates(mpCurrentKF, minScore)函数

/**
 * @brief 在闭环检测中找到与该关键帧可能闭环的关键帧（注意不和当前帧连接）
 * Step 1：找出和当前帧具有公共单词的所有关键帧，不包括与当前帧连接（也就是共视）的关键帧
 * Step 2：只和具有共同单词较多的（最大数目的80%以上）关键帧进行相似度计算 
 * Step 3：计算上述候选帧对应的共视关键帧组的总得分，只取最高组得分75%以上的组
 * Step 4：得到上述组中分数最高的关键帧作为闭环候选关键帧
 * @param[in] pKF               需要闭环检测的关键帧
 * @param[in] minScore          候选闭环关键帧帧和当前关键帧的BoW相似度至少要大于minScore
 * @return vector<KeyFrame*>    闭环候选关键帧
 */
vector<KeyFrame*> KeyFrameDatabase::DetectLoopCandidates(KeyFrame* pKF, float minScore)
{
    // 取出与当前关键帧相连（>15个共视地图点）的所有关键帧，这些相连关键帧都是局部相连，在闭环检测的时候将被剔除
    // 相连关键帧定义见 KeyFrame::UpdateConnections()
    set<KeyFrame*> spConnectedKeyFrames = pKF->GetConnectedKeyFrames();

    // 用于保存可能与当前关键帧形成闭环的候选帧（只要有相同的word，且不属于局部相连（共视）帧）
    list<KeyFrame*> lKFsSharingWords;

    // Search all keyframes that share a word with current keyframes
    // Discard keyframes connected to the query keyframe
    // Step 1：找出和当前帧具有公共单词的所有关键帧，不包括与当前帧连接（也就是共视）的关键帧
    {
        unique_lock<mutex> lock(mMutex);

        // words是检测图像是否匹配的枢纽，遍历该pKF的每一个word
        // mBowVec 内部实际存储的是std::map<WordId, WordValue>
        // WordId 和 WordValue 表示Word在叶子中的id 和权重
        for(DBoW2::BowVector::const_iterator vit=pKF->mBowVec.begin(), vend=pKF->mBowVec.end(); vit != vend; vit++)
        {
            // 提取所有包含该word的KeyFrame
            list<KeyFrame*> &lKFs =   mvInvertedFile[vit->first];
            // 然后对这些关键帧展开遍历
            for(list<KeyFrame*>::iterator lit=lKFs.begin(), lend= lKFs.end(); lit!=lend; lit++)
            {
                KeyFrame* pKFi=*lit;
                
                if(pKFi->mnLoopQuery!=pKF->mnId)    
                {
                    // 还没有标记为pKF的闭环候选帧
                    pKFi->mnLoopWords=0;
                    // 和当前关键帧共视的话不作为闭环候选帧
                    if(!spConnectedKeyFrames.count(pKFi))
                    {
                        // 没有共视就标记作为闭环候选关键帧，放到lKFsSharingWords里
                        pKFi->mnLoopQuery=pKF->mnId;
                        lKFsSharingWords.push_back(pKFi);
                    }
                }
                pKFi->mnLoopWords++;// 记录pKFi与pKF具有相同word的个数
            }
        }
    }

    // 如果没有关键帧和这个关键帧具有相同的单词,那么就返回空
    if(lKFsSharingWords.empty())
        return vector<KeyFrame*>();

    list<pair<float,KeyFrame*> > lScoreAndMatch;

    // Only compare against those keyframes that share enough words
    // Step 2：统计上述所有闭环候选帧中与当前帧具有共同单词最多的单词数，用来决定相对阈值 
    int maxCommonWords=0;
    for(list<KeyFrame*>::iterator lit=lKFsSharingWords.begin(), lend= lKFsSharingWords.end(); lit!=lend; lit++)
    {
        if((*lit)->mnLoopWords>maxCommonWords)
            maxCommonWords=(*lit)->mnLoopWords;
    }

    // 确定最小公共单词数为最大公共单词数目的0.8倍
    int minCommonWords = maxCommonWords*0.8f;

    int nscores=0;

    // Compute similarity score. Retain the matches whose score is higher than minScore
    // Step 3：遍历上述所有闭环候选帧，挑选出共有单词数大于minCommonWords且单词匹配度大于minScore存入lScoreAndMatch
    for(list<KeyFrame*>::iterator lit=lKFsSharingWords.begin(), lend= lKFsSharingWords.end(); lit!=lend; lit++)
    {
        KeyFrame* pKFi = *lit;

        // pKF只和具有共同单词较多（大于minCommonWords）的关键帧进行比较
        if(pKFi->mnLoopWords>minCommonWords)
        {
            nscores++;// 这个变量后面没有用到

            // 用mBowVec来计算两者的相似度得分
            float si = mpVoc->score(pKF->mBowVec,pKFi->mBowVec);

            pKFi->mLoopScore = si;
            if(si>=minScore)
                lScoreAndMatch.push_back(make_pair(si,pKFi));
        }
    }

    // 如果没有超过指定相似度阈值的，那么也就直接跳过去
    if(lScoreAndMatch.empty())
        return vector<KeyFrame*>();


    list<pair<float,KeyFrame*> > lAccScoreAndMatch;
    float bestAccScore = minScore;

    // Lets now accumulate score by covisibility
    // 单单计算当前帧和某一关键帧的相似性是不够的，这里将与关键帧相连（权值最高，共视程度最高）的前十个关键帧归为一组，计算累计得分
    // Step 4：计算上述候选帧对应的共视关键帧组的总得分，得到最高组得分bestAccScore，并以此决定阈值minScoreToRetain
    for(list<pair<float,KeyFrame*> >::iterator it=lScoreAndMatch.begin(), itend=lScoreAndMatch.end(); it!=itend; it++)
    {
        KeyFrame* pKFi = it->second;
        vector<KeyFrame*> vpNeighs = pKFi->GetBestCovisibilityKeyFrames(10);

        float bestScore = it->first; // 该组最高分数
        float accScore = it->first;  // 该组累计得分
        KeyFrame* pBestKF = pKFi;    // 该组最高分数对应的关键帧
        // 遍历共视关键帧，累计得分 
        for(vector<KeyFrame*>::iterator vit=vpNeighs.begin(), vend=vpNeighs.end(); vit!=vend; vit++)
        {
            KeyFrame* pKF2 = *vit;
            // 只有pKF2也在闭环候选帧中，且公共单词数超过最小要求，才能贡献分数
            if(pKF2->mnLoopQuery==pKF->mnId && pKF2->mnLoopWords>minCommonWords)
            {
                accScore+=pKF2->mLoopScore;
                // 统计得到组里分数最高的关键帧
                if(pKF2->mLoopScore>bestScore)
                {
                    pBestKF=pKF2;
                    bestScore = pKF2->mLoopScore;
                }
            }
        }

        lAccScoreAndMatch.push_back(make_pair(accScore,pBestKF));
        // 记录所有组中组得分最高的组，用于确定相对阈值
        if(accScore>bestAccScore)
            bestAccScore=accScore;
    }

    // Return all those keyframes with a score higher than 0.75*bestScore
    // 所有组中最高得分的0.75倍，作为最低阈值
    float minScoreToRetain = 0.75f*bestAccScore;

    set<KeyFrame*> spAlreadyAddedKF;
    vector<KeyFrame*> vpLoopCandidates;
    vpLoopCandidates.reserve(lAccScoreAndMatch.size());

    // Step 5：只取组得分大于阈值的组，得到组中分数最高的关键帧作为闭环候选关键帧
    for(list<pair<float,KeyFrame*> >::iterator it=lAccScoreAndMatch.begin(), itend=lAccScoreAndMatch.end(); it!=itend; it++)
    {
        if(it->first>minScoreToRetain)
        {
            KeyFrame* pKFi = it->second;
            // spAlreadyAddedKF 是为了防止重复添加
            if(!spAlreadyAddedKF.count(pKFi))
            {
                vpLoopCandidates.push_back(pKFi);
                spAlreadyAddedKF.insert(pKFi);
            }
        }
    }

    return vpLoopCandidates;
}

三、sim3的计算