ORBextractor.cc
Go to the documentation of this file.
1 
65 #include <opencv2/core/core.hpp>
66 #include <opencv2/highgui/highgui.hpp>
67 #include <opencv2/features2d/features2d.hpp>
68 #include <opencv2/imgproc/imgproc.hpp>
69 #include <vector>
70 #include <algorithm>
71 #include <iterator>
72 #include <set>
73 
74 #include "ORBextractor.h"
75 
76 
77 using namespace cv;
78 using namespace std;
79 
80 namespace rtabmap
81 {
82 
83 static float IC_Angle(const Mat& image, Point2f pt, const vector<int> & u_max, int halfPatchSize)
84 {
85  int m_01 = 0, m_10 = 0;
86 
87  const uchar* center = &image.at<uchar> (cvRound(pt.y), cvRound(pt.x));
88 
89  // Treat the center line differently, v=0
90  for (int u = -halfPatchSize; u <= halfPatchSize; ++u)
91  m_10 += u * center[u];
92 
93  // Go line by line in the circuI853lar patch
94  int step = (int)image.step1();
95  for (int v = 1; v <= halfPatchSize; ++v)
96  {
97  // Proceed over the two lines
98  int v_sum = 0;
99  int d = u_max[v];
100  for (int u = -d; u <= d; ++u)
101  {
102  int val_plus = center[u + v*step], val_minus = center[u - v*step];
103  v_sum += (val_plus - val_minus);
104  m_10 += u * (val_plus + val_minus);
105  }
106  m_01 += v * v_sum;
107  }
108 
109  return fastAtan2((float)m_01, (float)m_10);
110 }
111 
112 
113 const float factorPI = (float)(CV_PI/180.f);
114 static void computeOrbDescriptor(const KeyPoint& kpt,
115  const Mat& img, const Point* pattern,
116  uchar* desc)
117 {
118  float angle = (float)kpt.angle*factorPI;
119  float a = (float)cos(angle), b = (float)sin(angle);
120 
121  const uchar* center = &img.at<uchar>(cvRound(kpt.pt.y), cvRound(kpt.pt.x));
122  const int step = (int)img.step;
123 
124  #define GET_VALUE(idx) \
125  center[cvRound(pattern[idx].x*b + pattern[idx].y*a)*step + \
126  cvRound(pattern[idx].x*a - pattern[idx].y*b)]
127 
128 
129  for (int i = 0; i < 32; ++i, pattern += 16)
130  {
131  int t0, t1, val;
132  t0 = GET_VALUE(0); t1 = GET_VALUE(1);
133  val = t0 < t1;
134  t0 = GET_VALUE(2); t1 = GET_VALUE(3);
135  val |= (t0 < t1) << 1;
136  t0 = GET_VALUE(4); t1 = GET_VALUE(5);
137  val |= (t0 < t1) << 2;
138  t0 = GET_VALUE(6); t1 = GET_VALUE(7);
139  val |= (t0 < t1) << 3;
140  t0 = GET_VALUE(8); t1 = GET_VALUE(9);
141  val |= (t0 < t1) << 4;
142  t0 = GET_VALUE(10); t1 = GET_VALUE(11);
143  val |= (t0 < t1) << 5;
144  t0 = GET_VALUE(12); t1 = GET_VALUE(13);
145  val |= (t0 < t1) << 6;
146  t0 = GET_VALUE(14); t1 = GET_VALUE(15);
147  val |= (t0 < t1) << 7;
148 
149  desc[i] = (uchar)val;
150  }
151 
152  #undef GET_VALUE
153 }
154 
155 
156 static int bit_pattern_31_[256*4] =
157 {
158  8,-3, 9,5/*mean (0), correlation (0)*/,
159  4,2, 7,-12/*mean (1.12461e-05), correlation (0.0437584)*/,
160  -11,9, -8,2/*mean (3.37382e-05), correlation (0.0617409)*/,
161  7,-12, 12,-13/*mean (5.62303e-05), correlation (0.0636977)*/,
162  2,-13, 2,12/*mean (0.000134953), correlation (0.085099)*/,
163  1,-7, 1,6/*mean (0.000528565), correlation (0.0857175)*/,
164  -2,-10, -2,-4/*mean (0.0188821), correlation (0.0985774)*/,
165  -13,-13, -11,-8/*mean (0.0363135), correlation (0.0899616)*/,
166  -13,-3, -12,-9/*mean (0.121806), correlation (0.099849)*/,
167  10,4, 11,9/*mean (0.122065), correlation (0.093285)*/,
168  -13,-8, -8,-9/*mean (0.162787), correlation (0.0942748)*/,
169  -11,7, -9,12/*mean (0.21561), correlation (0.0974438)*/,
170  7,7, 12,6/*mean (0.160583), correlation (0.130064)*/,
171  -4,-5, -3,0/*mean (0.228171), correlation (0.132998)*/,
172  -13,2, -12,-3/*mean (0.00997526), correlation (0.145926)*/,
173  -9,0, -7,5/*mean (0.198234), correlation (0.143636)*/,
174  12,-6, 12,-1/*mean (0.0676226), correlation (0.16689)*/,
175  -3,6, -2,12/*mean (0.166847), correlation (0.171682)*/,
176  -6,-13, -4,-8/*mean (0.101215), correlation (0.179716)*/,
177  11,-13, 12,-8/*mean (0.200641), correlation (0.192279)*/,
178  4,7, 5,1/*mean (0.205106), correlation (0.186848)*/,
179  5,-3, 10,-3/*mean (0.234908), correlation (0.192319)*/,
180  3,-7, 6,12/*mean (0.0709964), correlation (0.210872)*/,
181  -8,-7, -6,-2/*mean (0.0939834), correlation (0.212589)*/,
182  -2,11, -1,-10/*mean (0.127778), correlation (0.20866)*/,
183  -13,12, -8,10/*mean (0.14783), correlation (0.206356)*/,
184  -7,3, -5,-3/*mean (0.182141), correlation (0.198942)*/,
185  -4,2, -3,7/*mean (0.188237), correlation (0.21384)*/,
186  -10,-12, -6,11/*mean (0.14865), correlation (0.23571)*/,
187  5,-12, 6,-7/*mean (0.222312), correlation (0.23324)*/,
188  5,-6, 7,-1/*mean (0.229082), correlation (0.23389)*/,
189  1,0, 4,-5/*mean (0.241577), correlation (0.215286)*/,
190  9,11, 11,-13/*mean (0.00338507), correlation (0.251373)*/,
191  4,7, 4,12/*mean (0.131005), correlation (0.257622)*/,
192  2,-1, 4,4/*mean (0.152755), correlation (0.255205)*/,
193  -4,-12, -2,7/*mean (0.182771), correlation (0.244867)*/,
194  -8,-5, -7,-10/*mean (0.186898), correlation (0.23901)*/,
195  4,11, 9,12/*mean (0.226226), correlation (0.258255)*/,
196  0,-8, 1,-13/*mean (0.0897886), correlation (0.274827)*/,
197  -13,-2, -8,2/*mean (0.148774), correlation (0.28065)*/,
198  -3,-2, -2,3/*mean (0.153048), correlation (0.283063)*/,
199  -6,9, -4,-9/*mean (0.169523), correlation (0.278248)*/,
200  8,12, 10,7/*mean (0.225337), correlation (0.282851)*/,
201  0,9, 1,3/*mean (0.226687), correlation (0.278734)*/,
202  7,-5, 11,-10/*mean (0.00693882), correlation (0.305161)*/,
203  -13,-6, -11,0/*mean (0.0227283), correlation (0.300181)*/,
204  10,7, 12,1/*mean (0.125517), correlation (0.31089)*/,
205  -6,-3, -6,12/*mean (0.131748), correlation (0.312779)*/,
206  10,-9, 12,-4/*mean (0.144827), correlation (0.292797)*/,
207  -13,8, -8,-12/*mean (0.149202), correlation (0.308918)*/,
208  -13,0, -8,-4/*mean (0.160909), correlation (0.310013)*/,
209  3,3, 7,8/*mean (0.177755), correlation (0.309394)*/,
210  5,7, 10,-7/*mean (0.212337), correlation (0.310315)*/,
211  -1,7, 1,-12/*mean (0.214429), correlation (0.311933)*/,
212  3,-10, 5,6/*mean (0.235807), correlation (0.313104)*/,
213  2,-4, 3,-10/*mean (0.00494827), correlation (0.344948)*/,
214  -13,0, -13,5/*mean (0.0549145), correlation (0.344675)*/,
215  -13,-7, -12,12/*mean (0.103385), correlation (0.342715)*/,
216  -13,3, -11,8/*mean (0.134222), correlation (0.322922)*/,
217  -7,12, -4,7/*mean (0.153284), correlation (0.337061)*/,
218  6,-10, 12,8/*mean (0.154881), correlation (0.329257)*/,
219  -9,-1, -7,-6/*mean (0.200967), correlation (0.33312)*/,
220  -2,-5, 0,12/*mean (0.201518), correlation (0.340635)*/,
221  -12,5, -7,5/*mean (0.207805), correlation (0.335631)*/,
222  3,-10, 8,-13/*mean (0.224438), correlation (0.34504)*/,
223  -7,-7, -4,5/*mean (0.239361), correlation (0.338053)*/,
224  -3,-2, -1,-7/*mean (0.240744), correlation (0.344322)*/,
225  2,9, 5,-11/*mean (0.242949), correlation (0.34145)*/,
226  -11,-13, -5,-13/*mean (0.244028), correlation (0.336861)*/,
227  -1,6, 0,-1/*mean (0.247571), correlation (0.343684)*/,
228  5,-3, 5,2/*mean (0.000697256), correlation (0.357265)*/,
229  -4,-13, -4,12/*mean (0.00213675), correlation (0.373827)*/,
230  -9,-6, -9,6/*mean (0.0126856), correlation (0.373938)*/,
231  -12,-10, -8,-4/*mean (0.0152497), correlation (0.364237)*/,
232  10,2, 12,-3/*mean (0.0299933), correlation (0.345292)*/,
233  7,12, 12,12/*mean (0.0307242), correlation (0.366299)*/,
234  -7,-13, -6,5/*mean (0.0534975), correlation (0.368357)*/,
235  -4,9, -3,4/*mean (0.099865), correlation (0.372276)*/,
236  7,-1, 12,2/*mean (0.117083), correlation (0.364529)*/,
237  -7,6, -5,1/*mean (0.126125), correlation (0.369606)*/,
238  -13,11, -12,5/*mean (0.130364), correlation (0.358502)*/,
239  -3,7, -2,-6/*mean (0.131691), correlation (0.375531)*/,
240  7,-8, 12,-7/*mean (0.160166), correlation (0.379508)*/,
241  -13,-7, -11,-12/*mean (0.167848), correlation (0.353343)*/,
242  1,-3, 12,12/*mean (0.183378), correlation (0.371916)*/,
243  2,-6, 3,0/*mean (0.228711), correlation (0.371761)*/,
244  -4,3, -2,-13/*mean (0.247211), correlation (0.364063)*/,
245  -1,-13, 1,9/*mean (0.249325), correlation (0.378139)*/,
246  7,1, 8,-6/*mean (0.000652272), correlation (0.411682)*/,
247  1,-1, 3,12/*mean (0.00248538), correlation (0.392988)*/,
248  9,1, 12,6/*mean (0.0206815), correlation (0.386106)*/,
249  -1,-9, -1,3/*mean (0.0364485), correlation (0.410752)*/,
250  -13,-13, -10,5/*mean (0.0376068), correlation (0.398374)*/,
251  7,7, 10,12/*mean (0.0424202), correlation (0.405663)*/,
252  12,-5, 12,9/*mean (0.0942645), correlation (0.410422)*/,
253  6,3, 7,11/*mean (0.1074), correlation (0.413224)*/,
254  5,-13, 6,10/*mean (0.109256), correlation (0.408646)*/,
255  2,-12, 2,3/*mean (0.131691), correlation (0.416076)*/,
256  3,8, 4,-6/*mean (0.165081), correlation (0.417569)*/,
257  2,6, 12,-13/*mean (0.171874), correlation (0.408471)*/,
258  9,-12, 10,3/*mean (0.175146), correlation (0.41296)*/,
259  -8,4, -7,9/*mean (0.183682), correlation (0.402956)*/,
260  -11,12, -4,-6/*mean (0.184672), correlation (0.416125)*/,
261  1,12, 2,-8/*mean (0.191487), correlation (0.386696)*/,
262  6,-9, 7,-4/*mean (0.192668), correlation (0.394771)*/,
263  2,3, 3,-2/*mean (0.200157), correlation (0.408303)*/,
264  6,3, 11,0/*mean (0.204588), correlation (0.411762)*/,
265  3,-3, 8,-8/*mean (0.205904), correlation (0.416294)*/,
266  7,8, 9,3/*mean (0.213237), correlation (0.409306)*/,
267  -11,-5, -6,-4/*mean (0.243444), correlation (0.395069)*/,
268  -10,11, -5,10/*mean (0.247672), correlation (0.413392)*/,
269  -5,-8, -3,12/*mean (0.24774), correlation (0.411416)*/,
270  -10,5, -9,0/*mean (0.00213675), correlation (0.454003)*/,
271  8,-1, 12,-6/*mean (0.0293635), correlation (0.455368)*/,
272  4,-6, 6,-11/*mean (0.0404971), correlation (0.457393)*/,
273  -10,12, -8,7/*mean (0.0481107), correlation (0.448364)*/,
274  4,-2, 6,7/*mean (0.050641), correlation (0.455019)*/,
275  -2,0, -2,12/*mean (0.0525978), correlation (0.44338)*/,
276  -5,-8, -5,2/*mean (0.0629667), correlation (0.457096)*/,
277  7,-6, 10,12/*mean (0.0653846), correlation (0.445623)*/,
278  -9,-13, -8,-8/*mean (0.0858749), correlation (0.449789)*/,
279  -5,-13, -5,-2/*mean (0.122402), correlation (0.450201)*/,
280  8,-8, 9,-13/*mean (0.125416), correlation (0.453224)*/,
281  -9,-11, -9,0/*mean (0.130128), correlation (0.458724)*/,
282  1,-8, 1,-2/*mean (0.132467), correlation (0.440133)*/,
283  7,-4, 9,1/*mean (0.132692), correlation (0.454)*/,
284  -2,1, -1,-4/*mean (0.135695), correlation (0.455739)*/,
285  11,-6, 12,-11/*mean (0.142904), correlation (0.446114)*/,
286  -12,-9, -6,4/*mean (0.146165), correlation (0.451473)*/,
287  3,7, 7,12/*mean (0.147627), correlation (0.456643)*/,
288  5,5, 10,8/*mean (0.152901), correlation (0.455036)*/,
289  0,-4, 2,8/*mean (0.167083), correlation (0.459315)*/,
290  -9,12, -5,-13/*mean (0.173234), correlation (0.454706)*/,
291  0,7, 2,12/*mean (0.18312), correlation (0.433855)*/,
292  -1,2, 1,7/*mean (0.185504), correlation (0.443838)*/,
293  5,11, 7,-9/*mean (0.185706), correlation (0.451123)*/,
294  3,5, 6,-8/*mean (0.188968), correlation (0.455808)*/,
295  -13,-4, -8,9/*mean (0.191667), correlation (0.459128)*/,
296  -5,9, -3,-3/*mean (0.193196), correlation (0.458364)*/,
297  -4,-7, -3,-12/*mean (0.196536), correlation (0.455782)*/,
298  6,5, 8,0/*mean (0.1972), correlation (0.450481)*/,
299  -7,6, -6,12/*mean (0.199438), correlation (0.458156)*/,
300  -13,6, -5,-2/*mean (0.211224), correlation (0.449548)*/,
301  1,-10, 3,10/*mean (0.211718), correlation (0.440606)*/,
302  4,1, 8,-4/*mean (0.213034), correlation (0.443177)*/,
303  -2,-2, 2,-13/*mean (0.234334), correlation (0.455304)*/,
304  2,-12, 12,12/*mean (0.235684), correlation (0.443436)*/,
305  -2,-13, 0,-6/*mean (0.237674), correlation (0.452525)*/,
306  4,1, 9,3/*mean (0.23962), correlation (0.444824)*/,
307  -6,-10, -3,-5/*mean (0.248459), correlation (0.439621)*/,
308  -3,-13, -1,1/*mean (0.249505), correlation (0.456666)*/,
309  7,5, 12,-11/*mean (0.00119208), correlation (0.495466)*/,
310  4,-2, 5,-7/*mean (0.00372245), correlation (0.484214)*/,
311  -13,9, -9,-5/*mean (0.00741116), correlation (0.499854)*/,
312  7,1, 8,6/*mean (0.0208952), correlation (0.499773)*/,
313  7,-8, 7,6/*mean (0.0220085), correlation (0.501609)*/,
314  -7,-4, -7,1/*mean (0.0233806), correlation (0.496568)*/,
315  -8,11, -7,-8/*mean (0.0236505), correlation (0.489719)*/,
316  -13,6, -12,-8/*mean (0.0268781), correlation (0.503487)*/,
317  2,4, 3,9/*mean (0.0323324), correlation (0.501938)*/,
318  10,-5, 12,3/*mean (0.0399235), correlation (0.494029)*/,
319  -6,-5, -6,7/*mean (0.0420153), correlation (0.486579)*/,
320  8,-3, 9,-8/*mean (0.0548021), correlation (0.484237)*/,
321  2,-12, 2,8/*mean (0.0616622), correlation (0.496642)*/,
322  -11,-2, -10,3/*mean (0.0627755), correlation (0.498563)*/,
323  -12,-13, -7,-9/*mean (0.0829622), correlation (0.495491)*/,
324  -11,0, -10,-5/*mean (0.0843342), correlation (0.487146)*/,
325  5,-3, 11,8/*mean (0.0929937), correlation (0.502315)*/,
326  -2,-13, -1,12/*mean (0.113327), correlation (0.48941)*/,
327  -1,-8, 0,9/*mean (0.132119), correlation (0.467268)*/,
328  -13,-11, -12,-5/*mean (0.136269), correlation (0.498771)*/,
329  -10,-2, -10,11/*mean (0.142173), correlation (0.498714)*/,
330  -3,9, -2,-13/*mean (0.144141), correlation (0.491973)*/,
331  2,-3, 3,2/*mean (0.14892), correlation (0.500782)*/,
332  -9,-13, -4,0/*mean (0.150371), correlation (0.498211)*/,
333  -4,6, -3,-10/*mean (0.152159), correlation (0.495547)*/,
334  -4,12, -2,-7/*mean (0.156152), correlation (0.496925)*/,
335  -6,-11, -4,9/*mean (0.15749), correlation (0.499222)*/,
336  6,-3, 6,11/*mean (0.159211), correlation (0.503821)*/,
337  -13,11, -5,5/*mean (0.162427), correlation (0.501907)*/,
338  11,11, 12,6/*mean (0.16652), correlation (0.497632)*/,
339  7,-5, 12,-2/*mean (0.169141), correlation (0.484474)*/,
340  -1,12, 0,7/*mean (0.169456), correlation (0.495339)*/,
341  -4,-8, -3,-2/*mean (0.171457), correlation (0.487251)*/,
342  -7,1, -6,7/*mean (0.175), correlation (0.500024)*/,
343  -13,-12, -8,-13/*mean (0.175866), correlation (0.497523)*/,
344  -7,-2, -6,-8/*mean (0.178273), correlation (0.501854)*/,
345  -8,5, -6,-9/*mean (0.181107), correlation (0.494888)*/,
346  -5,-1, -4,5/*mean (0.190227), correlation (0.482557)*/,
347  -13,7, -8,10/*mean (0.196739), correlation (0.496503)*/,
348  1,5, 5,-13/*mean (0.19973), correlation (0.499759)*/,
349  1,0, 10,-13/*mean (0.204465), correlation (0.49873)*/,
350  9,12, 10,-1/*mean (0.209334), correlation (0.49063)*/,
351  5,-8, 10,-9/*mean (0.211134), correlation (0.503011)*/,
352  -1,11, 1,-13/*mean (0.212), correlation (0.499414)*/,
353  -9,-3, -6,2/*mean (0.212168), correlation (0.480739)*/,
354  -1,-10, 1,12/*mean (0.212731), correlation (0.502523)*/,
355  -13,1, -8,-10/*mean (0.21327), correlation (0.489786)*/,
356  8,-11, 10,-6/*mean (0.214159), correlation (0.488246)*/,
357  2,-13, 3,-6/*mean (0.216993), correlation (0.50287)*/,
358  7,-13, 12,-9/*mean (0.223639), correlation (0.470502)*/,
359  -10,-10, -5,-7/*mean (0.224089), correlation (0.500852)*/,
360  -10,-8, -8,-13/*mean (0.228666), correlation (0.502629)*/,
361  4,-6, 8,5/*mean (0.22906), correlation (0.498305)*/,
362  3,12, 8,-13/*mean (0.233378), correlation (0.503825)*/,
363  -4,2, -3,-3/*mean (0.234323), correlation (0.476692)*/,
364  5,-13, 10,-12/*mean (0.236392), correlation (0.475462)*/,
365  4,-13, 5,-1/*mean (0.236842), correlation (0.504132)*/,
366  -9,9, -4,3/*mean (0.236977), correlation (0.497739)*/,
367  0,3, 3,-9/*mean (0.24314), correlation (0.499398)*/,
368  -12,1, -6,1/*mean (0.243297), correlation (0.489447)*/,
369  3,2, 4,-8/*mean (0.00155196), correlation (0.553496)*/,
370  -10,-10, -10,9/*mean (0.00239541), correlation (0.54297)*/,
371  8,-13, 12,12/*mean (0.0034413), correlation (0.544361)*/,
372  -8,-12, -6,-5/*mean (0.003565), correlation (0.551225)*/,
373  2,2, 3,7/*mean (0.00835583), correlation (0.55285)*/,
374  10,6, 11,-8/*mean (0.00885065), correlation (0.540913)*/,
375  6,8, 8,-12/*mean (0.0101552), correlation (0.551085)*/,
376  -7,10, -6,5/*mean (0.0102227), correlation (0.533635)*/,
377  -3,-9, -3,9/*mean (0.0110211), correlation (0.543121)*/,
378  -1,-13, -1,5/*mean (0.0113473), correlation (0.550173)*/,
379  -3,-7, -3,4/*mean (0.0140913), correlation (0.554774)*/,
380  -8,-2, -8,3/*mean (0.017049), correlation (0.55461)*/,
381  4,2, 12,12/*mean (0.01778), correlation (0.546921)*/,
382  2,-5, 3,11/*mean (0.0224022), correlation (0.549667)*/,
383  6,-9, 11,-13/*mean (0.029161), correlation (0.546295)*/,
384  3,-1, 7,12/*mean (0.0303081), correlation (0.548599)*/,
385  11,-1, 12,4/*mean (0.0355151), correlation (0.523943)*/,
386  -3,0, -3,6/*mean (0.0417904), correlation (0.543395)*/,
387  4,-11, 4,12/*mean (0.0487292), correlation (0.542818)*/,
388  2,-4, 2,1/*mean (0.0575124), correlation (0.554888)*/,
389  -10,-6, -8,1/*mean (0.0594242), correlation (0.544026)*/,
390  -13,7, -11,1/*mean (0.0597391), correlation (0.550524)*/,
391  -13,12, -11,-13/*mean (0.0608974), correlation (0.55383)*/,
392  6,0, 11,-13/*mean (0.065126), correlation (0.552006)*/,
393  0,-1, 1,4/*mean (0.074224), correlation (0.546372)*/,
394  -13,3, -9,-2/*mean (0.0808592), correlation (0.554875)*/,
395  -9,8, -6,-3/*mean (0.0883378), correlation (0.551178)*/,
396  -13,-6, -8,-2/*mean (0.0901035), correlation (0.548446)*/,
397  5,-9, 8,10/*mean (0.0949843), correlation (0.554694)*/,
398  2,7, 3,-9/*mean (0.0994152), correlation (0.550979)*/,
399  -1,-6, -1,-1/*mean (0.10045), correlation (0.552714)*/,
400  9,5, 11,-2/*mean (0.100686), correlation (0.552594)*/,
401  11,-3, 12,-8/*mean (0.101091), correlation (0.532394)*/,
402  3,0, 3,5/*mean (0.101147), correlation (0.525576)*/,
403  -1,4, 0,10/*mean (0.105263), correlation (0.531498)*/,
404  3,-6, 4,5/*mean (0.110785), correlation (0.540491)*/,
405  -13,0, -10,5/*mean (0.112798), correlation (0.536582)*/,
406  5,8, 12,11/*mean (0.114181), correlation (0.555793)*/,
407  8,9, 9,-6/*mean (0.117431), correlation (0.553763)*/,
408  7,-4, 8,-12/*mean (0.118522), correlation (0.553452)*/,
409  -10,4, -10,9/*mean (0.12094), correlation (0.554785)*/,
410  7,3, 12,4/*mean (0.122582), correlation (0.555825)*/,
411  9,-7, 10,-2/*mean (0.124978), correlation (0.549846)*/,
412  7,0, 12,-2/*mean (0.127002), correlation (0.537452)*/,
413  -1,-6, 0,-11/*mean (0.127148), correlation (0.547401)*/
414 };
415 
416 ORBextractor::ORBextractor(int _nfeatures, float _scaleFactor, int _nlevels,
417  int _iniThFAST, int _minThFAST, int _patchSize, int _edgeThreshold):
418  nfeatures(_nfeatures), scaleFactor(_scaleFactor), nlevels(_nlevels),
419  iniThFAST(_iniThFAST), minThFAST(_minThFAST), patchSize(_patchSize), edgeThreshold(_edgeThreshold)
420 {
422  mvScaleFactor.resize(nlevels);
423  mvLevelSigma2.resize(nlevels);
424  mvScaleFactor[0]=1.0f;
425  mvLevelSigma2[0]=1.0f;
426  for(int i=1; i<nlevels; i++)
427  {
430  }
431 
432  mvInvScaleFactor.resize(nlevels);
433  mvInvLevelSigma2.resize(nlevels);
434  for(int i=0; i<nlevels; i++)
435  {
438  }
439 
440  mvImagePyramid.resize(nlevels);
441 
442  mnFeaturesPerLevel.resize(nlevels);
443  float factor = 1.0f / scaleFactor;
444  float nDesiredFeaturesPerScale = nfeatures*(1 - factor)/(1 - (float)pow((double)factor, (double)nlevels));
445 
446  int sumFeatures = 0;
447  for( int level = 0; level < nlevels-1; level++ )
448  {
449  mnFeaturesPerLevel[level] = cvRound(nDesiredFeaturesPerScale);
450  sumFeatures += mnFeaturesPerLevel[level];
451  nDesiredFeaturesPerScale *= factor;
452  }
453  mnFeaturesPerLevel[nlevels-1] = std::max(nfeatures - sumFeatures, 0);
454 
455  const int npoints = 512;
456  const Point* pattern0 = (const Point*)bit_pattern_31_;
457  std::copy(pattern0, pattern0 + npoints, std::back_inserter(pattern));
458 
459  //This is for orientation
460  // pre-compute the end of a row in a circular patch
461  umax.resize(halfPatchSize + 1);
462 
463  int v, v0, vmax = cvFloor(float(halfPatchSize) * sqrt(2.f) / 2 + 1);
464  int vmin = cvCeil(float(halfPatchSize) * sqrt(2.f) / 2);
465  const double hp2 = halfPatchSize*halfPatchSize;
466  for (v = 0; v <= vmax; ++v)
467  umax[v] = cvRound(sqrt(hp2 - v * v));
468 
469  // Make sure we are symmetric
470  for (v = halfPatchSize, v0 = 0; v >= vmin; --v)
471  {
472  while (umax[v0] == umax[v0 + 1])
473  ++v0;
474  umax[v] = v0;
475  ++v0;
476  }
477 }
478 
479 static void computeOrientation(const Mat& image, vector<KeyPoint>& keypoints, const vector<int>& umax, int halfPatchSize)
480 {
481  for (vector<KeyPoint>::iterator keypoint = keypoints.begin(),
482  keypointEnd = keypoints.end(); keypoint != keypointEnd; ++keypoint)
483  {
484  keypoint->angle = IC_Angle(image, keypoint->pt, umax, halfPatchSize);
485  }
486 }
487 
489 {
490  const int halfX = ceil(static_cast<float>(UR.x-UL.x)/2);
491  const int halfY = ceil(static_cast<float>(BR.y-UL.y)/2);
492 
493  //Define boundaries of childs
494  n1.UL = UL;
495  n1.UR = cv::Point2i(UL.x+halfX,UL.y);
496  n1.BL = cv::Point2i(UL.x,UL.y+halfY);
497  n1.BR = cv::Point2i(UL.x+halfX,UL.y+halfY);
498  n1.vKeys.reserve(vKeys.size());
499 
500  n2.UL = n1.UR;
501  n2.UR = UR;
502  n2.BL = n1.BR;
503  n2.BR = cv::Point2i(UR.x,UL.y+halfY);
504  n2.vKeys.reserve(vKeys.size());
505 
506  n3.UL = n1.BL;
507  n3.UR = n1.BR;
508  n3.BL = BL;
509  n3.BR = cv::Point2i(n1.BR.x,BL.y);
510  n3.vKeys.reserve(vKeys.size());
511 
512  n4.UL = n3.UR;
513  n4.UR = n2.BR;
514  n4.BL = n3.BR;
515  n4.BR = BR;
516  n4.vKeys.reserve(vKeys.size());
517 
518  //Associate points to childs
519  for(size_t i=0;i<vKeys.size();i++)
520  {
521  const cv::KeyPoint &kp = vKeys[i];
522  if(kp.pt.x<n1.UR.x)
523  {
524  if(kp.pt.y<n1.BR.y)
525  n1.vKeys.push_back(kp);
526  else
527  n3.vKeys.push_back(kp);
528  }
529  else if(kp.pt.y<n1.BR.y)
530  n2.vKeys.push_back(kp);
531  else
532  n4.vKeys.push_back(kp);
533  }
534 
535  if(n1.vKeys.size()==1)
536  n1.bNoMore = true;
537  if(n2.vKeys.size()==1)
538  n2.bNoMore = true;
539  if(n3.vKeys.size()==1)
540  n3.bNoMore = true;
541  if(n4.vKeys.size()==1)
542  n4.bNoMore = true;
543 
544 }
545 
546 vector<cv::KeyPoint> ORBextractor::DistributeOctTree(const vector<cv::KeyPoint>& vToDistributeKeys, const int &minX,
547  const int &maxX, const int &minY, const int &maxY, const int &N, const int &level)
548 {
549  // Compute how many initial nodes
550  const int nIni = round(static_cast<float>(maxX-minX)/(maxY-minY));
551 
552  const float hX = static_cast<float>(maxX-minX)/nIni;
553 
554  list<ExtractorNode> lNodes;
555 
556  vector<ExtractorNode*> vpIniNodes;
557  vpIniNodes.resize(nIni);
558 
559  for(int i=0; i<nIni; i++)
560  {
561  ExtractorNode ni;
562  ni.UL = cv::Point2i(hX*static_cast<float>(i),0);
563  ni.UR = cv::Point2i(hX*static_cast<float>(i+1),0);
564  ni.BL = cv::Point2i(ni.UL.x,maxY-minY);
565  ni.BR = cv::Point2i(ni.UR.x,maxY-minY);
566  ni.vKeys.reserve(vToDistributeKeys.size());
567 
568  lNodes.push_back(ni);
569  vpIniNodes[i] = &lNodes.back();
570  }
571 
572  //Associate points to childs
573  for(size_t i=0;i<vToDistributeKeys.size();i++)
574  {
575  const cv::KeyPoint &kp = vToDistributeKeys[i];
576  vpIniNodes[kp.pt.x/hX]->vKeys.push_back(kp);
577  }
578 
579  list<ExtractorNode>::iterator lit = lNodes.begin();
580 
581  while(lit!=lNodes.end())
582  {
583  if(lit->vKeys.size()==1)
584  {
585  lit->bNoMore=true;
586  lit++;
587  }
588  else if(lit->vKeys.empty())
589  lit = lNodes.erase(lit);
590  else
591  lit++;
592  }
593 
594  bool bFinish = false;
595 
596  int iteration = 0;
597 
598  vector<pair<int,ExtractorNode*> > vSizeAndPointerToNode;
599  vSizeAndPointerToNode.reserve(lNodes.size()*4);
600 
601  while(!bFinish)
602  {
603  iteration++;
604 
605  int prevSize = lNodes.size();
606 
607  lit = lNodes.begin();
608 
609  int nToExpand = 0;
610 
611  vSizeAndPointerToNode.clear();
612 
613  while(lit!=lNodes.end())
614  {
615  if(lit->bNoMore)
616  {
617  // If node only contains one point do not subdivide and continue
618  lit++;
619  continue;
620  }
621  else
622  {
623  // If more than one point, subdivide
624  ExtractorNode n1,n2,n3,n4;
625  lit->DivideNode(n1,n2,n3,n4);
626 
627  // Add childs if they contain points
628  if(n1.vKeys.size()>0)
629  {
630  lNodes.push_front(n1);
631  if(n1.vKeys.size()>1)
632  {
633  nToExpand++;
634  vSizeAndPointerToNode.push_back(make_pair(n1.vKeys.size(),&lNodes.front()));
635  lNodes.front().lit = lNodes.begin();
636  }
637  }
638  if(n2.vKeys.size()>0)
639  {
640  lNodes.push_front(n2);
641  if(n2.vKeys.size()>1)
642  {
643  nToExpand++;
644  vSizeAndPointerToNode.push_back(make_pair(n2.vKeys.size(),&lNodes.front()));
645  lNodes.front().lit = lNodes.begin();
646  }
647  }
648  if(n3.vKeys.size()>0)
649  {
650  lNodes.push_front(n3);
651  if(n3.vKeys.size()>1)
652  {
653  nToExpand++;
654  vSizeAndPointerToNode.push_back(make_pair(n3.vKeys.size(),&lNodes.front()));
655  lNodes.front().lit = lNodes.begin();
656  }
657  }
658  if(n4.vKeys.size()>0)
659  {
660  lNodes.push_front(n4);
661  if(n4.vKeys.size()>1)
662  {
663  nToExpand++;
664  vSizeAndPointerToNode.push_back(make_pair(n4.vKeys.size(),&lNodes.front()));
665  lNodes.front().lit = lNodes.begin();
666  }
667  }
668 
669  lit=lNodes.erase(lit);
670  continue;
671  }
672  }
673 
674  // Finish if there are more nodes than required features
675  // or all nodes contain just one point
676  if((int)lNodes.size()>=N || (int)lNodes.size()==prevSize)
677  {
678  bFinish = true;
679  }
680  else if(((int)lNodes.size()+nToExpand*3)>N)
681  {
682 
683  while(!bFinish)
684  {
685 
686  prevSize = lNodes.size();
687 
688  vector<pair<int,ExtractorNode*> > vPrevSizeAndPointerToNode = vSizeAndPointerToNode;
689  vSizeAndPointerToNode.clear();
690 
691  sort(vPrevSizeAndPointerToNode.begin(),vPrevSizeAndPointerToNode.end());
692  for(int j=vPrevSizeAndPointerToNode.size()-1;j>=0;j--)
693  {
694  ExtractorNode n1,n2,n3,n4;
695  vPrevSizeAndPointerToNode[j].second->DivideNode(n1,n2,n3,n4);
696 
697  // Add childs if they contain points
698  if(n1.vKeys.size()>0)
699  {
700  lNodes.push_front(n1);
701  if(n1.vKeys.size()>1)
702  {
703  vSizeAndPointerToNode.push_back(make_pair(n1.vKeys.size(),&lNodes.front()));
704  lNodes.front().lit = lNodes.begin();
705  }
706  }
707  if(n2.vKeys.size()>0)
708  {
709  lNodes.push_front(n2);
710  if(n2.vKeys.size()>1)
711  {
712  vSizeAndPointerToNode.push_back(make_pair(n2.vKeys.size(),&lNodes.front()));
713  lNodes.front().lit = lNodes.begin();
714  }
715  }
716  if(n3.vKeys.size()>0)
717  {
718  lNodes.push_front(n3);
719  if(n3.vKeys.size()>1)
720  {
721  vSizeAndPointerToNode.push_back(make_pair(n3.vKeys.size(),&lNodes.front()));
722  lNodes.front().lit = lNodes.begin();
723  }
724  }
725  if(n4.vKeys.size()>0)
726  {
727  lNodes.push_front(n4);
728  if(n4.vKeys.size()>1)
729  {
730  vSizeAndPointerToNode.push_back(make_pair(n4.vKeys.size(),&lNodes.front()));
731  lNodes.front().lit = lNodes.begin();
732  }
733  }
734 
735  lNodes.erase(vPrevSizeAndPointerToNode[j].second->lit);
736 
737  if((int)lNodes.size()>=N)
738  break;
739  }
740 
741  if((int)lNodes.size()>=N || (int)lNodes.size()==prevSize)
742  bFinish = true;
743 
744  }
745  }
746  }
747 
748  // Retain the best point in each node
749  vector<cv::KeyPoint> vResultKeys;
750  vResultKeys.reserve(nfeatures);
751  for(list<ExtractorNode>::iterator lit=lNodes.begin(); lit!=lNodes.end(); lit++)
752  {
753  vector<cv::KeyPoint> &vNodeKeys = lit->vKeys;
754  cv::KeyPoint* pKP = &vNodeKeys[0];
755  float maxResponse = pKP->response;
756 
757  for(size_t k=1;k<vNodeKeys.size();k++)
758  {
759  if(vNodeKeys[k].response>maxResponse)
760  {
761  pKP = &vNodeKeys[k];
762  maxResponse = vNodeKeys[k].response;
763  }
764  }
765 
766  vResultKeys.push_back(*pKP);
767  }
768 
769  return vResultKeys;
770 }
771 
772 void ORBextractor::ComputeKeyPointsOctTree(vector<vector<KeyPoint> >& allKeypoints)
773 {
774  allKeypoints.resize(nlevels);
775 
776  const float W = 30;
777 
778  for (int level = 0; level < nlevels; ++level)
779  {
780  const int minBorderX = edgeThreshold-3;
781  const int minBorderY = minBorderX;
782  const int maxBorderX = mvImagePyramid[level].cols-edgeThreshold+3;
783  const int maxBorderY = mvImagePyramid[level].rows-edgeThreshold+3;
784 
785  vector<cv::KeyPoint> vToDistributeKeys;
786  vToDistributeKeys.reserve(nfeatures*10);
787 
788  const float width = (maxBorderX-minBorderX);
789  const float height = (maxBorderY-minBorderY);
790 
791  const int nCols = width/W;
792  const int nRows = height/W;
793  const int wCell = ceil(width/nCols);
794  const int hCell = ceil(height/nRows);
795 
796  for(int i=0; i<nRows; i++)
797  {
798  const float iniY =minBorderY+i*hCell;
799  float maxY = iniY+hCell+6;
800 
801  if(iniY>=maxBorderY-3)
802  continue;
803  if(maxY>maxBorderY)
804  maxY = maxBorderY;
805 
806  for(int j=0; j<nCols; j++)
807  {
808  const float iniX =minBorderX+j*wCell;
809  float maxX = iniX+wCell+6;
810  if(iniX>=maxBorderX-6)
811  continue;
812  if(maxX>maxBorderX)
813  maxX = maxBorderX;
814 
815  vector<cv::KeyPoint> vKeysCell;
816  FAST(mvImagePyramid[level].rowRange(iniY,maxY).colRange(iniX,maxX),
817  vKeysCell,iniThFAST,true);
818 
819  if(vKeysCell.empty())
820  {
821  FAST(mvImagePyramid[level].rowRange(iniY,maxY).colRange(iniX,maxX),
822  vKeysCell,minThFAST,true);
823  }
824 
825  if(!vKeysCell.empty())
826  {
827  for(vector<cv::KeyPoint>::iterator vit=vKeysCell.begin(); vit!=vKeysCell.end();vit++)
828  {
829  (*vit).pt.x+=j*wCell;
830  (*vit).pt.y+=i*hCell;
831  vToDistributeKeys.push_back(*vit);
832  }
833  }
834 
835  }
836  }
837 
838  vector<KeyPoint> & keypoints = allKeypoints[level];
839  keypoints.reserve(nfeatures);
840 
841  keypoints = DistributeOctTree(vToDistributeKeys, minBorderX, maxBorderX,
842  minBorderY, maxBorderY,mnFeaturesPerLevel[level], level);
843 
844  const int scaledPatchSize = patchSize*mvScaleFactor[level];
845 
846  // Add border to coordinates and scale information
847  const int nkps = keypoints.size();
848  for(int i=0; i<nkps ; i++)
849  {
850  keypoints[i].pt.x+=minBorderX;
851  keypoints[i].pt.y+=minBorderY;
852  keypoints[i].octave=level;
853  keypoints[i].size = scaledPatchSize;
854  }
855  }
856 
857  // compute orientations
858  for (int level = 0; level < nlevels; ++level)
860 }
861 
862 void ORBextractor::ComputeKeyPointsOld(std::vector<std::vector<KeyPoint> > &allKeypoints)
863 {
864  allKeypoints.resize(nlevels);
865 
866  float imageRatio = (float)mvImagePyramid[0].cols/mvImagePyramid[0].rows;
867 
868  for (int level = 0; level < nlevels; ++level)
869  {
870  const int nDesiredFeatures = mnFeaturesPerLevel[level];
871 
872  const int levelCols = sqrt((float)nDesiredFeatures/(5*imageRatio));
873  const int levelRows = imageRatio*levelCols;
874 
875  const int minBorderX = edgeThreshold;
876  const int minBorderY = minBorderX;
877  const int maxBorderX = mvImagePyramid[level].cols-edgeThreshold;
878  const int maxBorderY = mvImagePyramid[level].rows-edgeThreshold;
879 
880  const int W = maxBorderX - minBorderX;
881  const int H = maxBorderY - minBorderY;
882  const int cellW = ceil((float)W/levelCols);
883  const int cellH = ceil((float)H/levelRows);
884 
885  const int nCells = levelRows*levelCols;
886  const int nfeaturesCell = ceil((float)nDesiredFeatures/nCells);
887 
888  vector<vector<vector<KeyPoint> > > cellKeyPoints(levelRows, vector<vector<KeyPoint> >(levelCols));
889 
890  vector<vector<int> > nToRetain(levelRows,vector<int>(levelCols,0));
891  vector<vector<int> > nTotal(levelRows,vector<int>(levelCols,0));
892  vector<vector<bool> > bNoMore(levelRows,vector<bool>(levelCols,false));
893  vector<int> iniXCol(levelCols);
894  vector<int> iniYRow(levelRows);
895  int nNoMore = 0;
896  int nToDistribute = 0;
897 
898 
899  float hY = cellH + 6;
900 
901  for(int i=0; i<levelRows; i++)
902  {
903  const float iniY = minBorderY + i*cellH - 3;
904  iniYRow[i] = iniY;
905 
906  if(i == levelRows-1)
907  {
908  hY = maxBorderY+3-iniY;
909  if(hY<=0)
910  continue;
911  }
912 
913  float hX = cellW + 6;
914 
915  for(int j=0; j<levelCols; j++)
916  {
917  float iniX;
918 
919  if(i==0)
920  {
921  iniX = minBorderX + j*cellW - 3;
922  iniXCol[j] = iniX;
923  }
924  else
925  {
926  iniX = iniXCol[j];
927  }
928 
929 
930  if(j == levelCols-1)
931  {
932  hX = maxBorderX+3-iniX;
933  if(hX<=0)
934  continue;
935  }
936 
937 
938  Mat cellImage = mvImagePyramid[level].rowRange(iniY,iniY+hY).colRange(iniX,iniX+hX);
939 
940  cellKeyPoints[i][j].reserve(nfeaturesCell*5);
941 
942  FAST(cellImage,cellKeyPoints[i][j],iniThFAST,true);
943 
944  if(cellKeyPoints[i][j].size()<=3)
945  {
946  cellKeyPoints[i][j].clear();
947 
948  FAST(cellImage,cellKeyPoints[i][j],minThFAST,true);
949  }
950 
951 
952  const int nKeys = cellKeyPoints[i][j].size();
953  nTotal[i][j] = nKeys;
954 
955  if(nKeys>nfeaturesCell)
956  {
957  nToRetain[i][j] = nfeaturesCell;
958  bNoMore[i][j] = false;
959  }
960  else
961  {
962  nToRetain[i][j] = nKeys;
963  nToDistribute += nfeaturesCell-nKeys;
964  bNoMore[i][j] = true;
965  nNoMore++;
966  }
967 
968  }
969  }
970 
971 
972  // Retain by score
973 
974  while(nToDistribute>0 && nNoMore<nCells)
975  {
976  int nNewFeaturesCell = nfeaturesCell + ceil((float)nToDistribute/(nCells-nNoMore));
977  nToDistribute = 0;
978 
979  for(int i=0; i<levelRows; i++)
980  {
981  for(int j=0; j<levelCols; j++)
982  {
983  if(!bNoMore[i][j])
984  {
985  if(nTotal[i][j]>nNewFeaturesCell)
986  {
987  nToRetain[i][j] = nNewFeaturesCell;
988  bNoMore[i][j] = false;
989  }
990  else
991  {
992  nToRetain[i][j] = nTotal[i][j];
993  nToDistribute += nNewFeaturesCell-nTotal[i][j];
994  bNoMore[i][j] = true;
995  nNoMore++;
996  }
997  }
998  }
999  }
1000  }
1001 
1002  vector<KeyPoint> & keypoints = allKeypoints[level];
1003  keypoints.reserve(nDesiredFeatures*2);
1004 
1005  const int scaledPatchSize = patchSize*mvScaleFactor[level];
1006 
1007  // Retain by score and transform coordinates
1008  for(int i=0; i<levelRows; i++)
1009  {
1010  for(int j=0; j<levelCols; j++)
1011  {
1012  vector<KeyPoint> &keysCell = cellKeyPoints[i][j];
1013  KeyPointsFilter::retainBest(keysCell,nToRetain[i][j]);
1014  if((int)keysCell.size()>nToRetain[i][j])
1015  keysCell.resize(nToRetain[i][j]);
1016 
1017 
1018  for(size_t k=0, kend=keysCell.size(); k<kend; k++)
1019  {
1020  keysCell[k].pt.x+=iniXCol[j];
1021  keysCell[k].pt.y+=iniYRow[i];
1022  keysCell[k].octave=level;
1023  keysCell[k].size = scaledPatchSize;
1024  keypoints.push_back(keysCell[k]);
1025  }
1026  }
1027  }
1028 
1029  if((int)keypoints.size()>nDesiredFeatures)
1030  {
1031  KeyPointsFilter::retainBest(keypoints,nDesiredFeatures);
1032  keypoints.resize(nDesiredFeatures);
1033  }
1034  }
1035 
1036  // and compute orientations
1037  for (int level = 0; level < nlevels; ++level)
1039 }
1040 
1041 static void computeDescriptors(const Mat& image, vector<KeyPoint>& keypoints, Mat& descriptors,
1042  const vector<Point>& pattern)
1043 {
1044  descriptors = Mat::zeros((int)keypoints.size(), 32, CV_8UC1);
1045 
1046  for (size_t i = 0; i < keypoints.size(); i++)
1047  computeOrbDescriptor(keypoints[i], image, &pattern[0], descriptors.ptr((int)i));
1048 }
1049 
1050 void ORBextractor::operator()( InputArray _image, InputArray _mask, vector<KeyPoint>& _keypoints,
1051  OutputArray _descriptors)
1052 {
1053  if(_image.empty())
1054  return;
1055 
1056  Mat image = _image.getMat();
1057  assert(image.type() == CV_8UC1 );
1058 
1059  // Pre-compute the scale pyramid
1060  ComputePyramid(image);
1061 
1062  vector < vector<KeyPoint> > allKeypoints;
1063  ComputeKeyPointsOctTree(allKeypoints);
1064  //ComputeKeyPointsOld(allKeypoints);
1065 
1066  Mat descriptors;
1067 
1068  int nkeypoints = 0;
1069  for (int level = 0; level < nlevels; ++level)
1070  nkeypoints += (int)allKeypoints[level].size();
1071  if( nkeypoints == 0 )
1072  _descriptors.release();
1073  else
1074  {
1075  _descriptors.create(nkeypoints, 32, CV_8U);
1076  descriptors = _descriptors.getMat();
1077  }
1078 
1079  _keypoints.clear();
1080  _keypoints.reserve(nkeypoints);
1081 
1082  int offset = 0;
1083  for (int level = 0; level < nlevels; ++level)
1084  {
1085  vector<KeyPoint>& keypoints = allKeypoints[level];
1086  int nkeypointsLevel = (int)keypoints.size();
1087 
1088  if(nkeypointsLevel==0)
1089  continue;
1090 
1091  // preprocess the resized image
1092  Mat workingMat = mvImagePyramid[level].clone();
1093  GaussianBlur(workingMat, workingMat, Size(7, 7), 2, 2, BORDER_REFLECT_101);
1094 
1095  // Compute the descriptors
1096  Mat desc = descriptors.rowRange(offset, offset + nkeypointsLevel);
1097  computeDescriptors(workingMat, keypoints, desc, pattern);
1098 
1099  offset += nkeypointsLevel;
1100 
1101  // Scale keypoint coordinates
1102  if (level != 0)
1103  {
1104  float scale = mvScaleFactor[level]; //getScale(level, firstLevel, scaleFactor);
1105  for (vector<KeyPoint>::iterator keypoint = keypoints.begin(),
1106  keypointEnd = keypoints.end(); keypoint != keypointEnd; ++keypoint)
1107  keypoint->pt *= scale;
1108  }
1109  // And add the keypoints to the output
1110  _keypoints.insert(_keypoints.end(), keypoints.begin(), keypoints.end());
1111  }
1112 }
1113 
1114 void ORBextractor::ComputePyramid(cv::Mat image)
1115 {
1116  for (int level = 0; level < nlevels; ++level)
1117  {
1118  float scale = mvInvScaleFactor[level];
1119  Size sz(cvRound((float)image.cols*scale), cvRound((float)image.rows*scale));
1120  Size wholeSize(sz.width + edgeThreshold*2, sz.height + edgeThreshold*2);
1121  Mat temp(wholeSize, image.type()), masktemp;
1122  mvImagePyramid[level] = temp(Rect(edgeThreshold, edgeThreshold, sz.width, sz.height));
1123 
1124  // Compute the resized image
1125  if( level != 0 )
1126  {
1127  resize(mvImagePyramid[level-1], mvImagePyramid[level], sz, 0, 0, INTER_LINEAR);
1128 
1130  BORDER_REFLECT_101+BORDER_ISOLATED);
1131  }
1132  else
1133  {
1134  copyMakeBorder(image, temp, edgeThreshold, edgeThreshold, edgeThreshold, edgeThreshold,
1135  BORDER_REFLECT_101);
1136  }
1137  }
1138 
1139 }
1140 
1141 } //namespace rtabmap
int
int
H
set noclip points set clip one set noclip two set bar set border lt lw set xdata set ydata set zdata set x2data set y2data set boxwidth set dummy y set format x g set format y g set format x2 g set format y2 g set format z g set angles radians set nogrid set key title set key left top Right noreverse box linetype linewidth samplen spacing width set nolabel set noarrow set nologscale set logscale x set set pointsize set encoding default set nopolar set noparametric set set set set surface set nocontour set clabel set mapping cartesian set nohidden3d set cntrparam order set cntrparam linear set cntrparam levels auto set cntrparam points set size set set xzeroaxis lt lw set x2zeroaxis lt lw set yzeroaxis lt lw set y2zeroaxis lt lw set tics in set ticslevel set tics set mxtics default set mytics default set mx2tics default set my2tics default set xtics border mirror norotate autofreq set ytics border mirror norotate autofreq set ztics border nomirror norotate autofreq set nox2tics set noy2tics set timestamp bottom norotate set rrange[ *:*] noreverse nowriteback set trange[ *:*] noreverse nowriteback set urange[ *:*] noreverse nowriteback set vrange[ *:*] noreverse nowriteback set xlabel matrix size set x2label set timefmt d m y n H
rtabmap::ORBextractor::mvScaleFactor
std::vector< float > mvScaleFactor
Definition: ORBextractor.h:119
list::end
detail::list_iterator end() const
rtabmap::ORBextractor::nlevels
int nlevels
Definition: ORBextractor.h:107
rtabmap::computeDescriptors
static void computeDescriptors(const Mat &image, std::vector< KeyPoint > &keypoints, Mat &descriptors, const std::vector< Point > &pattern, int dsize, int WTA_K)
Definition: Orb.cpp:716
n1
n1
b
Array< int, 3, 1 > b
rtabmap::ORBextractor::mvInvLevelSigma2
std::vector< float > mvInvLevelSigma2
Definition: ORBextractor.h:122
rtabmap::FAST
Definition: Features2d.h:334
list
rtflann::uchar
unsigned char uchar
Definition: matrix.h:69
rtabmap::bit_pattern_31_
static int bit_pattern_31_[256 *4]
Definition: ORBextractor.cc:156
rtabmap::ORBextractor::mvInvScaleFactor
std::vector< float > mvInvScaleFactor
Definition: ORBextractor.h:120
size
Index size
ORBextractor.h
rtabmap::ORBextractor::DistributeOctTree
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)
Definition: ORBextractor.cc:546
rtabmap::ExtractorNode::UR
cv::Point2i UR
Definition: ORBextractor.h:48
rtabmap_netvlad.img
img
Definition: rtabmap_netvlad.py:78
rtabmap::ORBextractor::operator()
void operator()(cv::InputArray image, cv::InputArray mask, std::vector< cv::KeyPoint > &keypoints, cv::OutputArray descriptors)
Definition: ORBextractor.cc:1050
rtabmap::computeOrbDescriptor
static void computeOrbDescriptor(const KeyPoint &kpt, const Mat &img, const Point *pattern, uchar *desc)
Definition: ORBextractor.cc:114
y
Matrix3f y
rtabmap::ORBextractor::ComputeKeyPointsOctTree
void ComputeKeyPointsOctTree(std::vector< std::vector< cv::KeyPoint > > &allKeypoints)
Definition: ORBextractor.cc:772
rtabmap::ORBextractor::scaleFactor
double scaleFactor
Definition: ORBextractor.h:106
rtabmap::ORBextractor::halfPatchSize
int halfPatchSize
Definition: ORBextractor.h:112
rtabmap::ExtractorNode::BL
cv::Point2i BL
Definition: ORBextractor.h:48
rows
int rows
rtabmap::ExtractorNode
Definition: ORBextractor.h:40
glm::round
GLM_FUNC_DECL genType round(genType const &x)
glm::pow
GLM_FUNC_DECL genType pow(genType const &base, genType const &exponent)
desc
desc
rtabmap::ExtractorNode::bNoMore
bool bNoMore
Definition: ORBextractor.h:50
rtabmap::ORBextractor::mvImagePyramid
std::vector< cv::Mat > mvImagePyramid
Definition: ORBextractor.h:93
rtabmap::factorPI
const float factorPI
Definition: ORBextractor.cc:113
list::size
size_t size() const
n2
n2
BR
BearingRangeFactor< Pose, Point2 > BR
rtabmap::ORBextractor::pattern
std::vector< cv::Point > pattern
Definition: ORBextractor.h:103
scale
set noclip points set clip one set noclip two set bar set border lt lw set xdata set ydata set zdata set x2data set y2data set boxwidth set dummy y set format x g set format y g set format x2 g set format y2 g set format z g set angles radians set nogrid set key title set key left top Right noreverse box linetype linewidth samplen spacing width set nolabel set noarrow set nologscale set logscale x set set pointsize set encoding default set nopolar set noparametric set set set set surface set nocontour set clabel set mapping cartesian set nohidden3d set cntrparam order set cntrparam linear set cntrparam levels auto set cntrparam points set size set set xzeroaxis lt lw set x2zeroaxis lt lw set yzeroaxis lt lw set y2zeroaxis lt lw set tics in set ticslevel set tics scale
j
std::ptrdiff_t j
glm::sqrt
GLM_FUNC_DECL vecType< T, P > sqrt(vecType< T, P > const &x)
glm::ceil
GLM_FUNC_DECL genType ceil(genType const &x)
glm::max
GLM_FUNC_DECL genType max(genType const &x, genType const &y)
rtabmap::ExtractorNode::vKeys
std::vector< cv::KeyPoint > vKeys
Definition: ORBextractor.h:47
step
def step(data, isam, result, truth, currPoseIndex, isamArgs=())
d
d
level
level
x
x
rtabmap::ORBextractor::umax
std::vector< int > umax
Definition: ORBextractor.h:117
glm::angle
GLM_FUNC_DECL T angle(detail::tquat< T, P > const &x)
rtabmap::ORBextractor::minThFAST
int minThFAST
Definition: ORBextractor.h:109
anyset::size
size_t size() const
rtabmap::ORBextractor::nfeatures
int nfeatures
Definition: ORBextractor.h:105
rtabmap::ORBextractor::ComputeKeyPointsOld
void ComputeKeyPointsOld(std::vector< std::vector< cv::KeyPoint > > &allKeypoints)
Definition: ORBextractor.cc:862
rtabmap::ORBextractor::edgeThreshold
int edgeThreshold
Definition: ORBextractor.h:111
offset
set noclip points set clip one set noclip two set bar set border lt lw set xdata set ydata set zdata set x2data set y2data set boxwidth set dummy y set format x g set format y g set format x2 g set format y2 g set format z g set angles radians set nogrid set key title set key left top Right noreverse box linetype linewidth samplen spacing width set nolabel set noarrow set nologscale set logscale x set set pointsize set encoding default set nopolar set noparametric set set set set surface set nocontour set clabel set mapping cartesian set nohidden3d set cntrparam order set cntrparam linear set cntrparam levels auto set cntrparam points set size set set xzeroaxis lt lw set x2zeroaxis lt lw set yzeroaxis lt lw set y2zeroaxis lt lw set tics in set ticslevel set tics set mxtics default set mytics default set mx2tics default set my2tics default set xtics border mirror norotate autofreq set ytics border mirror norotate autofreq set ztics border nomirror norotate autofreq set nox2tics set noy2tics set timestamp bottom norotate offset
rtabmap::ExtractorNode::BR
cv::Point2i BR
Definition: ORBextractor.h:48
rtabmap::ORBextractor::iniThFAST
int iniThFAST
Definition: ORBextractor.h:108
f
Point2(* f)(const Point3 &, OptionalJacobian< 2, 3 >)
a
ArrayXXi a
rtabmap::computeOrbDescriptor
static void computeOrbDescriptor(const KeyPoint &kpt, const Mat &img, const Point *pattern, uchar *desc, int dsize, int WTA_K)
Definition: Orb.cpp:138
rtabmap::ORBextractor::ComputePyramid
void ComputePyramid(cv::Mat image)
Definition: ORBextractor.cc:1114
rtabmap::ExtractorNode::UL
cv::Point2i UL
Definition: ORBextractor.h:48
W
Key W(std::uint64_t j)
N
N
std
glm::cos
GLM_FUNC_DECL genType cos(genType const &angle)
v
Array< int, Dynamic, 1 > v
rtabmap::IC_Angle
static float IC_Angle(const Mat &image, Point2f pt, const vector< int > &u_max, int halfPatchSize)
Definition: ORBextractor.cc:83
rtabmap::ORBextractor::mvLevelSigma2
std::vector< float > mvLevelSigma2
Definition: ORBextractor.h:121
scan_step::second
@ second
GET_VALUE
#define GET_VALUE(idx)
float
float
Matrix< Scalar, Dynamic, Dynamic >
rtabmap::computeOrientation
static bool computeOrientation(Eigen::Vector3f A, Eigen::Vector3f E, Eigen::Quaternionf &orientation)
Definition: MadgwickFilter.cpp:151
cv
Definition: Features2d.h:41
rtabmap::ORBextractor::patchSize
int patchSize
Definition: ORBextractor.h:110
resize
void resize(const MatrixTraits &t)
cols
int cols
rtabmap
Definition: CameraARCore.cpp:35
list::empty
bool empty() const
rtabmap::ExtractorNode::DivideNode
void DivideNode(ExtractorNode &n1, ExtractorNode &n2, ExtractorNode &n3, ExtractorNode &n4)
Definition: ORBextractor.cc:488
i
int i
rtabmap::IC_Angle
static float IC_Angle(const Mat &image, const int half_k, Point2f pt, const std::vector< int > &u_max)
Definition: Orb.cpp:106
rtabmap::ORBextractor::mnFeaturesPerLevel
std::vector< int > mnFeaturesPerLevel
Definition: ORBextractor.h:115
glm::sin
GLM_FUNC_DECL genType sin(genType const &angle)
list::begin
detail::list_iterator begin() const


rtabmap
Author(s): Mathieu Labbe
autogenerated on Thu Jul 25 2024 02:50:14