ORB-SLAM3算法和代码学习——跟踪参考关键帧TrackReferenceKeyFrame

0总述

无论是跟踪恒速运动模型还是跟踪参考关键帧，本质上都是基于帧间匹配跟踪。

跟踪恒速模型是当前帧和上一帧之间的匹配，使用基于恒速模型计算得到的位姿作为优化的初始位姿，基于网格和搜索半径寻找匹配关系。

跟踪参考关键帧是当前帧和参考关键帧之间的匹配，虽然在时序上两个图像帧不连续，但是在空间上参考关键帧和当前帧的视野具有极高的重合度，因此可以作为投影匹配的参与对象，和跟踪恒速运动模型不同的是跟踪参考关键帧使用词袋向量搜索匹配关系，并使用上一帧lastframe的位姿作为当前帧位姿优化的初始位姿。

在得到两帧之间的匹配之后，跟踪恒速模型和跟踪参考关键帧的步骤基本就类似了，首先基于非线性优化计算当前帧的位姿，并过滤匹配误差较大的外点，得到最终的匹配数量，根据匹配数量是否满足要求判断是否跟踪成功。

进入跟踪参考关键帧TrackReferenceKeyFrame()函数的条件：

• 当前恒速模型为空且IMU没有初始化
• 或者当前帧距离重定位不久，与重定位只相差两帧

if((!mbVelocity && !pCurrentMap->isImuInitialized()) || mCurrentFrame.mnId<mnLastRelocFrameId+2)
{
   // 跟踪参考关键帧
   bOK = TrackReferenceKeyFrame();
}

1.参考关键帧的确定与更新

首先捋一下参考关键帧mpReferenceKF的确定与更新。

双目或单目初始化成功后，将初始帧设置为第一个参考关键帧，此后在此基础上进行更新。

如果跟踪彻底跪了或者接收到reset的命令，到了重新创建子地图CreateMapInAtlas()这一步，会先将参考关键帧设置为空，然后等着初始化再进行更新。

每当新创建关键帧时CreateNewKeyFrame()，会即时更新当前帧的参考关键帧

在跟踪局部地图时，更新局部地图中的关键帧UpdateLocalKeyFrames()后会将与当前图像帧共视程度最高的关键帧设置为当前帧的参考关键帧和最新关键帧

2.将当前帧的描述子转化为BoW向量

mCurrentFrame.ComputeBoW();

主要使用DBOW库的函数,具体不进行深究了，最终输出的是一个词袋向量记录当前图像包含的单词，以及特征向量即根据描述子计算得到的每个特征点对应的节点信息(该信息用于加速特征匹配)

mpORBvocabulary->transform(vCurrentDesc,// 当前的描述子vector
                                    mBowVec,// 输出，词袋向量，记录的是单词的id及其对应权重TF-IDF值
                                    mFeatVec,// 输出，记录node id及其对应的图像 feature对应的索引
                                    4);// 4表示从叶节点向前数的层数

3.通过词袋BoW加速当前帧与参考帧之间的特征点匹配

int nmatches = matcher.SearchByBoW(mpReferenceKF,mCurrentFrame,vpMapPointMatches);

这一步的作用等同于跟踪恒速运动模型中的SearchByProjection，目的在于寻找当前帧与参考关键帧之间存在的匹配关系，只不过这里使用的是基于词袋向量快速查找的方法。

3.1单词与词典树

参考链接：https://blog.csdn.net/u011341856/article/details/109405181

视觉词典的本质是一个通过聚类得到的KD树，通过离线训练，对各种类型的特征点对应的描述子进行聚类，划分成不同的簇，然后以簇的聚类中心作为子节点，再对子节点中的描述子进行聚类划分子节点的子节点，以此循环，直到满足条件。

1.词典的创建

如上图所示，假设在训练时一共使用了1000个描述子，首先我们将这1000个描述子进行聚类划分成3个簇，每个簇分别有200,300,500个元素，其中每个簇对应的聚类中心作为kd树第一层的字节点。

然后再对每个字节点对应的簇继续聚类，各自再划分为3个簇，然后形成第二层的9个子节点。

如果不再继续聚类的话，最终我们得到一个分支为3深度为2的kd树。而最下面一层，每个簇的聚类中心被曾为单词，即该图的kd树有9个单词。

当我们保存整个训练好的词汇树时，只要把树的结构和聚类中心都保存下来就行了，其中那些用来训练的描述子都可以扔掉了。

整个过程总结如下：

• step1.在根节点将样本聚为k类
• step2.对第一层的每个节点，把属于该节点的样本再聚为k类
• step3.重复step2，最后得到叶子层，每个叶子对应一个word，最终得到的树就是vocabulary tree

2.基于词袋向量搜索的特征匹配

假设对于当前帧图像提取了1000个特征点，对应的有1000个描述子，在ComputeBoW()函数中我们已经将整幅图像中的描述子转化为了词袋向量，同时记录了每个描述子对应的叶节点id。至于怎么计算的每个描述子对应的节点信息，就是和每一层节点(聚类中心描述子)计算描述子距离，和哪个节点的描述子距离最近该描述子就属于哪个节点。

在进行特征匹配时，为了加速匹配只计算属于同一节点的描述子以寻找匹配关系，这样就将1000对1000的暴力匹配转换为某个节点的小范围暴力匹配，节省计算时间。

每完成一个节点中的特征匹配，就要对这些匹配进行验证：

• 相互匹配的两个特征点的描述子距离要小于设定的阈值
• 最佳匹配的描述子距离要比较明显的小于次佳匹配的描述子距离(在SearchByBoW中最佳距离要小于0.7倍的次佳距离)

最后完成所有的特征匹配后需要对匹配的特征旋转偏差进行旋转一致性验证，然后记录最终的匹配关系，用于后续的非线性优化。

3.基于词袋向量的回环检测和重定位

参考链接：https://zhuanlan.zhihu.com/p/354616831

回环检测和重定位在本质上都是场景识别，在这两个模块中都使用了词袋向量进行了加速匹配。上面我们介绍了怎么为每一个特征点对应的描述子寻找对应的叶节点(也就是单词)，假如我们在一张图像中提取了1000个特征点，对应1000个描述子，通过计算我们将这1000个描述子分别找到了对应的叶节点或者说单词，这样最终一帧图像可以用若干个单词进行表示。

随着SLAM过程的进行会不断的创建关键帧，同时会将关键帧转化为词袋向量，在进行回环检测和重定位时会遍历所有关键帧，其中和当前帧公共单词数最多的关键帧就是当前帧的最佳候选关键帧。

这里只简单叙述一下原理，在后面重定位和回环检测部分代码解析时再进行详细分析。

4.基于非线性优化的位姿估计

在拿到匹配关系之后就是和跟踪恒速模型一样的老三样了，只不过位姿优化的初始值是上一帧的位姿，而非借助恒速模型

mCurrentFrame.SetPose(mLastFrame.GetPose());

非线性优化和跟踪恒速运动模型使用同一个优化函数

Optimizer::PoseOptimization(&mCurrentFrame);

在非线性优化函数中，会根据投影误差筛选外点，最后根据投影匹配的内点数量决定当前是否匹配成功。