Main MRPT website > C++ reference for MRPT 1.5.9
faster_corner_9.cpp
Go to the documentation of this file.
1 /* +---------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | http://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2017, Individual contributors, see AUTHORS file |
6  | See: http://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See details in http://www.mrpt.org/License |
8  +---------------------------------------------------------------------------+ */
9 
10 // ---------------------------------------------------------------------------
11 // LICENSING: This file is a slightly-modified version of part of libcvd,
12 // released under LGPL 2.1 by Edward Rosten
13 // ---------------------------------------------------------------------------
14 
15 #include <mrpt/utils/utils_defs.h>
16 #include <mrpt/system/memory.h>
17 #include "faster_corner_prototypes.h"
18 
19 #include <mrpt/utils/SSE_types.h>
20 #include <mrpt/system/memory.h>
21 #include "faster_corner_utilities.h"
22 #include "corner_9.h"
23 
24 using namespace std;
25 using namespace mrpt;
26 using namespace mrpt::utils;
27 
28 #if MRPT_HAS_SSE2 && MRPT_HAS_OPENCV
29 
30 template <bool Aligned>
31 void faster_corner_detect_9(const IplImage* I, mrpt::vision::TSimpleFeatureList & corners, int barrier, uint8_t octave, std::vector<size_t> * out_feats_index_by_row)
32 {
33  corners.reserve(corners.size()+500);
34  //corners.mark_kdtree_as_outdated();
35 
36  size_t *ptr_feat_index_by_row;
37  if (out_feats_index_by_row)
38  {
39  out_feats_index_by_row->resize(I->height);
40  ptr_feat_index_by_row = &(*out_feats_index_by_row)[0];
41  }
42  else {
43  ptr_feat_index_by_row = NULL;
44  }
45 
46  const int w = I->width;
47  const int stride = 3*I->widthStep; // 3*w;
48 
49  // The compiler refuses to reserve a register for this
50  const __m128i barriers = _mm_set1_epi8((uint8_t)barrier);
51 
52  int xend = I->width - 3;
53  xend -= (I->width-3) % 16;
54 
55  // 3 first rows have no features:
56  if (ptr_feat_index_by_row) {
57  *ptr_feat_index_by_row++ = corners.size();
58  *ptr_feat_index_by_row++ = corners.size();
59  *ptr_feat_index_by_row++ = corners.size();
60  }
61 
62  for(int y=3; y < I->height - 3; y++)
63  {
64  if (ptr_feat_index_by_row) // save index by row:
65  *ptr_feat_index_by_row++=corners.size();
66 
67  for(int x=3; x < 16; x++)
68  if(is_corner_9<Less>( (const uint8_t*)I->imageData+I->widthStep*y+x, I->widthStep, barrier) || is_corner_9<Greater>((const uint8_t*)I->imageData+I->widthStep*y+x, I->widthStep, barrier))
69  corners.push_back_fast(x<<octave, y<<octave);
70 
71  for(int x=16; x < xend; x+=16)
72  {
73  const uint8_t* p = (const uint8_t*)I->imageData+I->widthStep*y+x; //(const uint8_t*)I->imageData+I->widthStep*y+x;
74  __m128i lo, hi;
75  {
76  const __m128i here = load_si128<Aligned>((const __m128i*)(p));
77  lo = _mm_subs_epu8(here, barriers);
78  hi = _mm_adds_epu8(barriers, here);
79  }
80  unsigned int ans_0, ans_8, possible;
81  {
82  __m128i top = load_si128<Aligned>((const __m128i*)(p-stride));
83  __m128i bottom = load_si128<Aligned>((const __m128i*)(p+stride));
84 
85  CHECK_BARRIER(lo, hi, top, ans_0);
86  CHECK_BARRIER(lo, hi, bottom, ans_8);
87  possible = ans_0 | ans_8;
88  if (!possible)
89  continue;
90  }
91 
92  unsigned int ans_15, ans_1;
93  {
94  __m128i a = _mm_loadu_si128((const __m128i*)(p-1-stride));
95  __m128i c = _mm_insert_epi16(_mm_srli_si128(a,2), *(const unsigned short*)(p+15-stride), 7);
96  CHECK_BARRIER(lo, hi, a, ans_15);
97  CHECK_BARRIER(lo, hi, c, ans_1);
98  possible &= ans_8 | (ans_15 & ans_1);
99  if (!possible)
100  continue;
101  }
102 
103  unsigned int ans_9, ans_7;
104  {
105  __m128i d = _mm_loadu_si128((const __m128i*)(p-1+stride));
106  __m128i f = _mm_insert_epi16(_mm_srli_si128(d,2), *(const unsigned short*)(p+15+stride), 7);
107  CHECK_BARRIER(lo, hi, d, ans_9);
108  CHECK_BARRIER(lo, hi, f, ans_7);
109  possible &= ans_9 | (ans_0 & ans_1);
110  possible &= ans_7 | (ans_15 & ans_0);
111  if (!possible)
112  continue;
113  }
114 
115  unsigned int ans_12, ans_4;
116  {
117  __m128i left = _mm_loadu_si128((const __m128i*)(p-3));
118  __m128i right = _mm_loadu_si128((const __m128i*)(p+3));
119  CHECK_BARRIER(lo, hi, left, ans_12);
120  CHECK_BARRIER(lo, hi, right, ans_4);
121  possible &= ans_12 | (ans_4 & (ans_1 | ans_7));
122  possible &= ans_4 | (ans_12 & (ans_9 | ans_15));
123  if (!possible)
124  continue;
125  }
126 
127  unsigned int ans_14, ans_6;
128  {
129  __m128i ul = _mm_loadu_si128((const __m128i*)(p-2-2*w));
130  __m128i lr = _mm_loadu_si128((const __m128i*)(p+2+2*w));
131  CHECK_BARRIER(lo, hi, ul, ans_14);
132  CHECK_BARRIER(lo, hi, lr, ans_6);
133  {
134  const unsigned int ans_6_7 = ans_6 & ans_7;
135  possible &= ans_14 | (ans_6_7 & (ans_4 | (ans_8 & ans_9)));
136  possible &= ans_1 | (ans_6_7) | ans_12;
137  }
138  {
139  const unsigned int ans_14_15 = ans_14 & ans_15;
140  possible &= ans_6 | (ans_14_15 & (ans_12 | (ans_0 & ans_1)));
141  possible &= ans_9 | (ans_14_15) | ans_4;
142  }
143  if (!possible)
144  continue;
145  }
146 
147  unsigned int ans_10, ans_2;
148  {
149  __m128i ll = _mm_loadu_si128((const __m128i*)(p-2+2*w));
150  __m128i ur = _mm_loadu_si128((const __m128i*)(p+2-2*w));
151  CHECK_BARRIER(lo, hi, ll, ans_10);
152  CHECK_BARRIER(lo, hi, ur, ans_2);
153  {
154  const unsigned int ans_1_2 = ans_1 & ans_2;
155  possible &= ans_10 | (ans_1_2 & ((ans_0 & ans_15) | ans_4));
156  possible &= ans_12 | (ans_1_2) | (ans_6 & ans_7);
157  }
158  {
159  const unsigned int ans_9_10 = ans_9 & ans_10;
160  possible &= ans_2 | (ans_9_10 & ((ans_7 & ans_8) | ans_12));
161  possible &= ans_4 | (ans_9_10) | (ans_14 & ans_15);
162  }
163  possible &= ans_8 | ans_14 | ans_2;
164  possible &= ans_0 | ans_10 | ans_6;
165  if (!possible)
166  continue;
167  }
168 
169  unsigned int ans_13, ans_5;
170  {
171  __m128i g = _mm_loadu_si128((const __m128i*)(p-3-w));
172  __m128i l = _mm_loadu_si128((const __m128i*)(p+3+w));
173  CHECK_BARRIER(lo, hi, g, ans_13);
174  CHECK_BARRIER(lo, hi, l, ans_5);
175  const unsigned int ans_15_0 = ans_15 & ans_0;
176  const unsigned int ans_7_8 = ans_7 & ans_8;
177  {
178  const unsigned int ans_12_13 = ans_12 & ans_13;
179  possible &= ans_5 | (ans_12_13 & ans_14 & ((ans_15_0) | ans_10));
180  possible &= ans_7 | (ans_1 & ans_2) | (ans_12_13);
181  possible &= ans_2 | (ans_12_13) | (ans_7_8);
182  }
183  {
184  const unsigned int ans_4_5 = ans_4 & ans_5;
185  const unsigned int ans_9_10 = ans_9 & ans_10;
186  possible &= ans_13 | (ans_4_5 & ans_6 & ((ans_7_8) | ans_2));
187  possible &= ans_15 | (ans_4_5) | (ans_9_10);
188  possible &= ans_10 | (ans_4_5) | (ans_15_0);
189  possible &= ans_15 | (ans_9_10) | (ans_4_5);
190  }
191 
192  possible &= ans_8 | (ans_13 & ans_14) | ans_2;
193  possible &= ans_0 | (ans_5 & ans_6) | ans_10;
194  if (!possible)
195  continue;
196  }
197 
198 
199  unsigned int ans_11, ans_3;
200  {
201  __m128i ii = _mm_loadu_si128((const __m128i*)(p-3+w));
202  __m128i jj = _mm_loadu_si128((const __m128i*)(p+3-w));
203  CHECK_BARRIER(lo, hi, ii, ans_11);
204  CHECK_BARRIER(lo, hi, jj, ans_3);
205  {
206  const unsigned int ans_2_3 = ans_2 & ans_3;
207  possible &= ans_11 | (ans_2_3 & ans_4 & ((ans_0 & ans_1) | (ans_5 & ans_6)));
208  possible &= ans_13 | (ans_7 & ans_8) | (ans_2_3);
209  possible &= ans_8 | (ans_2_3) | (ans_13 & ans_14);
210  }
211  {
212  const unsigned int ans_11_12 = ans_11 & ans_12;
213  possible &= ans_3 | (ans_10 & ans_11_12 & ((ans_8 & ans_9) | (ans_13 & ans_14)));
214  possible &= ans_1 | (ans_11_12) | (ans_6 & ans_7);
215  possible &= ans_6 | (ans_0 & ans_1) | (ans_11_12);
216  }
217  {
218  const unsigned int ans_3_4 = ans_3 & ans_4;
219  possible &= ans_9 | (ans_3_4) | (ans_14 & ans_15);
220  possible &= ans_14 | (ans_8 & ans_9) | (ans_3_4);
221  }
222  {
223  const unsigned int ans_10_11 = ans_10 & ans_11;
224  possible &= ans_5 | (ans_15 & ans_0) | (ans_10_11);
225  possible &= ans_0 | (ans_10_11) | (ans_5 & ans_6);
226  }
227  if (!possible)
228  continue;
229 
230  }
231 
232  possible |= (possible >> 16);
233 
234  //if(possible & 0x0f) //Does this make it faster?
235  {
236  if(possible & (1<< 0))
237  corners.push_back_fast((x + 0)<<octave, y<<octave);
238  if(possible & (1<< 1))
239  corners.push_back_fast((x + 1)<<octave, y<<octave);
240  if(possible & (1<< 2))
241  corners.push_back_fast((x + 2)<<octave, y<<octave);
242  if(possible & (1<< 3))
243  corners.push_back_fast((x + 3)<<octave, y<<octave);
244  if(possible & (1<< 4))
245  corners.push_back_fast((x + 4)<<octave, y<<octave);
246  if(possible & (1<< 5))
247  corners.push_back_fast((x + 5)<<octave, y<<octave);
248  if(possible & (1<< 6))
249  corners.push_back_fast((x + 6)<<octave, y<<octave);
250  if(possible & (1<< 7))
251  corners.push_back_fast((x + 7)<<octave, y<<octave);
252  }
253  //if(possible & 0xf0) //Does this mak( , fast)r?
254  {
255  if(possible & (1<< 8))
256  corners.push_back_fast((x + 8)<<octave, y<<octave);
257  if(possible & (1<< 9))
258  corners.push_back_fast((x + 9)<<octave, y<<octave);
259  if(possible & (1<<10))
260  corners.push_back_fast((x +10)<<octave, y<<octave);
261  if(possible & (1<<11))
262  corners.push_back_fast((x +11)<<octave, y<<octave);
263  if(possible & (1<<12))
264  corners.push_back_fast((x +12)<<octave, y<<octave);
265  if(possible & (1<<13))
266  corners.push_back_fast((x +13)<<octave, y<<octave);
267  if(possible & (1<<14))
268  corners.push_back_fast((x +14)<<octave, y<<octave);
269  if(possible & (1<<15))
270  corners.push_back_fast((x +15)<<octave, y<<octave);
271  }
272  }
273 
274  for(int x=xend; x < I->width - 3; x++)
275  if(is_corner_9<Less>((const uint8_t*)I->imageData+I->widthStep*y+x, I->widthStep, barrier) || is_corner_9<Greater>((const uint8_t*)I->imageData+I->widthStep*y+x, I->widthStep, barrier))
276  corners.push_back_fast(x<<octave, y<<octave);
277  }
278 
279  // 3 last rows have no features:
280  if (ptr_feat_index_by_row) {
281  *ptr_feat_index_by_row++ = corners.size();
282  *ptr_feat_index_by_row++ = corners.size();
283  *ptr_feat_index_by_row++ = corners.size();
284  }
285 
286 }
287 
288 
289 #endif // MRPT_HAS_SSE2 && MRPT_HAS_OPENCV
290 
291 #if MRPT_HAS_OPENCV
292 
293 void fast_corner_detect_9(const IplImage* I, mrpt::vision::TSimpleFeatureList & corners, int barrier, uint8_t octave, std::vector<size_t> * out_feats_index_by_row)
294 {
295  if (I->width < 22)
296  {
297  fast_corner_detect_plain_9(I,corners,barrier,octave,out_feats_index_by_row);
298  return;
299  }
300  else if (I->width < 22 || I->height < 7)
301  return;
302 
303 #if MRPT_HAS_SSE2
304  if (mrpt::system::is_aligned<16>(I->imageData) && mrpt::system::is_aligned<16>(I->imageData+I->widthStep))
305  faster_corner_detect_9<true>(I, corners, barrier,octave,out_feats_index_by_row);
306  else
307  faster_corner_detect_9<false>(I, corners, barrier,octave,out_feats_index_by_row);
308 #else
309  fast_corner_detect_plain_9(I,corners,barrier,octave,out_feats_index_by_row);
310 #endif
311 }
312 
313 #endif
mrpt::system::is_aligned< 16 >
bool is_aligned< 16 >(const void *ptr)
Definition: memory.h:107
mrpt::utils
Classes for serialization, sockets, ini-file manipulation, streams, list of properties-values, timewatch, extensions to STL.
Definition: zip.h:16
mrpt::vision::TSimpleFeatureList_templ::push_back_fast
void push_back_fast(const FEATURE &f)
Definition: TSimpleFeature.h:157
SSE_types.h
std
STL namespace.
fast_corner_detect_9
void fast_corner_detect_9(const IplImage *I, mrpt::vision::TSimpleFeatureList &corners, int barrier, uint8_t octave, std::vector< size_t > *out_feats_index_by_row)
Definition: faster_corner_9.cpp:293
mrpt::vision::TSimpleFeatureList_templ::reserve
void reserve(size_t N)
Definition: TSimpleFeature.h:154
CHECK_BARRIER
#define CHECK_BARRIER(lo, hi, other, flags)
Definition: faster_corner_utilities.h:47
w
GLubyte GLubyte GLubyte GLubyte w
Definition: glext.h:3962
uint8_t
unsigned char uint8_t
Definition: rptypes.h:43
fast_corner_detect_plain_9
void fast_corner_detect_plain_9(const IplImage *i, TSimpleFeatureList &corners, int b, uint8_t octave, std::vector< size_t > *out_feats_index_by_row)
Definition: faster_corner_9_plain.cpp:28
utils_defs.h
faster_corner_utilities.h
c
const GLubyte * c
Definition: glext.h:5590
stride
GLsizei stride
Definition: glext.h:3702
g
GLubyte g
Definition: glext.h:5575
faster_corner_detect_9
void faster_corner_detect_9(const IplImage *I, mrpt::vision::TSimpleFeatureList &corners, int barrier, uint8_t octave, std::vector< size_t > *out_feats_index_by_row)
Definition: faster_corner_9.cpp:31
mrpt
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
Definition: CParticleFilter.h:16
corner_9.h
faster_corner_prototypes.h
y
GLenum GLint GLint y
Definition: glext.h:3516
mrpt::vision::TSimpleFeatureList_templ< TSimpleFeature >
mrpt::vision::TSimpleFeatureList_templ::size
size_t size() const
Definition: TSimpleFeature.h:150
x
GLenum GLint x
Definition: glext.h:3516
a
GLubyte GLubyte GLubyte a
Definition: glext.h:5575
p
GLfloat GLfloat p
Definition: glext.h:5587


Page generated by Doxygen 1.8.14 for MRPT 1.5.9 Git: 690a4699f Wed Apr 15 19:29:53 2020 +0200 at mié abr 15 19:30:12 CEST 2020