前言
ORB-SLAM3支持多地图系统，因此引进了地图融合线程。
地图融合线程会检测活跃地图和整个地图集是否存在共同区域，如果检测到共同区域发生在当前关键帧和活跃地图中，则执行闭环操作，如果检测到共同区域发生在当前帧和非活跃地图中，则执行地图融合操作。如果同时检测到闭环和地图融合，则忽略闭环执行地图融合操作。具体定义在KeyFrameDatabase.cc中。
1.函数声明
void KeyFrameDatabase::DetectNBestCandidates(KeyFrame *pKF, vector<KeyFrame *> &vpLoopCand, vector<KeyFrame *> &vpMergeCand, int nNumCandidates)
2.函数定义
下面的流程已经总结的很到位了！
具体流程如下：
1.统计与当前帧具有共同单词的关键帧，但是排除掉了与当前关键帧共视的关键帧，之后遍历这些共同单词，有该单词的关键帧进行标记（pKFi->mnPlaceRecognitionQuery = pKF->mnId;）将这个关键帧加入到列表中（lKFsSharingWords.push_back(pKFi);）记录有这个共同单词的关键帧的数量（pKFi->mnPlaceRecognitionWords++;）。该部分在一个锁中。若是最后没有统计到与当前关键帧具有公共单词的关键帧则直接返回。
排除了共视关键帧：目的是防止直接将共视的关键帧当作闭环关键帧。
// Step 1统计与当前关键帧有相同单词的关键帧
list<KeyFrame *> lKFsSharingWords;
// set<KeyFrame*> 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);
}*/
}
}
}
// 如果没有有公共单词的关键帧,直接返回
if (lKFsSharingWords.empty())
return;2.不断遍历前边筛选好的具有共同单词数的关键帧，逐一比较得到一个最大的共同的单词数（maxCommonWords），用最大单词数的0.8倍作为最小单词数用于后续的筛选，之后对满足大于最小单词数的关键帧进行相似度的计算，并加入到创建的新列表中（lScoreAndMatch.push_back(make_pair(si, pKFi));），如果经过筛选并没有满足条件的关键帧则返回。
// Only compare against those keyframes that share enough words
// Step 2 统计所有候选帧中与当前关键帧的公共单词数最多的单词数maxCommonWords,并筛选
int maxCommonWords = 0;
for (list<KeyFrame *>::iterator lit = lKFsSharingWords.begin(), lend = lKFsSharingWords.end(); lit != lend; lit++)
{
if ((*lit)->mnPlaceRecognitionWords > maxCommonWords)
maxCommonWords = (*lit)->mnPlaceRecognitionWords;
}
// 取0.8倍为阀值
int minCommonWords = maxCommonWords * 0.8f;
// 这里的pair是 <相似度,候选帧的指针> : 记录所有大于minCommonWords的候选帧与当前关键帧的相似度
list<pair<float, KeyFrame *>> lScoreAndMatch;
// 只是个统计变量,貌似没有用到
int nscores = 0;
// Compute similarity score.
// 对所有大于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));
}
}
// 如果为空,直接返回,表示没有符合上述条件的关键帧
if (lScoreAndMatch.empty())
return;3. 遍历上述已经筛选过得到的列表（lScoreAndMatch），获取列表中的关键帧的最好的10个共视关键帧（这些关键帧必须还得是之前被访问过的），统计一些变量（accScore、pBestKF、bestScore），并将其记录到变量lAccScoreAndMatch中。按照相似度对这个列表进行排序，遍历这个列表当遍历未越界以及（闭环候选容器的尺寸或者融合候选容器的尺寸不大于指定数量）就会已知循环。如果没有被添加过就要判断到底应该添加到哪个候选关键帧列表（回环候选关键帧和融合候选关键帧列表）中，已经添加过的就进行标记。
// Step 3 : 用小组得分排序得到top3总分里最高分的关键帧,作为候选帧
// 统计以组为单位的累计相似度和组内相似度最高的关键帧, 每个pair为<小组总相似度,组内相似度最高的关键帧指针>
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;
}
// cout << "Amount of candidates: " << lAccScoreAndMatch.size() << endl;
// 按相似度从大到小排序
lAccScoreAndMatch.sort(compFirst);
// 最后返回的变量, 记录回环的候选帧
vpLoopCand.reserve(nNumCandidates);
// 最后返回的变量, 记录融合候选帧
vpMergeCand.reserve(nNumCandidates);
// 避免重复添加
set<KeyFrame *> spAlreadyAddedKF;
// cout << "Candidates in score order " << endl;
// for(list<pair<float,KeyFrame*> >::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++;
}
}3.完整的代码解析
/*
找到N个融合候选N个回环候选
pKF 当前关键帧(我们要寻找这个关键帧的回环候选帧和融合候选帧)
vpLoopCand 记录找到的回环候选关键帧
vpMergeCand 记录找到的融合候选关键帧
nNumCandidates 期望的候选数目,即回环和候选分别应该有多少个
*/
void KeyFrameDatabase::DetectNBestCandidates(KeyFrame *pKF, vector<KeyFrame *> &vpLoopCand, vector<KeyFrame *> &vpMergeCand, int nNumCandidates)
{
// Step 1统计与当前关键帧有相同单词的关键帧
list<KeyFrame *> lKFsSharingWords;
// set<KeyFrame*> 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);
}*/
}
}
}
// 如果没有有公共单词的关键帧,直接返回
if (lKFsSharingWords.empty())
return;
// Only compare against those keyframes that share enough words
// Step 2 统计所有候选帧中与当前关键帧的公共单词数最多的单词数maxCommonWords,并筛选
int maxCommonWords = 0;
for (list<KeyFrame *>::iterator lit = lKFsSharingWords.begin(), lend = lKFsSharingWords.end(); lit != lend; lit++)
{
if ((*lit)->mnPlaceRecognitionWords > maxCommonWords)
maxCommonWords = (*lit)->mnPlaceRecognitionWords;
}
// 取0.8倍为阀值
int minCommonWords = maxCommonWords * 0.8f;
// 这里的pair是 <相似度,候选帧的指针> : 记录所有大于minCommonWords的候选帧与当前关键帧的相似度
list<pair<float, KeyFrame *>> lScoreAndMatch;
// 只是个统计变量,貌似没有用到
int nscores = 0;
// Compute similarity score.
// 对所有大于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));
}
}
// 如果为空,直接返回,表示没有符合上述条件的关键帧
if (lScoreAndMatch.empty())
return;
// Step 3 : 用小组得分排序得到top3总分里最高分的关键帧,作为候选帧
// 统计以组为单位的累计相似度和组内相似度最高的关键帧, 每个pair为<小组总相似度,组内相似度最高的关键帧指针>
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;
}
// cout << "Amount of candidates: " << lAccScoreAndMatch.size() << endl;
// 按相似度从大到小排序
lAccScoreAndMatch.sort(compFirst);
// 最后返回的变量, 记录回环的候选帧
vpLoopCand.reserve(nNumCandidates);
// 最后返回的变量, 记录融合候选帧
vpMergeCand.reserve(nNumCandidates);
// 避免重复添加
set<KeyFrame *> spAlreadyAddedKF;
// cout << "Candidates in score order " << endl;
// for(list<pair<float,KeyFrame*> >::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++;
}
}结束语
以上就是我学习到的内容，如果对您有帮助请多多支持我，如果哪里有问题欢迎大家在评论区积极讨论，我看到会及时回复。
标签：候选,关键帧,pKFi,lit,++,KeyFrameDatabase,单词 From： https://blog.csdn.net/2301_76831056/article/details/144911365