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
makeOffsets(int pixel[25],int rowStride,int patternSize)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
testCorner(const uchar * ptr,const int pixel[],int K,int N,int threshold)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<>
cornerScore(const uchar * ptr,const int pixel[],int threshold)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<>
cornerScore(const uchar * ptr,const int pixel[],int threshold)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<>
cornerScore(const uchar * ptr,const int pixel[],int threshold)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
360