← Back to branch summary

~paparazzi-uav/paparazzi/v5.0-manual

« back to all changes in this revision

Viewing changes to sw/ext/opencv_bebop/opencv/modules/features2d/src/fast_score.cpp

  • Committer: Paparazzi buildbot
  • Date: 2016-05-18 15:00:29 UTC
  • Revision ID: felix.ruess+docbot@gmail.com-20160518150029-e8lgzi5kvb4p7un9
Manual import commit 4b8bbb730080dac23cf816b98908dacfabe2a8ec from v5.0 branch.

Show diffs side-by-side

added added

removed removed

Lines of Context:
sw/ext/opencv_bebop/opencv/modules/features2d/src/fast_score.cpp
 
1
/* This is FAST corner detector, contributed to OpenCV by the author, Edward Rosten.
 
2
   Below is the original copyright and the references */
 
3
 
 
4
/*
 
5
Copyright (c) 2006, 2008 Edward Rosten
 
6
All rights reserved.
 
7
 
 
8
Redistribution and use in source and binary forms, with or without
 
9
modification, are permitted provided that the following conditions
 
10
are met:
 
11
 
 
12
    *Redistributions of source code must retain the above copyright
 
13
     notice, this list of conditions and the following disclaimer.
 
14
 
 
15
    *Redistributions in binary form must reproduce the above copyright
 
16
     notice, this list of conditions and the following disclaimer in the
 
17
     documentation and/or other materials provided with the distribution.
 
18
 
 
19
    *Neither the name of the University of Cambridge nor the names of
 
20
     its contributors may be used to endorse or promote products derived
 
21
     from this software without specific prior written permission.
 
22
 
 
23
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 
24
"AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 
25
LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
 
26
A PARTICULAR PURPOSE ARE DISCLAIMED.  IN NO EVENT SHALL THE COPYRIGHT OWNER OR
 
27
CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
 
28
EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
 
29
PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
 
30
PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
 
31
LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
 
32
NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
 
33
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
34
*/
 
35
 
 
36
/*
 
37
The references are:
 
38
 * Machine learning for high-speed corner detection,
 
39
   E. Rosten and T. Drummond, ECCV 2006
 
40
 * Faster and better: A machine learning approach to corner detection
 
41
   E. Rosten, R. Porter and T. Drummond, PAMI, 2009
 
42
*/
 
43
 
 
44
#include "fast_score.hpp"
 
45
 
 
46
#define VERIFY_CORNERS 0
 
47
 
 
48
namespace cv {
 
49
 
 
50
void makeOffsets(int pixel[25], int rowStride, int patternSize)
 
51
{
 
52
    static const int offsets16[][2] =
 
53
    {
 
54
        {0,  3}, { 1,  3}, { 2,  2}, { 3,  1}, { 3, 0}, { 3, -1}, { 2, -2}, { 1, -3},
 
55
        {0, -3}, {-1, -3}, {-2, -2}, {-3, -1}, {-3, 0}, {-3,  1}, {-2,  2}, {-1,  3}
 
56
    };
 
57
 
 
58
    static const int offsets12[][2] =
 
59
    {
 
60
        {0,  2}, { 1,  2}, { 2,  1}, { 2, 0}, { 2, -1}, { 1, -2},
 
61
        {0, -2}, {-1, -2}, {-2, -1}, {-2, 0}, {-2,  1}, {-1,  2}
 
62
    };
 
63
 
 
64
    static const int offsets8[][2] =
 
65
    {
 
66
        {0,  1}, { 1,  1}, { 1, 0}, { 1, -1},
 
67
        {0, -1}, {-1, -1}, {-1, 0}, {-1,  1}
 
68
    };
 
69
 
 
70
    const int (*offsets)[2] = patternSize == 16 ? offsets16 :
 
71
                              patternSize == 12 ? offsets12 :
 
72
                              patternSize == 8  ? offsets8  : 0;
 
73
 
 
74
    CV_Assert(pixel && offsets);
 
75
 
 
76
    int k = 0;
 
77
    for( ; k < patternSize; k++ )
 
78
        pixel[k] = offsets[k][0] + offsets[k][1] * rowStride;
 
79
    for( ; k < 25; k++ )
 
80
        pixel[k] = pixel[k - patternSize];
 
81
}
 
82
 
 
83
#if VERIFY_CORNERS
 
84
static void testCorner(const uchar* ptr, const int pixel[], int K, int N, int threshold) {
 
85
    // check that with the computed "threshold" the pixel is still a corner
 
86
    // and that with the increased-by-1 "threshold" the pixel is not a corner anymore
 
87
    for( int delta = 0; delta <= 1; delta++ )
 
88
    {
 
89
        int v0 = std::min(ptr[0] + threshold + delta, 255);
 
90
        int v1 = std::max(ptr[0] - threshold - delta, 0);
 
91
        int c0 = 0, c1 = 0;
 
92
 
 
93
        for( int k = 0; k < N; k++ )
 
94
        {
 
95
            int x = ptr[pixel[k]];
 
96
            if(x > v0)
 
97
            {
 
98
                if( ++c0 > K )
 
99
                    break;
 
100
                c1 = 0;
 
101
            }
 
102
            else if( x < v1 )
 
103
            {
 
104
                if( ++c1 > K )
 
105
                    break;
 
106
                c0 = 0;
 
107
            }
 
108
            else
 
109
            {
 
110
                c0 = c1 = 0;
 
111
            }
 
112
        }
 
113
        CV_Assert( (delta == 0 && std::max(c0, c1) > K) ||
 
114
                   (delta == 1 && std::max(c0, c1) <= K) );
 
115
    }
 
116
}
 
117
#endif
 
118
 
 
119
template<>
 
120
int cornerScore<16>(const uchar* ptr, const int pixel[], int threshold)
 
121
{
 
122
    const int K = 8, N = K*3 + 1;
 
123
    int k, v = ptr[0];
 
124
    short d[N];
 
125
    for( k = 0; k < N; k++ )
 
126
        d[k] = (short)(v - ptr[pixel[k]]);
 
127
 
 
128
#if CV_SSE2
 
129
    __m128i q0 = _mm_set1_epi16(-1000), q1 = _mm_set1_epi16(1000);
 
130
    for( k = 0; k < 16; k += 8 )
 
131
    {
 
132
        __m128i v0 = _mm_loadu_si128((__m128i*)(d+k+1));
 
133
        __m128i v1 = _mm_loadu_si128((__m128i*)(d+k+2));
 
134
        __m128i a = _mm_min_epi16(v0, v1);
 
135
        __m128i b = _mm_max_epi16(v0, v1);
 
136
        v0 = _mm_loadu_si128((__m128i*)(d+k+3));
 
137
        a = _mm_min_epi16(a, v0);
 
138
        b = _mm_max_epi16(b, v0);
 
139
        v0 = _mm_loadu_si128((__m128i*)(d+k+4));
 
140
        a = _mm_min_epi16(a, v0);
 
141
        b = _mm_max_epi16(b, v0);
 
142
        v0 = _mm_loadu_si128((__m128i*)(d+k+5));
 
143
        a = _mm_min_epi16(a, v0);
 
144
        b = _mm_max_epi16(b, v0);
 
145
        v0 = _mm_loadu_si128((__m128i*)(d+k+6));
 
146
        a = _mm_min_epi16(a, v0);
 
147
        b = _mm_max_epi16(b, v0);
 
148
        v0 = _mm_loadu_si128((__m128i*)(d+k+7));
 
149
        a = _mm_min_epi16(a, v0);
 
150
        b = _mm_max_epi16(b, v0);
 
151
        v0 = _mm_loadu_si128((__m128i*)(d+k+8));
 
152
        a = _mm_min_epi16(a, v0);
 
153
        b = _mm_max_epi16(b, v0);
 
154
        v0 = _mm_loadu_si128((__m128i*)(d+k));
 
155
        q0 = _mm_max_epi16(q0, _mm_min_epi16(a, v0));
 
156
        q1 = _mm_min_epi16(q1, _mm_max_epi16(b, v0));
 
157
        v0 = _mm_loadu_si128((__m128i*)(d+k+9));
 
158
        q0 = _mm_max_epi16(q0, _mm_min_epi16(a, v0));
 
159
        q1 = _mm_min_epi16(q1, _mm_max_epi16(b, v0));
 
160
    }
 
161
    q0 = _mm_max_epi16(q0, _mm_sub_epi16(_mm_setzero_si128(), q1));
 
162
    q0 = _mm_max_epi16(q0, _mm_unpackhi_epi64(q0, q0));
 
163
    q0 = _mm_max_epi16(q0, _mm_srli_si128(q0, 4));
 
164
    q0 = _mm_max_epi16(q0, _mm_srli_si128(q0, 2));
 
165
    threshold = (short)_mm_cvtsi128_si32(q0) - 1;
 
166
#else
 
167
    int a0 = threshold;
 
168
    for( k = 0; k < 16; k += 2 )
 
169
    {
 
170
        int a = std::min((int)d[k+1], (int)d[k+2]);
 
171
        a = std::min(a, (int)d[k+3]);
 
172
        if( a <= a0 )
 
173
            continue;
 
174
        a = std::min(a, (int)d[k+4]);
 
175
        a = std::min(a, (int)d[k+5]);
 
176
        a = std::min(a, (int)d[k+6]);
 
177
        a = std::min(a, (int)d[k+7]);
 
178
        a = std::min(a, (int)d[k+8]);
 
179
        a0 = std::max(a0, std::min(a, (int)d[k]));
 
180
        a0 = std::max(a0, std::min(a, (int)d[k+9]));
 
181
    }
 
182
 
 
183
    int b0 = -a0;
 
184
    for( k = 0; k < 16; k += 2 )
 
185
    {
 
186
        int b = std::max((int)d[k+1], (int)d[k+2]);
 
187
        b = std::max(b, (int)d[k+3]);
 
188
        b = std::max(b, (int)d[k+4]);
 
189
        b = std::max(b, (int)d[k+5]);
 
190
        if( b >= b0 )
 
191
            continue;
 
192
        b = std::max(b, (int)d[k+6]);
 
193
        b = std::max(b, (int)d[k+7]);
 
194
        b = std::max(b, (int)d[k+8]);
 
195
 
 
196
        b0 = std::min(b0, std::max(b, (int)d[k]));
 
197
        b0 = std::min(b0, std::max(b, (int)d[k+9]));
 
198
    }
 
199
 
 
200
    threshold = -b0-1;
 
201
#endif
 
202
 
 
203
#if VERIFY_CORNERS
 
204
    testCorner(ptr, pixel, K, N, threshold);
 
205
#endif
 
206
    return threshold;
 
207
}
 
208
 
 
209
template<>
 
210
int cornerScore<12>(const uchar* ptr, const int pixel[], int threshold)
 
211
{
 
212
    const int K = 6, N = K*3 + 1;
 
213
    int k, v = ptr[0];
 
214
    short d[N + 4];
 
215
    for( k = 0; k < N; k++ )
 
216
        d[k] = (short)(v - ptr[pixel[k]]);
 
217
#if CV_SSE2
 
218
    for( k = 0; k < 4; k++ )
 
219
        d[N+k] = d[k];
 
220
#endif
 
221
 
 
222
#if CV_SSE2
 
223
    __m128i q0 = _mm_set1_epi16(-1000), q1 = _mm_set1_epi16(1000);
 
224
    for( k = 0; k < 16; k += 8 )
 
225
    {
 
226
        __m128i v0 = _mm_loadu_si128((__m128i*)(d+k+1));
 
227
        __m128i v1 = _mm_loadu_si128((__m128i*)(d+k+2));
 
228
        __m128i a = _mm_min_epi16(v0, v1);
 
229
        __m128i b = _mm_max_epi16(v0, v1);
 
230
        v0 = _mm_loadu_si128((__m128i*)(d+k+3));
 
231
        a = _mm_min_epi16(a, v0);
 
232
        b = _mm_max_epi16(b, v0);
 
233
        v0 = _mm_loadu_si128((__m128i*)(d+k+4));
 
234
        a = _mm_min_epi16(a, v0);
 
235
        b = _mm_max_epi16(b, v0);
 
236
        v0 = _mm_loadu_si128((__m128i*)(d+k+5));
 
237
        a = _mm_min_epi16(a, v0);
 
238
        b = _mm_max_epi16(b, v0);
 
239
        v0 = _mm_loadu_si128((__m128i*)(d+k+6));
 
240
        a = _mm_min_epi16(a, v0);
 
241
        b = _mm_max_epi16(b, v0);
 
242
        v0 = _mm_loadu_si128((__m128i*)(d+k));
 
243
        q0 = _mm_max_epi16(q0, _mm_min_epi16(a, v0));
 
244
        q1 = _mm_min_epi16(q1, _mm_max_epi16(b, v0));
 
245
        v0 = _mm_loadu_si128((__m128i*)(d+k+7));
 
246
        q0 = _mm_max_epi16(q0, _mm_min_epi16(a, v0));
 
247
        q1 = _mm_min_epi16(q1, _mm_max_epi16(b, v0));
 
248
    }
 
249
    q0 = _mm_max_epi16(q0, _mm_sub_epi16(_mm_setzero_si128(), q1));
 
250
    q0 = _mm_max_epi16(q0, _mm_unpackhi_epi64(q0, q0));
 
251
    q0 = _mm_max_epi16(q0, _mm_srli_si128(q0, 4));
 
252
    q0 = _mm_max_epi16(q0, _mm_srli_si128(q0, 2));
 
253
    threshold = (short)_mm_cvtsi128_si32(q0) - 1;
 
254
#else
 
255
    int a0 = threshold;
 
256
    for( k = 0; k < 12; k += 2 )
 
257
    {
 
258
        int a = std::min((int)d[k+1], (int)d[k+2]);
 
259
        if( a <= a0 )
 
260
            continue;
 
261
        a = std::min(a, (int)d[k+3]);
 
262
        a = std::min(a, (int)d[k+4]);
 
263
        a = std::min(a, (int)d[k+5]);
 
264
        a = std::min(a, (int)d[k+6]);
 
265
        a0 = std::max(a0, std::min(a, (int)d[k]));
 
266
        a0 = std::max(a0, std::min(a, (int)d[k+7]));
 
267
    }
 
268
 
 
269
    int b0 = -a0;
 
270
    for( k = 0; k < 12; k += 2 )
 
271
    {
 
272
        int b = std::max((int)d[k+1], (int)d[k+2]);
 
273
        b = std::max(b, (int)d[k+3]);
 
274
        b = std::max(b, (int)d[k+4]);
 
275
        if( b >= b0 )
 
276
            continue;
 
277
        b = std::max(b, (int)d[k+5]);
 
278
        b = std::max(b, (int)d[k+6]);
 
279
 
 
280
        b0 = std::min(b0, std::max(b, (int)d[k]));
 
281
        b0 = std::min(b0, std::max(b, (int)d[k+7]));
 
282
    }
 
283
 
 
284
    threshold = -b0-1;
 
285
#endif
 
286
 
 
287
#if VERIFY_CORNERS
 
288
    testCorner(ptr, pixel, K, N, threshold);
 
289
#endif
 
290
    return threshold;
 
291
}
 
292
 
 
293
template<>
 
294
int cornerScore<8>(const uchar* ptr, const int pixel[], int threshold)
 
295
{
 
296
    const int K = 4, N = K*3 + 1;
 
297
    int k, v = ptr[0];
 
298
    short d[N];
 
299
    for( k = 0; k < N; k++ )
 
300
        d[k] = (short)(v - ptr[pixel[k]]);
 
301
 
 
302
#if CV_SSE2
 
303
    __m128i v0 = _mm_loadu_si128((__m128i*)(d+1));
 
304
    __m128i v1 = _mm_loadu_si128((__m128i*)(d+2));
 
305
    __m128i a = _mm_min_epi16(v0, v1);
 
306
    __m128i b = _mm_max_epi16(v0, v1);
 
307
    v0 = _mm_loadu_si128((__m128i*)(d+3));
 
308
    a = _mm_min_epi16(a, v0);
 
309
    b = _mm_max_epi16(b, v0);
 
310
    v0 = _mm_loadu_si128((__m128i*)(d+4));
 
311
    a = _mm_min_epi16(a, v0);
 
312
    b = _mm_max_epi16(b, v0);
 
313
    v0 = _mm_loadu_si128((__m128i*)(d));
 
314
    __m128i q0 = _mm_min_epi16(a, v0);
 
315
    __m128i q1 = _mm_max_epi16(b, v0);
 
316
    v0 = _mm_loadu_si128((__m128i*)(d+5));
 
317
    q0 = _mm_max_epi16(q0, _mm_min_epi16(a, v0));
 
318
    q1 = _mm_min_epi16(q1, _mm_max_epi16(b, v0));
 
319
    q0 = _mm_max_epi16(q0, _mm_sub_epi16(_mm_setzero_si128(), q1));
 
320
    q0 = _mm_max_epi16(q0, _mm_unpackhi_epi64(q0, q0));
 
321
    q0 = _mm_max_epi16(q0, _mm_srli_si128(q0, 4));
 
322
    q0 = _mm_max_epi16(q0, _mm_srli_si128(q0, 2));
 
323
    threshold = (short)_mm_cvtsi128_si32(q0) - 1;
 
324
#else
 
325
    int a0 = threshold;
 
326
    for( k = 0; k < 8; k += 2 )
 
327
    {
 
328
        int a = std::min((int)d[k+1], (int)d[k+2]);
 
329
        if( a <= a0 )
 
330
            continue;
 
331
        a = std::min(a, (int)d[k+3]);
 
332
        a = std::min(a, (int)d[k+4]);
 
333
        a0 = std::max(a0, std::min(a, (int)d[k]));
 
334
        a0 = std::max(a0, std::min(a, (int)d[k+5]));
 
335
    }
 
336
 
 
337
    int b0 = -a0;
 
338
    for( k = 0; k < 8; k += 2 )
 
339
    {
 
340
        int b = std::max((int)d[k+1], (int)d[k+2]);
 
341
        b = std::max(b, (int)d[k+3]);
 
342
        if( b >= b0 )
 
343
            continue;
 
344
        b = std::max(b, (int)d[k+4]);
 
345
 
 
346
        b0 = std::min(b0, std::max(b, (int)d[k]));
 
347
        b0 = std::min(b0, std::max(b, (int)d[k+5]));
 
348
    }
 
349
 
 
350
    threshold = -b0-1;
 
351
#endif
 
352
 
 
353
#if VERIFY_CORNERS
 
354
    testCorner(ptr, pixel, K, N, threshold);
 
355
#endif
 
356
    return threshold;
 
357
}
 
358
 
 
359
} // namespace cv