DetectRelocalizationCandidates(Frame *F, Map* pMap)是ORB-SLAM3中负责为当前帧寻找潜在重定位候选关键帧的核心函数。它的实现逻辑主要涉及基于词袋向量的相似性检索以及后续对候选关键帧的筛选与排序。由于这个函数通常在系统跟踪失败lost时被调用它的主要任务是从关键帧数据库中快速找出与当前帧在视觉上最相似的几个关键帧为后续精确的位姿估计如MLPnP提供初始的参考目标。这个函数的实现逻辑主要分为以下几步。 核心流程初步检索查找具有公共单词的关键帧函数首先会利用倒排索引Inverted Index进行快速检索具体步骤如下遍历当前帧的词袋向量mBowVec中的每一个视觉单词Word。在关键帧数据库的倒排文件mvInvertedFile中找到所有包含该单词的关键帧列表。统计这些关键帧与当前帧的公共单词数量并记录下来。过滤与筛选第一步检索出的候选关键帧数量可能很多因此需要进行筛选排除掉那些相关性较低的关键帧。其具体策略在不同版本的ORB-SLAM如2和3中略有不同基于最佳得分的阈值过滤通常会先计算所有候选关键帧与当前帧的相似度得分并找到其中的最高分。然后只有得分高于最高分一定比例例如75%的关键帧才会被保留下来。利用共视关系聚合关键步骤由于具有重叠视野的关键帧视觉上会很相似如果它们都被选为候选会导致候选集过于冗余。因此函数会利用共视图Covisibility Graph将这些相互关联的关键帧进行分组聚合。在每个组中只选择得分最高的关键帧作为该组的代表以此来降低候选集的冗余度并提升重定位的效率。返回候选关键帧列表经过以上步骤的层层筛选最终会生成一个精简的、高质量的候选关键帧列表vpCandidateKFs并将其返回给调用者通常是Tracking线程的Relocalization()函数。 关于Map* pMap参数你提到的函数签名中包含了Map* pMap参数。在ORB-SLAM3引入多地图Atlas系统后这个参数的作用在于指定要在哪一个地图中检索候选关键帧而不再像ORB-SLAM2那样只在全局唯一的地图中检索。在Relocalization()函数中调用此函数时传入的参数通常是当前活跃地图mpAtlas-GetCurrentMap()。 总结总的来说DetectRelocalizationCandidates这个函数的工作机制可以概括为先将当前帧的“视觉摘要”词袋向量作为一个“查询”在数据库的“索引”倒排文件中进行快速查找再通过一系列筛选和聚合策略最终返回一个精简的、代表不同视觉区域的“最佳匹配”候选关键帧列表。步骤核心操作目的1. 初步检索遍历当前帧的每个视觉单词通过倒排索引找出包含这些单词的所有关键帧。快速缩小搜索范围定位所有“可能”相似的关键帧。2. 过滤与聚合根据相似度得分进行阈值过滤并利用共视图对候选关键帧进行分组聚合保留每组中得分最高的帧。剔除弱相关帧并减少由高重叠度带来的候选冗余提升后续处理效率。3. 结果返回将筛选后的关键帧列表vpCandidateKFs返回给调用函数。为后续的特征匹配和MLPnP位姿求解提供输入。补充不同的版本以下是ORB-SLAM3中DetectRelocalizationCandidates函数的完整代码实现和详细解读。 完整代码cpp// KeyFrameDatabase.cc vectorKeyFrame* KeyFrameDatabase::DetectRelocalizationCandidates(Frame *F) { // 用于存储与当前帧共享单词的关键帧列表 listKeyFrame* lKFsSharingWords; // 步骤1: 通过倒排索引检索候选关键帧 { 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(), lendlKFs.end(); lit ! lend; lit) { KeyFrame* pKF *lit; if(pKF-isBad()) continue; // 避免重复添加同一个关键帧 if(find(lKFsSharingWords.begin(), lKFsSharingWords.end(), pKF) lKFsSharingWords.end()) { lKFsSharingWords.push_back(pKF); } } } } if(lKFsSharingWords.empty()) return vectorKeyFrame*(); // 步骤2: 计算并筛选相似度得分 // 存储相似度得分, 关键帧指针的配对 listpairfloat, KeyFrame* lScoreAndMatch; for(listKeyFrame*::iterator litlKFsSharingWords.begin(), lendlKFsSharingWords.end(); lit ! lend; lit) { KeyFrame* pKF *lit; // 计算当前帧与候选关键帧的相似度得分 float si mpVoc-score(F-mBowVec, pKF-mBowVec); // 按得分降序存储 lScoreAndMatch.push_back(make_pair(si, pKF)); } lScoreAndMatch.sort(KeyFrameDatabase::sortScoreDesc); // 获取最高得分 float bestScore lScoreAndMatch.begin()-first; // 保留得分 最高分 * 0.75 的关键帧 float th 0.75f * bestScore; listpairfloat, KeyFrame* lAccScoreAndMatch; // 存储每个共视图组的总得分 mapKeyFrame*, float mpAccScore; for(listpairfloat, KeyFrame* ::iterator itlScoreAndMatch.begin(), itendlScoreAndMatch.end(); it ! itend; it) { float si it-first; KeyFrame* pKF it-second; // 如果得分低于阈值后续帧得分更低直接跳出 if(si th) break; // 步骤3: 基于共视图进行分组聚合 // 遍历该关键帧的共视关键帧权重 80 for(mapKeyFrame*, int::const_iterator vitpKF-GetConnectedKeyFrames().begin(), vendpKF-GetConnectedKeyFrames().end(); vit ! vend; vit) { KeyFrame* pKF2 vit-first; if(pKF2-isBad()) continue; // 累加该共视组的得分 if(mpAccScore.find(pKF2) mpAccScore.end()) { mpAccScore[pKF2] si; lAccScoreAndMatch.push_back(make_pair(si, pKF2)); } else { mpAccScore[pKF2] si; } } } // 步骤4: 排序并提取最终候选 // 按累积得分降序排序 lAccScoreAndMatch.sort(KeyFrameDatabase::sortScoreDesc); vectorKeyFrame* vpRelocCandidates; // 取累积得分最高的前3个作为最终候选 int nTop min(3, (int)lAccScoreAndMatch.size()); for(listpairfloat, KeyFrame* ::iterator itlAccScoreAndMatch.begin(), itendlAccScoreAndMatch.end(); it ! itend nTop 0; it, nTop--) { vpRelocCandidates.push_back(it-second); } return vpRelocCandidates; } 逐步骤详细解读步骤1倒排索引检索cppfor (auto vit F-mBowVec.begin(); vit ! F-mBowVec.end(); vit) { listKeyFrame* lKFs mvInvertedFile[vit-first]; // 将包含该单词的所有关键帧加入候选列表 }这里利用倒排文件Inverted File进行快速检索。倒排文件的本质是“单词ID → 包含该单词的所有关键帧列表”的映射。原理遍历当前帧的每个视觉单词从倒排文件中找出所有包含该单词的关键帧。这些关键帧就构成了初始候选集。步骤2相似度得分计算与过滤cppfloat si mpVoc-score(F-mBowVec, pKF-mBowVec);使用词袋向量计算当前帧与每个候选关键帧的相似度得分。得分反映了两帧在视觉内容上的重叠程度。过滤策略取最高分bestScore只保留得分 0.75 * bestScore的关键帧。这一步剔除了与当前帧差异较大的帧缩小候选范围。这个75%的阈值在ORB-SLAM中被广泛使用是经过实验验证的有效参数。步骤3共视图分组聚合——核心创新这是该函数最精妙的设计。由于关键帧之间存在视觉重叠即多个关键帧可能看到同一片区域如果直接返回所有高分帧会导致候选集高度冗余。解决方案cpp// 遍历该关键帧的所有共视关键帧 for (auto vit pKF-GetConnectedKeyFrames().begin(); vit ! vend; vit) { KeyFrame* pKF2 vit-first; mpAccScore[pKF2] si; // 累加该组的得分 }逻辑解释一个关键帧pKF在共视图中有许多相连的关键帧它们共享地图点当前帧与pKF相似而pKF又与其共视关键帧pKF2相似因此pKF2也应当被视为有效候选将pKF的得分累加到它的每个共视关键帧上形成累积得分最终按累积得分排序取前3名作为最终候选这种分组策略比DBoW2原始的“按时间邻近分组”更加合理因为它利用了真实的共视关系而非时间关系。步骤4返回精简的候选列表最终只返回累积得分最高的3个关键帧作为候选。这个数量是在精度和效率之间的平衡——候选太少可能错过正确匹配太多则会增加后续计算的负担。 总结步骤核心操作目的1. 倒排索引检索遍历当前帧的每个单词从mvInvertedFile中取出包含该单词的关键帧快速缩小搜索范围找出所有可能相似的帧2. 得分计算与过滤计算词袋相似度保留得分 最高分×75%的帧剔除弱相关帧3. 共视图分组聚合将得分累加到每个关键帧的共视关键帧上减少视觉重叠带来的冗余候选提高可靠性4. 返回最终候选取累积得分最高的3个关键帧为后续特征匹配和位姿估计提供精简的输入这个函数的核心思想是用倒排索引做粗筛用词袋得分做精筛用共视图做聚合最终输出一个高质量的精简候选列表。ORB-SLAM3 版本2在 ORB-SLAM3 中KeyFrameDatabase::DetectRelocalizationCandidates(Frame *F, Map* pMap)函数的核心作用是在系统跟踪丢失时从地图中快速、准确地找出与当前帧最像的几个关键帧为重定位提供“候选”。与ORB-SLAM2相比ORB-SLAM3版本的函数主要变化在于引入了多地图Atlas系统的支持将检索范围限制在指定的pMap内。其算法流程是一个层层筛选的精巧设计从全局粗筛到得分过滤再到共视分组聚合最终输出一个高质量的精简候选列表。⚙️ 完整代码 (基于ORB-SLAM3源码)由于源码长度限制此处提供完整代码并整合了关键注释该函数位于KeyFrameDatabase.cc文件中cppvectorKeyFrame* KeyFrameDatabase::DetectRelocalizationCandidates(Frame *F, Map* pMap) { // 存储所有与当前帧有公共单词的关键帧 listKeyFrame* lKFsSharingWords; // 步骤1: 利用倒排索引进行全局粗筛 { 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(), lendlKFs.end(); lit!lend; lit) { KeyFrame* pKFi *lit; // 多地图核心只考虑属于指定地图 pMap 的关键帧 if(pKFi-GetMap() pMap) { if(pKFi-mnRelocQuery ! F-mnId) { pKFi-mnRelocWords 0; pKFi-mnRelocQuery F-mnId; lKFsSharingWords.push_back(pKFi); } pKFi-mnRelocWords; // 累计公共单词数 } } } } if(lKFsSharingWords.empty()) return vectorKeyFrame*(); // 步骤2: 基于公共单词数进行初筛 // 找出最大公共单词数并设定一个比例阈值(如80%)剔除公共单词过少的关键帧 int maxCommonWords 0; for(listKeyFrame*::iterator litlKFsSharingWords.begin(); lit!lKFsSharingWords.end(); lit) if((*lit)-mnRelocWords maxCommonWords) maxCommonWords (*lit)-mnRelocWords; int minCommonWords maxCommonWords * 0.8f; // 计算通过初筛的关键帧的相似度得分 listpairfloat, KeyFrame* lScoreAndMatch; for(listKeyFrame*::iterator litlKFsSharingWords.begin(); lit!lKFsSharingWords.end(); lit) { if((*lit)-mnRelocWords minCommonWords) { float si mpVoc-score(F-mBowVec, (*lit)-mBowVec); lScoreAndMatch.push_back(make_pair(si, *lit)); } } if(lScoreAndMatch.empty()) return vectorKeyFrame*(); // 步骤3: 利用共视图进行分组聚合 // 这个步骤是算法的精髓用来解决候选帧彼此之间视觉重叠过高的问题 listpairfloat, KeyFrame* lAccScoreAndMatch; float bestAccScore 0; for(listpairfloat, KeyFrame* ::iterator itlScoreAndMatch.begin(); it!lScoreAndMatch.end(); it) { KeyFrame* pKFi it-second; // 取出与该候选帧共视程度最高的10个关键帧 vectorKeyFrame* vpNeighs pKFi-GetBestCovisibilityKeyFrames(10); float bestScore it-first; float accScore bestScore; KeyFrame* pBestKF pKFi; // 将候选帧的得分累加到它自己和它的共视关键帧上 for(vectorKeyFrame*::iterator vitvpNeighs.begin(); vit!vpNeighs.end(); vit) { KeyFrame* pKF2 *vit; if(pKF2-mnRelocQuery ! F-mnId || pKF2-isBad()) continue; accScore pKF2-mRelocScore; if(pKF2-mRelocScore bestScore) { pBestKF pKF2; bestScore pKF2-mRelocScore; } } lAccScoreAndMatch.push_back(make_pair(accScore, pBestKF)); if(accScore bestAccScore) bestAccScore accScore; } // 步骤4: 提取最终候选 // 返回累积得分超过最佳得分一定比例(如75%)的关键帧并用set去重 float minScoreToRetain 0.75f * bestAccScore; setKeyFrame* spAlreadyAddedKF; vectorKeyFrame* vpRelocCandidates; for(listpairfloat, KeyFrame* ::iterator itlAccScoreAndMatch.begin(); it!lAccScoreAndMatch.end(); it) { if(it-first minScoreToRetain) { KeyFrame* pKFi it-second; if(!spAlreadyAddedKF.count(pKFi)) { vpRelocCandidates.push_back(pKFi); spAlreadyAddedKF.insert(pKFi); } } } return vpRelocCandidates; } 核心步骤详解1. 全局粗筛倒排索引快速定位这一步是效率的保障。系统使用倒排文件Inverted File它就像一个“单词 → 包含它的所有关键帧”的大词典。函数遍历当前帧的每个视觉单词从这个“大词典”里瞬间就能捞出所有包含该单词的关键帧。此函数最关键的改动在于捞取时会检查pKFi-GetMap() pMap确保只从指定的地图中寻找候选这是多地图系统的要求。2. 得分计算与过滤从倒排索引捞出的帧数量可能很大需要进一步筛选。首先系统会统计每个候选帧与当前帧的公共单词数并只保留那些公共单词数超过最高值80%的帧。然后为保留下来的每一帧计算一个精确的相似度得分si mpVoc-score这个得分反映了两个词袋向量的相似程度得分越高意味着两帧在视觉上越接近。3. 共视图分组聚合——算法的核心创新这是该函数最精妙的设计。如果只按单个帧的得分排序最终候选列表里可能会挤满来自同一小片区域视觉上重叠很高的关键帧造成冗余。因此算法引入了共视图Covisibility Graph进行聚合。具体做法是对于每个高分候选帧取出它在共视图中关系最紧密的10个邻居关键帧然后将该候选帧的得分累加到它自己和这些邻居身上形成一个累积得分。这就像一个“拉票”机制一个候选帧得分高它所在的视觉区域的所有关键帧都会受益。最后用累积得分最高的帧代表整个区域这极大地提升了候选帧的多样性和鲁棒性。4. 输出精简候选列表最后一步是从累积得分中筛选出得分超过最佳累积得分75%的关键帧并利用std::set去重得到最终的候选列表。这个列表通常只有3-5个关键帧它们代表了与当前帧视觉相似且空间分布比较分散的几个不同区域为后续的特征匹配和MLPnP位姿求解提供了高质量的输入。 总结DetectRelocalizationCandidates通过倒排索引 → 得分过滤 → 共视聚合的流程为ORB-SLAM3的重定位提供了高效且精准的“检索”能力其多地图支持是该函数在ORB-SLAM3中的重要演进。