ORB_SLAM3 闭环检测

• ORB SLAM3系统初始化
• ORB SLAM3 构建Frame
• ORB_SLAM3 单目初始化
• ORB_SLAM3 双目匹配
• ORB_SLAM3_IMU预积分理论推导(预积分项)
• ORB_SLAM3_IMU预积分理论推导(噪声分析)
• ORB_SLAM3_IMU预积分理论推导(更新)
• ORB_SLAM3_IMU预积分理论推导(残差)
• ORB_SLAM3_优化方法 Pose优化
• ORB_SLAM3 闭环检测

DetectNBestCandidates

DetectNBestCandidates主要是用来闭环检测得到候选关键帧，和闭环有关的变量如下：

• mnPlaceRecognitionQuery：与其有公共单词的关键帧
• mnPlaceRecognitionWords：与其有多少公共单词数
• mPlaceRecognitionScore：与其的词袋相似度

1.闭环检测示意图

2.步骤

1. 通过词袋的BowVec找出与当前关键帧具有相同单词的所有关键帧，并统计对应的相同单词数量，但不包括对应的共视关键帧spConnectedKF，得到lKFsSharingWords。
   

    // Step 1统计与当前关键帧有相同单词的关键帧
    list<KeyFrame *> lKFsSharingWords;
    // set spInsertedKFsSharing;
    //  当前关键帧的共视关键帧(避免将当前关键帧的共视关键帧加入回环检测)
    set<KeyFrame *> spConnectedKF;

    // Search all keyframes that share a word with current frame
    {
        unique_lock<mutex> lock(mMutex);
        // 拿到当前关键帧的共视关键帧
        spConnectedKF = pKF->GetConnectedKeyFrames();

        // 遍历当前关键帧bow向量的每个单词
        for (DBoW2::BowVector::const_iterator vit = pKF->mBowVec.begin(), vend = pKF->mBowVec.end(); vit != vend; vit++)
        { // 拿到当前单词的逆向索引(所有有当前单词的关键帧)
            list<KeyFrame *> &lKFs = mvInvertedFile[vit->first];
            // 遍历每个有该单词的关键帧
            for (list<KeyFrame *>::iterator lit = lKFs.begin(), lend = lKFs.end(); lit != lend; lit++)
            {
                KeyFrame *pKFi = *lit;
                /*if(spConnectedKF.find(pKFi) != spConnectedKF.end())
                {
                    continue;
                }*/
                // 如果此关键帧没有被当前关键帧访问过(防止重复添加)
                if (pKFi->mnPlaceRecognitionQuery != pKF->mnId)
                {
                    // 初始化公共单词数为0
                    pKFi->mnPlaceRecognitionWords = 0;
                    // 如果该关键帧不是当前关键帧的共视关键帧
                    if (!spConnectedKF.count(pKFi))
                    {
                        // 标记该关键帧被当前关键帧访问到（也就是有公共单词）
                        pKFi->mnPlaceRecognitionQuery = pKF->mnId;
                        // 把当前关键帧添加到有公共单词的关键帧列表中
                        lKFsSharingWords.push_back(pKFi);
                    }
                }
                // 递增该关键帧与当前关键帧的公共单词数
                pKFi->mnPlaceRecognitionWords++;
                /*if(spInsertedKFsSharing.find(pKFi) == spInsertedKFsSharing.end())
                {
                    lKFsSharingWords.push_back(pKFi);
                    spInsertedKFsSharing.insert(pKFi);
                }*/
            }
        }
    }

2. 计算当前关键帧与具有一定公共单词数的关键帧的BowVec的分数，得到lScoreAndMatch。
   • 用最大公共单词数maxCommonWords的0.8(minCommonWords)动态过滤掉那些公共单词数少的
     

    for (list<KeyFrame *>::iterator lit = lKFsSharingWords.begin(), lend = lKFsSharingWords.end(); lit != lend; lit++)
    {
        KeyFrame *pKFi = *lit;
        // 如果当前帧的公共单词数大于minCommonWords
        if (pKFi->mnPlaceRecognitionWords > minCommonWords)
        {
            nscores++; //未使用
            // 计算相似度
            float si = mpVoc->score(pKF->mBowVec, pKFi->mBowVec);
            // 记录该候选帧与当前帧的相似度
            pKFi->mPlaceRecognitionScore = si;
            // 记录到容器里, 每个元素是<相似度,候选帧的指针>
            lScoreAndMatch.push_back(make_pair(si, pKFi));
        }
    }

3. 单个关键帧相似可能不太准确，那么就组队，对lScoreAndMatch中的关键帧，将候选关键帧周围的共视关键帧(需要和当前关键帧有公共的单词，且单词数超过minCommonWords)拉入，统计每一组的分数，并用每一组分数最大的那帧代表该组，选为候选关键帧
   

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

    // Lets now accumulate score by covisibility
    // 变量所有被lScoreAndMatch记录的pair <相似度,候选关键帧>
    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 = bestScore;
        // 初始化组内相似度最高的帧为该候选关键帧本身
        KeyFrame *pBestKF = pKFi;
        // 遍历与当前关键帧共视关系最好的10帧
        for (vector<KeyFrame *>::iterator vit = vpNeighs.begin(), vend = vpNeighs.end(); vit != vend; vit++)
        {
            KeyFrame *pKF2 = *vit;
            // 如果该关键帧没有被当前关键帧访问过(也就是没有公共单词)则跳过
            if (pKF2->mnPlaceRecognitionQuery != pKF->mnId)
                continue;
            // 累加小组总分
            accScore += pKF2->mPlaceRecognitionScore;
            // 如果大于组内最高分,则更新当前最高分记录
            if (pKF2->mPlaceRecognitionScore > bestScore)
            {
                pBestKF = pKF2;
                bestScore = pKF2->mPlaceRecognitionScore;
            }
        }
        // 统计以组为单位的累计相似度和组内相似度最高的关键帧, 每个pair为<小组总相似度,组内相似度最高的关键帧指针>
        lAccScoreAndMatch.push_back(make_pair(accScore, pBestKF));
        // 统计最高得分, 这个bestAccSocre没有用到
        if (accScore > bestAccScore)
            bestAccScore = accScore;
    }

4. 对lScoreAndMatch安装分数排序
   • 如果和当前关键帧是同一张地图，就添加到闭环候选帧vpLoopBowCand中
   • 如果和当前关键帧是不同张地图，就添加到合并候选帧vpMergeBowCand中

    //  按相似度从大到小排序
    lAccScoreAndMatch.sort(compFirst);
    // 最后返回的变量, 记录回环的候选帧
    vpLoopCand.reserve(nNumCandidates);
    // 最后返回的变量, 记录融合候选帧
    vpMergeCand.reserve(nNumCandidates);
    // 避免重复添加
    set<KeyFrame *> spAlreadyAddedKF;
    // cout << "Candidates in score order " << endl;
    // for(list >::iterator it=lAccScoreAndMatch.begin(), itend=lAccScoreAndMatch.end(); it!=itend; it++)
    int i = 0;
    list<pair<float, KeyFrame *>>::iterator it = lAccScoreAndMatch.begin();
    // 遍历lAccScoreAndMatch中所有的pair, 每个pair为<小组总相似度,组内相似度最高的关键帧指针>，nNumCandidates默认为3
    while (i < lAccScoreAndMatch.size() && (vpLoopCand.size() < nNumCandidates || vpMergeCand.size() < nNumCandidates))
    {
        // cout << "Accum score: " << it->first << endl;
        //  拿到候选关键帧的指针
        KeyFrame *pKFi = it->second;
        if (pKFi->isBad())
            continue;

        // 如果没有被重复添加
        if (!spAlreadyAddedKF.count(pKFi))
        {
            // 如果候选帧与当前关键帧在同一个地图里,且候选者数量还不足够
            if (pKF->GetMap() == pKFi->GetMap() && vpLoopCand.size() < nNumCandidates)
            {
                // 添加到回环候选帧里
                vpLoopCand.push_back(pKFi);
            }
            // 如果候选者与当前关键帧不再同一个地图里, 且候选者数量还不足够, 且候选者所在地图不是bad
            else if (pKF->GetMap() != pKFi->GetMap() && vpMergeCand.size() < nNumCandidates && !pKFi->GetMap()->IsBad())
            {
                // 添加到融合候选帧里
                vpMergeCand.push_back(pKFi);
            }
            // 防止重复添加
            spAlreadyAddedKF.insert(pKFi);
        }
        i++;
        it++;
    }