orb-slam系列 LoopClosing线程 DetectLoop(十)

DetectLoop

/*******************************************************************************
 *                 功能:检测是否产生了回环
 *                 检测回环的步骤:
 *                             1  检测上次回环发生是否离当前关键帧足够长时间    并且满足当前关键帧总数量大于10
 *                             2  找出当前关键帧的共视关键帧,并找出其中的最小得分
 *                             3  根据最小得分寻找回环候选帧   具体见含函数DetectLoopCandidates
 * 				4  在候选回环关键帧中寻找具有连续性的关键帧
 * 						这里将候选回环关键帧和他的共视关键帧组成一个候选组
 *                                             一个组和另一个组是连续的,是指他们至少存在一个共视关键帧
 *                                       如果两个组之间存在足够多的帧是共视关键帧,则证明两个组之间是完全连续组,则说明发生了回环
 *			候选关键帧需要进行连续性检验的原因: 
 * 				我们通过聚类相连候选帧,可以将一些得分很高但却相对独立的枕给去掉这些帧与其他帧相对没有关联，而我们知道事实上回环处会
 * 			有一个时间和空间上的连续性，因此对于正确的回环来讲，这些相似性评分较高的帧是错误关键帧。
 * 
 ******************************************************************************/
bool LoopClosing::DetectLoop()
{
  //提出待检测回环的关键帧
    {
        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
    //如果刚刚发生了回环   上次回环之后通过的关键帧帧数不超过10  则将该关键帧添加到关键帧集中  将该关键帧设为可擦除关键帧 
    // 或者map中关键帧总共还没有10帧，则不进行闭环检测
    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
    // 当前关键帧的共视关键帧
    const vector<KeyFrame*> vpConnectedKeyFrames = mpCurrentKF->GetVectorCovisibleKeyFrames();  //权重排序的共视关键帧
    // 得到当前关键帧的BOW向量
    const DBoW2::BowVector &CurrentBowVec = mpCurrentKF->mBowVec;
    // 最低得分
    float minScore = 1;
    // 循环每个共视关键帧  计算每个共视关键帧与当前待检测回环关键帧之间的BOW得分  并得到其中最小的得分
    for(size_t i=0; i<vpConnectedKeyFrames.size(); i++)
    {
        KeyFrame* pKF = vpConnectedKeyFrames[i];
        if(pKF->isBad())
            continue;
	// 共视关键帧的BOW向量
        const DBoW2::BowVector &BowVec = pKF->mBowVec;
	//得到共视关键帧和当前关键帧的BOW向量得分
        float score = mpORBVocabulary->score(CurrentBowVec, BowVec);

        if(score<minScore)    //得到最小的得分
            minScore = score;
    }

    // Query the database imposing the minimum score    在关键帧数据集中查找当前关键帧的回环候选关键帧  最小得分大于minScore  （潜在回环帧）
    vector<KeyFrame*> vpCandidateKFs = mpKeyFrameDB->DetectLoopCandidates(mpCurrentKF, minScore);    
    //最小得分的意义是  词袋节点相连

    // If there are no loop candidates, just add new keyframe and return 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
    // 步骤4：在候选帧中检测具有连续性的候选帧
    // 1、每个候选帧将与自己相连的关键帧构成一个“子候选组spCandidateGroup”，vpCandidateKFs-->spCandidateGroup
    // 2、检测“子候选组”中每一个关键帧是否存在于“连续组”，如果存在nCurrentConsistency++，则将该“子候选组”放入“当前连续组vCurrentConsistentGroups”
    // 3、如果nCurrentConsistency大于等于3，那么该”子候选组“代表的候选帧过关，进入mvpEnoughConsistentCandidates
    //筛选后得到的具有连续性的候选帧
    //  候选关键帧需要进行连续性检验的原因: 
    // 我们通过聚类相连候选帧,可以将一些得分很高但却相对独立的枕给去掉这些帧与其他帧相对没有关联，而我们知道事实上回环处会
    // 有一个时间和空间上的连续性，因此对于正确的回环来讲，这些相似性评分较高的帧是错误关键帧。
    mvpEnoughConsistentCandidates.clear();  
    // ConsistentGroup数据类型为pair<set<KeyFrame*>,int>
    // ConsistentGroup.first对应每个“连续组”中的关键帧，ConsistentGroup.second为当前该连续组与其他连续组之间连续的连续组数量
    // 当前的连续组   连续的候选回环帧组
    vector<ConsistentGroup> vCurrentConsistentGroups;
    vector<bool> vbConsistentGroup(mvConsistentGroups.size(),false);

    //每一个候选回环帧
    for(size_t i=0, iend=vpCandidateKFs.size(); i<iend; i++)
    {
        KeyFrame* pCandidateKF = vpCandidateKFs[i];
	// 候选关键帧的共视关键帧以及候选关键帧构成了"子候选组"
        set<KeyFrame*> spCandidateGroup = pCandidateKF->GetConnectedKeyFrames();
        spCandidateGroup.insert(pCandidateKF);

        bool bEnoughConsistent = false;
        bool bConsistentForSomeGroup = false;
	//遍历之前的"子连续组"
        for(size_t iG=0, iendG=mvConsistentGroups.size(); iG<iendG; iG++)
        {
	  //之前的子连续组
            set<KeyFrame*> sPreviousGroup = mvConsistentGroups[iG].first;

            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)  // 如果与之前的连续组是连续的  则将它加入到当前连续组中
            {
                int nPreviousConsistency = mvConsistentGroups[iG].second;
                int nCurrentConsistency = nPreviousConsistency + 1;
                if(!vbConsistentGroup[iG])   // 如果当前连续组没有在当前连续组集中,则将其加入
                {
                    ConsistentGroup cg = make_pair(spCandidateGroup,nCurrentConsistency);
                    vCurrentConsistentGroups.push_back(cg);    //当前连续组
                    vbConsistentGroup[iG]=true; //this avoid to include the same group more than once 
                }
                // 如果与当前连续组连续的其他连续组之间连续数量大于某一阈值,则说明他有足够多的连续组  将其加入足够连续组集   mnCovisibilityConsistencyTh=3
                if(nCurrentConsistency>=mnCovisibilityConsistencyTh && !bEnoughConsistent)  
                {
                    mvpEnoughConsistentCandidates.push_back(pCandidateKF);
                    bEnoughConsistent=true; //this avoid to insert the same candidate more than once
                }
            }
        }

        // If the group is not consistent with any previous group insert with consistency counter set to zero
        //创建组
        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
    {
        return true;
    }

    mpCurrentKF->SetErase();  // 设置当前关键帧可以被擦除,与刚进行检测时形成呼应
    return false;
}

1）检测上次回环是否超过了10帧
2）找共视帧(有门槛15点)，计算共视帧当前帧的bow得分，得到最小得分

DetectLoopCandidates(KeyFrame* pKF, float minScore)

/************************************************************************
 *          功能: 得到回环候选帧
 *          将所有与当前帧具有公共单词id的所有关键帧(不包括与当前关键帧链接共视的关键帧)都设为候选关键帧,然后进行筛选
 *           筛选条件:
 *                    1  根据共有单词数来筛选   筛选最大共有单词数0.8倍以上的所有关键帧为候选关键帧
 *                    2  根据候选关键帧和当前待回环关键帧之间的BOW得分来筛选候选关键帧(大于阈值minScore得分的关键帧)
 *                    3  根据候选关键帧的前10个共视关键帧的累积回环得分来筛选回环候选关键帧(大于0.75最大累积得分的所有回环候选帧,并将得分大于当
 *                               前候选关键帧的共视关键帧代替当前候选关键帧)
 * 
 **************************************************************************************/
vector<KeyFrame*> KeyFrameDatabase::DetectLoopCandidates(KeyFrame* pKF, float minScore)
{
  //  与本关键帧相关联的所有关键帧(相关联是指存在15个以上的共视地图点)   非回环帧  待回环帧的关联帧
    set<KeyFrame*> spConnectedKeyFrames = pKF->GetConnectedKeyFrames();
    // 所有回环候选帧
    list<KeyFrame*> lKFsSharingWords;

    // Search all keyframes that share a word with current keyframes
    // Discard keyframes connected to the query keyframe
    // 用来查找  回环帧的端口 关键帧
    {
        unique_lock<mutex> lock(mMutex);

        for(DBoW2::BowVector::const_iterator vit=pKF->mBowVec.begin(), vend=pKF->mBowVec.end(); vit != vend; vit++)  //  关键帧的所有BOW向量  节点
        {  
	  //  寻找每一BOW向量所在词典节点中所有的关键帧序列   
            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) //pKFi还没有标记为pKF的候选帧  该关键帧还没有加入lKFsSharingWords容器
                {
                    pKFi->mnLoopWords=0;
		    //  找出和当前帧具有公共单词的所有关键帧（不包括与当前帧链接的关键帧(共视关键帧)）
                    if(!spConnectedKeyFrames.count(pKFi))  // 如果关联关键帧中不存在pKFi关键帧,则将pKFi关键帧的回环id设为当前待回环关键帧
                    {
                        pKFi->mnLoopQuery=pKF->mnId;       //节点属于同一个 但是没有相连关系  把回环id赋值当前帧
                        lKFsSharingWords.push_back(pKFi);   // 将本关键帧加入回环列表中
                    }
                }
                pKFi->mnLoopWords++;   //这个关键帧的回环单词数+1
            }
        }
    }

    if(lKFsSharingWords.empty())
        return vector<KeyFrame*>();

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

    // Only compare against those keyframes that share enough words   得到所有回环候选关键帧中最大的回环单词数
    int maxCommonWords=0;
    for(list<KeyFrame*>::iterator lit=lKFsSharingWords.begin(), lend= lKFsSharingWords.end(); lit!=lend; lit++)
    {
        if((*lit)->mnLoopWords>maxCommonWords)
            maxCommonWords=(*lit)->mnLoopWords;
    }
    // 回环候选帧最小的回环单词数
    int minCommonWords = maxCommonWords*0.8f;
    
    int nscores=0;

    // Compute similarity score. Retain the matches whose score is higher than minScore
    for(list<KeyFrame*>::iterator lit=lKFsSharingWords.begin(), lend= lKFsSharingWords.end(); lit!=lend; lit++)
    {
      //  循环回环候选帧中所有的帧
        KeyFrame* pKFi = *lit;
	// 如果回环候选帧中的帧回环单词数大于最小回环单词数
        if(pKFi->mnLoopWords>minCommonWords)
        {
            nscores++;
	    // 检测待回环关键帧与当前候选回环关键帧的BOW得分
            float si = mpVoc->score(pKF->mBowVec,pKFi->mBowVec);

            pKFi->mLoopScore = si;   //回环BOW得分
            if(si>=minScore)    // 将得分小于最小BOW阈值的候选回环关键帧删除
                lScoreAndMatch.push_back(make_pair(si,pKFi));   //将bow大约最大bow值0.8倍的 关键帧挑出来 作为潜在的回环检测点
        }
    }

    if(lScoreAndMatch.empty())
        return vector<KeyFrame*>();

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

    // Lets now accumulate score by covisibility
    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);   // 检测候选回环关键帧的前10帧共视关键帧
	// 当前回环候选帧的最高分(与回环候选帧共视帧的前10帧中与当前待回环关键帧回环得分中的最高分)
        float bestScore = it->first;
	// 当前回环关键帧的累计得分(与回环候选帧共视帧的前10帧如果也与当前帧构成回环,则将它的得分累计进来)
        float accScore = it->first;
	// 最高回环得分的关键帧
        KeyFrame* pBestKF = pKFi;
        for(vector<KeyFrame*>::iterator vit=vpNeighs.begin(), vend=vpNeighs.end(); vit!=vend; vit++)// 检测候选回环关键帧的前10帧共视关键帧
        {   //累加得分  从候选回环帧中选出最佳值
            KeyFrame* pKF2 = *vit;
            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
    float minScoreToRetain = 0.75f*bestAccScore;

    set<KeyFrame*> spAlreadyAddedKF;
    vector<KeyFrame*> vpLoopCandidates;   //当前帧与   回环检测帧进行匹配 前 25%
    vpLoopCandidates.reserve(lAccScoreAndMatch.size());
    //  根据累计得分对其进行筛选  只取前75%的关键帧
    for(list<pair<float,KeyFrame*> >::iterator it=lAccScoreAndMatch.begin(), itend=lAccScoreAndMatch.end(); it!=itend; it++)
    {
        if(it->first>minScoreToRetain)
        {
            KeyFrame* pKFi = it->second;
            if(!spAlreadyAddedKF.count(pKFi))
            {
                vpLoopCandidates.push_back(pKFi);
                spAlreadyAddedKF.insert(pKFi);
            }
        }
    }


    return vpLoopCandidates;
}

1）与本关键帧相关联的所有关键帧(相关联是指存在15个以上的共视地图点) spConnectedKeyFrames
2）与当前帧所有的词节点相联系的关键帧，查看是否带有本帧回环帧序列的标记，没有的话，更改标记，加入到lKFsSharingWords，此帧的回环数加1.（节点属于一个，但不属于spConnectedKeyFrames）
3）在与待回环帧有相同词节点的帧lKFsSharingWords中，遍历循环，找到最大的回环单词数。设置0.8倍为最小回环单词数。满足条件的lKFsSharingWords帧得分加1，计算待回环帧与候选回环帧的bow得分，将其存入lScoreAndMatch，作为潜在回环点。
4）对lScoreAndMatch中的每一帧，选择出最佳共视的10帧，对这10帧中的bow得分累加，选取出其中单个得分最高的一帧，赋值到lAccScoreAndMatch。并将最高分赋给bestAccScore。相当于10帧变为一组。
5）对bestAccScore乘0.75设置阈值，当大于阈值，帧存入 spAlreadyAddedKF 和 vpLoopCandidates

3）vpCandidateKFs就是通过DetectLoopCandidates得到的 vpLoopCandidates 潜在回环帧。
4）每一个候选回环帧的共视帧（15阈值）组成子候选组，假设已经创建了vCurrentConsistentGroups组（初始是后续创建），之前的连续组是否包括子候选组中的帧。
如果之前子连续组中包含"子候选组"中的帧,则说明该关键帧组与之前的组是连续的 bConsistent=true;
不包括，需要创建新组。
假如与之前的连续组是连续的 则将它加入到当前连续组中。如果当前连续组没有在当前连续组集中,则将其加入。
大于三个 是足够连续 将待回环帧赋入mvpEnoughConsistentCandidates

只要子候选组里有一个帧在已存在的连续帧组里，整组加入，连续组的连续性加1.

5）更新连续组mvConsistentGroups