词袋模型与重定位
我们在分析轨迹跟踪的总体过程时， 看到轨迹跟踪器有三种估计相机位姿的方式，上一节中我们分析了其中的匀速运动模型。 此外，还有根据关键帧进行定位和词袋模型。根据关键帧进行定位可以说是介于匀速运动模型和词袋模型之间一种定位方式，其逻辑比较简单，就是跟参考关键帧进行特征点匹配优化相机位姿。 不再详细介绍了。本文将详细分析通过词袋模型进行重定位的过程。
有时相机可能运动过快，导致里程计跟丢了，此时需要快速地找到一个相似的帧作参考来估计相机位姿，我们称这一过程为重定位Relocalization。 在不考虑计算时间的情况下，我们完全可以拿着当前帧跟历史的帧逐一进行特征点匹配，找到匹配点对最多的那个作为新的参考帧。 随着系统运行时间的增长，这种大力出奇迹的方法就会显得越来越笨拙，前端里程计的实时性根本顾不上。词袋模型的引入可以极大的加快重定位的计算速度。
本文中先了解一下词袋模型的基本思想，再结合代码分析函数 Relocalization 的实现过程。 然后结合 DBoW2 的论文，看看 ORB-SLAM2 所用的词袋模型。 最后分析 ORB-SLAM2 如何根据词袋计算候选关键帧。关于词袋模型更详细的内容，我们将放到附录中介绍。
1. 初看词袋模型
重定位的本质还是一个字——搜。从历史的关键帧中搜出一个可以拿来重新估计相机位姿的关键帧。为了加快搜索的速度，最直接的想法就是降低搜索空间。 所能做的事情无非是，对帧进行编码，把无序变成有序。然后建立编码之间的查找映射关系，忽略掉那些明显与当前帧没有关系的帧。 此外，我们还可以适当的降低精度来换取速度。因为搜索的目标是，找到一个可用的关键帧，不是那个匹配度最佳的。而这两个加速的套路，都是词袋模型所擅长的。
最早词袋模型(Bag of Words, BoW)是文本检索领域中经典的分析方法之一，采用统计关键词出现频率的方法，对文本进行向量表示进而达到分类检索的目的。 比如下面这两个新闻的标题，如果计算机知道梅西是一个足球明星，詹姆斯是一个篮球明星的话。在不理解人话的情况下，它也可以做出判断，认定左边是一篇关于足球的新闻，右边是篮球的。
 |
 |
词袋模型的大体意思就是把一段文本中出现的单词都装到一个袋子里，不管它们的语法规则、语言逻辑。然后拿一个表单，来数袋子里的单词，对应到表单中的元素数量。 还是针对上面这两个新闻的标题，现在我们有一个表单 \(\{ 足球明星、足球俱乐部、足球赛事、篮球明星、篮球俱乐部、篮球赛事 \}\)。 左侧的标题里一共出现了梅西、斯卡洛尼两个足球明星，没提到俱乐部和赛事，对应有计数 \(\{ 2, 0, 0, 0, 0, 0 \}\)。右侧的标题里出现了詹姆斯一个篮球明星， 湖人和灰熊两个俱乐部，没有提到赛事，有计数\(\{ 0, 0, 0, 1, 2, 0\}\)。我们不妨把这两个计数看做是两个6维的向量，那么我们就可以有各种手段来评估这两个向量之间的距离。 只要能够计算距离，那么什么聚类呀、分类器呀等等机器学习的方法就都可以拿来鼓捣了。本质上，在提取文本的特征，构建特征向量。
看到词袋模型在文本分类中这么好用， Csurka 等人在 2004 年的时候把它引入到视觉图像分类中了。 在2012年深度学习的方法取得突破性进展以前，词袋模型可以说是最好的图像分类方法了。 ORB-SLAM2 选用的 DBoW2，需要离线的训练一个字典文件，把 BRIEF 描述子转化成单词，保存在一个树形的结构中。 使用时先对图像提取 ORB 特征点，然后统计其中各个 BRIEF 描述子对应单词出现的频率，构建图像总体的特征向量。根据这个特征向量，我们就可以对图像进行分类。
在 ORB-SLAM2 看来，相机位姿变化不大时，采集的图像应该很像，并且存在很多匹配的特征点，表现在词袋模型上就是，它们的图像特征向量比较接近。 但是反过来如果两幅图的图像特征向量很接近，并不意味着采集它们时相机的位姿差异不大。所以，ORB-SLAM2 的重定位过程需要先通过词袋模型得到一个候选集， 再逐个进行特征点匹配，找到候选集中匹配点最多的那个作为新的参考帧。
2. 重定位总体过程
相比于我们一开始提到的暴力枚举的方法而言，词袋模型减少了很多搜索空间。现在，我们来具体看一下完成重定位任务的函数 Relocalization，了解一下它的总体过程。 下面是该函数的代码片段，在函数一开始，先通过类 Frame 的接口 ComputeBoW 计算当前帧 mCurrentFrame 的特征向量。然后根据词袋模型，通过关键帧数据库 mpKeyFrameDB 查询候选关键帧。
bool Tracking::Relocalization() {
mCurrentFrame.ComputeBoW();
vector<KeyFrame*> vpCandidateKFs = mpKeyFrameDB->DetectRelocalizationCandidates(&mCurrentFrame);
// 要是候选集是空的，就不要再扯了
if(vpCandidateKFs.empty())
return false;
然后定义了一些局部变量，用于后续筛选关键帧和地图点。
const int nKFs = vpCandidateKFs.size();
ORBmatcher matcher(0.75,true);
vector<PnPsolver*> vpPnPsolvers;
vpPnPsolvers.resize(nKFs);
vector<vector<MapPoint*>> vvpMapPointMatches;
vvpMapPointMatches.resize(nKFs);
vector<bool> vbDiscarded;
vbDiscarded.resize(nKFs);
左侧的代码片段中，nKFs 表示候选关键帧的数量，matcher 是后续筛选关键帧时需要用到的 ORB 特征点匹配器。 后面我们会看到，针对每个候选关键帧，都会构建一个 PnP 问题的求解器 PnPsolver，来根据匹配的3D地图点和当前帧特征点的2D像素坐标估计相机位姿。 这些 PnPSolver 都被保存在容器 vpPnPsolvers 中。我们会专门讨论 PnP 问题，常见的求解方法， 以及类 PnPSolver 的实现。 容器 vvpMapPointMatches 与各个候选关键帧一一对应，记录了它们与当前帧匹配的地图点列表，容器 vbDiscarded 也是与候选集中的元素一一对应的， 当某个关键帧因为匹配的特征点数量不足而被抛弃掉，就在 vbDiscarded 中将之即为 true。
int nCandidates=0;
for(int i = 0; i < nKFs; i++) {
KeyFrame *pKF = vpCandidateKFs[i];
if (pKF->isBad()) {
vbDiscarded[i] = true;
continue;
}
int nmatches = matcher.SearchByBoW(pKF,mCurrentFrame,vvpMapPointMatches[i]);
if (nmatches < 15) {
vbDiscarded[i] = true;
continue;
}
PnPsolver* pSolver = new PnPsolver(mCurrentFrame,vvpMapPointMatches[i]);
pSolver->SetRansacParameters(0.99,10,300,4,0.5,5.991);
vpPnPsolvers[i] = pSolver;
nCandidates++;
}
接下来遍历各个候选关键帧。局部变量 nCandidates 用于计数有效的候选关键帧数量。因为关键帧可能在 LOCAL MAPPING, LOOP CLOSING 两个线程中被剔除掉， 所以有一个接口 isBad 用于判定关键帧是否还能用。若不能用，就在 vbDiscarded 中标记下来。
然后调用 ORBmatcher 对象的 SearchByBoW 计算候选关键帧与当前帧的匹配点对。 该函数与我们在上一节中分析的 SearchByProjection 的功能类似， 只是通过词袋向量替代3D投影来限定特征点的搜索范围。如果匹配点对不足 15 个则抛弃对应的候选关键帧。
最后为通过筛选的候选关键帧构建 PnPSolver 对象，方便后续计算相机位姿。第28行中，传了一堆参数给函数 SetRansacParameters， 我们放到下一节讨论。
bool bMatch = false;
ORBmatcher matcher2(0.9,true);
while (nCandidates > 0 && !bMatch) {
for (int i = 0; i < nKFs; i++) {
if(vbDiscarded[i])
continue;
完成初次筛选之后，函数 Relocalization 又构造了一个特征点匹配器 matcher2，用于进一步根据 SearchByProjection 对候选关键帧进行过滤。 布尔型变量 bMatch 用于标记是否已经找到了一个合适的关键帧。
接着在两层循环中对候选关键帧进行筛选。内层的 for 循环完成位姿估计、特征点匹配、筛选。while 只关心当前是否还有候选帧(nCandidates > 0)，以及是否找到了关键帧(!bMatch)。
vector<bool> vbInliers; int nInliers; bool bNoMore;
PnPsolver* pSolver = vpPnPsolvers[i];
cv::Mat Tcw = pSolver->iterate(5,bNoMore,vbInliers,nInliers);
if (bNoMore || Tcw.empty()) {
vbDiscarded[i]=true;
nCandidates--;
}
对每个候选帧都通过 PnPsolver 迭代5次。如果求解器达到了最大迭代次数，仍然没有满足之前通过 SetRansacParameters 设定的迭代终止条件时，就会将变量 bNoMore 置为 true， 此时我们可以认为该候选帧不能够估计出当前帧的相机位姿，在41到44行中将之舍去。
求解器的每次迭代都会输出一个相机位姿 Tcw，同时在局部的容器 vbInliers 中记录下匹配的特征点是否为内点，并由 nInliers 记录内点数量。 我在第41行中增加了一个判定条件 Tcw.empty()。原来的代码是要在接下来的代码中判定 Tcw 非空才进行筛选操作。 我想，既然求解器都放弃了，给了一个空的 Tcw，干脆直接将之抛弃得了。主要是我不喜欢看到代码缩进了很多层。 如果求解器成功的给出了一个位姿估计，就先通过下面的代码片段记录下该位姿以及匹配的地图点。
Tcw.copyTo(mCurrentFrame.mTcw);
set<MapPoint*> sFound;
const int np = vbInliers.size();
for (int j = 0; j < np; j++) {
if(vbInliers[j]) {
mCurrentFrame.mvpMapPoints[j]=vvpMapPointMatches[i][j];
sFound.insert(vvpMapPointMatches[i][j]);
} else {
mCurrentFrame.mvpMapPoints[j]=NULL;
} }
int nGood = Optimizer::PoseOptimization(&mCurrentFrame);
if (nGood < 10)
continue;
for(int io = 0; io < mCurrentFrame.N; io++)
if(mCurrentFrame.mvbOutlier[io])
mCurrentFrame.mvpMapPoints[io]=static_cast<MapPoint*>(NULL);
然后通过 PoseOptimization 对估计的位姿进行图优化，它将返回优化后仍然匹配的地图点的数量。 如果优化之后，匹配点对不足 10 个，认为当前的位姿估计不够准确，需要在下一轮的最外层 while 循环中进一步优化。58到60行剔除掉那些被 PoseOptimization 抛弃的地图点。
如下面的代码片段所示，如果匹配点对不足50个，就通过刚刚构建的第二个特征点匹配器 matcher2 进行投影匹配，获得更多的匹配点对。局部变量 nadditional 记录了新增的匹配点数量。 这次调用的这个重载函数 SearchByProjection 的套路与我们在上一节中匹配当前帧与上一帧的特征点是一样的。不同之处在于，这里是当前帧和候选关键帧的匹配。 局部变量 sFound 是一个 std::set 的容器，记录了已经找到的匹配地图点。
如果新增的匹配地图点比较多的话，Relocalization 就再给候选关键帧两次机会，再次调用 SearchByProjection 和 PoseOptimization 继续搜索匹配的地图点并进行位姿优化。
if (nGood < 50) {
int nadditional = matcher2.SearchByProjection(mCurrentFrame,vpCandidateKFs[i],sFound,10,100);
if (nadditional + nGood >=50) {
nGood = Optimizer::PoseOptimization(&mCurrentFrame);
if (nGood > 30 && nGood < 50) {
sFound.clear();
for (int ip = 0; ip < mCurrentFrame.N; ip++)
if(mCurrentFrame.mvpMapPoints[ip])
sFound.insert(mCurrentFrame.mvpMapPoints[ip]);
nadditional = matcher2.SearchByProjection(mCurrentFrame,vpCandidateKFs[i],sFound,3,64);
// Final optimization
if (nGood + nadditional >= 50) {
nGood = Optimizer::PoseOptimization(&mCurrentFrame);
for(int io = 0; io < mCurrentFrame.N; io++)
if (mCurrentFrame.mvbOutlier[io])
mCurrentFrame.mvpMapPoints[io] = NULL;
}
} }
// If the pose is supported by enough inliers stop ransacs and continue
if (nGood >= 50) {
bMatch = true;
break;
} } }
如上面的代码片段所示，如果最后找到足够多的匹配点，就认为成功找到了可以重定位的参考关键帧。最后，我们根据局部变量 bMatch 的情况，或返回 false 表示重定位失败， 或返回 true 并记录下当前帧的 id 表示最后一次成功重定位。
if(!bMatch)
return false;
mnLastRelocFrameId = mCurrentFrame.mnId;
return true;
}
上述过程大体上可以分为5步：
- 计算当前帧的特征向量；
- 通过词袋模型计算候选关键帧；
- 通过 SearchByBoW 进行初步筛选；
- 针对各个候选帧通过 PnP 估计相机位姿，通过 PoseOptimization 优化相机位姿，剔除掉内点数量不足的候选帧；
- 重复第4步，直到候选集为空或者找到一个合适的关键帧
3. 再看词袋模型
右图是 DBoW2 的词袋模型示意图。 它由一棵词汇树(Vocabulary Tree)、逆向索引表(Inverse Indexes)以及一个正向索引表(Direct Indexes)三个部分构成。
词汇树的叶子节点表示词袋的单词(words)，它是由一堆图片离线训练出来的。首先对训练的图片计算 ORB 特征点，然后把这些特征点放在一起，通过 K-means 的方法对它们聚类， 将之分为 \(k\) 类。接着对每个聚类，再次通过 K-means 的方法进行聚类。如此重复\(L_w\)次，就得到了深度为\(L_w\)的树，除了叶子之外，每个节点都有 \(k\) 个子节点。 最后得到了\(W = 2^{L_w + 1}\)个叶子节点，将 ORB 特征点的 BRIEF 描述子划分成了\(W\)个单词。
逆向索引表和正向索引表建立了单词和图像之间的双向索引关系，则是针对在线进行图像检索，设计的数据结构。
逆向索引表对应着每个单词，记录着它出现在各个图像中的权重。比如，上图中 'Word 1' 在 'Image 68'、'Image 82' 等图像中出现了，在这两个图像中的权重分别是 0.79 和 0.73。 DBoW2 以TF-IDF(Term Frequency - Inverse Document Frequency)的形式计算单词\(i\)出现在图像\(d\)中的权重:
$$ v_d^i = \frac{n_d^i}{n_d} \log {\frac{N}{N^i}} $$其中，\(n_d\)表示图像\(d\)中特征点数量，\(n_d^i\)则是图像\(d\)中单词\(i\)出现的次数，\(\frac{n_d^i}{n_d}\)就是所谓的 TF(Term Frequency) 表示单词\(i\)出现的频率。 \(N\)为离线训练字典文件时所用的特征点数量，\(N_i\)是训练时单词\(i\)的数量，\(\log {\frac{N}{N^i}}\)是 IDF(Inverse Document Frequency)。IDF 的逻辑在于， 如果某个单词出现在字典中的频率越低，其辨识度就越高，权重应该更大。
正向索引表对应着每个图像，记录着它所包含的词汇树中的各级节点。对于图像\(d\)，计算出它的各个特征点，然后根据 Hamming 距离从词汇树的根节点到叶子节点， 将所有特征点的 BRIEF 描述子逐级遍历一遍，得到图像的特征向量\(\boldsymbol{v_d} \in \mathbb{R}^W\)。正向索引表记录了遍历到的各个节点计数，描述了图像的特征。 它可以用于加速特征匹配，前面提到的 SearchByBoW() 就是利用了这个数据结构，压缩了搜索空间。
在 ORB-SLAM2 中， 每个帧(类 Frame)和关键帧(类 KeyFrame )都有一个字段 mBowVec 和 mFeatVec， 它们可以理解为这里的正向索引表，由接口 ComputeBoW 计算。mBowVec 记录的是图像中出现的单词 id 及其权重， mFeatVec 记录的则是图像中出现的词汇树中的节点 id 及其权重。 关键帧数据库(类 KeyFrameDatabase)中的字段 mvInvertedFile 可以理解为这里的逆向索引表。
4. 根据词袋模型计算候选帧
前面提到的根据词袋模型计算候选关键帧的函数 DetectRelocalizationCandidates 是关键帧数据库的一个接口，如下面的代码片段所示，只有一个参数 F， 返回的是一个关键帧对象列表，记录了与 F 相似的候选关键帧。
vector<KeyFrame*> KeyFrameDatabase::DetectRelocalizationCandidates(Frame *F) {
首先，遍历输入的帧 F 中的字段 mBowVec，查找与 F 具有相同单词的关键帧，记录在局部的容器 lKFsSharingWords 中。 如下面的第 6 行所示，对于出现在 F 中的每个单词，都通过字段 mvInvertedFile 查询其它观测到该单词的关键帧。 然后在一个 for 循环中记录到容器 lKFsSharingWords 中。如果，最后该容器仍然是空的，意味着目前数据库中没有一个关键帧与帧 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];
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*>();
然后统计容器 lKFsSharingWords 中与 F 共享单词数最多的关键帧。并据此指定一个阈值，minCommonWords，用于剔除掉那些共享单词数较少的关键帧。
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;
如下面的第32行所示，对于那些大于阈值 minCommonWords 的关键帧，计算它们与帧 F 的相似度，保存在容器 lScoreAndMatch 中。
list<pair<float,KeyFrame*>> lScoreAndMatch;
int nscores=0;
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);
pKFi->mRelocScore=si;
lScoreAndMatch.push_back(make_pair(si,pKFi));
}
}
if (lScoreAndMatch.empty())
return vector<KeyFrame*>();
在 ORB-SLAM2 中，认为如果当前帧与一个关键帧的匹配度很高，那么它与该关键帧在共视图中的一级邻接关键帧也应当有一定的匹配度。 所以在下面的这次遍历中，选取了与候选关键帧共视程度最高的10个关键帧，计算累计的相似度。并统计累计相似度最高的关键帧，用局部变量 bestAccScore 记录最高的累计相似度。
list<pair<float,KeyFrame*>> lAccScoreAndMatch;
float bestAccScore = 0;
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);
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->mnRelocQuery != F->mnId)
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;
}
最后，构建了阈值 minScoreToRetain 保留了那些累计相似度大于 0.75 * bestAccScore 的关键帧。
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;
}
5. 完
本文中，我们先简单介绍了词袋模型的基本思想，并分析了重定位的详细过程。然后，分析了 ORB-SLAM2 所用的 DBoW2 的词袋模型基本结构，并详细研究了候选关键帧的计算过程。