计算候选闭环关键帧
我们一直在说，前端里程计存在累积误差的问题，长时间运行建出来的地图误差会很大。后端优化则是消除累计误差的一个主要手段。 它要求我们建图的时候走一个环路，过一段时间要回到一个之前到过的地方。这样后端就可以把当前的位姿估计与历史的位姿建立起联系，从而构建一个带有环路的位姿图。 之后就是构建优化问题迭代求解了。
要想进行后端优化，首先 SLAM 系统要能够意识到自己回到了原来的地方，也就是说它要能够检测出路径的闭环。这个过程和我们人类在沙漠里容易迷路的道理类似。 走在沙漠里，哪儿跟哪儿都一样，很难说是不是踩到了自己的脚印。自身的定位都不准，就更不要说建图了。如果走在陌生的城市里，我很容易根据周围的环境判定自己是不是曾今来过。 但要是认错了地方，我们仍然会迷路的。所以一个误检率较低的闭环检测功能，对于需要长时间运行、大场景构建的应用而言是很重要的。
后端闭环检测的需求与我们在前端讨论的重定位需求是一致的， 都是从历史的关键帧中搜出一个和当前帧关系比较大的来重新估计相机的位姿。只是此时，后端的主要目的是提高系统的定位和建图精度，没有实时性的要求。 我们在后台可以慢慢算，多方比较之后选出好几个候选关键帧，综合地修正相机位姿。 本文中，我们将详细介绍 ORB-SLAM2 中如何根据一致共视组(ConsistentGroup)搜索候选闭环关键帧。
1. 筛选方法简述
一个很朴素的想法，如果回到了曾今到过的地方，那么应该能够在历史的关键帧中找到一个与当前帧极度相似的关键帧。所以我们就需要有个判据来评价两个关键帧的相似度，然后选择一个阈值， 把所有相似度超过该阈值的关键帧都认为是潜在的闭环关键帧。由于错误的闭环将给系统引入很大的偏差，所以还需要对这些潜在的闭环关键帧进行较为严格的筛选，降低误检率。
ORB-SLAM2 采用词袋模型计算帧之间的相似度。它对一堆图片通过 K-means 的方法逐层训练得到一颗词汇树， 其叶子结点被称为单词，可能对应着多种特征点。对于每帧图片都会根据这颗词汇树计算一个词袋向量，描述图像中出现的单词id及其权重。如果两帧图像很相似，那么它们将具有很多相同的词汇， 表现的效果是根据词袋向量计算的距离很接近。
因为存在很多不确定的因素，所以我们不能设定一个固定的相似度阈值。在 LoopClosing 中，在共视图上计算当前帧与所有邻接关键帧的相似度，取其中最低的相似度作为阈值\(s_{min}\)。 关键帧的生成并不是孤立的，它不仅会继承上一帧中匹配的地图点， 还会在 LocalMapping 中根据局部地图生成新地图点。 所以在LoopClosing中当前关键帧会与最近一段时间生成的关键帧有共视关系。如果闭环发生了，对应的关键帧的相似度应当不低于\(s_{min}\)。
为了降低误检率，只有连续多次通过上述的判定条件的关键帧被认定是候选的闭环关键帧。同样因为一些不确定的因素存在，很可能找不到一个关键帧能够满足这个连续多次通过的判定。 ORB-SLAM2 引入了一致共视组(ConsistentGroup)，把一个关键帧及其在共视图中的邻接关键帧一起称为一个组(Group)（包含关键帧本身）。 如果一个Group连续三次有关键帧通过了上述相似度判定，我们就认为构建该Group的关键帧通过了连续性判定，对应的关键帧被称为闭环关键帧。
2. 源码解析
LoopClosing 在其成员函数 DetectLoop() 中完成了闭环候选帧的筛选工作。下面是该函数的代码片段，一开始先从队列 mlpLoopKeyFrameQueue 中取出一个关键帧。 这和 LocalMapping 处理新关键帧的套路是一样的。 LoopClosing 通过成员函数 InsertKeyFrame() 接收新的关键帧放置到缓存队列中，然后在 DetectLoop() 中消费。 通常函数InsertKeyFrame()是在 LOCAL_MAPPING 线程中调用的，所以需要一个互斥量 mMutexLoopQueue 来保护公共资源。
bool LoopClosing::DetectLoop() {
{
unique_lock<mutex> lock(mMutexLoopQueue);
mpCurrentKF = mlpLoopKeyFrameQueue.front();
mlpLoopKeyFrameQueue.pop_front();
// Avoid that a keyframe can be erased while it is being process by this thread
mpCurrentKF->SetNotErase();
}
由于 LocalMapping 中会对关键帧进行筛选，剔除掉那些冗余的关键帧。 为了防止本线程中，正在进行闭环检测的关键帧被剔除掉，上面代码的第7行调用了类 KeyFrame 的接口 SetNotErase 标记一下。由于相机的运动和空间的连续性，一旦产生了闭环， 那么一段时间之内很容易再次检测到闭环。如下面的代码片段所示，为了防止位姿图过于稠密，DetectLoop 要求前后两次闭环之间要相差十帧。
if(mpCurrentKF->mnId < (mLastLoopKFid+10)) {
mpKeyFrameDB->add(mpCurrentKF);
mpCurrentKF->SetErase();
return false;
}
然后，通过关键帧的接口 GetVectorCovisibleKeyFrames() 获取当前帧在共视图上的一级邻接，并用 CurrentBowVec 记录当前帧的词袋向量。 在一个 for 循环里通过词袋模型计算当前帧与一级邻接的相似度，统计其中的最小值。
const vector<KeyFrame*> vpConnectedKeyFrames = mpCurrentKF->GetVectorCovisibleKeyFrames();
const DBoW2::BowVector & CurrentBowVec = mpCurrentKF->mBowVec;
float minScore = 1;
for(size_t i = 0; i < vpConnectedKeyFrames.size(); i++) {
KeyFrame* pKF = vpConnectedKeyFrames[i];
if (pKF->isBad()) continue;
const DBoW2::BowVector & BowVec = pKF->mBowVec;
float score = mpORBVocabulary->score(CurrentBowVec, BowVec);
if (score < minScore)
minScore = score;
}
接着，根据 minScore 从数据库中查询除了当前帧及其一级邻接关键帧之外关键帧作为候选。要是没找着就不折腾了。
vector<KeyFrame*> vpCandidateKFs = mpKeyFrameDB->DetectLoopCandidates(mpCurrentKF, minScore);
if(vpCandidateKFs.empty()) {
mpKeyFrameDB->add(mpCurrentKF);
mvConsistentGroups.clear();
mpCurrentKF->SetErase();
return false;
}
下面才是本文的重点，根据一致共视组(ConsistentGroup)对候选关键帧进行筛选。通过筛选的关键帧将被记录到容器 mvpEnoughConsistentCandidates，所以要先清空它。 临时构建的容器 vCurrentConsistentGroups 用于记录当前候选帧构成的组。LoopClosing 的成员容器 mvConsistentGroups 中记录的是上次的一致共视组。 这里创建一个 bool 类型的容器与之一一对应，是为了防止重复记录组。
mvpEnoughConsistentCandidates.clear();
vector<ConsistentGroup> vCurrentConsistentGroups;
vector<bool> vbConsistentGroup(mvConsistentGroups.size(),false);
在接下来的 for 循环中遍历所有的候选关键帧，计算它们的一致共视组(ConsistentGroup)，包括其以及邻接(第38行)，也包含它们自己(第39行)。 临时变量 bEnoughConsistent 和 bConsistentForSomeGroup 将分别用于标记spCandidateGroup是否具有足够的连续性(连续3次通过判定)，或者是一个新的Group。
for (size_t i = 0; i < vpCandidateKFs.size(); i++) {
KeyFrame* pCandidateKF = vpCandidateKFs[i];
set<KeyFrame*> spCandidateGroup = pCandidateKF->GetConnectedKeyFrames();
spCandidateGroup.insert(pCandidateKF);
bool bEnoughConsistent = false;
bool bConsistentForSomeGroup = false;
准备好必要数据之后，在下面的 for 循环中遍历上次的一致共视组。mvConsistentGroups 中的元素是一个二元组(std::pair)， 记录着一致共视组中的关键帧集合以及连续通过判定的次数。
for(size_t iG = 0; iG < mvConsistentGroups.size(); iG++) {
set<KeyFrame*> sPreviousGroup = mvConsistentGroups[iG].first;
检查组的一致性，并设置相应的标记。
bool bConsistent = false;
for(auto sit = spCandidateGroup.begin(); sit != spCandidateGroup.end();sit++) {
if (sPreviousGroup.count(*sit)) {
bConsistent=true;
bConsistentForSomeGroup=true;
break;
} }
如果上次的一致共视组与当前正在考察的组存在一致关系，我们记录下当前考察的组到容器 vCurrentConsistentGroups 中，同时增加连续性计数。 当连续性计数超出了设定的阈值(3)，则认为该组对应的关键帧是一个不错的候选帧，记录到容器 mvpEnoughConsistentCandidates 中。
if(bConsistent) {
int nPreviousConsistency = mvConsistentGroups[iG].second;
int nCurrentConsistency = nPreviousConsistency + 1;
if (!vbConsistentGroup[iG]) {
ConsistentGroup cg = make_pair(spCandidateGroup,nCurrentConsistency);
vCurrentConsistentGroups.push_back(cg);
vbConsistentGroup[iG]=true; //this avoid to include the same group more than once
}
if (nCurrentConsistency >= mnCovisibilityConsistencyTh && !bEnoughConsistent) {
mvpEnoughConsistentCandidates.push_back(pCandidateKF);
bEnoughConsistent=true; //this avoid to insert the same candidate more than once
}
} }
如果遍历完上次的一致共视组，没发现谁与当前考察的组具有一致关系，就认为遇到了一个新的组，记录到vCurrentConsistentGroups 中，并记连续性计数为0。 完成所有的候选一致共视组的遍历之后，更新mvConsistentGroups为vCurrentConsistentGroups。
if (!bConsistentForSomeGroup) {
ConsistentGroup cg = make_pair(spCandidateGroup,0);
vCurrentConsistentGroups.push_back(cg);
} }
mvConsistentGroups = vCurrentConsistentGroups;
最后根据容器mvpEnoughConsistentCandidates的大小，即候选闭环关键帧的数量，判定是否可能有闭环，并返回。
mpKeyFrameDB->add(mpCurrentKF);
if (mvpEnoughConsistentCandidates.empty()) {
mpCurrentKF->SetErase();
return false;
} else {
return true;
} }
3. 完
LoopClosing 通过词袋模型评价两个关键帧之间的相似性，取当前帧与其一级邻接关键帧的最低相似度，作为初步筛选候选闭环关键帧的阈值。为了降低闭环的误检率， ORB-SLAM2 还根据一致共视组(ConsistentGroup)的概念对这些候选帧进行筛选，保留下那些与当前帧具有一定相似度，并且连续三帧都符合一致共视条件的关键帧。