ORB-SLAM2源码解读（4）：LocalClosing

 

VO总是会有累计误差，而LoopClosing通过检测是否曾经来过此处，进行后端优化，可以将这个累计误差缩小到一个可接受的范围内。闭环是一个比BA更加强烈、更加准确的约束，从而使得Slam系统应对大范围场景时，拥有更高的鲁棒性和可用性。

整个LoopClosing模块是在线程中完成，并在创建线程时调用LoopClosing::Run()函数让其运行。

mptLoopClosing = new thread(&ORB_SLAM2::LoopClosing::Run,mpLoopCloser)

 

 

---

一、DetectLoop（）

闭环条件检测

主要流程：

1、检测当前关键帧在 Covisibility 图中的附近关键帧，并会依次计算当前关键帧和每一个附近关键帧的BoW分值，通过我们所得到分数的最低分，到数据库中查询，查找出所有大于该最低分的关键帧作为候选帧。

2、在候选帧中检测具有连续性的候选帧。

0、从队列中取出一个关键帧，原队列少一个mpCurrentKF

    {
        // 从队列中取出一个关键帧mpCurrentKF，原队列少一个
        unique_lock<mutex> lock(mMutexLoopQueue);
        mpCurrentKF = mlpLoopKeyFrameQueue.front();
        mlpLoopKeyFrameQueue.pop_front();
        mpCurrentKF->SetNotErase();//避免处理该关键帧时被擦除
    }

1、如果地图中的关键帧数小于10或者距离上次闭环少于10帧，那么不进行闭环检测

    if(mpCurrentKF->mnId<mLastLoopKFid+10)
    {
        mpKeyFrameDB->add(mpCurrentKF);//关键帧数据库里加入当前关键帧
        mpCurrentKF->SetErase();
        return false;//输出错误，不进行loop detection
    }

2、遍历所有共视关键帧，计算当前关键帧与每个共视关键的bow相似度得分，并得到最低得分minScore

    const vector<KeyFrame*> vpConnectedKeyFrames = mpCurrentKF->GetVectorCovisibleKeyFrames();//Convisible图中与当前帧相连的关键帧
    const DBoW2::BowVector &CurrentBowVec = mpCurrentKF->mBowVec;//当前帧的bow向量 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;//相连关键帧的bow向量 pKF->mBowVec

        float score = mpORBVocabulary->score(CurrentBowVec, BowVec);//核心函数：计算当前帧bow向量与相连关键帧bow向量的相似度score

        if(score<minScore)
            minScore = score;
    }

3、在所有关键帧中找出闭环备选帧vpCandidateKFs（大于minscore）

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

    重点函数DetectLoopCandidates（）

 

4、对候选关键帧集进行连续性检测（并且如果一旦有一个闭环候选关键帧被检测到3次，系统就认为检测到闭环）

    （1）每个候选帧将与自己相连的关键帧构成一个“子候选组spCandidateGroup”，vpCandidateKFs-->spCandidateGroup

        KeyFrame* pCandidateKF = vpCandidateKFs[i];//候选关键帧

        set<KeyFrame*> spCandidateGroup = pCandidateKF->GetConnectedKeyFrames();
        spCandidateGroup.insert(pCandidateKF);

   （2）检测“子候选组”中每一个关键帧是否存在于“连续组”，如果存在nCurrentConsistency++，则将该“子候选组”放入“当前连续组vCurrentConsistentGroups”

        // 遍历之前的“子连续组”
        for(size_t iG=0, iendG=mvConsistentGroups.size(); iG<iendG; iG++)
        {
            // 取出一个之前的子连续组
            set<KeyFrame*> sPreviousGroup = mvConsistentGroups[iG].first;

            // 遍