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
47namespace cv
48{
49
50const float HARRIS_K = 0.04f;
51
52template<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
60static bool
61ocl_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
84static bool
85ocl_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
104static bool
105ocl_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 */
129static void
130HarrisResponses(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
175static 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
213static void
214computeOrbDescriptors( 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
347static 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
374static 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
635static 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
647static inline float getScale(int level, int firstLevel, double scaleFactor)
648{
649    return (float)std::pow(scaleFactor, (double)(level - firstLevel));
650}
651
652
653class ORB_Impl : public ORB
654{
655public:
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
663    void setMaxFeatures(int maxFeatures) { nfeatures = maxFeatures; }
664    int getMaxFeatures() const { return nfeatures; }
665
666    void setScaleFactor(double scaleFactor_) { scaleFactor = scaleFactor_; }
667    double getScaleFactor() const { return scaleFactor; }
668
669    void setNLevels(int nlevels_) { nlevels = nlevels_; }
670    int getNLevels() const { return nlevels; }
671
672    void setEdgeThreshold(int edgeThreshold_) { edgeThreshold = edgeThreshold_; }
673    int getEdgeThreshold() const { return edgeThreshold; }
674
675    void setFirstLevel(int firstLevel_) { firstLevel = firstLevel_; }
676    int getFirstLevel() const { return firstLevel; }
677
678    void setWTA_K(int wta_k_) { wta_k = wta_k_; }
679    int getWTA_K() const { return wta_k; }
680
681    void setScoreType(int scoreType_) { scoreType = scoreType_; }
682    int getScoreType() const { return scoreType; }
683
684    void setPatchSize(int patchSize_) { patchSize = patchSize_; }
685    int getPatchSize() const { return patchSize; }
686
687    void setFastThreshold(int fastThreshold_) { fastThreshold = fastThreshold_; }
688    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
701protected:
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
714int ORB_Impl::descriptorSize() const
715{
716    return kBytes;
717}
718
719int ORB_Impl::descriptorType() const
720{
721    return CV_8U;
722}
723
724int ORB_Impl::defaultNorm() const
725{
726    return NORM_HAMMING;
727}
728
729static 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
738typedef union if32_t
739{
740    int i;
741    float f;
742}
743if32_t;
744
745static 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 */
768static 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 */
945void 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
1177Ptr<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