嗯要做一个早睡早起身体棒每天精力充沛的好LZ。认认真真完成学业O(∩_∩)O哈哈~关键帧数据是通过预先训练好的词典维护一个vector的链表每个链表对应一个vocabulary的id对应存储含有这个vocabulary的关键帧链表。#ifndef KEYFRAMEDATABASE_H #define KEYFRAMEDATABASE_H #include vector #include list #include set #include KeyFrame.h #include Frame.h #include ORBVocabulary.h #includemutex namespace ORB_SLAM2 { class KeyFrame; class Frame; class KeyFrameDatabase { public: KeyFrameDatabase(const ORBVocabulary voc); void add(KeyFrame* pKF); void erase(KeyFrame* pKF); void clear(); // Loop Detection std::vectorKeyFrame * DetectLoopCandidates(KeyFrame* pKF, float minScore); // Relocalization std::vectorKeyFrame* DetectRelocalizationCandidates(Frame* F); protected: // Associated vocabulary const ORBVocabulary* mpVoc; // Inverted file std::vectorlistKeyFrame* mvInvertedFile; // Mutex std::mutex mMutex; }; } //namespace ORB_SLAM #endif#include KeyFrameDatabase.h #include KeyFrame.h #include Thirdparty/DBoW2/DBoW2/BowVector.h #includemutex using namespace std; namespace ORB_SLAM2 { //构造函数传入的是ORBVocabulary //关键帧数据库通过预先训练好的词典维护一个向量 mvInvertedFile KeyFrameDatabase::KeyFrameDatabase (const ORBVocabulary voc): mpVoc(voc) { mvInvertedFile.resize(voc.size());// } //根据词袋模型增加关键帧 void KeyFrameDatabase::add(KeyFrame *pKF) { unique_lockmutex lock(mMutex); //mvInvertedFile[i]表示第i个WordId的所有关键帧, vit-first取出的就是WordID for(DBoW2::BowVector::const_iterator vit pKF-mBowVec.begin(), vendpKF-mBowVec.end(); vit!vend; vit) mvInvertedFile[vit-first].push_back(pKF); } //删除对应的关键帧 void KeyFrameDatabase::erase(KeyFrame* pKF) { unique_lockmutex lock(mMutex); // Erase elements in the Inverse File for the entry //对整个vector进行遍历 for(DBoW2::BowVector::const_iterator vitpKF-mBowVec.begin(), vendpKF-mBowVec.end(); vit!vend; vit) { // List of keyframes that share the word //对每个voc进行再次遍历 listKeyFrame* lKFs mvInvertedFile[vit-first]; //如果list中存在指定KF,删除后重新检索其他list for(listKeyFrame*::iterator litlKFs.begin(), lend lKFs.end(); lit!lend; lit) { if(pKF*lit) { lKFs.erase(lit); break; } } } } //清除对应的mvInvertedFile,重新初始化mvInvertedFile的尺寸 void KeyFrameDatabase::clear() { mvInvertedFile.clear(); mvInvertedFile.resize(mpVoc-size()); } //闭环检测 //需要参考关键帧去寻找闭环候选帧 vectorKeyFrame* KeyFrameDatabase::DetectLoopCandidates(KeyFrame* pKF, float minScore) { setKeyFrame* spConnectedKeyFrames pKF-GetConnectedKeyFrames(); listKeyFrame* lKFsSharingWords; // Search all keyframes that share a word with current keyframes // Discard keyframes connected to the query keyframe { unique_lockmutex lock(mMutex); //找出与当前关键帧pKF有公共单词的所有关键帧pKFi,不包括与当前帧相连的关键帧 for(DBoW2::BowVector::const_iterator vitpKF-mBowVec.begin(), vendpKF-mBowVec.end(); vit ! vend; vit) { listKeyFrame* lKFs mvInvertedFile[vit-first]; for(listKeyFrame*::iterator litlKFs.begin(), lend lKFs.end(); lit!lend; lit) { KeyFrame* pKFi*lit; if(pKFi-mnLoopQuery!pKF-mnId) { pKFi-mnLoopWords0; if(!spConnectedKeyFrames.count(pKFi)) { pKFi-mnLoopQuerypKF-mnId; lKFsSharingWords.push_back(pKFi); } } pKFi-mnLoopWords; } } } if(lKFsSharingWords.empty()) return vectorKeyFrame*(); listpairfloat,KeyFrame* lScoreAndMatch; // Only compare against those keyframes that share enough words // 统计所有闭环候选帧中与pKF具有共同单词最多的单词数 int maxCommonWords0; for(listKeyFrame*::iterator litlKFsSharingWords.begin(), lend lKFsSharingWords.end(); lit!lend; lit) { if((*lit)-mnLoopWordsmaxCommonWords) maxCommonWords(*lit)-mnLoopWords; } //设定一个阈值只有当共有单词数大于0.8*maxCommonWords以及匹配得分大于给定的minScore的关键帧存入IScoreAndMatch int minCommonWords maxCommonWords*0.8f; int nscores0; // Compute similarity score. Retain the matches whose score is higher than minScore for(listKeyFrame*::iterator litlKFsSharingWords.begin(), lend lKFsSharingWords.end(); lit!lend; lit) { KeyFrame* pKFi *lit; if(pKFi-mnLoopWordsminCommonWords) { nscores; float si mpVoc-score(pKF-mBowVec,pKFi-mBowVec); pKFi-mLoopScore si; if(siminScore) lScoreAndMatch.push_back(make_pair(si,pKFi)); } } if(lScoreAndMatch.empty()) return vectorKeyFrame*(); listpairfloat,KeyFrame* lAccScoreAndMatch; float bestAccScore minScore; // Lets now accumulate score by covisibility // 对于上文筛选出来的pKFi,每一个都要抽取出自身的共视共享地图点最多的前10帧 //关键帧分为一组计算该组整体得分与pKF比较的记为bestAccScore; for(listpairfloat,KeyFrame* ::iterator itlScoreAndMatch.begin(), itendlScoreAndMatch.end(); it!itend; it) { KeyFrame* pKFi it-second; vectorKeyFrame* vpNeighs pKFi-GetBestCovisibilityKeyFrames(10); float bestScore it-first; float accScore it-first; KeyFrame* pBestKF pKFi; for(vectorKeyFrame*::iterator vitvpNeighs.begin(), vendvpNeighs.end(); vit!vend; vit) { KeyFrame* pKF2 *vit; if(pKF2-mnLoopQuerypKF-mnId pKF2-mnLoopWordsminCommonWords) { accScorepKF2-mLoopScore; if(pKF2-mLoopScorebestScore) { pBestKFpKF2; bestScore pKF2-mLoopScore; } } } lAccScoreAndMatch.push_back(make_pair(accScore,pBestKF)); if(accScorebestAccScore) bestAccScoreaccScore; } // Return all those keyframes with a score higher than 0.75*bestScore //所有组得分大于0.75*bestAccScore的均当做闭环候选帧 float minScoreToRetain 0.75f*bestAccScore; setKeyFrame* spAlreadyAddedKF; vectorKeyFrame* vpLoopCandidates; vpLoopCandidates.reserve(lAccScoreAndMatch.size()); for(listpairfloat,KeyFrame* ::iterator itlAccScoreAndMatch.begin(), itendlAccScoreAndMatch.end(); it!itend; it) { if(it-firstminScoreToRetain) { KeyFrame* pKFi it-second; if(!spAlreadyAddedKF.count(pKFi)) { vpLoopCandidates.push_back(pKFi); spAlreadyAddedKF.insert(pKFi); } } } return vpLoopCandidates; } //检测重定位候选帧 //闭环候选帧和重定位候选帧之间的区别是闭环候选帧是从关键帧中选取 //重定位候选帧是从普通帧中选取 //其他的流程和闭环候选帧选取方式相同 vectorKeyFrame* KeyFrameDatabase::DetectRelocalizationCandidates(Frame *F) { listKeyFrame* lKFsSharingWords; // Search all keyframes that share a word with current frame { unique_lockmutex lock(mMutex); for(DBoW2::BowVector::const_iterator vitF-mBowVec.begin(), vendF-mBowVec.end(); vit ! vend; vit) { listKeyFrame* lKFs mvInvertedFile[vit-first]; for(listKeyFrame*::iterator litlKFs.begin(), lend lKFs.end(); lit!lend; lit) { KeyFrame* pKFi*lit; if(pKFi-mnRelocQuery!F-mnId) { pKFi-mnRelocWords0; pKFi-mnRelocQueryF-mnId; lKFsSharingWords.push_back(pKFi); } pKFi-mnRelocWords; } } } if(lKFsSharingWords.empty()) return vectorKeyFrame*(); // Only compare against those keyframes that share enough words int maxCommonWords0; for(listKeyFrame*::iterator litlKFsSharingWords.begin(), lend lKFsSharingWords.end(); lit!lend; lit) { if((*lit)-mnRelocWordsmaxCommonWords) maxCommonWords(*lit)-mnRelocWords; } int minCommonWords maxCommonWords*0.8f; listpairfloat,KeyFrame* lScoreAndMatch; int nscores0; // Compute similarity score. for(listKeyFrame*::iterator litlKFsSharingWords.begin(), lend lKFsSharingWords.end(); lit!lend; lit) { KeyFrame* pKFi *lit; if(pKFi-mnRelocWordsminCommonWords) { nscores; float si mpVoc-score(F-mBowVec,pKFi-mBowVec); pKFi-mRelocScoresi; lScoreAndMatch.push_back(make_pair(si,pKFi)); } } if(lScoreAndMatch.empty()) return vectorKeyFrame*(); listpairfloat,KeyFrame* lAccScoreAndMatch; float bestAccScore 0; // Lets now accumulate score by covisibility for(listpairfloat,KeyFrame* ::iterator itlScoreAndMatch.begin(), itendlScoreAndMatch.end(); it!itend; it) { KeyFrame* pKFi it-second; vectorKeyFrame* vpNeighs pKFi-GetBestCovisibilityKeyFrames(10); float bestScore it-first; float accScore bestScore; KeyFrame* pBestKF pKFi; for(vectorKeyFrame*::iterator vitvpNeighs.begin(), vendvpNeighs.end(); vit!vend; vit) { KeyFrame* pKF2 *vit; if(pKF2-mnRelocQuery!F-mnId) continue; accScorepKF2-mRelocScore; if(pKF2-mRelocScorebestScore) { pBestKFpKF2; bestScore pKF2-mRelocScore; } } lAccScoreAndMatch.push_back(make_pair(accScore,pBestKF)); if(accScorebestAccScore) bestAccScoreaccScore; } // Return all those keyframes with a score higher than 0.75*bestScore float minScoreToRetain 0.75f*bestAccScore; setKeyFrame* spAlreadyAddedKF; vectorKeyFrame* vpRelocCandidates; vpRelocCandidates.reserve(lAccScoreAndMatch.size()); for(listpairfloat,KeyFrame* ::iterator itlAccScoreAndMatch.begin(), itendlAccScoreAndMatch.end(); it!itend; it) { const float si it-first; if(siminScoreToRetain) { KeyFrame* pKFi it-second; if(!spAlreadyAddedKF.count(pKFi)) { vpRelocCandidates.push_back(pKFi); spAlreadyAddedKF.insert(pKFi); } } } return vpRelocCandidates; } } //namespace ORB_SLAM