Code Reading: ORB-SLAM回环检测源码阅读＋注释

之前研究过一些回环检测的内容，首先要看的自然是用词袋回环的鼻祖和正当继承人(没有冒犯VINS和LDSO的意思）ORB-SLAM。下面是我的代码注释。因为代码都是自己手打的，不是在源码上注释的，所以一些我觉得不是太重要的被略过了，可能也会有一些typo.

ORB的回环策略比较偏向seq-SLAM的思路，通过共视帧打包的关系，比较每个包的相似值，而非只是关注单帧和单帧的匹配，这个思路是比较合适的，但是VINS和LDSO两位后来者用实际行动证明了我不太看中你这种思路，两个都没有用。后续我会介绍一些VINS和LDSO里的回环方法。

System Constructor

// System constructor
System::System()
{

    mpVocabulary = new ORBVocabulary();	// -- create keyframe database
    mpVocabulary->LoadFromTextFile(strVocFile); // load file

    mpLoopCloser = new LoopClosing(...);
    mptLoopClosing = new thread(&ORB_SLAM2:LoopClosing::Run, mploopCloser); // a thread

    // -- mpTracker, mpLocalMapper, mpLoopCloser have to know each other's pointer
    ...
}

ComputeBow

在系统中的多个地方会计算Frame或者Keyframe的Bow，具体如下：

void Frame::ComputeBow()
{
    if(mBowVec.empty())
    {	// -- mDescriptors: cv::Mat
        vector<cv::Mat> vCurrentDesc = Converter::toDescriptorVector(mDescriptors);
        mpORBVocabulary->transform(vCurrentDesc, mBowVec, mFeatVec, 4);
    }
}

// -- LocalMapping::ProcessNewKeyFrame() 调用
// -- Tracking::CreatInitialMapMonocular() 调用，开始两个关键帧
// -- Tracking::TrackReferenceKeyFrame()
// -- Tracking::Relocalization()
void KeyFrame::ComputeBow()
{
    if(mBowVec.empty() || mFeatVec.empty())
    {
        同上；
	}
}

LoopClosing

void LoopClosing::Run()
{
	whlie(1)
    {
        if(CheckNewKeyFrames())	// -- lock and check LoopKeyFrameQueue
        {
            if(DetectLoop())
            {
                if(ComputeSim3())
                {
                    CorrectLoop();
                }
            }
        }
    }
}

bool LoopClosing::DetectLoop()
{
    {	// -- 得到当前帧
        unique_lock<mutex> lock(...); // -- lock
        mpCurrentKF = mlpLoopKeyFrameQueue.front();
        mlpLoopKeyFrameQueue.pop_front();
        mpCurrentKF->SetNotErase(); // -- 暂时不销毁
    }

    if(mpCurrentKF->mnId<mLastLoopKFid + 10)	// -- 如果上次发生回环在10kf以内，回false
    {
        mpKeyFrameDB->add(mpCurrentKF);	//加入到关键帧数据集里面
        mpCurrentKF->SetErase();	//销毁咯
        return false;
    }

    // -- 计算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(socre < minScore)
        {
            minScore = score;
        }
    }

    //	-- 用currentKF和minScore找回环候选帧
    vector<KeyFrame*> vpCanndidateKFs = mpKeyFrameDB->DetectLoopCandidates(mpCurrentKF, minScore);

    // -- 如果没有候选帧，回家吃饭
    if(vpCandidateKFs.empty())
    {
        mpKeyFrameDB->add(mpCurrentKF);
        mvConsistentGroup.clear();
        mpCurrentKF->SetErase();
        return false;
    }

    //TO BE CONTINUED
    ...
}

KeyframeDatabase

初始化

KeyFrameDatabae::KeyFrameDatabase (const ORBVocabulary &voc) : mpVoc(&voc)
{
    mvInvertedFile.resize(voc.size());
}

mvInvertedFile是一个按单次ID顺序存储的vector，每一个空间存储一个KeyFrame*的list。

add函数存储关键帧指针到每一个ID的对应list里面。

void KeyFrameDatabase::add(KeyFrame *pKF)
{
    unique_lock<mutex> lock(mMutex);
    for(auto vit = pKF->mBowVec.begin(), vend; ;vit++ )
    {
        mvInvertedFile[vit->first].push_back(pKF);	// -- std::vector<list<KeyFrame*>>
    }
}

void KeyFrameDatabase::erase(KeyFrame* pKF)
{
    unique_lock<mutex> lock(mMutex);
    for(auto vit = pKF->mBoWVec.begin(), vend;; vit++)
    {
        list<KeyFrame*> &lKFs = mvInvertedFile[vit->first]; // -- 获取存有该ID单词的list
        for(auto lit=lKFs.begin(),lend;;lit++)
        {
            找到pKF然后删除这个迭代器;
        }
    }
}

看如果用这个数据集来寻找回环候选帧

vector<KeyFrame*> KeyFrameDatabase::detectLoopCandidate(KeyFrame* pKF, float minScore)
{
    set<KeyFrame*> spConnectedKeyframes = pKF->GetConnectedKeyFrames();
   	list<KeyFrame*> lKFsSharingWords;//找到有相同单次的帧加入list

    // -- 找到所有和当前帧有相同word的帧
    {
        unique_lock<mutex> lock<mMutex>;
        for(auto vit=pkF->mBowVec, vend; vit != vend; vit++)	//遍历当前帧的words
        {
        	list<KeyFrame*> &lKFs = mvInvertedFile[vit->first];	// 找到对应的list
            for(auto lit = lKFs.begin()..; ;lit++)	//遍历list
            {
                KeyFrame* pKFi = *lit;
                if(pKFi->mnLoopQuery!=pKF->mnId)//如果这个关键帧的**最近询问**不是当前帧-111
                {
                    pKFi->mnLoopWords = 0;// 第一次找到单词
                    if(!spConnectedKeyFrames.count(pKFi))//查询不是当前帧的共视帧
                    {
                        pKFi->mnLoopQuery = pKF->mnId;//和-111遥相呼应，不重复操作
                        lKFSSharingWords.push_back(pKFi);//加入set
                    }
                }
                pKFi->mnLoopWords++;//记录一共共有的单词个数
            }
        }
	}

    if(lKFsSharingWords.empty())	// -- 没有任何帧有相同的单词，GG
    {
        return vector<KeyFrame*>();
    }

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

    int maxCommonWords = 0;
    {
    	//遍历找到共享单词最多的更新给maxCommonWords;
    }
    int minCommonWords = maxCommonWords * .8f; // 至少是最多共视的80%
    int nscore = 0;

    //轮询lKFsSharingWords
    //计算相似得分，保留得分比minScore高的
    for(auto lit = lKFsSharingWords.begin(), lend; ; lit++)
    {
        KeyFrame* pKFi = *lit;
        if(pKFi->mnLoopWords > minCommonWords)
        {
            nscores++;
            float si = mpVoc->score(pKF->mBowVec, pKFi->mBoWVec); //计算得分
            pKFi->mLoopScore = si;
            if(si >= minScore)
            {
                lScoreAndMatch.push_back(make_pair(si, pKFi)); //记录得分和大于minScore的回环帧
            }
        }
    }

    if(lScoreAndMatch.empty()) // -- 没有得分够的，GG
    {
        return vector<KeyFrame*>();
    }

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

    //翻云覆雨，用共视估计得分
    for(auto it = lScoreAndMatch.begin(), itend; ; it++) //遍历lScoreAndMatch
    {
        KeyFrame* pKFi = it->second;
        vector<KeyFrame*> vpNeighs = pKFi->GetBestCovisibilityKeyFrames(10); // -- 找10个共视最多的帧 这里Neigh应该是想说Neighbor
        float bestScore = it->first;
        float accScore = it->first;	// -- accumulate累计的分数
        KeyFrame* pBestKF = pKFi;
        for(auto vit = vpNeights.begin(), vend; ;vit++)
        {
            KeyFrame* pKF2 = *vit;
            if(pKF2->mnLoopQuery == pKF->mnId && pKF2->mnLoopWords > minCommonWords)	//共视的帧也和当前帧有很强的BoW的反应
            {
                accScore += pKF2->mLoopScore;
                if(pkF2->mLoopScore > bestScore)
                {
                    pBestKF = pKF2; //更新最强回环帧
                    bestScore = pKF2->mLoopScore;
                }
            }
        }
        lAccScoreAndMatch.push_back(make_pair(accScore, pBestKF)); //把之前算的每一帧的，以最近10个共视帧的分时累计分数，并选出10帧中个分数最高的做pointer
        if(accScore > bestAccScore) // -- 更新累计最高分
        {
            bestAccScore = accScore;
        }
	}

    float minScoreToRetain = 0.75f * bestAccSCore;

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

    for(auto it = lAccScoreAndMatch.begin...;;) //遍历lAccScoreAndMatch
    {
        if(it->first > minScoreToRetain)
        {
            KeyFrame* pKFi = it->second;
            if(!spAlreadyAddedKF.count(pKFi)) // -- 滤除一样的帧
            {
                vpLoopCandiates.push_back(pKFi);
                spAlreadyAddedKF.insert(pkFi);
            }
        }
    }

    return vpLoopCandidates;
}