ORB_SLAM2修(Ru)仙(Keng)之路(一):重定位候選關鍵幀選取函式註釋
阿新 • • 發佈:2019-02-09
自從入了ORB_SLAM的坑以來,深感,在室外使用VO導航的艱辛,ORB_SLAM程式碼前後看了有一個月了,現在逐漸把一些函式的程式碼註釋記錄在這裡,希望能給後面入坑的同志們些借鑑吧,當然自己也還有很多不明白的地方,所以,如果註釋有錯的,還請指出。
程式碼塊
/**
* 函式功能:從關鍵幀庫中找出與當前幀匹配得最好的候選關鍵幀序列
* 函式輸入:當前幀
* 函式輸出:關鍵幀庫中與當前幀匹配較好的候選關鍵幀序列
* 幾個變數含義:(1)mBowVec:BoW向量,裡面儲存的是從BoW詞袋中查詢到的“單詞”
* (2)mvInvertedFile:是一個向量,裡面存放的是一個個單詞,每個單詞對應一個list連結串列,每個連結串列裡存放的是擁有該單詞的關鍵幀
* 即,每一個單詞都對應著所有看到該單詞的關鍵幀
* (3)pKFi->mnRelocWords:該關鍵幀與當前幀所共有的單詞數
* */
vector<KeyFrame*> KeyFrameDatabase::DetectRelocalizationCandidates(Frame *F)
{
list<KeyFrame*> lKFsSharingWords;//與當前幀具有公共單詞的所有關鍵幀
{
///遍歷該幀看到的所有單詞
unique_lock<mutex> lock(mMutex);
for(DBoW2::BowVector::const_iterator vit=F->mBowVec.begin(), vend=F->mBowVec.end(); vit != vend; vit++)
{
list <KeyFrame*> &lKFs = mvInvertedFile[vit->first];//找到這個單詞所對應的所有關鍵幀
///遍歷該單詞的所有關鍵幀,並打上標籤即:pKFi->mnRelocQuery=F->mnId,為了防止lKFsSharingWords中重複新增
for(list<KeyFrame*>::iterator lit=lKFs.begin(), lend= lKFs.end(); lit!=lend; lit++)
{
KeyFrame* pKFi=*lit;
if (pKFi->mnRelocQuery!=F->mnId)//為了不對同一個關鍵幀重複檢測,這裡打一個標籤
{
pKFi->mnRelocWords=0;
pKFi->mnRelocQuery=F->mnId;
lKFsSharingWords.push_back(pKFi);
}
pKFi->mnRelocWords++;//如果當前幀與該幀每共有一個單詞,則加一
}
}
}
///如果關鍵幀庫裡所有的關鍵幀與當前幀都沒有公共單詞,則返回空
if(lKFsSharingWords.empty())
return vector<KeyFrame*>();
int maxCommonWords=0;//最大共有單詞數量
///遍歷所有與當前幀具有公共單詞的關鍵幀,並找與當前幀具有的最大公共單詞數
for(list<KeyFrame*>::iterator lit=lKFsSharingWords.begin(), lend= lKFsSharingWords.end(); lit!=lend; lit++)
{
if((*lit)->mnRelocWords>maxCommonWords)
maxCommonWords=(*lit)->mnRelocWords;
}
int minCommonWords = maxCommonWords*0.8f;//最小共有單詞數取最大共有單詞數的0.8倍
list<pair<float,KeyFrame*> > lScoreAndMatch;
int nscores=0;
///找出共有單詞數在minCommonWords與maxCommonWords之間的關鍵幀,作為“得分較高的關鍵幀”,
/// 並且計算當前幀與該關鍵幀的得分,將得分與該關鍵幀組成pair存到lScoreAndMatch中
for(list<KeyFrame*>::iterator lit=lKFsSharingWords.begin(), lend= lKFsSharingWords.end(); lit!=lend; lit++)
{
KeyFrame* pKFi = *lit;
if(pKFi->mnRelocWords>minCommonWords)
{
nscores++;
float si = mpVoc->score(F->mBowVec,pKFi->mBowVec);//比較兩個vector向量的得分
pKFi->mRelocScore=si;
lScoreAndMatch.push_back(make_pair(si,pKFi));
}
}
///這一個應該是不會發生的,因為至少有一個關鍵幀與當前幀的共有單詞數符合上邊的閾值範圍,就是共有單詞數最多的那個關鍵幀
if(lScoreAndMatch.empty())
return vector<KeyFrame*>();
list<pair<float,KeyFrame*> > lAccScoreAndMatch;
float bestAccScore = 0;
///遍歷得分較高的關鍵幀所對應的10個最佳共視關鍵幀,這樣lScoreAndMatch序列中“被共視”的越多的關鍵幀將越有可能被選為候選關鍵幀
/// 注:這一塊看的不是特別明白,如有錯誤請指出,謝謝
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個最佳共視關鍵幀
float bestScore = it->first;
float accScore = bestScore;
KeyFrame* pBestKF = pKFi;
///遍歷該幀對應的共視關鍵幀
for(vector<KeyFrame*>::iterator vit=vpNeighs.begin(), vend=vpNeighs.end(); vit!=vend; vit++)
{
KeyFrame* pKF2 = *vit;
if(pKF2==NULL)//add by yuxi
continue;
if(pKF2->mnRelocQuery!=F->mnId)//如果該幀不在lKFsSharingWords中,則無須再處理
continue;
accScore+=pKF2->mRelocScore;//將pKFi的得分與它的共視關鍵幀得分累加
if(pKF2->mRelocScore>bestScore)//如果共視關鍵幀的得分超過pKFi,則將pKFi替換掉
{
pBestKF=pKF2;
bestScore = pKF2->mRelocScore;
}
}
lAccScoreAndMatch.push_back(make_pair(accScore,pBestKF));
if(accScore>bestAccScore)
bestAccScore=accScore;
}
///將lAccScoreAndMatch accScore中滿足閾值的得分作為候選關鍵幀,返回
float minScoreToRetain = 0.75f*bestAccScore;
set<KeyFrame*> spAlreadyAddedKF;
vector<KeyFrame*> vpRelocCandidates;
vpRelocCandidates.reserve(lAccScoreAndMatch.size());
for(list<pair<float,KeyFrame*> >::iterator it=lAccScoreAndMatch.begin(), itend=lAccScoreAndMatch.end(); it!=itend; it++)
{
const float &si = it->first;
if(si>minScoreToRetain)
{
KeyFrame* pKFi = it->second;
if(!spAlreadyAddedKF.count(pKFi))
{
vpRelocCandidates.push_back(pKFi);
spAlreadyAddedKF.insert(pKFi);
}
}
}
return vpRelocCandidates;
}