ORB-SLAM2源码逐行解析系列（十九）：闭环线程

最新推荐文章于 2024-05-18 16:39:33 发布

LDST_CSDN

最新推荐文章于 2024-05-18 16:39:33 发布

阅读量288

点赞数

文章标签：人工智能计算机视觉自动驾驶

本文链接：https://blog.csdn.net/LDST_CSDN/article/details/130672422

版权

1. 关于闭环线程

闭环是SLAM系统中非常重要的部分，它用来判断机器人是否经过同一地点，从而进行调整。ORB-SLAM2中的闭环线程主要包括如下三个部分：

闭环检测：检测机器人是否到达起始位置，获取闭环候选关键帧
闭环矫正：用检测到的闭环关系对所有关键帧位姿和地图点进行矫正，将闭环误差分摊到每一帧及其相邻帧上
全局BA优化：完成闭环矫正后，对所有关键帧的位姿和地图点进行全局优化

2. 主线程

（1）大致流程

在闭环检测缓冲队列中含有关键帧的前提下，依次执行闭环检测(DetectLoop)，计算Sim3(ComputeSim3)以及闭环矫正(CorrectLoop)。

（2）代码实现

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())
               {
   
                   // Perform loop fusion and pose graph optimization
                   CorrectLoop();
               }
            }
        }

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

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

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

	}

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

3. 闭环检测DetectLoop

3.1 寻找初始闭环候选关键帧

主要是根据公共单词进行搜索，并根据相应阈值进行筛选，以得到初始的闭环候选关键帧。对于当前关键帧，首先需要明确的是它的共视关键帧、相连关键帧以及具有公共单词关键帧的区别。

当前帧的共视关键帧：与当前帧具有共视关系，即能够共同观测到某一地图点的关键帧
当前帧的相连关键帧：当前帧的共视关键帧中，共视程度大于15的关键帧（至少共同观测到15个地图点）
与当前帧具有公共单词的关键帧：是指与当前帧存在相同特征的关键帧，与共视关键帧、相连关键帧没有必然联系

闭环候选关键帧的筛选阈值如下，必须要经过下面3个阈值的筛选才能成为闭环候选关键帧：

minScore：当前帧的相连关键帧中，与当前帧的BoW相似度得分的最小值
minCommonWords：与当前关键帧具有公共单词，但不与其相连的关键帧中，具有最大公共单词数的0.8倍
minScoreToRetain：对于满足上述条件的闭环候选关键帧，将与其具有最强共视关系的前10个关键帧归为一组，BoW相似度得分累积最高组中BoW相似度得分最大值的0.75倍

代码实现

/**
 * @brief 在闭环检测中找到与该关键帧可能闭环的候选关键帧(注意不和当前帧连接)
 * Step 1：找出和当前帧具有公共单词的所有关键帧，不包括与当前帧连接的关键帧
 * Step 2：只和具有共同单词较多的(最大数目的80%以上)关键帧进行BoW相似度计算 
 * Step 3：计算上述候选帧对应的共视关键帧组的总得分，只取最高组得分75%以上的组
 * Step 4：得到上述组中分数最高的关键帧作为闭环候选关键帧
 * @param[in] pKF               需要闭环检测的关键帧
 * @param[in] minScore          候选闭环关键帧和当前关键帧的BoW相似度至少要大于minScore
 * @return vector<KeyFrame*>    闭环候选关键帧
 */
vector<KeyFrame*> KeyFrameDatabase::DetectLoopCandidates(KeyFrame* pKF, float minScore)
{
   
/*
 取出与当前关键帧相连的所有关键帧，这些相连关键帧都是局部相连，在闭环检测的时候将被剔除
 GetVectorCovisibleKeyFrames与GetConnectedKeyFrames得到的都是当前帧的共视关键帧(>15个共视地图点)
 只不过GetConnectedKeyFrames是按照权重从大到小排列的，而GetVectorCovisibleKeyFrames没有排序
*/
    set<KeyFrame*> spConnectedKeyFrames = pKF->GetConnectedKeyFrames();
    // 共视帧: 与当前帧需要有共视地图点(共同观测到的地图点)
    // 相连帧: 与当前帧的共视地图点需要超过15个
    // 公共单词: 与当前帧存在相同特征的关键帧，不一定与当前帧共视，也不一定相连
    // 用于保存可能与当前关键帧形成闭环的候选帧(只要有相同的word，且不属于局部相连(共视)帧)
    list<KeyFrame*> lKFsSharingWords;
    
    // Step 1：找出和当前帧具有公共单词的所有关键帧，不包括与当前帧连接的关键帧(不在spConnectedKeyFrames中)
    {
   
        unique_lock<mutex> lock(mMutex);
        // 遍历该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++)
        {
   
            // 通过倒排索引mvInvertedFile提取所有包含该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);
                    }
                }
                // 对于当前word自增之后，由于已经做了标记(pKFi->mnLoopQuery=pKF->mnId)
                // 所以如果下一word也被看到，则再次自增，而不用进入上面的if中清0
                pKFi->mnLoopWords++;  // 记录pKFi与pKF具有相同word的个数
            }
        }
    }
    // 如果没有关键帧和这个关键帧具有相同的单词,说明没有候选闭环关键帧，那么就返回空
    if(lKFsSharingWords.empty())
        return vector<KeyFrame*>();
    
    // 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;  // 这个变量后面没有用到
    
    // 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来计算两者的BoW相似度得分
            float si = mpVoc->score(pKF->mBowVec,pKFi->mBowVec);
            pKFi->mLoopScore = si;  // 更新闭环候选帧与当前帧的BoW相似度得分(后面会用得到)
            if(si>=minScore)
                // 进一步地，获取BoW相似度得分大于minScore的关键帧
                lScoreAndMatch.push_back(make_pair(si,pKFi));
        }
    }
	// 如果没有超过指定相似度阈值的，那么也就直接跳过去
    if(lScoreAndMatch.empty())
        return vector<KeyFrame*>();
    
    // Step 4：以上述候选关键帧的前10个共视关键帧为一组，计算每个组中与当前关键帧的累积BoW相似度
    // 获取累积相似度最高的得分bestAccScore及其对应组中与当前帧BoW相似度最高的关键帧pBestKF
    for(list<pair<float,KeyFrame*> >::iterator it=lScoreAndMatch.begin(),
        itend=lScoreAndMatch.end(); it!=itend; it++)
    {
   
		KeyFrame* pKFi = it->second;
        // 获取当前候选关键帧共视程度最高的前10个共视关键帧，以此为一组
        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)
            {
   
                // 累计当前组的BoW相似度得分
                accScore+=pKF2->mLoopScore;
                // 统计得到组里分数最高的关键帧
                if(pKF2->mLoopScore>bestScore)
                {
   
                    pBestKF=pKF2;
                    bestScore = pKF2->mLoopScore;
                }
            }
        }
        // first: 每个组的累积BoW相似度得分  second: 该组BoW相似度得分最高的候选关键帧
        lAccScoreAndMatch.push_back(make_pair(accScore,pBestKF));
        // 记录所有组中累积得分最高的组，用于确定相对阈值
        if(accScore>bestAccScore)
            bestAccScore=accScore;
    }
    
    // 所有组中最高得分的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;
}

3.2 闭环检测 —— 进一步验证闭环候选关键帧

在得到初始的闭环候选关键帧之后，需要从中选取满足连续性条件的关键帧作为闭环候选关键帧。对于连续性条件，首先需要明确如下几个概念：

本次闭环检测的子候选组spCandidateGroup：上述得到的初始闭环候选关键帧中，每个候选关键帧和它的相连关键帧构成本次闭环检测的子候选组
上次闭环检测的子连续组sPreviousGroup：来自于上次闭环检测的连续组链mvConsistentGroups
本次闭环检测的连续组链vCurrentConsistentGroups：若本次闭环检测的子候选组与上次闭环检测的子连续组相连，即有一帧共同存在于本次的子候选组与上次的子连续组中，则将所有满足该条件的子候选组及对应的连续长度存放到本次闭环的连续组链中。
上次闭环检测的连续组链mvConsistentGroups：由本次闭环的连续组链赋值而来，初始时为空