• Home
  • Line#
  • Scopes#
  • Navigate#
  • Raw
  • Download
1 /*********************************************************************
2 * Software License Agreement (BSD License)
3 *
4 *  Copyright (c) 2009, Willow Garage, Inc.
5 *  All rights reserved.
6 *
7 *  Redistribution and use in source and binary forms, with or without
8 *  modification, are permitted provided that the following conditions
9 *  are met:
10 *
11 *   * Redistributions of source code must retain the above copyright
12 *     notice, this list of conditions and the following disclaimer.
13 *   * Redistributions in binary form must reproduce the above
14 *     copyright notice, this list of conditions and the following
15 *     disclaimer in the documentation and/or other materials provided
16 *     with the distribution.
17 *   * Neither the name of the Willow Garage nor the names of its
18 *     contributors may be used to endorse or promote products derived
19 *     from this software without specific prior written permission.
20 *
21 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32 *  POSSIBILITY OF SUCH DAMAGE.
33 *********************************************************************/
34 
35 /** Authors: Ethan Rublee, Vincent Rabaud, Gary Bradski */
36 
37 #include "precomp.hpp"
38 #include "opencl_kernels_features2d.hpp"
39 #include <iterator>
40 
41 #ifndef CV_IMPL_ADD
42 #define CV_IMPL_ADD(x)
43 #endif
44 
45 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
46 
47 namespace cv
48 {
49 
50 const float HARRIS_K = 0.04f;
51 
copyVectorToUMat(const std::vector<_Tp> & v,OutputArray um)52 template<typename _Tp> inline void copyVectorToUMat(const std::vector<_Tp>& v, OutputArray um)
53 {
54     if(v.empty())
55         um.release();
56     else
57         Mat(1, (int)(v.size()*sizeof(v[0])), CV_8U, (void*)&v[0]).copyTo(um);
58 }
59 
60 static bool
ocl_HarrisResponses(const UMat & imgbuf,const UMat & layerinfo,const UMat & keypoints,UMat & responses,int nkeypoints,int blockSize,float harris_k)61 ocl_HarrisResponses(const UMat& imgbuf,
62                     const UMat& layerinfo,
63                     const UMat& keypoints,
64                     UMat& responses,
65                     int nkeypoints, int blockSize, float harris_k)
66 {
67     size_t globalSize[] = {nkeypoints};
68 
69     float scale = 1.f/((1 << 2) * blockSize * 255.f);
70     float scale_sq_sq = scale * scale * scale * scale;
71 
72     ocl::Kernel hr_ker("ORB_HarrisResponses", ocl::features2d::orb_oclsrc,
73                 format("-D ORB_RESPONSES -D blockSize=%d -D scale_sq_sq=%.12ef -D HARRIS_K=%.12ff", blockSize, scale_sq_sq, harris_k));
74     if( hr_ker.empty() )
75         return false;
76 
77     return hr_ker.args(ocl::KernelArg::ReadOnlyNoSize(imgbuf),
78                 ocl::KernelArg::PtrReadOnly(layerinfo),
79                 ocl::KernelArg::PtrReadOnly(keypoints),
80                 ocl::KernelArg::PtrWriteOnly(responses),
81                 nkeypoints).run(1, globalSize, 0, true);
82 }
83 
84 static bool
ocl_ICAngles(const UMat & imgbuf,const UMat & layerinfo,const UMat & keypoints,UMat & responses,const UMat & umax,int nkeypoints,int half_k)85 ocl_ICAngles(const UMat& imgbuf, const UMat& layerinfo,
86              const UMat& keypoints, UMat& responses,
87              const UMat& umax, int nkeypoints, int half_k)
88 {
89     size_t globalSize[] = {nkeypoints};
90 
91     ocl::Kernel icangle_ker("ORB_ICAngle", ocl::features2d::orb_oclsrc, "-D ORB_ANGLES");
92     if( icangle_ker.empty() )
93         return false;
94 
95     return icangle_ker.args(ocl::KernelArg::ReadOnlyNoSize(imgbuf),
96                 ocl::KernelArg::PtrReadOnly(layerinfo),
97                 ocl::KernelArg::PtrReadOnly(keypoints),
98                 ocl::KernelArg::PtrWriteOnly(responses),
99                 ocl::KernelArg::PtrReadOnly(umax),
100                 nkeypoints, half_k).run(1, globalSize, 0, true);
101 }
102 
103 
104 static bool
ocl_computeOrbDescriptors(const UMat & imgbuf,const UMat & layerInfo,const UMat & keypoints,UMat & desc,const UMat & pattern,int nkeypoints,int dsize,int wta_k)105 ocl_computeOrbDescriptors(const UMat& imgbuf, const UMat& layerInfo,
106                           const UMat& keypoints, UMat& desc, const UMat& pattern,
107                           int nkeypoints, int dsize, int wta_k)
108 {
109     size_t globalSize[] = {nkeypoints};
110 
111     ocl::Kernel desc_ker("ORB_computeDescriptor", ocl::features2d::orb_oclsrc,
112                          format("-D ORB_DESCRIPTORS -D WTA_K=%d", wta_k));
113     if( desc_ker.empty() )
114         return false;
115 
116     return desc_ker.args(ocl::KernelArg::ReadOnlyNoSize(imgbuf),
117                          ocl::KernelArg::PtrReadOnly(layerInfo),
118                          ocl::KernelArg::PtrReadOnly(keypoints),
119                          ocl::KernelArg::PtrWriteOnly(desc),
120                          ocl::KernelArg::PtrReadOnly(pattern),
121                          nkeypoints, dsize).run(1, globalSize, 0, true);
122 }
123 
124 
125 /**
126  * Function that computes the Harris responses in a
127  * blockSize x blockSize patch at given points in the image
128  */
129 static void
HarrisResponses(const Mat & img,const std::vector<Rect> & layerinfo,std::vector<KeyPoint> & pts,int blockSize,float harris_k)130 HarrisResponses(const Mat& img, const std::vector<Rect>& layerinfo,
131                 std::vector<KeyPoint>& pts, int blockSize, float harris_k)
132 {
133     CV_Assert( img.type() == CV_8UC1 && blockSize*blockSize <= 2048 );
134 
135     size_t ptidx, ptsize = pts.size();
136 
137     const uchar* ptr00 = img.ptr<uchar>();
138     int step = (int)(img.step/img.elemSize1());
139     int r = blockSize/2;
140 
141     float scale = 1.f/((1 << 2) * blockSize * 255.f);
142     float scale_sq_sq = scale * scale * scale * scale;
143 
144     AutoBuffer<int> ofsbuf(blockSize*blockSize);
145     int* ofs = ofsbuf;
146     for( int i = 0; i < blockSize; i++ )
147         for( int j = 0; j < blockSize; j++ )
148             ofs[i*blockSize + j] = (int)(i*step + j);
149 
150     for( ptidx = 0; ptidx < ptsize; ptidx++ )
151     {
152         int x0 = cvRound(pts[ptidx].pt.x);
153         int y0 = cvRound(pts[ptidx].pt.y);
154         int z = pts[ptidx].octave;
155 
156         const uchar* ptr0 = ptr00 + (y0 - r + layerinfo[z].y)*step + x0 - r + layerinfo[z].x;
157         int a = 0, b = 0, c = 0;
158 
159         for( int k = 0; k < blockSize*blockSize; k++ )
160         {
161             const uchar* ptr = ptr0 + ofs[k];
162             int Ix = (ptr[1] - ptr[-1])*2 + (ptr[-step+1] - ptr[-step-1]) + (ptr[step+1] - ptr[step-1]);
163             int Iy = (ptr[step] - ptr[-step])*2 + (ptr[step-1] - ptr[-step-1]) + (ptr[step+1] - ptr[-step+1]);
164             a += Ix*Ix;
165             b += Iy*Iy;
166             c += Ix*Iy;
167         }
168         pts[ptidx].response = ((float)a * b - (float)c * c -
169                                harris_k * ((float)a + b) * ((float)a + b))*scale_sq_sq;
170     }
171 }
172 
173 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
174 
ICAngles(const Mat & img,const std::vector<Rect> & layerinfo,std::vector<KeyPoint> & pts,const std::vector<int> & u_max,int half_k)175 static void ICAngles(const Mat& img, const std::vector<Rect>& layerinfo,
176                      std::vector<KeyPoint>& pts, const std::vector<int> & u_max, int half_k)
177 {
178     int step = (int)img.step1();
179     size_t ptidx, ptsize = pts.size();
180 
181     for( ptidx = 0; ptidx < ptsize; ptidx++ )
182     {
183         const Rect& layer = layerinfo[pts[ptidx].octave];
184         const uchar* center = &img.at<uchar>(cvRound(pts[ptidx].pt.y) + layer.y, cvRound(pts[ptidx].pt.x) + layer.x);
185 
186         int m_01 = 0, m_10 = 0;
187 
188         // Treat the center line differently, v=0
189         for (int u = -half_k; u <= half_k; ++u)
190             m_10 += u * center[u];
191 
192         // Go line by line in the circular patch
193         for (int v = 1; v <= half_k; ++v)
194         {
195             // Proceed over the two lines
196             int v_sum = 0;
197             int d = u_max[v];
198             for (int u = -d; u <= d; ++u)
199             {
200                 int val_plus = center[u + v*step], val_minus = center[u - v*step];
201                 v_sum += (val_plus - val_minus);
202                 m_10 += u * (val_plus + val_minus);
203             }
204             m_01 += v * v_sum;
205         }
206 
207         pts[ptidx].angle = fastAtan2((float)m_01, (float)m_10);
208     }
209 }
210 
211 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
212 
213 static void
computeOrbDescriptors(const Mat & imagePyramid,const std::vector<Rect> & layerInfo,const std::vector<float> & layerScale,std::vector<KeyPoint> & keypoints,Mat & descriptors,const std::vector<Point> & _pattern,int dsize,int wta_k)214 computeOrbDescriptors( const Mat& imagePyramid, const std::vector<Rect>& layerInfo,
215                        const std::vector<float>& layerScale, std::vector<KeyPoint>& keypoints,
216                        Mat& descriptors, const std::vector<Point>& _pattern, int dsize, int wta_k )
217 {
218     int step = (int)imagePyramid.step;
219     int j, i, nkeypoints = (int)keypoints.size();
220 
221     for( j = 0; j < nkeypoints; j++ )
222     {
223         const KeyPoint& kpt = keypoints[j];
224         const Rect& layer = layerInfo[kpt.octave];
225         float scale = 1.f/layerScale[kpt.octave];
226         float angle = kpt.angle;
227 
228         angle *= (float)(CV_PI/180.f);
229         float a = (float)cos(angle), b = (float)sin(angle);
230 
231         const uchar* center = &imagePyramid.at<uchar>(cvRound(kpt.pt.y*scale) + layer.y,
232                                                       cvRound(kpt.pt.x*scale) + layer.x);
233         float x, y;
234         int ix, iy;
235         const Point* pattern = &_pattern[0];
236         uchar* desc = descriptors.ptr<uchar>(j);
237 
238     #if 1
239         #define GET_VALUE(idx) \
240                (x = pattern[idx].x*a - pattern[idx].y*b, \
241                 y = pattern[idx].x*b + pattern[idx].y*a, \
242                 ix = cvRound(x), \
243                 iy = cvRound(y), \
244                 *(center + iy*step + ix) )
245     #else
246         #define GET_VALUE(idx) \
247             (x = pattern[idx].x*a - pattern[idx].y*b, \
248             y = pattern[idx].x*b + pattern[idx].y*a, \
249             ix = cvFloor(x), iy = cvFloor(y), \
250             x -= ix, y -= iy, \
251             cvRound(center[iy*step + ix]*(1-x)*(1-y) + center[(iy+1)*step + ix]*(1-x)*y + \
252                     center[iy*step + ix+1]*x*(1-y) + center[(iy+1)*step + ix+1]*x*y))
253     #endif
254 
255         if( wta_k == 2 )
256         {
257             for (i = 0; i < dsize; ++i, pattern += 16)
258             {
259                 int t0, t1, val;
260                 t0 = GET_VALUE(0); t1 = GET_VALUE(1);
261                 val = t0 < t1;
262                 t0 = GET_VALUE(2); t1 = GET_VALUE(3);
263                 val |= (t0 < t1) << 1;
264                 t0 = GET_VALUE(4); t1 = GET_VALUE(5);
265                 val |= (t0 < t1) << 2;
266                 t0 = GET_VALUE(6); t1 = GET_VALUE(7);
267                 val |= (t0 < t1) << 3;
268                 t0 = GET_VALUE(8); t1 = GET_VALUE(9);
269                 val |= (t0 < t1) << 4;
270                 t0 = GET_VALUE(10); t1 = GET_VALUE(11);
271                 val |= (t0 < t1) << 5;
272                 t0 = GET_VALUE(12); t1 = GET_VALUE(13);
273                 val |= (t0 < t1) << 6;
274                 t0 = GET_VALUE(14); t1 = GET_VALUE(15);
275                 val |= (t0 < t1) << 7;
276 
277                 desc[i] = (uchar)val;
278             }
279         }
280         else if( wta_k == 3 )
281         {
282             for (i = 0; i < dsize; ++i, pattern += 12)
283             {
284                 int t0, t1, t2, val;
285                 t0 = GET_VALUE(0); t1 = GET_VALUE(1); t2 = GET_VALUE(2);
286                 val = t2 > t1 ? (t2 > t0 ? 2 : 0) : (t1 > t0);
287 
288                 t0 = GET_VALUE(3); t1 = GET_VALUE(4); t2 = GET_VALUE(5);
289                 val |= (t2 > t1 ? (t2 > t0 ? 2 : 0) : (t1 > t0)) << 2;
290 
291                 t0 = GET_VALUE(6); t1 = GET_VALUE(7); t2 = GET_VALUE(8);
292                 val |= (t2 > t1 ? (t2 > t0 ? 2 : 0) : (t1 > t0)) << 4;
293 
294                 t0 = GET_VALUE(9); t1 = GET_VALUE(10); t2 = GET_VALUE(11);
295                 val |= (t2 > t1 ? (t2 > t0 ? 2 : 0) : (t1 > t0)) << 6;
296 
297                 desc[i] = (uchar)val;
298             }
299         }
300         else if( wta_k == 4 )
301         {
302             for (i = 0; i < dsize; ++i, pattern += 16)
303             {
304                 int t0, t1, t2, t3, u, v, k, val;
305                 t0 = GET_VALUE(0); t1 = GET_VALUE(1);
306                 t2 = GET_VALUE(2); t3 = GET_VALUE(3);
307                 u = 0, v = 2;
308                 if( t1 > t0 ) t0 = t1, u = 1;
309                 if( t3 > t2 ) t2 = t3, v = 3;
310                 k = t0 > t2 ? u : v;
311                 val = k;
312 
313                 t0 = GET_VALUE(4); t1 = GET_VALUE(5);
314                 t2 = GET_VALUE(6); t3 = GET_VALUE(7);
315                 u = 0, v = 2;
316                 if( t1 > t0 ) t0 = t1, u = 1;
317                 if( t3 > t2 ) t2 = t3, v = 3;
318                 k = t0 > t2 ? u : v;
319                 val |= k << 2;
320 
321                 t0 = GET_VALUE(8); t1 = GET_VALUE(9);
322                 t2 = GET_VALUE(10); t3 = GET_VALUE(11);
323                 u = 0, v = 2;
324                 if( t1 > t0 ) t0 = t1, u = 1;
325                 if( t3 > t2 ) t2 = t3, v = 3;
326                 k = t0 > t2 ? u : v;
327                 val |= k << 4;
328 
329                 t0 = GET_VALUE(12); t1 = GET_VALUE(13);
330                 t2 = GET_VALUE(14); t3 = GET_VALUE(15);
331                 u = 0, v = 2;
332                 if( t1 > t0 ) t0 = t1, u = 1;
333                 if( t3 > t2 ) t2 = t3, v = 3;
334                 k = t0 > t2 ? u : v;
335                 val |= k << 6;
336 
337                 desc[i] = (uchar)val;
338             }
339         }
340         else
341             CV_Error( Error::StsBadSize, "Wrong wta_k. It can be only 2, 3 or 4." );
342         #undef GET_VALUE
343     }
344 }
345 
346 
initializeOrbPattern(const Point * pattern0,std::vector<Point> & pattern,int ntuples,int tupleSize,int poolSize)347 static void initializeOrbPattern( const Point* pattern0, std::vector<Point>& pattern, int ntuples, int tupleSize, int poolSize )
348 {
349     RNG rng(0x12345678);
350     int i, k, k1;
351     pattern.resize(ntuples*tupleSize);
352 
353     for( i = 0; i < ntuples; i++ )
354     {
355         for( k = 0; k < tupleSize; k++ )
356         {
357             for(;;)
358             {
359                 int idx = rng.uniform(0, poolSize);
360                 Point pt = pattern0[idx];
361                 for( k1 = 0; k1 < k; k1++ )
362                     if( pattern[tupleSize*i + k1] == pt )
363                         break;
364                 if( k1 == k )
365                 {
366                     pattern[tupleSize*i + k] = pt;
367                     break;
368                 }
369             }
370         }
371     }
372 }
373 
374 static int bit_pattern_31_[256*4] =
375 {
376     8,-3, 9,5/*mean (0), correlation (0)*/,
377     4,2, 7,-12/*mean (1.12461e-05), correlation (0.0437584)*/,
378     -11,9, -8,2/*mean (3.37382e-05), correlation (0.0617409)*/,
379     7,-12, 12,-13/*mean (5.62303e-05), correlation (0.0636977)*/,
380     2,-13, 2,12/*mean (0.000134953), correlation (0.085099)*/,
381     1,-7, 1,6/*mean (0.000528565), correlation (0.0857175)*/,
382     -2,-10, -2,-4/*mean (0.0188821), correlation (0.0985774)*/,
383     -13,-13, -11,-8/*mean (0.0363135), correlation (0.0899616)*/,
384     -13,-3, -12,-9/*mean (0.121806), correlation (0.099849)*/,
385     10,4, 11,9/*mean (0.122065), correlation (0.093285)*/,
386     -13,-8, -8,-9/*mean (0.162787), correlation (0.0942748)*/,
387     -11,7, -9,12/*mean (0.21561), correlation (0.0974438)*/,
388     7,7, 12,6/*mean (0.160583), correlation (0.130064)*/,
389     -4,-5, -3,0/*mean (0.228171), correlation (0.132998)*/,
390     -13,2, -12,-3/*mean (0.00997526), correlation (0.145926)*/,
391     -9,0, -7,5/*mean (0.198234), correlation (0.143636)*/,
392     12,-6, 12,-1/*mean (0.0676226), correlation (0.16689)*/,
393     -3,6, -2,12/*mean (0.166847), correlation (0.171682)*/,
394     -6,-13, -4,-8/*mean (0.101215), correlation (0.179716)*/,
395     11,-13, 12,-8/*mean (0.200641), correlation (0.192279)*/,
396     4,7, 5,1/*mean (0.205106), correlation (0.186848)*/,
397     5,-3, 10,-3/*mean (0.234908), correlation (0.192319)*/,
398     3,-7, 6,12/*mean (0.0709964), correlation (0.210872)*/,
399     -8,-7, -6,-2/*mean (0.0939834), correlation (0.212589)*/,
400     -2,11, -1,-10/*mean (0.127778), correlation (0.20866)*/,
401     -13,12, -8,10/*mean (0.14783), correlation (0.206356)*/,
402     -7,3, -5,-3/*mean (0.182141), correlation (0.198942)*/,
403     -4,2, -3,7/*mean (0.188237), correlation (0.21384)*/,
404     -10,-12, -6,11/*mean (0.14865), correlation (0.23571)*/,
405     5,-12, 6,-7/*mean (0.222312), correlation (0.23324)*/,
406     5,-6, 7,-1/*mean (0.229082), correlation (0.23389)*/,
407     1,0, 4,-5/*mean (0.241577), correlation (0.215286)*/,
408     9,11, 11,-13/*mean (0.00338507), correlation (0.251373)*/,
409     4,7, 4,12/*mean (0.131005), correlation (0.257622)*/,
410     2,-1, 4,4/*mean (0.152755), correlation (0.255205)*/,
411     -4,-12, -2,7/*mean (0.182771), correlation (0.244867)*/,
412     -8,-5, -7,-10/*mean (0.186898), correlation (0.23901)*/,
413     4,11, 9,12/*mean (0.226226), correlation (0.258255)*/,
414     0,-8, 1,-13/*mean (0.0897886), correlation (0.274827)*/,
415     -13,-2, -8,2/*mean (0.148774), correlation (0.28065)*/,
416     -3,-2, -2,3/*mean (0.153048), correlation (0.283063)*/,
417     -6,9, -4,-9/*mean (0.169523), correlation (0.278248)*/,
418     8,12, 10,7/*mean (0.225337), correlation (0.282851)*/,
419     0,9, 1,3/*mean (0.226687), correlation (0.278734)*/,
420     7,-5, 11,-10/*mean (0.00693882), correlation (0.305161)*/,
421     -13,-6, -11,0/*mean (0.0227283), correlation (0.300181)*/,
422     10,7, 12,1/*mean (0.125517), correlation (0.31089)*/,
423     -6,-3, -6,12/*mean (0.131748), correlation (0.312779)*/,
424     10,-9, 12,-4/*mean (0.144827), correlation (0.292797)*/,
425     -13,8, -8,-12/*mean (0.149202), correlation (0.308918)*/,
426     -13,0, -8,-4/*mean (0.160909), correlation (0.310013)*/,
427     3,3, 7,8/*mean (0.177755), correlation (0.309394)*/,
428     5,7, 10,-7/*mean (0.212337), correlation (0.310315)*/,
429     -1,7, 1,-12/*mean (0.214429), correlation (0.311933)*/,
430     3,-10, 5,6/*mean (0.235807), correlation (0.313104)*/,
431     2,-4, 3,-10/*mean (0.00494827), correlation (0.344948)*/,
432     -13,0, -13,5/*mean (0.0549145), correlation (0.344675)*/,
433     -13,-7, -12,12/*mean (0.103385), correlation (0.342715)*/,
434     -13,3, -11,8/*mean (0.134222), correlation (0.322922)*/,
435     -7,12, -4,7/*mean (0.153284), correlation (0.337061)*/,
436     6,-10, 12,8/*mean (0.154881), correlation (0.329257)*/,
437     -9,-1, -7,-6/*mean (0.200967), correlation (0.33312)*/,
438     -2,-5, 0,12/*mean (0.201518), correlation (0.340635)*/,
439     -12,5, -7,5/*mean (0.207805), correlation (0.335631)*/,
440     3,-10, 8,-13/*mean (0.224438), correlation (0.34504)*/,
441     -7,-7, -4,5/*mean (0.239361), correlation (0.338053)*/,
442     -3,-2, -1,-7/*mean (0.240744), correlation (0.344322)*/,
443     2,9, 5,-11/*mean (0.242949), correlation (0.34145)*/,
444     -11,-13, -5,-13/*mean (0.244028), correlation (0.336861)*/,
445     -1,6, 0,-1/*mean (0.247571), correlation (0.343684)*/,
446     5,-3, 5,2/*mean (0.000697256), correlation (0.357265)*/,
447     -4,-13, -4,12/*mean (0.00213675), correlation (0.373827)*/,
448     -9,-6, -9,6/*mean (0.0126856), correlation (0.373938)*/,
449     -12,-10, -8,-4/*mean (0.0152497), correlation (0.364237)*/,
450     10,2, 12,-3/*mean (0.0299933), correlation (0.345292)*/,
451     7,12, 12,12/*mean (0.0307242), correlation (0.366299)*/,
452     -7,-13, -6,5/*mean (0.0534975), correlation (0.368357)*/,
453     -4,9, -3,4/*mean (0.099865), correlation (0.372276)*/,
454     7,-1, 12,2/*mean (0.117083), correlation (0.364529)*/,
455     -7,6, -5,1/*mean (0.126125), correlation (0.369606)*/,
456     -13,11, -12,5/*mean (0.130364), correlation (0.358502)*/,
457     -3,7, -2,-6/*mean (0.131691), correlation (0.375531)*/,
458     7,-8, 12,-7/*mean (0.160166), correlation (0.379508)*/,
459     -13,-7, -11,-12/*mean (0.167848), correlation (0.353343)*/,
460     1,-3, 12,12/*mean (0.183378), correlation (0.371916)*/,
461     2,-6, 3,0/*mean (0.228711), correlation (0.371761)*/,
462     -4,3, -2,-13/*mean (0.247211), correlation (0.364063)*/,
463     -1,-13, 1,9/*mean (0.249325), correlation (0.378139)*/,
464     7,1, 8,-6/*mean (0.000652272), correlation (0.411682)*/,
465     1,-1, 3,12/*mean (0.00248538), correlation (0.392988)*/,
466     9,1, 12,6/*mean (0.0206815), correlation (0.386106)*/,
467     -1,-9, -1,3/*mean (0.0364485), correlation (0.410752)*/,
468     -13,-13, -10,5/*mean (0.0376068), correlation (0.398374)*/,
469     7,7, 10,12/*mean (0.0424202), correlation (0.405663)*/,
470     12,-5, 12,9/*mean (0.0942645), correlation (0.410422)*/,
471     6,3, 7,11/*mean (0.1074), correlation (0.413224)*/,
472     5,-13, 6,10/*mean (0.109256), correlation (0.408646)*/,
473     2,-12, 2,3/*mean (0.131691), correlation (0.416076)*/,
474     3,8, 4,-6/*mean (0.165081), correlation (0.417569)*/,
475     2,6, 12,-13/*mean (0.171874), correlation (0.408471)*/,
476     9,-12, 10,3/*mean (0.175146), correlation (0.41296)*/,
477     -8,4, -7,9/*mean (0.183682), correlation (0.402956)*/,
478     -11,12, -4,-6/*mean (0.184672), correlation (0.416125)*/,
479     1,12, 2,-8/*mean (0.191487), correlation (0.386696)*/,
480     6,-9, 7,-4/*mean (0.192668), correlation (0.394771)*/,
481     2,3, 3,-2/*mean (0.200157), correlation (0.408303)*/,
482     6,3, 11,0/*mean (0.204588), correlation (0.411762)*/,
483     3,-3, 8,-8/*mean (0.205904), correlation (0.416294)*/,
484     7,8, 9,3/*mean (0.213237), correlation (0.409306)*/,
485     -11,-5, -6,-4/*mean (0.243444), correlation (0.395069)*/,
486     -10,11, -5,10/*mean (0.247672), correlation (0.413392)*/,
487     -5,-8, -3,12/*mean (0.24774), correlation (0.411416)*/,
488     -10,5, -9,0/*mean (0.00213675), correlation (0.454003)*/,
489     8,-1, 12,-6/*mean (0.0293635), correlation (0.455368)*/,
490     4,-6, 6,-11/*mean (0.0404971), correlation (0.457393)*/,
491     -10,12, -8,7/*mean (0.0481107), correlation (0.448364)*/,
492     4,-2, 6,7/*mean (0.050641), correlation (0.455019)*/,
493     -2,0, -2,12/*mean (0.0525978), correlation (0.44338)*/,
494     -5,-8, -5,2/*mean (0.0629667), correlation (0.457096)*/,
495     7,-6, 10,12/*mean (0.0653846), correlation (0.445623)*/,
496     -9,-13, -8,-8/*mean (0.0858749), correlation (0.449789)*/,
497     -5,-13, -5,-2/*mean (0.122402), correlation (0.450201)*/,
498     8,-8, 9,-13/*mean (0.125416), correlation (0.453224)*/,
499     -9,-11, -9,0/*mean (0.130128), correlation (0.458724)*/,
500     1,-8, 1,-2/*mean (0.132467), correlation (0.440133)*/,
501     7,-4, 9,1/*mean (0.132692), correlation (0.454)*/,
502     -2,1, -1,-4/*mean (0.135695), correlation (0.455739)*/,
503     11,-6, 12,-11/*mean (0.142904), correlation (0.446114)*/,
504     -12,-9, -6,4/*mean (0.146165), correlation (0.451473)*/,
505     3,7, 7,12/*mean (0.147627), correlation (0.456643)*/,
506     5,5, 10,8/*mean (0.152901), correlation (0.455036)*/,
507     0,-4, 2,8/*mean (0.167083), correlation (0.459315)*/,
508     -9,12, -5,-13/*mean (0.173234), correlation (0.454706)*/,
509     0,7, 2,12/*mean (0.18312), correlation (0.433855)*/,
510     -1,2, 1,7/*mean (0.185504), correlation (0.443838)*/,
511     5,11, 7,-9/*mean (0.185706), correlation (0.451123)*/,
512     3,5, 6,-8/*mean (0.188968), correlation (0.455808)*/,
513     -13,-4, -8,9/*mean (0.191667), correlation (0.459128)*/,
514     -5,9, -3,-3/*mean (0.193196), correlation (0.458364)*/,
515     -4,-7, -3,-12/*mean (0.196536), correlation (0.455782)*/,
516     6,5, 8,0/*mean (0.1972), correlation (0.450481)*/,
517     -7,6, -6,12/*mean (0.199438), correlation (0.458156)*/,
518     -13,6, -5,-2/*mean (0.211224), correlation (0.449548)*/,
519     1,-10, 3,10/*mean (0.211718), correlation (0.440606)*/,
520     4,1, 8,-4/*mean (0.213034), correlation (0.443177)*/,
521     -2,-2, 2,-13/*mean (0.234334), correlation (0.455304)*/,
522     2,-12, 12,12/*mean (0.235684), correlation (0.443436)*/,
523     -2,-13, 0,-6/*mean (0.237674), correlation (0.452525)*/,
524     4,1, 9,3/*mean (0.23962), correlation (0.444824)*/,
525     -6,-10, -3,-5/*mean (0.248459), correlation (0.439621)*/,
526     -3,-13, -1,1/*mean (0.249505), correlation (0.456666)*/,
527     7,5, 12,-11/*mean (0.00119208), correlation (0.495466)*/,
528     4,-2, 5,-7/*mean (0.00372245), correlation (0.484214)*/,
529     -13,9, -9,-5/*mean (0.00741116), correlation (0.499854)*/,
530     7,1, 8,6/*mean (0.0208952), correlation (0.499773)*/,
531     7,-8, 7,6/*mean (0.0220085), correlation (0.501609)*/,
532     -7,-4, -7,1/*mean (0.0233806), correlation (0.496568)*/,
533     -8,11, -7,-8/*mean (0.0236505), correlation (0.489719)*/,
534     -13,6, -12,-8/*mean (0.0268781), correlation (0.503487)*/,
535     2,4, 3,9/*mean (0.0323324), correlation (0.501938)*/,
536     10,-5, 12,3/*mean (0.0399235), correlation (0.494029)*/,
537     -6,-5, -6,7/*mean (0.0420153), correlation (0.486579)*/,
538     8,-3, 9,-8/*mean (0.0548021), correlation (0.484237)*/,
539     2,-12, 2,8/*mean (0.0616622), correlation (0.496642)*/,
540     -11,-2, -10,3/*mean (0.0627755), correlation (0.498563)*/,
541     -12,-13, -7,-9/*mean (0.0829622), correlation (0.495491)*/,
542     -11,0, -10,-5/*mean (0.0843342), correlation (0.487146)*/,
543     5,-3, 11,8/*mean (0.0929937), correlation (0.502315)*/,
544     -2,-13, -1,12/*mean (0.113327), correlation (0.48941)*/,
545     -1,-8, 0,9/*mean (0.132119), correlation (0.467268)*/,
546     -13,-11, -12,-5/*mean (0.136269), correlation (0.498771)*/,
547     -10,-2, -10,11/*mean (0.142173), correlation (0.498714)*/,
548     -3,9, -2,-13/*mean (0.144141), correlation (0.491973)*/,
549     2,-3, 3,2/*mean (0.14892), correlation (0.500782)*/,
550     -9,-13, -4,0/*mean (0.150371), correlation (0.498211)*/,
551     -4,6, -3,-10/*mean (0.152159), correlation (0.495547)*/,
552     -4,12, -2,-7/*mean (0.156152), correlation (0.496925)*/,
553     -6,-11, -4,9/*mean (0.15749), correlation (0.499222)*/,
554     6,-3, 6,11/*mean (0.159211), correlation (0.503821)*/,
555     -13,11, -5,5/*mean (0.162427), correlation (0.501907)*/,
556     11,11, 12,6/*mean (0.16652), correlation (0.497632)*/,
557     7,-5, 12,-2/*mean (0.169141), correlation (0.484474)*/,
558     -1,12, 0,7/*mean (0.169456), correlation (0.495339)*/,
559     -4,-8, -3,-2/*mean (0.171457), correlation (0.487251)*/,
560     -7,1, -6,7/*mean (0.175), correlation (0.500024)*/,
561     -13,-12, -8,-13/*mean (0.175866), correlation (0.497523)*/,
562     -7,-2, -6,-8/*mean (0.178273), correlation (0.501854)*/,
563     -8,5, -6,-9/*mean (0.181107), correlation (0.494888)*/,
564     -5,-1, -4,5/*mean (0.190227), correlation (0.482557)*/,
565     -13,7, -8,10/*mean (0.196739), correlation (0.496503)*/,
566     1,5, 5,-13/*mean (0.19973), correlation (0.499759)*/,
567     1,0, 10,-13/*mean (0.204465), correlation (0.49873)*/,
568     9,12, 10,-1/*mean (0.209334), correlation (0.49063)*/,
569     5,-8, 10,-9/*mean (0.211134), correlation (0.503011)*/,
570     -1,11, 1,-13/*mean (0.212), correlation (0.499414)*/,
571     -9,-3, -6,2/*mean (0.212168), correlation (0.480739)*/,
572     -1,-10, 1,12/*mean (0.212731), correlation (0.502523)*/,
573     -13,1, -8,-10/*mean (0.21327), correlation (0.489786)*/,
574     8,-11, 10,-6/*mean (0.214159), correlation (0.488246)*/,
575     2,-13, 3,-6/*mean (0.216993), correlation (0.50287)*/,
576     7,-13, 12,-9/*mean (0.223639), correlation (0.470502)*/,
577     -10,-10, -5,-7/*mean (0.224089), correlation (0.500852)*/,
578     -10,-8, -8,-13/*mean (0.228666), correlation (0.502629)*/,
579     4,-6, 8,5/*mean (0.22906), correlation (0.498305)*/,
580     3,12, 8,-13/*mean (0.233378), correlation (0.503825)*/,
581     -4,2, -3,-3/*mean (0.234323), correlation (0.476692)*/,
582     5,-13, 10,-12/*mean (0.236392), correlation (0.475462)*/,
583     4,-13, 5,-1/*mean (0.236842), correlation (0.504132)*/,
584     -9,9, -4,3/*mean (0.236977), correlation (0.497739)*/,
585     0,3, 3,-9/*mean (0.24314), correlation (0.499398)*/,
586     -12,1, -6,1/*mean (0.243297), correlation (0.489447)*/,
587     3,2, 4,-8/*mean (0.00155196), correlation (0.553496)*/,
588     -10,-10, -10,9/*mean (0.00239541), correlation (0.54297)*/,
589     8,-13, 12,12/*mean (0.0034413), correlation (0.544361)*/,
590     -8,-12, -6,-5/*mean (0.003565), correlation (0.551225)*/,
591     2,2, 3,7/*mean (0.00835583), correlation (0.55285)*/,
592     10,6, 11,-8/*mean (0.00885065), correlation (0.540913)*/,
593     6,8, 8,-12/*mean (0.0101552), correlation (0.551085)*/,
594     -7,10, -6,5/*mean (0.0102227), correlation (0.533635)*/,
595     -3,-9, -3,9/*mean (0.0110211), correlation (0.543121)*/,
596     -1,-13, -1,5/*mean (0.0113473), correlation (0.550173)*/,
597     -3,-7, -3,4/*mean (0.0140913), correlation (0.554774)*/,
598     -8,-2, -8,3/*mean (0.017049), correlation (0.55461)*/,
599     4,2, 12,12/*mean (0.01778), correlation (0.546921)*/,
600     2,-5, 3,11/*mean (0.0224022), correlation (0.549667)*/,
601     6,-9, 11,-13/*mean (0.029161), correlation (0.546295)*/,
602     3,-1, 7,12/*mean (0.0303081), correlation (0.548599)*/,
603     11,-1, 12,4/*mean (0.0355151), correlation (0.523943)*/,
604     -3,0, -3,6/*mean (0.0417904), correlation (0.543395)*/,
605     4,-11, 4,12/*mean (0.0487292), correlation (0.542818)*/,
606     2,-4, 2,1/*mean (0.0575124), correlation (0.554888)*/,
607     -10,-6, -8,1/*mean (0.0594242), correlation (0.544026)*/,
608     -13,7, -11,1/*mean (0.0597391), correlation (0.550524)*/,
609     -13,12, -11,-13/*mean (0.0608974), correlation (0.55383)*/,
610     6,0, 11,-13/*mean (0.065126), correlation (0.552006)*/,
611     0,-1, 1,4/*mean (0.074224), correlation (0.546372)*/,
612     -13,3, -9,-2/*mean (0.0808592), correlation (0.554875)*/,
613     -9,8, -6,-3/*mean (0.0883378), correlation (0.551178)*/,
614     -13,-6, -8,-2/*mean (0.0901035), correlation (0.548446)*/,
615     5,-9, 8,10/*mean (0.0949843), correlation (0.554694)*/,
616     2,7, 3,-9/*mean (0.0994152), correlation (0.550979)*/,
617     -1,-6, -1,-1/*mean (0.10045), correlation (0.552714)*/,
618     9,5, 11,-2/*mean (0.100686), correlation (0.552594)*/,
619     11,-3, 12,-8/*mean (0.101091), correlation (0.532394)*/,
620     3,0, 3,5/*mean (0.101147), correlation (0.525576)*/,
621     -1,4, 0,10/*mean (0.105263), correlation (0.531498)*/,
622     3,-6, 4,5/*mean (0.110785), correlation (0.540491)*/,
623     -13,0, -10,5/*mean (0.112798), correlation (0.536582)*/,
624     5,8, 12,11/*mean (0.114181), correlation (0.555793)*/,
625     8,9, 9,-6/*mean (0.117431), correlation (0.553763)*/,
626     7,-4, 8,-12/*mean (0.118522), correlation (0.553452)*/,
627     -10,4, -10,9/*mean (0.12094), correlation (0.554785)*/,
628     7,3, 12,4/*mean (0.122582), correlation (0.555825)*/,
629     9,-7, 10,-2/*mean (0.124978), correlation (0.549846)*/,
630     7,0, 12,-2/*mean (0.127002), correlation (0.537452)*/,
631     -1,-6, 0,-11/*mean (0.127148), correlation (0.547401)*/
632 };
633 
634 
makeRandomPattern(int patchSize,Point * pattern,int npoints)635 static void makeRandomPattern(int patchSize, Point* pattern, int npoints)
636 {
637     RNG rng(0x34985739); // we always start with a fixed seed,
638                          // to make patterns the same on each run
639     for( int i = 0; i < npoints; i++ )
640     {
641         pattern[i].x = rng.uniform(-patchSize/2, patchSize/2+1);
642         pattern[i].y = rng.uniform(-patchSize/2, patchSize/2+1);
643     }
644 }
645 
646 
getScale(int level,int firstLevel,double scaleFactor)647 static inline float getScale(int level, int firstLevel, double scaleFactor)
648 {
649     return (float)std::pow(scaleFactor, (double)(level - firstLevel));
650 }
651 
652 
653 class ORB_Impl : public ORB
654 {
655 public:
ORB_Impl(int _nfeatures,float _scaleFactor,int _nlevels,int _edgeThreshold,int _firstLevel,int _WTA_K,int _scoreType,int _patchSize,int _fastThreshold)656     explicit ORB_Impl(int _nfeatures, float _scaleFactor, int _nlevels, int _edgeThreshold,
657              int _firstLevel, int _WTA_K, int _scoreType, int _patchSize, int _fastThreshold) :
658         nfeatures(_nfeatures), scaleFactor(_scaleFactor), nlevels(_nlevels),
659         edgeThreshold(_edgeThreshold), firstLevel(_firstLevel), wta_k(_WTA_K),
660         scoreType(_scoreType), patchSize(_patchSize), fastThreshold(_fastThreshold)
661     {}
662 
setMaxFeatures(int maxFeatures)663     void setMaxFeatures(int maxFeatures) { nfeatures = maxFeatures; }
getMaxFeatures() const664     int getMaxFeatures() const { return nfeatures; }
665 
setScaleFactor(double scaleFactor_)666     void setScaleFactor(double scaleFactor_) { scaleFactor = scaleFactor_; }
getScaleFactor() const667     double getScaleFactor() const { return scaleFactor; }
668 
setNLevels(int nlevels_)669     void setNLevels(int nlevels_) { nlevels = nlevels_; }
getNLevels() const670     int getNLevels() const { return nlevels; }
671 
setEdgeThreshold(int edgeThreshold_)672     void setEdgeThreshold(int edgeThreshold_) { edgeThreshold = edgeThreshold_; }
getEdgeThreshold() const673     int getEdgeThreshold() const { return edgeThreshold; }
674 
setFirstLevel(int firstLevel_)675     void setFirstLevel(int firstLevel_) { firstLevel = firstLevel_; }
getFirstLevel() const676     int getFirstLevel() const { return firstLevel; }
677 
setWTA_K(int wta_k_)678     void setWTA_K(int wta_k_) { wta_k = wta_k_; }
getWTA_K() const679     int getWTA_K() const { return wta_k; }
680 
setScoreType(int scoreType_)681     void setScoreType(int scoreType_) { scoreType = scoreType_; }
getScoreType() const682     int getScoreType() const { return scoreType; }
683 
setPatchSize(int patchSize_)684     void setPatchSize(int patchSize_) { patchSize = patchSize_; }
getPatchSize() const685     int getPatchSize() const { return patchSize; }
686 
setFastThreshold(int fastThreshold_)687     void setFastThreshold(int fastThreshold_) { fastThreshold = fastThreshold_; }
getFastThreshold() const688     int getFastThreshold() const { return fastThreshold; }
689 
690     // returns the descriptor size in bytes
691     int descriptorSize() const;
692     // returns the descriptor type
693     int descriptorType() const;
694     // returns the default norm type
695     int defaultNorm() const;
696 
697     // Compute the ORB_Impl features and descriptors on an image
698     void detectAndCompute( InputArray image, InputArray mask, std::vector<KeyPoint>& keypoints,
699                      OutputArray descriptors, bool useProvidedKeypoints=false );
700 
701 protected:
702 
703     int nfeatures;
704     double scaleFactor;
705     int nlevels;
706     int edgeThreshold;
707     int firstLevel;
708     int wta_k;
709     int scoreType;
710     int patchSize;
711     int fastThreshold;
712 };
713 
descriptorSize() const714 int ORB_Impl::descriptorSize() const
715 {
716     return kBytes;
717 }
718 
descriptorType() const719 int ORB_Impl::descriptorType() const
720 {
721     return CV_8U;
722 }
723 
defaultNorm() const724 int ORB_Impl::defaultNorm() const
725 {
726     return NORM_HAMMING;
727 }
728 
uploadORBKeypoints(const std::vector<KeyPoint> & src,std::vector<Vec3i> & buf,OutputArray dst)729 static void uploadORBKeypoints(const std::vector<KeyPoint>& src, std::vector<Vec3i>& buf, OutputArray dst)
730 {
731     size_t i, n = src.size();
732     buf.resize(std::max(buf.size(), n));
733     for( i = 0; i < n; i++ )
734         buf[i] = Vec3i(cvRound(src[i].pt.x), cvRound(src[i].pt.y), src[i].octave);
735     copyVectorToUMat(buf, dst);
736 }
737 
738 typedef union if32_t
739 {
740     int i;
741     float f;
742 }
743 if32_t;
744 
uploadORBKeypoints(const std::vector<KeyPoint> & src,const std::vector<float> & layerScale,std::vector<Vec4i> & buf,OutputArray dst)745 static void uploadORBKeypoints(const std::vector<KeyPoint>& src,
746                                const std::vector<float>& layerScale,
747                                std::vector<Vec4i>& buf, OutputArray dst)
748 {
749     size_t i, n = src.size();
750     buf.resize(std::max(buf.size(), n));
751     for( i = 0; i < n; i++ )
752     {
753         int z = src[i].octave;
754         float scale = 1.f/layerScale[z];
755         if32_t angle;
756         angle.f = src[i].angle;
757         buf[i] = Vec4i(cvRound(src[i].pt.x*scale), cvRound(src[i].pt.y*scale), z, angle.i);
758     }
759     copyVectorToUMat(buf, dst);
760 }
761 
762 
763 /** Compute the ORB_Impl keypoints on an image
764  * @param image_pyramid the image pyramid to compute the features and descriptors on
765  * @param mask_pyramid the masks to apply at every level
766  * @param keypoints the resulting keypoints, clustered per level
767  */
computeKeyPoints(const Mat & imagePyramid,const UMat & uimagePyramid,const Mat & maskPyramid,const std::vector<Rect> & layerInfo,const UMat & ulayerInfo,const std::vector<float> & layerScale,std::vector<KeyPoint> & allKeypoints,int nfeatures,double scaleFactor,int edgeThreshold,int patchSize,int scoreType,bool useOCL,int fastThreshold)768 static void computeKeyPoints(const Mat& imagePyramid,
769                              const UMat& uimagePyramid,
770                              const Mat& maskPyramid,
771                              const std::vector<Rect>& layerInfo,
772                              const UMat& ulayerInfo,
773                              const std::vector<float>& layerScale,
774                              std::vector<KeyPoint>& allKeypoints,
775                              int nfeatures, double scaleFactor,
776                              int edgeThreshold, int patchSize, int scoreType,
777                              bool useOCL, int fastThreshold  )
778 {
779     int i, nkeypoints, level, nlevels = (int)layerInfo.size();
780     std::vector<int> nfeaturesPerLevel(nlevels);
781 
782     // fill the extractors and descriptors for the corresponding scales
783     float factor = (float)(1.0 / scaleFactor);
784     float ndesiredFeaturesPerScale = nfeatures*(1 - factor)/(1 - (float)std::pow((double)factor, (double)nlevels));
785 
786     int sumFeatures = 0;
787     for( level = 0; level < nlevels-1; level++ )
788     {
789         nfeaturesPerLevel[level] = cvRound(ndesiredFeaturesPerScale);
790         sumFeatures += nfeaturesPerLevel[level];
791         ndesiredFeaturesPerScale *= factor;
792     }
793     nfeaturesPerLevel[nlevels-1] = std::max(nfeatures - sumFeatures, 0);
794 
795     // Make sure we forget about what is too close to the boundary
796     //edge_threshold_ = std::max(edge_threshold_, patch_size_/2 + kKernelWidth / 2 + 2);
797 
798     // pre-compute the end of a row in a circular patch
799     int halfPatchSize = patchSize / 2;
800     std::vector<int> umax(halfPatchSize + 2);
801 
802     int v, v0, vmax = cvFloor(halfPatchSize * std::sqrt(2.f) / 2 + 1);
803     int vmin = cvCeil(halfPatchSize * std::sqrt(2.f) / 2);
804     for (v = 0; v <= vmax; ++v)
805         umax[v] = cvRound(std::sqrt((double)halfPatchSize * halfPatchSize - v * v));
806 
807     // Make sure we are symmetric
808     for (v = halfPatchSize, v0 = 0; v >= vmin; --v)
809     {
810         while (umax[v0] == umax[v0 + 1])
811             ++v0;
812         umax[v] = v0;
813         ++v0;
814     }
815 
816     allKeypoints.clear();
817     std::vector<KeyPoint> keypoints;
818     std::vector<int> counters(nlevels);
819     keypoints.reserve(nfeaturesPerLevel[0]*2);
820 
821     for( level = 0; level < nlevels; level++ )
822     {
823         int featuresNum = nfeaturesPerLevel[level];
824         Mat img = imagePyramid(layerInfo[level]);
825         Mat mask = maskPyramid.empty() ? Mat() : maskPyramid(layerInfo[level]);
826 
827         // Detect FAST features, 20 is a good threshold
828         {
829         Ptr<FastFeatureDetector> fd = FastFeatureDetector::create(fastThreshold, true);
830         fd->detect(img, keypoints, mask);
831         }
832 
833         // Remove keypoints very close to the border
834         KeyPointsFilter::runByImageBorder(keypoints, img.size(), edgeThreshold);
835 
836         // Keep more points than necessary as FAST does not give amazing corners
837         KeyPointsFilter::retainBest(keypoints, scoreType == ORB_Impl::HARRIS_SCORE ? 2 * featuresNum : featuresNum);
838 
839         nkeypoints = (int)keypoints.size();
840         counters[level] = nkeypoints;
841 
842         float sf = layerScale[level];
843         for( i = 0; i < nkeypoints; i++ )
844         {
845             keypoints[i].octave = level;
846             keypoints[i].size = patchSize*sf;
847         }
848 
849         std::copy(keypoints.begin(), keypoints.end(), std::back_inserter(allKeypoints));
850     }
851 
852     std::vector<Vec3i> ukeypoints_buf;
853 
854     nkeypoints = (int)allKeypoints.size();
855     if(nkeypoints == 0)
856     {
857         return;
858     }
859     Mat responses;
860     UMat ukeypoints, uresponses(1, nkeypoints, CV_32F);
861 
862     // Select best features using the Harris cornerness (better scoring than FAST)
863     if( scoreType == ORB_Impl::HARRIS_SCORE )
864     {
865         if( useOCL )
866         {
867             uploadORBKeypoints(allKeypoints, ukeypoints_buf, ukeypoints);
868             useOCL = ocl_HarrisResponses( uimagePyramid, ulayerInfo, ukeypoints,
869                                           uresponses, nkeypoints, 7, HARRIS_K );
870             if( useOCL )
871             {
872                 CV_IMPL_ADD(CV_IMPL_OCL);
873                 uresponses.copyTo(responses);
874                 for( i = 0; i < nkeypoints; i++ )
875                     allKeypoints[i].response = responses.at<float>(i);
876             }
877         }
878 
879         if( !useOCL )
880             HarrisResponses(imagePyramid, layerInfo, allKeypoints, 7, HARRIS_K);
881 
882         std::vector<KeyPoint> newAllKeypoints;
883         newAllKeypoints.reserve(nfeaturesPerLevel[0]*nlevels);
884 
885         int offset = 0;
886         for( level = 0; level < nlevels; level++ )
887         {
888             int featuresNum = nfeaturesPerLevel[level];
889             nkeypoints = counters[level];
890             keypoints.resize(nkeypoints);
891             std::copy(allKeypoints.begin() + offset,
892                       allKeypoints.begin() + offset + nkeypoints,
893                       keypoints.begin());
894             offset += nkeypoints;
895 
896             //cull to the final desired level, using the new Harris scores.
897             KeyPointsFilter::retainBest(keypoints, featuresNum);
898 
899             std::copy(keypoints.begin(), keypoints.end(), std::back_inserter(newAllKeypoints));
900         }
901         std::swap(allKeypoints, newAllKeypoints);
902     }
903 
904     nkeypoints = (int)allKeypoints.size();
905     if( useOCL )
906     {
907         UMat uumax;
908         if( useOCL )
909             copyVectorToUMat(umax, uumax);
910 
911         uploadORBKeypoints(allKeypoints, ukeypoints_buf, ukeypoints);
912         useOCL = ocl_ICAngles(uimagePyramid, ulayerInfo, ukeypoints, uresponses, uumax,
913                               nkeypoints, halfPatchSize);
914 
915         if( useOCL )
916         {
917             CV_IMPL_ADD(CV_IMPL_OCL);
918             uresponses.copyTo(responses);
919             for( i = 0; i < nkeypoints; i++ )
920                 allKeypoints[i].angle = responses.at<float>(i);
921         }
922     }
923 
924     if( !useOCL )
925     {
926         ICAngles(imagePyramid, layerInfo, allKeypoints, umax, halfPatchSize);
927     }
928 
929     for( i = 0; i < nkeypoints; i++ )
930     {
931         float scale = layerScale[allKeypoints[i].octave];
932         allKeypoints[i].pt *= scale;
933     }
934 }
935 
936 
937 /** Compute the ORB_Impl features and descriptors on an image
938  * @param img the image to compute the features and descriptors on
939  * @param mask the mask to apply
940  * @param keypoints the resulting keypoints
941  * @param descriptors the resulting descriptors
942  * @param do_keypoints if true, the keypoints are computed, otherwise used as an input
943  * @param do_descriptors if true, also computes the descriptors
944  */
detectAndCompute(InputArray _image,InputArray _mask,std::vector<KeyPoint> & keypoints,OutputArray _descriptors,bool useProvidedKeypoints)945 void ORB_Impl::detectAndCompute( InputArray _image, InputArray _mask,
946                                  std::vector<KeyPoint>& keypoints,
947                                  OutputArray _descriptors, bool useProvidedKeypoints )
948 {
949     CV_Assert(patchSize >= 2);
950 
951     bool do_keypoints = !useProvidedKeypoints;
952     bool do_descriptors = _descriptors.needed();
953 
954     if( (!do_keypoints && !do_descriptors) || _image.empty() )
955         return;
956 
957     //ROI handling
958     const int HARRIS_BLOCK_SIZE = 9;
959     int halfPatchSize = patchSize / 2;
960     int border = std::max(edgeThreshold, std::max(halfPatchSize, HARRIS_BLOCK_SIZE/2))+1;
961 
962     bool useOCL = ocl::useOpenCL();
963 
964     Mat image = _image.getMat(), mask = _mask.getMat();
965     if( image.type() != CV_8UC1 )
966         cvtColor(_image, image, COLOR_BGR2GRAY);
967 
968     int i, level, nLevels = this->nlevels, nkeypoints = (int)keypoints.size();
969     bool sortedByLevel = true;
970 
971     if( !do_keypoints )
972     {
973         // if we have pre-computed keypoints, they may use more levels than it is set in parameters
974         // !!!TODO!!! implement more correct method, independent from the used keypoint detector.
975         // Namely, the detector should provide correct size of each keypoint. Based on the keypoint size
976         // and the algorithm used (i.e. BRIEF, running on 31x31 patches) we should compute the approximate
977         // scale-factor that we need to apply. Then we should cluster all the computed scale-factors and
978         // for each cluster compute the corresponding image.
979         //
980         // In short, ultimately the descriptor should
981         // ignore octave parameter and deal only with the keypoint size.
982         nLevels = 0;
983         for( i = 0; i < nkeypoints; i++ )
984         {
985             level = keypoints[i].octave;
986             CV_Assert(level >= 0);
987             if( i > 0 && level < keypoints[i-1].octave )
988                 sortedByLevel = false;
989             nLevels = std::max(nLevels, level);
990         }
991         nLevels++;
992     }
993 
994     std::vector<Rect> layerInfo(nLevels);
995     std::vector<int> layerOfs(nLevels);
996     std::vector<float> layerScale(nLevels);
997     Mat imagePyramid, maskPyramid;
998     UMat uimagePyramid, ulayerInfo;
999 
1000     int level_dy = image.rows + border*2;
1001     Point level_ofs(0,0);
1002     Size bufSize((image.cols + border*2 + 15) & -16, 0);
1003 
1004     for( level = 0; level < nLevels; level++ )
1005     {
1006         float scale = getScale(level, firstLevel, scaleFactor);
1007         layerScale[level] = scale;
1008         Size sz(cvRound(image.cols/scale), cvRound(image.rows/scale));
1009         Size wholeSize(sz.width + border*2, sz.height + border*2);
1010         if( level_ofs.x + wholeSize.width > bufSize.width )
1011         {
1012             level_ofs = Point(0, level_ofs.y + level_dy);
1013             level_dy = wholeSize.height;
1014         }
1015 
1016         Rect linfo(level_ofs.x + border, level_ofs.y + border, sz.width, sz.height);
1017         layerInfo[level] = linfo;
1018         layerOfs[level] = linfo.y*bufSize.width + linfo.x;
1019         level_ofs.x += wholeSize.width;
1020     }
1021     bufSize.height = level_ofs.y + level_dy;
1022 
1023     imagePyramid.create(bufSize, CV_8U);
1024     if( !mask.empty() )
1025         maskPyramid.create(bufSize, CV_8U);
1026 
1027     Mat prevImg = image, prevMask = mask;
1028 
1029     // Pre-compute the scale pyramids
1030     for (level = 0; level < nLevels; ++level)
1031     {
1032         Rect linfo = layerInfo[level];
1033         Size sz(linfo.width, linfo.height);
1034         Size wholeSize(sz.width + border*2, sz.height + border*2);
1035         Rect wholeLinfo = Rect(linfo.x - border, linfo.y - border, wholeSize.width, wholeSize.height);
1036         Mat extImg = imagePyramid(wholeLinfo), extMask;
1037         Mat currImg = extImg(Rect(border, border, sz.width, sz.height)), currMask;
1038 
1039         if( !mask.empty() )
1040         {
1041             extMask = maskPyramid(wholeLinfo);
1042             currMask = extMask(Rect(border, border, sz.width, sz.height));
1043         }
1044 
1045         // Compute the resized image
1046         if( level != firstLevel )
1047         {
1048             resize(prevImg, currImg, sz, 0, 0, INTER_LINEAR);
1049             if( !mask.empty() )
1050             {
1051                 resize(prevMask, currMask, sz, 0, 0, INTER_LINEAR);
1052                 if( level > firstLevel )
1053                     threshold(currMask, currMask, 254, 0, THRESH_TOZERO);
1054             }
1055 
1056             copyMakeBorder(currImg, extImg, border, border, border, border,
1057                            BORDER_REFLECT_101+BORDER_ISOLATED);
1058             if (!mask.empty())
1059                 copyMakeBorder(currMask, extMask, border, border, border, border,
1060                                BORDER_CONSTANT+BORDER_ISOLATED);
1061         }
1062         else
1063         {
1064             copyMakeBorder(image, extImg, border, border, border, border,
1065                            BORDER_REFLECT_101);
1066             if( !mask.empty() )
1067                 copyMakeBorder(mask, extMask, border, border, border, border,
1068                                BORDER_CONSTANT+BORDER_ISOLATED);
1069         }
1070         prevImg = currImg;
1071         prevMask = currMask;
1072     }
1073 
1074     if( useOCL )
1075         copyVectorToUMat(layerOfs, ulayerInfo);
1076 
1077     if( do_keypoints )
1078     {
1079         if( useOCL )
1080             imagePyramid.copyTo(uimagePyramid);
1081 
1082         // Get keypoints, those will be far enough from the border that no check will be required for the descriptor
1083         computeKeyPoints(imagePyramid, uimagePyramid, maskPyramid,
1084                          layerInfo, ulayerInfo, layerScale, keypoints,
1085                          nfeatures, scaleFactor, edgeThreshold, patchSize, scoreType, useOCL, fastThreshold);
1086     }
1087     else
1088     {
1089         KeyPointsFilter::runByImageBorder(keypoints, image.size(), edgeThreshold);
1090 
1091         if( !sortedByLevel )
1092         {
1093             std::vector<std::vector<KeyPoint> > allKeypoints(nLevels);
1094             nkeypoints = (int)keypoints.size();
1095             for( i = 0; i < nkeypoints; i++ )
1096             {
1097                 level = keypoints[i].octave;
1098                 CV_Assert(0 <= level);
1099                 allKeypoints[level].push_back(keypoints[i]);
1100             }
1101             keypoints.clear();
1102             for( level = 0; level < nLevels; level++ )
1103                 std::copy(allKeypoints[level].begin(), allKeypoints[level].end(), std::back_inserter(keypoints));
1104         }
1105     }
1106 
1107     if( do_descriptors )
1108     {
1109         int dsize = descriptorSize();
1110 
1111         nkeypoints = (int)keypoints.size();
1112         if( nkeypoints == 0 )
1113         {
1114             _descriptors.release();
1115             return;
1116         }
1117 
1118         _descriptors.create(nkeypoints, dsize, CV_8U);
1119         std::vector<Point> pattern;
1120 
1121         const int npoints = 512;
1122         Point patternbuf[npoints];
1123         const Point* pattern0 = (const Point*)bit_pattern_31_;
1124 
1125         if( patchSize != 31 )
1126         {
1127             pattern0 = patternbuf;
1128             makeRandomPattern(patchSize, patternbuf, npoints);
1129         }
1130 
1131         CV_Assert( wta_k == 2 || wta_k == 3 || wta_k == 4 );
1132 
1133         if( wta_k == 2 )
1134             std::copy(pattern0, pattern0 + npoints, std::back_inserter(pattern));
1135         else
1136         {
1137             int ntuples = descriptorSize()*4;
1138             initializeOrbPattern(pattern0, pattern, ntuples, wta_k, npoints);
1139         }
1140 
1141         for( level = 0; level < nLevels; level++ )
1142         {
1143             // preprocess the resized image
1144             Mat workingMat = imagePyramid(layerInfo[level]);
1145 
1146             //boxFilter(working_mat, working_mat, working_mat.depth(), Size(5,5), Point(-1,-1), true, BORDER_REFLECT_101);
1147             GaussianBlur(workingMat, workingMat, Size(7, 7), 2, 2, BORDER_REFLECT_101);
1148         }
1149 
1150         if( useOCL )
1151         {
1152             imagePyramid.copyTo(uimagePyramid);
1153             std::vector<Vec4i> kptbuf;
1154             UMat ukeypoints, upattern;
1155             copyVectorToUMat(pattern, upattern);
1156             uploadORBKeypoints(keypoints, layerScale, kptbuf, ukeypoints);
1157 
1158             UMat udescriptors = _descriptors.getUMat();
1159             useOCL = ocl_computeOrbDescriptors(uimagePyramid, ulayerInfo,
1160                                                ukeypoints, udescriptors, upattern,
1161                                                nkeypoints, dsize, wta_k);
1162             if(useOCL)
1163             {
1164                 CV_IMPL_ADD(CV_IMPL_OCL);
1165             }
1166         }
1167 
1168         if( !useOCL )
1169         {
1170             Mat descriptors = _descriptors.getMat();
1171             computeOrbDescriptors(imagePyramid, layerInfo, layerScale,
1172                                   keypoints, descriptors, pattern, dsize, wta_k);
1173         }
1174     }
1175 }
1176 
create(int nfeatures,float scaleFactor,int nlevels,int edgeThreshold,int firstLevel,int wta_k,int scoreType,int patchSize,int fastThreshold)1177 Ptr<ORB> ORB::create(int nfeatures, float scaleFactor, int nlevels, int edgeThreshold,
1178            int firstLevel, int wta_k, int scoreType, int patchSize, int fastThreshold)
1179 {
1180     return makePtr<ORB_Impl>(nfeatures, scaleFactor, nlevels, edgeThreshold,
1181                              firstLevel, wta_k, scoreType, patchSize, fastThreshold);
1182 }
1183 
1184 }
1185