ORB-SLAM2系列第五章—— 特征匹配

系列文章目录

ORB-SLAM2系列第一章——简介
ORB-SLAM2系列第二章——ORB 特征点提取
ORB-SLAM2系列第三章—— 地图初始化
ORB-SLAM2系列第四章—— 地图点、关键帧、图结构
ORB-SLAM2系列第五章—— 特征匹配

---

---

前言

迎浏览我的SLAM专栏，包括slam安装运行、代码注释、原理详解，一起加油淦穿SLAM。

---

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

初始化搜索匹配，对应函数 SearchForInitialization

快速搜索候选匹配特征点，对应函数 GetFeaturesInArea

通过角度一致性过滤外点中的错误分析
原因：错误把长度为30的直方图（单个bin角度范围12°）变成了长度为12（单个bin角度范围30°），
影响：弱化了对角度不一致性的错误剔除，导致只有角度差超过30°（本来是12°）才会被去除。
rotHist 预分配了30个bin，实际第13-30个都没有用到
修改前打印输出

总结，修改后代码
能够更有效的滤掉方向不一致的匹配对，更精确了。
举个例子，假如方向差别6°，
原来的代码：factor = 1/30，假设rot =6°,
计算得到 所在 bin =0; 和正常的角度±1°（bin=0）混在一起，不会被滤掉
改进后代码 factor = 1/12，假设rot =6°,
计算得到 所在 bin =1
会被当做异常值滤掉
经过方向一致性检测后删除的匹配对

二、通过视觉词袋进行特征匹配

1.直观理解词袋

词袋的大概流程

为什么叫 bag of words 而不是 list of words 或者 array of words？
因为丢弃了Word出现的排列顺序、位置等因素，只考虑出现的频率，大大简化了表达，节省了存储空间，在分析对比相似度的时候非常高
效:
https://gurus.pyimagesearch.com/the-bag-of-visual-words-model/

2.为什么要研究BoW？

闭环检测：核心就是判断两张图片是否是同一个场景，也就是判断图像的相似性。
如何计算两张图的相似性？
类帧差方法
不行，这样做有很多问题
视角变化
没办法将两张图的像素进行一一匹配，直接帧差时几乎不能保证是同一个像素点的差。

光照变换
同一视角，不同时间的光照对比

相机曝光不同

词袋法
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，说明精度是有保证的。实际应用中效果非常不错。
缺点：
需要提前加载离线训练好的词袋字典，增加了存储空间。但是带来的优势远大于劣势，而且也有不少改进方法比如用二进制存储等来压缩词
袋，减少存储空间，提升加载速度。

3.如何制作、生成BoW？

1.为什么 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的尺度问题。论文中作者也提到了FAST + BRIEF的特征点主要问题是没有旋转不变性和尺度
不变性。不过目前都解决了。
总之，BRIEF的闭环效果值得信赖！

2.离线训练 vocabulary tree（也称为字典）
首先图像提取ORB 特征点，将描述子通过 k-means 进行聚类，根据设定的树的分支数和深度，从叶子节点开始聚类一直到根节点，最后得
到一个非常大的 vocabulary tree，

• 遍历所有的训练图像，对每幅图像提取ORB特征点。
• 设定vocabulary tree的分支数K和深度L。将特征点的每个描述子用 K-means聚类，变成K个集合，作为vocabulary
  tree 的第1层级，然 后对每个集合重复该聚类操作，就得到了vocabulary
  tree的第2层级，继续迭代最后得到满足条件的vocabulary
  tree，它的规模通常比较大，比如ORB-SLAM2使用的离线字典就有108万+ 个节点。
• 离根节点最远的一层节点称为叶子或者单词 Word。根据每个Word 在训练集中的相关程度给定一个权重weight，训练集里出现的次数越
  多，说明辨别力越差，给与的权重越低。

3.在线图像生成BoW向量

• 1、对新来的一帧图像进行ORB特征提取，得到一定数量（一般几百个）的特征点，描述子维度和vocabulary tree中的一致
• 2、对于每个特征点的描述子，从离线创建好的vocabulary tree中开始找自己的位置，从根节点开始，用该描述子和每个节点的描述子计算
  汉明距离，选择汉明距离最小的作为自己所在的节点，一直遍历到叶子节点。

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

村民，村长，市长，省长 类比

4.源码解析

一个描述子转化为Word id， Word weight，节点所属的父节点（距离叶子深度为level up深度的节点）id 对应的实现代码见：

/**
* @brief 将描述子转化为Word id， Word weight，节点所属的父节点id（这里的父节点不是叶子的上一层，它距离叶子深度为levelsup）
*
* @tparam TDescriptor
* @tparam F
* @param[in] feature 特征描述子
* @param[in & out] word_id Word id
* @param[in & out] weight Word 权重
* @param[in & out] nid 记录当前描述子转化为Word后所属的 node id，它距离叶子深度为levelsup
* @param[in] levelsup 距离叶子的深度
*/
template<class TDescriptor, class F>
void TemplatedVocabulary<TDescriptor,F>::transform(const TDescriptor &feature,
WordId &word_id, WordValue &weight, NodeId *nid, int levelsup) const
{
// propagate the feature down the tree
vector<NodeId> nodes;
typename vector<NodeId>::const_iterator nit;
// level at which the node must be stored in nid, if given
// m_L: depth levels, m_L = 6 in ORB-SLAM2
// nid_level 当前特征点转化为的Word 所属的 node id，方便索引
const int nid_level = m_L - levelsup;
if(nid_level <= 0 && nid != NULL) *nid = 0; // root
NodeId final_id = 0; // root
int current_level = 0;
do
{
++current_level;
nodes = m_nodes[final_id].children;
final_id = nodes[0];
// 取当前节点第一个子节点的描述子距离初始化最佳（小）距离
double best_d = F::distance(feature, m_nodes[final_id].descriptor);
// 遍历nodes中所有的描述子，找到最小距离对应的描述子
for(nit = nodes.begin() + 1; nit != nodes.end(); ++nit)
{
NodeId id = *nit;
double d = F::distance(feature, m_nodes[id].descriptor);
if(d < best_d)
{
best_d = d;
final_id = id;
}
}
// 记录当前描述子转化为Word后所属的 node id，它距离叶子深度为levelsup
if(nid != NULL && current_level == nid_level)
*nid = final_id;
} while( !m_nodes[final_id].isLeaf() );
// turn node id into word id
// 取出 vocabulary tree中node距离当前feature 描述子距离最小的那个node的 Word id 和 weight
word_id = m_nodes[final_id].word_id;
weight = m_nodes[final_id].weight;
}

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

/**
* @brief 将一幅图像所有的特征点转化为BowVector和FeatureVector
*
* @tparam TDescriptor
* @tparam F
* @param[in] features 图像中所有的特征点
* @param[in & out] v BowVector
* @param[in & out] fv FeatureVector
* @param[in] levelsup 距离叶子的深度
*/
template<class TDescriptor, class F>
void TemplatedVocabulary<TDescriptor,F>::transform(
const std::vector<TDescriptor>& features,
BowVector &v, FeatureVector &fv, int levelsup) const
{
v.clear();
fv.clear();
if(empty()) // safe for subclasses
{
return;
}
// normalize
// 根据选择的评分类型来确定是否需要将BowVector 归一化
LNorm norm;
bool must = m_scoring_object->mustNormalize(norm);
typename vector<TDescriptor>::const_iterator fit;
if(m_weighting == TF || m_weighting == TF_IDF)
{
unsigned int i_feature = 0;
// 遍历图像中所有的特征点
for(fit = features.begin(); fit < features.end(); ++fit, ++i_feature)
{
WordId id; // 叶子节点的Word id
NodeId nid; // FeatureVector 里的NodeId，用于加速搜索
WordValue w; // 叶子节点Word对应的权重
// 将当前描述子转化为Word id， Word weight，节点所属的父节点id（这里的父节点不是叶子的上一层，它距离叶子深度为
levelsup）
// w is the idf value if TF_IDF, 1 if TF
transform(*fit, id, w, &nid, levelsup);
if(w > 0) // not stopped
{
// 如果Word 权重大于0，将其添加到BowVector 和 FeatureVector
v.addWeight(id, w);
fv.addFeature(nid, i_feature);
}
}
if(!v.empty() && !must)
{
// unnecessary when normalizing
const double nd = v.size();
for(BowVector::iterator vit = v.begin(); vit != v.end(); vit++)
vit->second /= nd;
}
}
// 省略掉了IDF || BINARY情况的代码
if(must) v.normalize(norm);
}

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

理解词袋向量BowVector
它内部实际存储的是这个
std::map<WordId, WordValue>
其中 WordId 和 WordValue 表示Word在所有叶子中距离最近的叶子的id 和权重（后面解释）。
同一个Word id 的权重是累加更新的，见代码

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

理解特征向量FeatureVector

内部实际是个
std::map<NodeId, std::vector >
其中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 下所有特征点在图像中的索引。见代码

void FeatureVector::addFeature(NodeId id, unsigned int i_feature)
{
FeatureVector::iterator vit = this->lower_bound(id);
// 将同样node id下的特征放在一个vector里
if(vit != this->end() && vit->first == id)
{
vit->second.push_back(i_feature);
}
else
{
vit = this->insert(vit, FeatureVector::value_type(id,
std::vector<unsigned int>() ));
vit->second.push_back(i_feature);
}
}

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

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

4.词典树的保存和加载

我们将 vocabulary tree翻译为词典树
1.如何保存训练好的词典树存储为txt文件？

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

2.如何加载训练好的词典树文件？

/**
* @brief 加载训练好的 vocabulary tree，txt格式
*
* @tparam TDescriptor
* @tparam F
* @param[in] filename vocabulary tree 文件名称
* @return true 加载成功
* @return false 加载失败
*/
template<class TDescriptor, class F>
bool TemplatedVocabulary<TDescriptor,F>::loadFromTextFile(const std::string &filename)
{
ifstream f;
f.open(filename.c_str());
if(f.eof())
return false;
m_words.clear();
m_nodes.clear();
string s;
getline(f,s);
stringstream ss;
ss << s;
ss >> m_k; // 树的分支数目
ss >> m_L; // 树的深度
int n1, n2;
ss >> n1;
ss >> n2;
if(m_k<0 || m_k>20 || m_L<1 || m_L>10 || n1<0 || n1>5 || n2<0 || n2>3)
{
std::cerr << "Vocabulary loading failure: This is not a correct text file!" << endl;
return false;
}
m_scoring = (ScoringType)n1; // 评分类型
m_weighting = (WeightingType)n2; // 权重类型
createScoringObject();
// 总共节点（nodes）数，是一个等比数列求和
//! bug 没有包含最后叶子节点数，应该改为 ((pow((double)m_k, (double)m_L + 2) - 1)/(m_k - 1))
//! 但是没有影响，因为这里只是reserve，实际存储是一步步resize实现
int expected_nodes =
(int)((pow((double)m_k, (double)m_L + 1) - 1)/(m_k - 1));
m_nodes.reserve(expected_nodes);
// 预分配空间给 单词（叶子）数
m_words.reserve(pow((double)m_k, (double)m_L + 1));
// 第一个节点是根节点，id设为0
m_nodes.resize(1);
m_nodes[0].id = 0;
while(!f.eof())
{
string snode;
getline(f,snode);
stringstream ssnode;
ssnode << snode;
// nid 表示当前节点id，实际是读取顺序，从0开始
int nid = m_nodes.size();
// 节点size 加1
m_nodes.resize(m_nodes.size()+1);
m_nodes[nid].id = nid;
// 读每行的第1个数字，表示父节点id
int pid ;
ssnode >> pid;
// 记录节点id的相互父子关系
m_nodes[nid].parent = pid;
m_nodes[pid].children.push_back(nid);
// 读取第2个数字，表示是否是叶子（Word）
int nIsLeaf;
ssnode >> nIsLeaf;
// 每个特征点描述子是256 bit，一个字节对应8 bit，所以一个特征点需要32个字节存储。
// 这里 F::L=32，也就是读取32个字节，最后以字符串形式存储在ssd
stringstream ssd;
for(int iD=0;iD<F::L;iD++)
{
string sElement;
ssnode >> sElement;
ssd << sElement << " ";
}
// 将ssd存储在该节点的描述子
F::fromString(m_nodes[nid].descriptor, ssd.str());
// 读取最后一个数字：节点的权重（Word才有）
ssnode >> m_nodes[nid].weight;
if(nIsLeaf>0)
{
// 如果是叶子（Word），存储到m_words
int wid = m_words.size();
m_words.resize(wid+1);
//存储Word的id，具有唯一性
m_nodes[nid].word_id = wid;
//构建 vector<Node*> m_words，存储word所在node的指针
m_words[wid] = &m_nodes[nid];
}
else
{
//非叶子节点，直接分配 m_k个分支
m_nodes[nid].children.reserve(m_k);
}
}
return true;
}

关于权重类型 和 评分类型

/// Weighting type
enum WeightingType
{
TF_IDF, //0
TF, //1
IDF, //2
BINARY //3
};
/// Scoring type
enum ScoringType
{
L1_NORM, //0
L2_NORM, //1
CHI_SQUARE, //2
KL, //3
BHATTACHARYYA,//4
DOT_PRODUCT, //5
};

有个地方需要注意：
ORB-SLAM2 中词典初始化时：k = 10, int L = 5, weighting = TF_IDF, scoring = L1_NORM

/**
* Initiates an empty vocabulary
* @param k branching factor
* @param L depth levels
* @param weighting weighting type
* @param scoring scoring type
*/
TemplatedVocabulary(int k = 10, int L = 5,
WeightingType weighting = TF_IDF, ScoringType scoring = L1_NORM);
// levelsup = 4
mpORBvocabulary->transform(vCurrentDesc,mBowVec,mFeatVec,4);

但是实际上ORB-SLAM2 使用时是使用的加载的词典ORBvoc.txt

10 6 0 0
0 0 252 188 188 242 169 109 85 143 187 191 164 25 222 255 72 27 129 215 237 16 58 111 219 51 219 211 85 127
192 112 134 34 0
0 0 93 125 221 103 180 14 111 184 112 234 255 76 215 115 153 115 22 196 124 110 233 240 249 46 237 239 101 20
104 243 66 33 0
....

所以真正使用的词典参数是：k = 10, int L = 6, weighting = TF_IDF, scoring = L1_NORM, levelsup=4

---

5.倒排索引

// mvInvertedFile[i]表示包含了第i个word id的所有关键帧
std::vector<list<KeyFrame*> > mvInvertedFile;
直接索引表direct index 和 逆向（倒排）索引表inverse index。
直接索引表 是以 图像为基础的，存储图像中的特征以及和该图像相关联的节点 node（vocabulary tree的某一层级）。
直接索引的优势：
通过同一个节点下特征点描述子对应关系 加速几何关系验证，比如用BOW快速匹配
逆向（倒排）索引表 是以 Word 为基础，存储Word的权重以及该Word出现在哪个图像里。这对于查询数据库非常方便，因为它可以很方便
对比哪些图像有共同的Word

三、局部建图线程中搜索匹配三角化总结

对应函数 ORBmatcher::SearchForTriangulation

闭环线程中Sim3搜索匹配，在闭环线程中使用。目的是为了进一步搜索匹配更多地图点，对应函数 SearchBySim3