- ORB SLAM3系统初始化
- ORB SLAM3 构建Frame
- ORB_SLAM3 单目初始化
- ORB_SLAM3 双目匹配
- ORB_SLAM3_IMU预积分理论推导(预积分项)
- ORB_SLAM3_IMU预积分理论推导(噪声分析)
- ORB_SLAM3_IMU预积分理论推导(更新)
- ORB_SLAM3_IMU预积分理论推导(残差)
- ORB_SLAM3_优化方法 Pose优化
- ORB_SLAM3 闭环检测
DetectNBestCandidates
DetectNBestCandidates主要是用来闭环检测得到候选关键帧，和闭环有关的变量如下：
- mnPlaceRecognitionQuery：与其有公共单词的关键帧
- mnPlaceRecognitionWords：与其有多少公共单词数
- mPlaceRecognitionScore：与其的词袋相似度
1.闭环检测示意图
2.步骤
- 通过
词袋的BowVec找出与当前关键帧具有相同单词的所有关键帧，并统计对应的相同单词数量，但不包括对应的共视关键帧spConnectedKF，得到lKFsSharingWords。
// 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);
}*/
}
}
}
- 计算
当前关键帧与具有一定公共单词数的关键帧的BowVec的分数，得到lScoreAndMatch。- 用
最大公共单词数maxCommonWords的0.8(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));
}
}
- 单个关键帧相似可能不太准确，那么就
组队，对lScoreAndMatch中的关键帧，将候选关键帧周围的共视关键帧(需要和当前关键帧有公共的单词，且单词数超过minCommonWords)拉入，统计每一组的分数，并用每一组分数最大的那帧代表该组，选为候选关键帧
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;
}
- 对
lScoreAndMatch安装分数排序- 如果和当前关键帧是同一张地图，就添加到闭环候选帧
vpLoopBowCand中 - 如果和当前关键帧是不同张地图，就添加到合并候选帧
vpMergeBowCand中
- 如果和当前关键帧是同一张地图，就添加到闭环候选帧
// 按相似度从大到小排序
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++;
}
![基于粒子群优化算法的配电网光伏储能双层优化配置模型[IEEE33节点]（选址定容）(Matlab代码实现)](https://img-blog.csdnimg.cn/d478c2c49d894ac3b9740b19a7e998ac.png)
![[论文阅读] (31)李沐老师视频学习——4.研究的艺术·理由、论据和担保](https://img-blog.csdnimg.cn/4d4a9a2edc814a68bfb40eac06927424.png#pic_center)
![[创业之路-73] ：如何判断一个公司或团队是熵减：凝聚力强、上下一心，还是，熵增：一盘散沙、乌合之众？](https://img-blog.csdnimg.cn/2f6b4562c3ef468e8e327a32df5ef5af.png)
