SLAM常见问题(一):SearchByBoW

ORB-SLAM中使用了多种特征匹配的奇技淫巧,其中之一就是利用词袋信息进行引导匹配SearchByBoW:利用了BOW里的正向引导进行两帧之间的匹配,核心点在于位于同一个节点处的特征才有可能属于同一匹配,相较于暴力匹配匹配速度更快。

注意:每幅图像都可以通过ComputeBoW得到其对应的词袋向量。featureVector存储的是节点的索引值以及对应图像feature对应的索引向量,即map<node_id,vector<featureID>。这样的话就可以根据两帧图像的node_id来初步确定二者共有的特征点,然后根据该id取出vector<featureID>,根据featureID找到图像上的特征点以及描述子,通过比较二者描述子距离来判定该特征点是否为匹配点,若距离小于某一阈值,则二者为匹配对。

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
/**
* @brief Bag of Words Representation
* 计算词袋mBowVec和mFeatVec
* @see CreateInitialMapMonocular() TrackReferenceKeyFrame() Relocalization()
*/
//同样的,关键帧也有构造函数void KeyFrame::ComputeBoW()
void Frame::ComputeBoW()
{
if(mBowVec.empty())
{
// 将描述子mDescriptors转换为DBOW要求的输入格式
vector<cv::Mat> vCurrentDesc = Converter::toDescriptorVector(mDescriptors);
// 转换成词袋向量mBowVec以及特征向量mFeatVec
// mBowVec存储着单词及其对应的权重TF-IDF值
// mFeatVec存储节点ID以及对应对应图像feature对应的索引向量
mpORBvocabulary->transform(vCurrentDesc,mBowVec,mFeatVec,4);
}
}

该函数在Tracking线程中的TrackReferenceKeyFrame()/Relocalization()进行调用(注意:LoopClosing线程中ComputeSim3()也会调用该函数,与上述二者的区别在于,ComputeSim3()中的SearchByBoW是寻找关键帧之间的匹配,而非关键帧与当前帧之间的匹配)。

下面给出ORB-SLAM2中用于关键帧与当前帧进行词袋引导匹配的源码:

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
/**
* @brief 通过词袋,对关键帧的特征点进行跟踪
*
* 通过bow对pKF和F中的特征点进行快速匹配(不属于同一node的特征点直接跳过匹配) \n
* 对属于同一node的特征点通过描述子距离进行匹配 \n
* 根据匹配,用pKF中特征点对应的MapPoint去更新F中特征点对应的MapPoints \n
* 每个特征点都对应一个MapPoint,因此pKF中每个特征点的MapPoint也就是F中对应点的MapPoint \n
* 通过距离阈值、比例阈值和角度投票进行剔除误匹配
* @param pKF KeyFrame
* @param F Current Frame
* @param vpMapPointMatches F中MapPoints对应的匹配,NULL表示未匹配
* @return 成功匹配的数量
*/
//const int ORBmatcher::TH_HIGH = 100;
//const int ORBmatcher::TH_LOW = 50;
//const int ORBmatcher::HISTO_LENGTH = 30;
int ORBmatcher::SearchByBoW(KeyFrame* pKF,Frame &F, vector<MapPoint*> &vpMapPointMatches)
{
// vpMapPointsKF:获取输入关键帧匹配到的地图点
const vector<MapPoint*> vpMapPointsKF = pKF->GetMapPointMatches();

// 初始化当前帧MapPoints对应的匹配NULL
vpMapPointMatches = vector<MapPoint*>(F.N,static_cast<MapPoint*>(NULL));

// FeatureVector数据类型 map<node_id,vector<featureID>,
// 可以快速根据node_id找到属于该node的特征点
const DBoW2::FeatureVector &vFeatVecKF = pKF->mFeatVec;

int nmatches=0;

vector<int> rotHist[HISTO_LENGTH];
for(int i=0;i<HISTO_LENGTH;i++)
rotHist[i].reserve(500);
const float factor = HISTO_LENGTH/360.0f;

// We perform the matching over ORB that belong to the same vocabulary node
// (at a certain level)
// 建立迭代器,将属于同一节点(特定层)的ORB特征进行匹配
DBoW2::FeatureVector::const_iterator KFit = vFeatVecKF.begin();
DBoW2::FeatureVector::const_iterator Fit = F.mFeatVec.begin();
DBoW2::FeatureVector::const_iterator KFend = vFeatVecKF.end();
DBoW2::FeatureVector::const_iterator Fend = F.mFeatVec.end();

while(KFit != KFend && Fit != Fend)
{
// 步骤1:分别取出属于同一node的ORB特征点(只有属于同一node,才有可能是匹配点)
// first表示node_id,只有node_id相同才表示这些特征点位于同一层
if(KFit->first == Fit->first)
{
// second中记录了这些特征对应图像中的ID
const vector<unsigned int> vIndicesKF = KFit->second;
const vector<unsigned int> vIndicesF = Fit->second;

// 步骤2:遍历KF中属于该node的特征点
for(size_t iKF=0; iKF<vIndicesKF.size(); iKF++)
{
// 获取关键帧上某一个特征点的ID
const unsigned int realIdxKF = vIndicesKF[iKF];
// 根据该ID得到该特征对应的MapPoint
MapPoint* pMP = vpMapPointsKF[realIdxKF];

if(!pMP) //不存在
continue;

if(pMP->isBad())//被标记为坏点
continue;

// 根据该ID取出KF中该特征对应的描述子
const cv::Mat &dKF= pKF->mDescriptors.row(realIdxKF);

int bestDist1=256; // 最好的距离(最小距离)
int bestIdxF =-1 ;
int bestDist2=256; // 倒数第二好距离(倒数第二小距离)

// 步骤3:遍历当前帧中属于该node的特征点,找到了最佳匹配点
for(size_t iF=0; iF<vIndicesF.size(); iF++)
{
// 取出当前帧上位于该node上的某一个特征点的ID
const unsigned int realIdxF = vIndicesF[iF];
// 表明这个点已经被匹配过了,不再匹配,加快速度
if(vpMapPointMatches[realIdxF])
continue;
// 取出F中该特征对应的描述子
const cv::Mat &dF = F.mDescriptors.row(realIdxF);

// 计算描述子距离,这里是汉明距离,若非二进制描述子可选择用其他距离
const int dist = DescriptorDistance(dKF,dF);

// 下面的操作就是分别获得最小bestDist1以及次小bestDist2的描述子距离
// dist < bestDist1 < bestDist2,更新bestDist1 bestDist2
if(dist<bestDist1)
{
bestDist2=bestDist1;
bestDist1=dist;
bestIdxF=realIdxF;
}
else if(dist<bestDist2)// bestDist1 < dist < bestDist2,更新bestDist2
{
bestDist2=dist;
}
}

// 步骤4:根据描述子距离阈值和角度投票剔除误匹配
// 最小的描述子距离小于一个阈值
if(bestDist1<=TH_LOW)
{
// trick!
// 最佳匹配比次佳匹配明显要好,那么最佳匹配才真正靠谱
if(static_cast<float>(bestDist1)<
mfNNratio*static_cast<float>(bestDist2))
{
// 步骤5:更新当前帧特征点的MapPoint
// 记录了特征点ID以及对应的特征点
vpMapPointMatches[bestIdxF]=pMP;

// 获得关键帧上的特征点位置
const cv::KeyPoint &kp = pKF->mvKeysUn[realIdxKF];

//
if(mbCheckOrientation)
{
// trick!
// angle:每个特征点在提取描述子时的旋转主方向角度,
// 如果图像旋转了,这个角度将发生改变
// 所有的特征点的角度变化应该是一致的,
// 通过直方图统计得到最准确的角度变化值
// 该特征点的角度变化值
float rot = kp.angle-F.mvKeys[bestIdxF].angle;
if(rot<0.0)
rot+=360.0f;
int bin = round(rot*factor);// 将rot分配到bin组
if(bin==HISTO_LENGTH)
bin=0;
assert(bin>=0 && bin<HISTO_LENGTH);
rotHist[bin].push_back(bestIdxF);
}
// 匹配点+1
nmatches++;
}
}

}

KFit++;
Fit++;
}
else if(KFit->first < Fit->first)
{
KFit = vFeatVecKF.lower_bound(Fit->first);
}
else
{
Fit = F.mFeatVec.lower_bound(KFit->first);
}
}

// 根据方向剔除误匹配的点,即删除那些不属于特征点角度变化最多的三个类别的匹配点
if(mbCheckOrientation)
{
int ind1=-1;
int ind2=-1;
int ind3=-1;

// 计算rotHist中最大的三个的index
ComputeThreeMaxima(rotHist,HISTO_LENGTH,ind1,ind2,ind3);

for(int i=0; i<HISTO_LENGTH; i++)
{
// 如果特征点的旋转角度变化量属于这三个组,则保留
if(i==ind1 || i==ind2 || i==ind3)
continue;

// 将除了ind1 ind2 ind3以外的匹配点去掉
for(size_t j=0, jend=rotHist[i].size(); j<jend; j++)
{
vpMapPointMatches[rotHist[i][j]]=static_cast<MapPoint*>(NULL);
nmatches--;
}
}
}

return nmatches;
}

另外,LoopClosing线程中ComputeSim3()调用的SearchByBoW的函数声明为:

1
int ORBmatcher::SearchByBoW(KeyFrame *pKF1, KeyFrame *pKF2, vector<MapPoint *> &vpMatches12)

参考链接

  1. https://blog.csdn.net/qq_24893115/article/details/52629248