关于ORBSLAM2的学习已经有一段时间了，开始是照着网上各路大神的博客，对着代码梳理了下流程，算是对这个代码的工作过程有了个了解，接下来详细阅读源码，学习各部分的具体实现。这部分是特征点的提取和描述子的计算。
ORBSLAM使用的是ORB特征，即Oriented FAST and Rotated BRIEF，是对FAST特征点与BREIF特征描述子的一种结合与改进。关于特征点其实就是点的提取以及描述子的计算，这两部分网上都有很多详细的博客介绍，高博的书里也比较介绍了这部分的原理，这里就不废话了，贴几个写的比较好的博客：
-
http://www.cnblogs.com/ronny/p/4083537.html
-
http://www.cnblogs.com/ronny/p/4078710.html
-
http://www.cnblogs.com/ronny/p/4081362.html
本文主要详细介绍下ORBSLAM2代码中的实现，首先是头文件的定义
ORBextractor.h
复制代码
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// 分配四叉树时用到的结点类型 class ExtractorNode { public: ExtractorNode() : bNoMore(false) {} void DivideNode(ExtractorNode &n1, ExtractorNode &n2, ExtractorNode &n3, ExtractorNode &n4); std::vector<cv::KeyPoint> vKeys; cv::Point2i UL, UR, BL, BR; std::list<ExtractorNode>::iterator lit; bool bNoMore; }; class ORBextractor { public: enum {HARRIS_SCORE=0, FAST_SCORE=1 }; //nfeatures ORB特征点数量 scaleFactor相邻层的放大倍数 nlevels层数 iniThFAST提取FAST角点时初始阈值 minThFAST提取FAST角点时更小的阈值 //设置两个阈值的原因是在FAST提取角点进行分块后有可能在某个块中在原始阈值情况下提取不到角点，使用更小的阈值进一步提取 //构造函数 ORBextractor(int nfeatures, float scaleFactor, int nlevels, int iniThFAST, int minThFAST); ~ORBextractor(){} // Compute the ORB features and descriptors on an image. // ORB are dispersed on the image using an octree. // Mask is ignored in the current implementation. //重载了()运算符，作为对外接口 void operator()( cv::InputArray image, cv::InputArray mask, std::vector<cv::KeyPoint>& keypoints, cv::OutputArray descriptors); int inline GetLevels(){ return nlevels;} float inline GetScaleFactor(){ return scaleFactor;} std::vector<float> inline GetScaleFactors(){ return mvScaleFactor; } std::vector<float> inline GetInverseScaleFactors(){ return mvInvScaleFactor; } std::vector<float> inline GetScaleSigmaSquares(){ return mvLevelSigma2; } std::vector<float> inline GetInverseScaleSigmaSquares(){ return mvInvLevelSigma2; } //图像金字塔 存放各层的图片 std::vector<cv::Mat> mvImagePyramid; protected: //计算高斯金字塔 void ComputePyramid(cv::Mat image); //计算关键点并用四叉树进行存储 void ComputeKeyPointsOctTree(std::vector<std::vector<cv::KeyPoint> >& allKeypoints); //将关键点分配到四叉树 std::vector<cv::KeyPoint> DistributeOctTree(const std::vector<cv::KeyPoint>& vToDistributeKeys, const int &minX, const int &maxX, const int &minY, const int &maxY, const int &nFeatures, const int &level); void ComputeKeyPointsOld(std::vector<std::vector<cv::KeyPoint> >& allKeypoints); //存储关键点附近patch的点对 std::vector<cv::Point> pattern; //提取特征点的最大数量 int nfeatures; //每层之间的缩放比例 double scaleFactor; //高斯金字塔的层数 int nlevels; //iniThFAST提取FAST角点时初始阈值 int iniThFAST; //minThFAST提取FAST角点时更小的阈值 int minThFAST; //每层的特征数量 std::vector<int> mnFeaturesPerLevel; //Patch圆的最大坐标 std::vector<int> umax; //每层的相对于原始图像的缩放比例 std::vector<float> mvScaleFactor; //每层的相对于原始图像的缩放比例的倒数 std::vector<float> mvInvScaleFactor; std::vector<float> mvLevelSigma2; std::vector<float> mvInvLevelSigma2; };
该类实现了ORB特征点的提取，外部调用时只需要调用ORB的构造函数ORBextractor(int nfeatures, float scaleFactor, int nlevels,
int iniThFAST, int minThFAST);
和外部接口函数void operator()( cv::InputArray image, cv::InputArray mask,std::vector<cv::KeyPoint>& keypoints,cv::OutputArray descriptors);其他的各个参数的含义在代码注释中已经给出。
首先看构造函数：
复制代码
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
78ORBextractor::ORBextractor(int _nfeatures, float _scaleFactor, int _nlevels, int _iniThFAST, int _minThFAST): nfeatures(_nfeatures), scaleFactor(_scaleFactor), nlevels(_nlevels), iniThFAST(_iniThFAST), minThFAST(_minThFAST) { //计算每一层相对于原始图片的放大倍数 mvScaleFactor.resize(nlevels); mvLevelSigma2.resize(nlevels); mvScaleFactor[0]=1.0f; mvLevelSigma2[0]=1.0f; for(int i=1; i<nlevels; i++) { mvScaleFactor[i]=mvScaleFactor[i-1]*scaleFactor; mvLevelSigma2[i]=mvScaleFactor[i]*mvScaleFactor[i]; } //计算每一层相对于原始图片放大倍数的逆 mvInvScaleFactor.resize(nlevels); mvInvLevelSigma2.resize(nlevels); for(int i=0; i<nlevels; i++) { mvInvScaleFactor[i]=1.0f/mvScaleFactor[i]; mvInvLevelSigma2[i]=1.0f/mvLevelSigma2[i]; } mvImagePyramid.resize(nlevels); mnFeaturesPerLevel.resize(nlevels); float factor = 1.0f / scaleFactor; //scaleFactor：1.2 nfeatures：1000 nlevels：8 // 第一层：nfeatures*(1 - factor)/(1 - (float)pow((double)factor, (double)nlevels)) // 第二层：nfeatures*(1 - factor)/(1 - (float)pow((double)factor, (double)nlevels))*q // 第三层：nfeatures*(1 - factor)/(1 - (float)pow((double)factor, (double)nlevels))*q*q // ......... // 第nlevels层：nfeatures*(1 - factor)/(1 - (float)pow((double)factor, (double)nlevels))*q^nlevels // 那么前nlevels层的和为总的特征点数量nfeatures（等比数列的前n相和） // 可以看出ORB特征点是如何实现尺度不变性的，原始图像那一层特征点最多，依次递减 // 主要是将每层的特征点数量进行均匀控制 float nDesiredFeaturesPerScale = nfeatures*(1 - factor)/(1 - (float)pow((double)factor, (double)nlevels)); //计算第一层特征点的数量 int sumFeatures = 0; for( int level = 0; level < nlevels-1; level++ ) { mnFeaturesPerLevel[level] = cvRound(nDesiredFeaturesPerScale); //mnFeaturesPerLevel数组存储每层特征点的数量 sumFeatures += mnFeaturesPerLevel[level]; nDesiredFeaturesPerScale *= factor; } mnFeaturesPerLevel[nlevels-1] = std::max(nfeatures - sumFeatures, 0); //复制训练的模板 const int npoints = 512; const Point* pattern0 = (const Point*)bit_pattern_31_; std::copy(pattern0, pattern0 + npoints, std::back_inserter(pattern)); //This is for orientation 用于计算特征点方向 // pre-compute the end of a row in a circular patch //用于计算特征方向时，每个v坐标对应最大的u坐标 umax.resize(HALF_PATCH_SIZE + 1); // 将v坐标划分为两部分进行计算，主要为了确保计算特征主方向的时候，x,y方向对称 //cvFloor是取不大于参数的最大整数值 int v, v0, vmax = cvFloor(HALF_PATCH_SIZE * sqrt(2.f) / 2 + 1); //cvCeil 是取不小于参数的最小整数值 int vmin = cvCeil(HALF_PATCH_SIZE * sqrt(2.f) / 2); //利用勾股定理计算 const double hp2 = HALF_PATCH_SIZE*HALF_PATCH_SIZE; //V坐标的第一部分 for (v = 0; v <= vmax; ++v) umax[v] = cvRound(sqrt(hp2 - v * v)); // Make sure we are symmetric 确保对称 // V坐标的第二部分 for (v = HALF_PATCH_SIZE, v0 = 0; v >= vmin; --v) { while (umax[v0] == umax[v0 + 1]) ++v0; umax[v] = v0; ++v0; } }
构造函数中传入基本参数，如提取的特征点数量、高斯金字塔图像之间的比例因子、金字塔层数及角点检测阈值等，为了防止出现检测不到角点的情况，特别设置了两个阈值。在构造函数中，计算了图像金字塔每一层图像的缩放倍数、每一层图像的特征点数量（通过等比数列的方式）、接下来计算umax参数，用于后续计算特征点主方向和描述子时用到。
接下来是对外的特征提取接口，即重载的operator()运算符：
复制代码
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
63void ORBextractor::operator()( InputArray _image, InputArray _mask, vector<KeyPoint>& _keypoints, OutputArray _descriptors) { if(_image.empty()) return; Mat image = _image.getMat(); assert(image.type() == CV_8UC1 ); // Pre-compute the scale pyramid 构建高斯金字塔 ComputePyramid(image); //计算关键点并生成四叉树 vector < vector<KeyPoint> > allKeypoints; ComputeKeyPointsOctTree(allKeypoints); //ComputeKeyPointsOld(allKeypoints); Mat descriptors; int nkeypoints = 0; for (int level = 0; level < nlevels; ++level) nkeypoints += (int)allKeypoints[level].size(); if( nkeypoints == 0 ) _descriptors.release(); else { _descriptors.create(nkeypoints, 32, CV_8U); descriptors = _descriptors.getMat(); } _keypoints.clear(); _keypoints.reserve(nkeypoints); //计算每个关键点对应的描述子 int offset = 0; for (int level = 0; level < nlevels; ++level) //逐层计算 { vector<KeyPoint>& keypoints = allKeypoints[level]; int nkeypointsLevel = (int)keypoints.size(); if(nkeypointsLevel==0) continue; // preprocess the resized image Mat workingMat = mvImagePyramid[level].clone(); // 进行高斯模糊，用BORDER_REFLECT_101方法处理边缘 GaussianBlur(workingMat, workingMat, Size(7, 7), 2, 2, BORDER_REFLECT_101); // Compute the descriptors Mat desc = descriptors.rowRange(offset, offset + nkeypointsLevel); //offset是偏移量，此处取出的是该层的描述子， //计算描述子 computeDescriptors(workingMat, keypoints, desc, pattern); offset += nkeypointsLevel; // Scale keypoint coordinates if (level != 0) { float scale = mvScaleFactor[level]; for (vector<KeyPoint>::iterator keypoint = keypoints.begin(), keypointEnd = keypoints.end(); keypoint != keypointEnd; ++keypoint) keypoint->pt *= scale; // 按缩放因子进行坐标缩放 } // And add the keypoints to the output _keypoints.insert(_keypoints.end(), keypoints.begin(), keypoints.end()); } }
这个函数主要做了这么几件事：
- 计算高斯图像金字塔
- 提取特征点并生成四叉树
- 逐层计算特征点描述子
一个一个分析，首先构建高斯金字塔，主要是根据缩放因子对图像进行缩放处理，这个比较简单，直接计算每层图像应该有的尺寸，然后调用resize函数实现
复制代码
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24void ORBextractor::ComputePyramid(cv::Mat image) { // 每一层根据尺度因子重新计算图像大小，之后调用resize函数 // 计算结果存入std::vector<cv::Mat> mvImagePyramid 中 for (int level = 0; level < nlevels; ++level) { float scale = mvInvScaleFactor[level]; Size sz(cvRound((float) image.cols * scale), cvRound((float) image.rows * scale)); Size wholeSize(sz.width + EDGE_THRESHOLD * 2, sz.height + EDGE_THRESHOLD * 2); Mat temp(wholeSize, image.type()), masktemp; mvImagePyramid[level] = temp(Rect(EDGE_THRESHOLD, EDGE_THRESHOLD, sz.width, sz.height)); // Compute the resized image if (level != 0) { resize(mvImagePyramid[level - 1], mvImagePyramid[level], sz, 0, 0, INTER_LINEAR); copyMakeBorder(mvImagePyramid[level], temp, EDGE_THRESHOLD, EDGE_THRESHOLD, EDGE_THRESHOLD, EDGE_THRESHOLD, BORDER_REFLECT_101 + BORDER_ISOLATED); } else { copyMakeBorder(image, temp, EDGE_THRESHOLD, EDGE_THRESHOLD, EDGE_THRESHOLD, EDGE_THRESHOLD, BORDER_REFLECT_101); } } }
第二步是提取特征点，作者将金字塔中每一张图像按照尺寸分成一定大小的格子cell，然后在每一个cell内提取FAST特征点，目的是保证特征点的均匀分布，不至于过度集中，之后将一层中的特征点分配到四叉树中，所有层的特征点提取完毕后，统一计算特征点的主方向。
复制代码
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
93void ORBextractor::ComputeKeyPointsOctTree(vector<vector<KeyPoint> >& allKeypoints) { allKeypoints.resize(nlevels); //设定每个格子的大小 const float W = 30; //每层分别提取特征点 for (int level = 0; level < nlevels; ++level) { //设定该层图像中检测的X，Y最大最小坐标 const int minBorderX = EDGE_THRESHOLD-3; const int minBorderY = minBorderX; const int maxBorderX = mvImagePyramid[level].cols-EDGE_THRESHOLD+3; const int maxBorderY = mvImagePyramid[level].rows-EDGE_THRESHOLD+3; // 用于分配的关键点 vector<cv::KeyPoint> vToDistributeKeys; vToDistributeKeys.reserve(nfeatures*10); const float width = (maxBorderX-minBorderX); const float height = (maxBorderY-minBorderY); // 将待检测区域划分为格子的行列数 const int nCols = width/W; const int nRows = height/W; // 重新计算每个格子的大小 const int wCell = ceil(width/nCols); const int hCell = ceil(height/nRows); // 在每个格子内进行fast特征检测 for(int i=0; i<nRows; i++) { const float iniY =minBorderY+i*hCell; float maxY = iniY+hCell+6; if(iniY>=maxBorderY-3) continue; if(maxY>maxBorderY) maxY = maxBorderY; for(int j=0; j<nCols; j++) { const float iniX =minBorderX+j*wCell; float maxX = iniX+wCell+6; if(iniX>=maxBorderX-6) continue; if(maxX>maxBorderX) maxX = maxBorderX; vector<cv::KeyPoint> vKeysCell; FAST(mvImagePyramid[level].rowRange(iniY,maxY).colRange(iniX,maxX), vKeysCell,iniThFAST,true); // 如果检测为空就降低阈值再进行检测 if(vKeysCell.empty()) { FAST(mvImagePyramid[level].rowRange(iniY,maxY).colRange(iniX,maxX), vKeysCell,minThFAST,true); } //如果检测不为空就将检测到的特征点放到vToDistributeKeys中 if(!vKeysCell.empty()) { for(vector<cv::KeyPoint>::iterator vit=vKeysCell.begin(); vit!=vKeysCell.end();vit++) { (*vit).pt.x+=j*wCell; (*vit).pt.y+=i*hCell; vToDistributeKeys.push_back(*vit); } } } } vector<KeyPoint> & keypoints = allKeypoints[level]; keypoints.reserve(nfeatures); keypoints = DistributeOctTree(vToDistributeKeys, minBorderX, maxBorderX, minBorderY, maxBorderY,mnFeaturesPerLevel[level], level); //计算特征点Patch的大小，根据每层的尺度的不同而不同 const int scaledPatchSize = PATCH_SIZE*mvScaleFactor[level]; // Add border to coordinates and scale information //考虑边界信息进去计算特征点的位置 const int nkps = keypoints.size(); for(int i=0; i<nkps ; i++) { keypoints[i].pt.x+=minBorderX; //x坐标 keypoints[i].pt.y+=minBorderY; //y坐标 keypoints[i].octave=level; //特征点所在的层数 keypoints[i].size = scaledPatchSize;//特征点Patch的大小将来计算描述子时使用 } } // compute orientations 计算特征点的方向 for (int level = 0; level < nlevels; ++level) computeOrientation(mvImagePyramid[level], allKeypoints[level], umax); }
关键部分在于将提取到的特征点分配给四叉树，保证每个结点里都有一个特征，而如果某个结点内的特征比较多的话，则计算特征点的响应值，保留最大的，实现了对特征的均匀化。
复制代码
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
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223std::vector<cv::KeyPoint> ORBextractor::distributeQuadTree(const std::vector<cv::KeyPoint>& vec_to_distribute_keys, const int &min_x, const int &max_x, const int &min_y, const int &max_y, const int &feature_num, const int &level) { // 计算初始时有几个节点 const int init_node_num = round(static_cast<float>(max_x - min_x) / (max_y - min_y)); // 得到节点之间的间隔 const float interval_x = static_cast<float>(max_x - min_x) / init_node_num; std::vector<ExtractorNode*> init_nodes; init_nodes.resize(init_node_num); // 划分之后包含的节点 std::list<ExtractorNode> list_nodes; for (int i = 0; i < init_node_num; i++) { ExtractorNode ni; ni.UL_ = cv::Point2i(interval_x*static_cast<float>(i), 0); ni.UR_ = cv::Point2i(interval_x*static_cast<float>(i + 1), 0); ni.BL_ = cv::Point2i(ni.UL_.x, max_y - min_y); ni.BR_ = cv::Point2i(ni.UR_.x, max_y - min_y); ni.vec_keys_.reserve(vec_to_distribute_keys.size()); list_nodes.push_back(ni); init_nodes[i] = &list_nodes.back(); } //将点分配给子节点 for (size_t i = 0; i < vec_to_distribute_keys.size(); i++) { const cv::KeyPoint &kp = vec_to_distribute_keys[i]; init_nodes[kp.pt.x / interval_x]->vec_keys_.push_back(kp); } std::list<ExtractorNode>::iterator lit = list_nodes.begin(); while (lit != list_nodes.end()) { // 如果只含一个特征点的时候，则不再划分 if (lit->vec_keys_.size() == 1) { lit->is_no_more_ = true; lit++; } else if (lit->vec_keys_.empty()) lit = list_nodes.erase(lit); else lit++; } bool is_finish = false; int iteration = 0; std::vector<std::pair<int, ExtractorNode*> > keys_size_and_node;//节点及对应包含的特征数 keys_size_and_node.reserve(list_nodes.size() * 4); while (!is_finish) { iteration++; // 初始节点个数，用于判断是否节点再一次进行了划分 int prev_size = list_nodes.size(); lit = list_nodes.begin(); // 表示节点分解次数 int to_expand_num = 0; keys_size_and_node.clear(); while (lit != list_nodes.end()) { if (lit->is_no_more_) { // 表面只有一个特征点，则不再划分 lit++; continue; } else { // 如果超过一个特征点，则继续划分 ExtractorNode n1, n2, n3, n4; lit->divideNode(n1, n2, n3, n4); // 对划分之后的节点进行判断，是否含有特征点，含有特征点则添加节点 if (n1.vec_keys_.size() > 0) { list_nodes.push_front(n1); if (n1.vec_keys_.size() > 1) { to_expand_num++; keys_size_and_node.push_back(std::make_pair(n1.vec_keys_.size(), &list_nodes.front())); list_nodes.front().node_iter_ = list_nodes.begin(); } } if (n2.vec_keys_.size() > 0) { list_nodes.push_front(n2); if (n2.vec_keys_.size() > 1) { to_expand_num++; keys_size_and_node.push_back(std::make_pair(n2.vec_keys_.size(), &list_nodes.front())); list_nodes.front().node_iter_ = list_nodes.begin(); } } if (n3.vec_keys_.size() > 0) { list_nodes.push_front(n3); if (n3.vec_keys_.size() > 1) { to_expand_num++; keys_size_and_node.push_back(std::make_pair(n3.vec_keys_.size(), &list_nodes.front())); list_nodes.front().node_iter_ = list_nodes.begin(); } } if (n4.vec_keys_.size() > 0) { list_nodes.push_front(n4); if (n4.vec_keys_.size() > 1) { to_expand_num++; keys_size_and_node.push_back(std::make_pair(n4.vec_keys_.size(), &list_nodes.front())); list_nodes.front().node_iter_ = list_nodes.begin(); } } lit = list_nodes.erase(lit); continue; } } // 当节点个数大于需分配的特征数或者所有的节点只有一个特征点（节点不能划分）的时候，则结束。 if ((int)list_nodes.size() >= feature_num || (int)list_nodes.size() == prev_size) { is_finish = true; } else if (((int)list_nodes.size() + to_expand_num * 3) > feature_num)//节点展开次数乘以3用于表明下一次的节点分解可能超过特征数，即为最后一次分解 { while (!is_finish) { prev_size = list_nodes.size(); std::vector<std::pair<int, ExtractorNode*> > prev_size_and_node = keys_size_and_node; keys_size_and_node.clear(); sort(prev_size_and_node.begin(), prev_size_and_node.end()); for (int j = prev_size_and_node.size() - 1; j >= 0; j--) { ExtractorNode n1, n2, n3, n4; prev_size_and_node[j].second->divideNode(n1, n2, n3, n4); // 划分之后进一步的判断 if (n1.vec_keys_.size() > 0) { list_nodes.push_front(n1); if (n1.vec_keys_.size() > 1) { keys_size_and_node.push_back(std::make_pair(n1.vec_keys_.size(), &list_nodes.front())); list_nodes.front().node_iter_ = list_nodes.begin(); } } if (n2.vec_keys_.size() > 0) { list_nodes.push_front(n2); if (n2.vec_keys_.size() > 1) { keys_size_and_node.push_back(std::make_pair(n2.vec_keys_.size(), &list_nodes.front())); list_nodes.front().node_iter_ = list_nodes.begin(); } } if (n3.vec_keys_.size() > 0) { list_nodes.push_front(n3); if (n3.vec_keys_.size() > 1) { keys_size_and_node.push_back(std::make_pair(n3.vec_keys_.size(), &list_nodes.front())); list_nodes.front().node_iter_ = list_nodes.begin(); } } if (n4.vec_keys_.size() > 0) { list_nodes.push_front(n4); if (n4.vec_keys_.size() > 1) { keys_size_and_node.push_back(std::make_pair(n4.vec_keys_.size(), &list_nodes.front())); list_nodes.front().node_iter_ = list_nodes.begin(); } } list_nodes.erase(prev_size_and_node[j].second->node_iter_); if ((int)list_nodes.size() >= feature_num) break; } if ((int)list_nodes.size() >= feature_num || (int)list_nodes.size() == prev_size) is_finish = true; } } } // 保留每个节点下最好的特征点 std::vector<cv::KeyPoint> result_keys; result_keys.reserve(features_num_); for (std::list<ExtractorNode>::iterator lit = list_nodes.begin(); lit != list_nodes.end(); lit++) { std::vector<cv::KeyPoint> &node_keys = lit->vec_keys_; cv::KeyPoint* keypoint = &node_keys[0]; float max_response = keypoint->response; for (size_t k = 1; k < node_keys.size(); k++) { if (node_keys[k].response > max_response) { keypoint = &node_keys[k]; max_response = node_keys[k].response; } } result_keys.push_back(*keypoint); } return result_keys; }
特征提取完之后，对金字塔的每一层图像中的特征点进行特征点方向计算，添加旋转不变性。
复制代码
1
2
3
4
5
6
7static void computeOrientation(const Mat &image, vector<KeyPoint> &keypoints, const vector<int> &umax) { for (vector<KeyPoint>::iterator keypoint = keypoints.begin(), keypointEnd = keypoints.end(); keypoint != keypointEnd; ++keypoint) { keypoint->angle = IC_Angle(image, keypoint->pt, umax); } }
具体计算过程：
复制代码
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// 灰度质心法计算特征点方向 static float IC_Angle(const cv::Mat& image, cv::Point2f pt, const std::vector<int> & u_max) { int m_01 = 0, m_10 = 0; // 得到中心位置 const uchar* center = &image.at<uchar>(cvRound(pt.y), cvRound(pt.x)); // 对 v=0 这一行单独计算 for (int u = -HALF_PATCH_SIZE; u <= HALF_PATCH_SIZE; ++u) m_10 += u * center[u]; // 要注意图像的step不一定是图像的宽度 int step = (int)image.step1(); for (int v = 1; v <= HALF_PATCH_SIZE; ++v) { // 上下和左右两条线同时计算 int v_sum = 0; int d = u_max[v]; for (int u = -d; u <= d; ++u) { int val_plus = center[u + v*step], val_minus = center[u - v*step]; v_sum += (val_plus - val_minus); //计算上下的时候v是有符号的，所以这边是减 m_10 += u * (val_plus + val_minus); //这边加是由于u本身有符号 } m_01 += v * v_sum; } return cv::fastAtan2((float)m_01, (float)m_10); }
计算时相当于以图像的u轴为对称轴，循环v后每次计算对称的两行，具体计算符号在代码中标出。
最后则是计算描述子，按照预定的选取点对的方式，比较所选的点对的灰度值大小。
复制代码
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
47static void computeOrbDescriptor(const KeyPoint &kpt, const Mat &img, const Point *pattern, uchar *desc) { float angle = (float) kpt.angle * factorPI; float a = (float) cos(angle), b = (float) sin(angle); const uchar *center = &img.at<uchar>(cvRound(kpt.pt.y), cvRound(kpt.pt.x)); const int step = (int) img.step; #define GET_VALUE(idx) center[cvRound(pattern[idx].x*b + pattern[idx].y*a)*step + cvRound(pattern[idx].x*a - pattern[idx].y*b)] for (int i = 0; i < 32; ++i, pattern += 16) { int t0, t1, val; t0 = GET_VALUE(0); t1 = GET_VALUE(1); val = t0 < t1; t0 = GET_VALUE(2); t1 = GET_VALUE(3); val |= (t0 < t1) << 1; t0 = GET_VALUE(4); t1 = GET_VALUE(5); val |= (t0 < t1) << 2; t0 = GET_VALUE(6); t1 = GET_VALUE(7); val |= (t0 < t1) << 3; t0 = GET_VALUE(8); t1 = GET_VALUE(9); val |= (t0 < t1) << 4; t0 = GET_VALUE(10); t1 = GET_VALUE(11); val |= (t0 < t1) << 5; t0 = GET_VALUE(12); t1 = GET_VALUE(13); val |= (t0 < t1) << 6; t0 = GET_VALUE(14); t1 = GET_VALUE(15); val |= (t0 < t1) << 7; desc[i] = (uchar) val; } #undef GET_VALUE }
最后
以上就是鲜艳酸奶最近收集整理的关于ORBSLAM2源码学习（1）ORBextractor的全部内容，更多相关ORBSLAM2源码学习（1）ORBextractor内容请搜索靠谱客的其他文章。
本图文内容来源于网友提供，作为学习参考使用，或来自网络收集整理，版权属于原作者所有。
发表评论 取消回复