WHCSRL 技术网

ORB_SLAM2源码分析特征匹配(五)

目录

一、单目初始化中的特征匹配SearchForInitialization

二、跟踪(TrackwithModel)

 TrackReferenceKeyFrame

三、词袋介绍BoW

1、直观理解词袋

2、词袋基本思想

3、从字典结构到k-d树

K-means聚类

4、相似度计算TF-IDF

5、总结词袋模型

四、词袋大概流程

闭环检测:

加速匹配

如何制作、生成BoW? 为什么 BOW一般用 BRIEF描述子?

速度方面

在精度方面

可视化效果

离线训练 vocabulary tree(也称为字典)

在线图像生成BoW向量

源码解析 

理解词袋向量BowVector

理解特征向量FeatureVector

词典树的保存和加载


一、单目初始化中的特征匹配SearchForInitialization

 计算描述子的距离(汉明距离)距离最短则为匹配点

 1、匹配点要大于100才能进行初始化

如上图所示,找出第二张图片中对应第一张图片的特征点,并在特征点周围以100为半径,做一个框框,在这个范围里面找符合要求的特征点。

快速匹配和候选特征点GetFeaturesInArea

 2、挑选出来的特征点所属金字塔必须为第0层

3、剔除那些在所选格子内但是不在搜索范围内的点

4、最优距离要小于50,计算最优距离和最次距离的比值

5、统计匹配点方向的直方图

        计算第一张图片FAST特征点方向和第二张图片FAST特征点方向,并将第一张特征点的方向向量移动到第二张图片上面,计算两个方向向量的角度,做一个直方图。

6、统计特征点数量最多的三个方向

7、判断第二多的数量<0.1第一多的数量。符合则证明第一多为主方向

8、判断第三多的数量<0.1第一多的数量。符合则证明第一多和第二多的主方向

二、跟踪(TrackwithModel

根据匀速模型来计算初始位姿,然后通过投影的形式搜索匹配点

 TrackReferenceKeyFrame

1、前面EPnP得到的内容将不再进行跟踪搜索

2、这里需要估计关键帧地图点在当前在帧图像金字塔的层数

图像金字塔的层数怎么的来:通过地图点离相机光心的距离计算而来

3、和前面的seachByProjection类似,将关键帧地图点投影到当前帧,然后在一定范围内搜索

三、词袋介绍BoW

1、直观理解词袋

 如何找到匹配的图像

特征匹配

不同的光照强度都会有影响,而且匹配时间较长

词袋模型(BoW)

2、词袋基本思想

从单词概念引入基本思想

如何定量描述

s(a,b)=1-frac{1}{W}left | a-b 
ight |_{1}

s---评分计算函数

a&b----二进制向量

W---向量维度

1---向量L1范数,各元素绝对值之和

由此定义了描述向量的相似性,即图像的相似程度

3、从字典结构到k-d树

k:每层的节点数为k

d:数结构一共有d 层

单词:局部相邻特征点的集合

功能:把图片中所有的行人归到人这个类中

字典:具有一定结构的单词索引,从大量图片训练而来

那么有了一个特征点,如何找到匹配的单词

从字典结构到K-d树(两种索引方式)

K-means聚类

 

1、首先随机生成K个聚类中心点

2、根据聚类中心点,将数据分为K类。分类原则是数据离哪个中心点近就将它分为哪一类

3、再根据分好的类别数据,重新计算聚类的类别中心点

4、不断的重复2和3步,直到中心点不再变化

注:刚开始生成的中心点不同对后面也会有不同的影响

4、相似度计算TF-IDF

TF:某单词在一副图像中出现的频率越高,该单词就更具代表性

IDF:某单词在字典中出现的频率越低,就说明该单词在字典中更具有代表性

图片维度

                                                                TF_{i}=frac{n_{i}}{n} 

字典维度

                                                             IDF_{i}=lgfrac{n}{n_{i}}

 权重

                                                           eta _{i}=TF_{i}	imes IDF_{i}                                                      

相似度计算 BoW向量(bog of words)

BoW向量组成如下,两元素分别为单词和权重

A=left [ left ( omega _{1} ,eta _{1}
ight ),left (omega _{2} ,eta _{2}
ight ) ... left (omega _{N} ,eta _{N}
ight )
ight ]=v_{A}

该向量描述了一张图像,包含图像中有哪些单词,以及对应的权重是多少,进而,通过人为规定范数的形式,就能够计算出两张图片的相似程度

5、总结词袋模型

step1、从大量的图片中提取特征采用聚类法生成单词构建字典

step2、处理某一帧图片,采用TF-IDF计算单词权重

step3、生成该帧的BoW向量,更新正向索引和倒排索引的内容

生成BoW向量就是图片的新名片,可以用来回环检测、匹配图像、优化位姿、寻找特征点等

四、词袋大概流程

为什么叫 bag of words 而不是 list of words 或者 array of words?

因为丢弃了Word出现的排列顺序、位置等因素,只考虑出现的频率,大大简化了表达,节省了存储空 间,在分析对比相似度的时候非常高效

闭环检测:

核心就是判断两张图片是否是同一个场景,也就是判断图像的相似性。

 如何计算两张图片的相似性

Bag of words 可以解决这个问题。是以图像特征集合作为visual words,只关心图像中有没有这些 words,有多少次,更符合人类认知方式,对不同光照、视角变换、季节更替等非常鲁棒。

加速匹配

ORB-SLAM2代码中使用的 SearchByBoW(用于关键帧跟踪、重定位、闭环检测SIM3计算),以及局部 地图里的SearchForTriangulation,内部实现主要是利用了 BoW中的FeatureVector 来加速特征匹配。

使用FeatureVector 避免了所有特征点的两两匹配,只比较同一个节点下的特征点,极大加速了匹配效 率,至于匹配精度,论文 《Bags of Binary Words for Fast Place Recognition in Image Sequences 》 中提到在26292 张图片里的 false positive 为0,说明精度是有保证的。实际应用中效果非常不错。

缺点

需要提前加载离线训练好的词袋字典,增加了存储空间。但是带来的优势远大于劣势,而且也有不少改 进方法比如用二进制存储等来压缩词袋,减少存储空间,提升加载速度。

如何制作、生成BoW? 为什么 BOW一般用 BRIEF描述子?

速度方面

因为计算和匹配都非常快,论文中说大概一个关键点计算256位的描述子只需要 17.3µs 因为都是二进制描述子,距离描述通过汉明距离,使用异或操作即可,速度非常快。 而SIFT, SURF 描述子都是浮点型,需要计算欧式距离,会慢很多。

在Intel Core i7 ,2.67GHz CPU上,使用FAST+BRIEF 特征,在26300帧图像中 特征提取+词袋位置识别 耗时 22ms 每帧。

在精度方面

先上结论:闭环效果并不比SIFT, SURF之类的高精度特征点差。

具体来看一下对比: 以下对比来自论文《2012,Bags of Binary Words for Fast Place Recognition in Image Sequences, IEEE TRANSACTIONS ON ROBOTICS》

三种描述子 BRIEF,,SURF64 ,U-SURF128 使用同样的参数,在训练数据集NewCollege,Bicocca25b 上的 Precision-recall 曲线

其中SURF64:带旋转不变性的 64 维描述子

U-SURF128:不带旋转不变性的128维描述子

在两个数据中,SURF64 都明显比 U-SURF128 表现的好(曲线下面积更大),可以看到在Bicocca25b 数据集上,BRIEF明显比 U-SURF128 好,比SURF64 也稍微好一些,在NewCollege 数据集上 SURF64 比 BRIEF 更好一点,但是BRIEF也仍然不错。

总之,BRIEF 和 SURF64 效果基本相差不大,可以说打个平手。 

可视化效果

可视化看一下效果

下图左边图片对是BRIEF 在vocabulary 中同样的Word下的回环匹配结果,同样的特征连成了一条线。

下图右边图像对是同样数据集中SURF64 的闭环匹配结果。

第一行 来看,尽管有一定视角变换,BRIEF 和 SURF64 的匹配结果接近

第二行:BRIEF成功进行了闭环,但是SURF64 没有闭环。原因是SURF64 没有得到足够多的匹配关系。

第三行:BRIEF 闭环失败而SURF64闭环成功。

我们分析一下原因:主要和近景远景有关。因为BRIEF相比SURF64没有尺度不变性,所以在尺度变换较 大的近景很容易匹配失败,比如第三行。而在中景和远景,由于尺度变化不大,BRIEF 表现接近甚至优 于SURF64

 不过,我们通过图像金字塔可以解决上述BRIEF的尺度问题。论文中作者也提到了ORB + BRIEF的特征点 主要问题是没有旋转不变性和尺度不变性。不过目前都解决了。

总之,BRIEF的闭环效果值得信赖!

离线训练 vocabulary tree(也称为字典)

首先图像提取ORB 特征点,将描述子通过 k-means 进行聚类,根据设定的树的分支数和深度,从叶子 节点开始聚类一直到根节点,最后得到一个非常大的 vocabulary tree,

1、遍历所有的训练图像,对每幅图像提取ORB特征点。

2、设定vocabulary tree的分支数K和深度L。将特征点的每个描述子用 K-means聚类,变成 K个集合, 作为vocabulary tree 的第1层级,然后对每个集合重复该聚类操作,就得到了vocabulary tree的第2层 级,继续迭代最后得到满足条件的vocabulary tree,它的规模通常比较大,比如ORB-SLAM2使用的离 线字典就有108万+ 个节点。

3、离根节点最远的一层节点称为叶子或者单词 Word。根据每个Word 在训练集中的相关程度给定一个 权重weight,训练集里出现的次数越多,说明辨别力越差,给与的权重越低。

在线图像生成BoW向量

1、对新来的一帧图像进行ORB特征提取,得到一定数量(一般几百个)的特征点,描述子维度和 vocabulary tree中的一致

2、对于每个特征点的描述子,从离线创建好的vocabulary tree中开始找自己的位置,从根节点开始, 用该描述子和每个节点的描述子计算汉明距离,选择汉明距离最小的作为自己所在的节点,一直遍历到 叶子节点。

整个过程是这样的,见下图。紫色的线表示 一个特征点从根节点到叶子节点的过程。

源码解析 

一个描述子转化为Word id, Word weight,节点所属的父节点(距离叶子深度为level up深度的节点) id 对应的实现代码见:

  1. /**
  2. * @brief 将描述子转化为Word id, Word weight,节点所属的父节点id(这里的父节点不是叶子的上
  3. 一层,它距离叶子深度为levelsup)
  4. * @tparam TDescriptor
  5. * @tparam F
  6. * @param[in] feature 特征描述子
  7. * @param[in & out] word_id Word id
  8. * @param[in & out] weight Word 权重
  9. * @param[in & out] nid 记录当前描述子转化为Word后所属的 node id,它距离
  10. 叶子深度为levelsup
  11. * @param[in] levelsup 距离叶子的深度
  12. */
  13. template<class TDescriptor, class F>
  14. void TemplatedVocabulary<TDescriptor,F>::transform(const TDescriptor &feature,
  15. WordId &word_id, WordValue &weight, NodeId *nid, int levelsup) const
  16. {
  17. // propagate the feature down the tree
  18. vector<NodeId> nodes;
  19. typename vector<NodeId>::const_iterator nit;
  20. // level at which the node must be stored in nid, if given
  21. // m_L: depth levels, m_L = 6 in ORB-SLAM2
  22. // nid_level 当前特征点转化为的Word 所属的 node id,方便索引
  23. const int nid_level = m_L - levelsup;
  24. if(nid_level <= 0 && nid != NULL) *nid = 0; // root
  25. NodeId final_id = 0; // root
  26. int current_level = 0;
  27. do
  28. {
  29. ++current_level;
  30. nodes = m_nodes[final_id].children;
  31. final_id = nodes[0];
  32. // 取当前节点第一个子节点的描述子距离初始化最佳(小)距离
  33. double best_d = F::distance(feature, m_nodes[final_id].descriptor);
  34. // 遍历nodes中所有的描述子,找到最小距离对应的描述子
  35. for(nit = nodes.begin() + 1; nit != nodes.end(); ++nit)
  36. {
  37. NodeId id = *nit;
  38. double d = F::distance(feature, m_nodes[id].descriptor);
  39. if(d < best_d)
  40. {
  41. best_d = d;
  42. final_id = id;
  43. }
  44. }
  45. // 记录当前描述子转化为Word后所属的 node id,它距离叶子深度为levelsup
  46. if(nid != NULL && current_level == nid_level)
  47. *nid = final_id;
  48. } while( !m_nodes[final_id].isLeaf() );
  49. // turn node id into word id
  50. // 取出 vocabulary tree中node距离当前feature 描述子距离最小的那个node的 Word id 和
  51. weight
  52. word_id = m_nodes[final_id].word_id;
  53. weight = m_nodes[final_id].weight;
  54. }

一幅图像里所有特征点转化为两个std::map容器 BowVector 和 FeatureVector 的代码见:

  1. /**
  2. * @brief 将一幅图像所有的特征点转化为BowVector和FeatureVector
  3. *
  4. * @tparam TDescriptor
  5. * @tparam F
  6. * @param[in] features 图像中所有的特征点
  7. * @param[in & out] v BowVector
  8. * @param[in & out] fv FeatureVector
  9. * @param[in] levelsup 距离叶子的深度
  10. */
  11. template<class TDescriptor, class F>
  12. void TemplatedVocabulary<TDescriptor,F>::transform(
  13. const std::vector<TDescriptor>& features,
  14. BowVector &v, FeatureVector &fv, int levelsup) const
  15. {
  16. v.clear();
  17. fv.clear();
  18. if(empty()) // safe for subclasses
  19. {
  20. return;
  21. }
  22. // normalize
  23. // 根据选择的评分类型来确定是否需要将BowVector 归一化
  24. LNorm norm;
  25. bool must = m_scoring_object->mustNormalize(norm);
  26. typename vector<TDescriptor>::const_iterator fit;
  27. if(m_weighting == TF || m_weighting == TF_IDF)
  28. {
  29. unsigned int i_feature = 0;
  30. // 遍历图像中所有的特征点
  31. for(fit = features.begin(); fit < features.end(); ++fit, ++i_feature)
  32. {
  33. WordId id; // 叶子节点的Word id
  34. NodeId nid; // FeatureVector 里的NodeId,用于加速搜索
  35. WordValue w; // 叶子节点Word对应的权重
  36. // 将当前描述子转化为Word id, Word weight,节点所属的父节点id(这里的父节点不是叶的上一层,它距离叶子深度为levelsup)
  37. // w is the idf value if TF_IDF, 1 if TF
  38. transform(*fit, id, w, &nid, levelsup);
  39. if(w > 0) // not stopped
  40. {
  41. // 如果Word 权重大于0,将其添加到BowVector 和 FeatureVector
  42. v.addWeight(id, w);
  43. fv.addFeature(nid, i_feature);
  44. }
  45. }
  46. if(!v.empty() && !must)
  47. {
  48. // unnecessary when normalizing
  49. const double nd = v.size();
  50. for(BowVector::iterator vit = v.begin(); vit != v.end(); vit++)
  51. vit->second /= nd;
  52. }
  53. }
  54. // 省略掉了IDF || BINARY情况的代码
  55. if(must) v.normalize(norm);
  56. }

相当于将当前图像信息进行了压缩,并且对后面特征点快速匹配、闭环检测、重定位意义重大。 这两个容器非常重要,下面一个个来解释:

理解词袋向量BowVector

它内部实际存储的是这个

std::map<WordId,Woedvalue>

其中 WordId 和 WordValue 表示Word在所有叶子中距离最近的叶子的id 和权重(后面解释)。 同一个Word id 的权重是累加更新的,见代码

  1. void BowVector::addWeight(WordId id, WordValue v)
  2. {
  3. // 返回指向大于等于id的第一个值的位置
  4. BowVector::iterator vit = this->lower_bound(id);
  5. // http://www.cplusplus.com/reference/map/map/key_comp/
  6. if(vit != this->end() && !(this->key_comp()(id, vit->first)))
  7. {
  8. // 如果id = vit->first, 说明是同一个Word,权重更新
  9. vit->second += v;
  10. }
  11. else
  12. {
  13. // 如果该Word id不在BowVector中,新添加进来
  14. this->insert(vit, BowVector::value_type(id, v));
  15. }
  16. }

理解特征向量FeatureVector

内部实际是个

std::map <Nodeid,std:;vector<unsigned int>>

其中NodeId 并不是该叶子节点直接的父节点id,而是距离叶子节点深度为level up对应的node 的id, 对应上面 vocabulary tree 图示里的 Word's node id。为什么不直接设置为父节点?因为后面搜索该 Word 的匹配点的时候是在和它具有同样node id下面所有子节点中的Word 进行匹配,搜索区域见图示 中的 Word's search region。所以搜索范围大小是根据level up来确定的,level up 值越大,搜索范围越广,速度越慢;level up 值越小,搜索范围越小,速度越快,但能够匹配的特征就越少。 另外 std::vector 中实际存的是NodeId 下所有特征点在图像中的索引。见代码

  1. void FeatureVector::addFeature(NodeId id, unsigned int i_feature)
  2. {
  3. FeatureVector::iterator vit = this->lower_bound(id);
  4. // 将同样node id下的特征放在一个vector里
  5. if(vit != this->end() && vit->first == id)
  6. {
  7. vit->second.push_back(i_feature);
  8. }
  9. else
  10. {
  11. vit = this->insert(vit, FeatureVector::value_type(id,
  12. std::vector<unsigned int>() ));
  13. vit->second.push_back(i_feature);
  14. }
  15. }

FeatureVector主要用于不同图像特征点快速匹配,加速几何关系验证,比如 ORBmatcher::SearchByBoW 中是这样用的

  1. DBoW2::FeatureVector::const_iterator f1it = vFeatVec1.begin();
  2. DBoW2::FeatureVector::const_iterator f2it = vFeatVec2.begin();
  3. DBoW2::FeatureVector::const_iterator f1end = vFeatVec1.end();
  4. DBoW2::FeatureVector::const_iterator f2end = vFeatVec2.end();
  5. while(f1it != f1end && f2it != f2end)
  6. {
  7. // Step 1:分别取出属于同一node的ORB特征点(只有属于同一node,才有可能是匹配点)
  8. if(f1it->first == f2it->first)
  9. // Step 2:遍历KF中属于该node的特征点
  10. for(size_t i1=0, iend1=f1it->second.size(); i1<iend1; i1++)
  11. {
  12. const size_t idx1 = f1it->second[i1];
  13. MapPoint* pMP1 = vpMapPoints1[idx1];
  14. // 省略
  15. // ..........

词典树的保存和加载

我们将 vocabulary tree翻译为词典树

如何保存训练好的词典树存储为txt文件?

  1. template<class TDescriptor, class F>
  2. void TemplatedVocabulary<TDescriptor,F>::saveToTextFile(const std::string
  3. &filename) const
  4. {
  5. fstream f;
  6. f.open(filename.c_str(),ios_base::out);
  7. // 第一行打印 树的分支数、深度、评分方式、权重计算方式
  8. f << m_k << " " << m_L << " " << " " << m_scoring << " " << m_weighting <<
  9. endl;
  10. for(size_t i=1; i<m_nodes.size();i++)
  11. {
  12. const Node& node = m_nodes[i];
  13. // 每行第1个数字为父节点id
  14. f << node.parent << " ";
  15. // 每行第2个数字标记是(1)否(0)为叶子(Word)
  16. if(node.isLeaf())
  17. f << 1 << " ";
  18. else
  19. f << 0 << " ";
  20. // 接下来存储256位描述子,最后存储节点权重
  21. f << F::toString(node.descriptor) << " " << (double)node.weight << endl;
  22. }
  23. f.close();
  24. }

如何加载训练好的词典树文件?

  1. /**
  2. * @brief 加载训练好的 vocabulary tree,txt格式
  3. *
  4. * @tparam TDescriptor
  5. * @tparam F
  6. * @param[in] filename vocabulary tree 文件名称
  7. * @return true 加载成功
  8. * @return false 加载失败
  9. */
  10. template<class TDescriptor, class F>
  11. bool TemplatedVocabulary<TDescriptor,F>::loadFromTextFile(const std::string
  12. &filename)
  13. {
  14. ifstream f;
  15. f.open(filename.c_str());
  16. if(f.eof())
  17. return false;
  18. m_words.clear();
  19. m_nodes.clear();
  20. string s;
  21. getline(f,s);
  22. stringstream ss;
  23. ss << s;
  24. ss >> m_k; // 树的分支数目
  25. ss >> m_L; // 树的深度
  26. int n1, n2;
  27. ss >> n1;
  28. ss >> n2;
  29. if(m_k<0 || m_k>20 || m_L<1 || m_L>10 || n1<0 || n1>5 || n2<0 || n2>3)
  30. {
  31. std::cerr << "Vocabulary loading failure: This is not a correct text
  32. file!" << endl;
  33. return false;
  34. }
  35. m_scoring = (ScoringType)n1; // 评分类型
  36. m_weighting = (WeightingType)n2; // 权重类型
  37. createScoringObject();
  38. // 总共节点(nodes)数,是一个等比数列求和
  39. //! bug 没有包含最后叶子节点数,应该改为 ((pow((double)m_k, (double)m_L + 2) -
  40. 1)/(m_k - 1))
  41. //! 但是没有影响,因为这里只是reserve,实际存储是一步步resize实现
  42. int expected_nodes =
  43. (int)((pow((double)m_k, (double)m_L + 1) - 1)/(m_k - 1));
  44. m_nodes.reserve(expected_nodes);
  45. // 预分配空间给 单词(叶子)数
  46. m_words.reserve(pow((double)m_k, (double)m_L + 1));
  47. // 第一个节点是根节点,id设为0
  48. m_nodes.resize(1);
  49. m_nodes[0].id = 0;
  50. while(!f.eof())
  51. {
  52. string snode;
  53. getline(f,snode);
  54. stringstream ssnode;
  55. ssnode << snode;
  56. // nid 表示当前节点id,实际是读取顺序,从0开始
  57. int nid = m_nodes.size();
  58. // 节点size 加1
  59. m_nodes.resize(m_nodes.size()+1);
  60. m_nodes[nid].id = nid;
  61. // 读每行的第1个数字,表示父节点id
  62. int pid ;
  63. ssnode >> pid;
  64. // 记录节点id的相互父子关系
  65. m_nodes[nid].parent = pid;
  66. m_nodes[pid].children.push_back(nid);
  67. // 读取第2个数字,表示是否是叶子(Word)
  68. int nIsLeaf;
  69. ssnode >> nIsLeaf;
  70. // 每个特征点描述子是256 bit,一个字节对应8 bit,所以一个特征点需要32个字节存储。
  71. // 这里 F::L=32,也就是读取32个字节,最后以字符串形式存储在ssd
  72. stringstream ssd;
  73. for(int iD=0;iD<F::L;iD++)
  74. {
  75. string sElement;
  76. ssnode >> sElement;
  77. ssd << sElement << " ";
  78. }
  79. // 将ssd存储在该节点的描述子
  80. F::fromString(m_nodes[nid].descriptor, ssd.str())
  81. // 读取最后一个数字:节点的权重(Word才有)
  82. ssnode >> m_nodes[nid].weight;
  83. if(nIsLeaf>0)
  84. {
  85. // 如果是叶子(Word),存储到m_words
  86. int wid = m_words.size();
  87. m_words.resize(wid+1);
  88. //存储Word的id,具有唯一性
  89. m_nodes[nid].word_id = wid;
  90. //构建 vector<Node*> m_words,存储word所在node的指针
  91. m_words[wid] = &m_nodes[nid];
  92. }
  93. else
  94. {
  95. //非叶子节点,直接分配 m_k个分支
  96. m_nodes[nid].children.reserve(m_k);
  97. }
  98. }
  99. return true;
  100. }
推荐阅读