Main MRPT website > C++ reference for MRPT 1.5.7
faster_corner_10.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 "faster_corner_utilities.h"
21 #include "corner_10.h"
22 
23 using namespace std;
24 using namespace mrpt;
25 using namespace mrpt::utils;
26 
27 #if MRPT_HAS_SSE2 && MRPT_HAS_OPENCV
28 
29 template <bool Aligned>
30 void faster_corner_detect_10(const IplImage* I, mrpt::vision::TSimpleFeatureList & corners, int barrier, uint8_t octave, std::vector<size_t> * out_feats_index_by_row)
31 {
32  MRPT_UNUSED_PARAM(octave);
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 
47  // 3 first rows have no features:
48  if (ptr_feat_index_by_row) {
49  *ptr_feat_index_by_row++ = corners.size();
50  *ptr_feat_index_by_row++ = corners.size();
51  *ptr_feat_index_by_row++ = corners.size();
52  }
53 
54  const int w = I->width;
55  const int stride = 3*I->widthStep; // 3*w;
56 
57  // The compiler refuses to reserve a register for this
58  const __m128i barriers = _mm_set1_epi8((uint8_t)barrier);
59 
60  int xend = I->width - 3;
61  xend -= (I->width-3) % 16;
62 
63  for(int y=3; y < I->height - 3; y++)
64  {
65  if (ptr_feat_index_by_row) // save index by row:
66  *ptr_feat_index_by_row++=corners.size();
67 
68  for(int x=3; x < 16; x++)
69  if(is_corner_10<Less>((const uint8_t*)I->imageData+I->widthStep*y+x, I->widthStep, barrier) || is_corner_10<Greater>((const uint8_t*)I->imageData+I->widthStep*y+x, I->widthStep, barrier))
70  corners.push_back_fast(x, y);
71 
72  for(int x=16; x < xend; x+=16)
73  {
74  const uint8_t* p = (const uint8_t*)I->imageData+I->widthStep*y+x;
75  __m128i lo, hi;
76  {
77  const __m128i here = load_si128<Aligned>((const __m128i*)(p));
78  lo = _mm_subs_epu8(here, barriers);
79  hi = _mm_adds_epu8(barriers, here);
80  }
81  unsigned int ans_b, ans_e;
82  {
83  __m128i top = load_si128<Aligned>((const __m128i*)(p-stride));
84  __m128i bottom = load_si128<Aligned>((const __m128i*)(p+stride));
85 
86  CHECK_BARRIER(lo, hi, top, ans_b);
87  CHECK_BARRIER(lo, hi, bottom, ans_e);
88  if (!(ans_b | ans_e))
89  continue;
90  }
91 
92  unsigned int ans_m, ans_p, possible;
93  {
94  __m128i ul = _mm_loadu_si128((const __m128i*)(p-2-2*w));
95  __m128i lr = _mm_loadu_si128((const __m128i*)(p+2+2*w));
96  CHECK_BARRIER(lo, hi, ul, ans_m);
97  CHECK_BARRIER(lo, hi, lr, ans_p);
98  possible = (ans_m & ans_b) | (ans_e & ans_p);
99  if (!possible)
100  continue;
101  }
102 
103  unsigned int ans_o, ans_n;
104  {
105  __m128i ll = _mm_loadu_si128((const __m128i*)(p-2+2*w));
106  __m128i ur = _mm_loadu_si128((const __m128i*)(p+2-2*w));
107  CHECK_BARRIER(lo, hi, ll, ans_o);
108  CHECK_BARRIER(lo, hi, ur, ans_n);
109  possible &= ans_o | (ans_b & ans_n);
110  possible &= ans_n | (ans_e & ans_o);
111  if (!possible)
112  continue;
113  }
114 
115  unsigned int ans_h, ans_k;
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_h);
120  CHECK_BARRIER(lo, hi, right, ans_k);
121  possible &= ans_h | (ans_n & ans_k & ans_p);
122  possible &= ans_k | (ans_m & ans_h & ans_o);
123  if (!possible)
124  continue;
125  }
126 
127  unsigned int ans_a, ans_c;
128  {
129  __m128i a = _mm_loadu_si128((const __m128i*)(p-1-stride));
130  __m128i c = _mm_insert_epi16(_mm_srli_si128(a,2), *(const unsigned short*)(p+15-stride), 7);
131  //__m128i c = _mm_loadu_si128((const __m128i*)(p+1-stride));
132  CHECK_BARRIER(lo, hi, a, ans_a);
133  CHECK_BARRIER(lo, hi, c, ans_c);
134  possible &= ans_a | (ans_e & ans_p);
135  possible &= ans_c | (ans_o & ans_e);
136  if (!possible)
137  continue;
138  }
139 
140  unsigned int ans_d, ans_f;
141  {
142  __m128i d = _mm_loadu_si128((const __m128i*)(p-1+stride));
143  __m128i f = _mm_insert_epi16(_mm_srli_si128(d,2), *(const unsigned short*)(p+15+stride), 7);
144  //__m128i f = _mm_loadu_si128((const __m128i*)(p+1+stride));
145  CHECK_BARRIER(lo, hi, d, ans_d);
146  CHECK_BARRIER(lo, hi, f, ans_f);
147  const unsigned int ans_abc = ans_a & ans_b & ans_c;
148  possible &= ans_d | (ans_abc & ans_n);
149  possible &= ans_f | (ans_m & ans_abc);
150  if (!possible)
151  continue;
152  }
153 
154  unsigned int ans_g, ans_i;
155  {
156  __m128i g = _mm_loadu_si128((const __m128i*)(p-3-w));
157  __m128i ii = _mm_loadu_si128((const __m128i*)(p-3+w));
158  CHECK_BARRIER(lo, hi, g, ans_g);
159  CHECK_BARRIER(lo, hi, ii, ans_i);
160  possible &= ans_g | (ans_f & ans_p & ans_k);
161  possible &= ans_i | (ans_c & ans_n & ans_k);
162  if (!possible)
163  continue;
164  }
165 
166  unsigned int ans_j, ans_l;
167  {
168  __m128i jj = _mm_loadu_si128((const __m128i*)(p+3-w));
169  __m128i l = _mm_loadu_si128((const __m128i*)(p+3+w));
170  CHECK_BARRIER(lo, hi, jj, ans_j);
171  CHECK_BARRIER(lo, hi, l, ans_l);
172  const unsigned int ans_ghi = ans_g & ans_h & ans_i;
173  possible &= ans_j | (ans_d & ans_o & ans_ghi);
174  possible &= ans_l | (ans_m & ans_a & ans_ghi);
175  if (!possible)
176  continue;
177  }
178 
179  possible |= (possible >> 16);
180  //if(possible & 0x0f) //Does this make it faster?
181  {
182  if(possible & (1<< 0))
183  corners.push_back_fast(x + 0, y);
184  if(possible & (1<< 1))
185  corners.push_back_fast(x + 1, y);
186  if(possible & (1<< 2))
187  corners.push_back_fast(x + 2, y);
188  if(possible & (1<< 3))
189  corners.push_back_fast(x + 3, y);
190  if(possible & (1<< 4))
191  corners.push_back_fast(x + 4, y);
192  if(possible & (1<< 5))
193  corners.push_back_fast(x + 5, y);
194  if(possible & (1<< 6))
195  corners.push_back_fast(x + 6, y);
196  if(possible & (1<< 7))
197  corners.push_back_fast(x + 7, y);
198  }
199  //if(possible & 0xf0) //Does this mak( , fast)r?
200  {
201  if(possible & (1<< 8))
202  corners.push_back_fast(x + 8, y);
203  if(possible & (1<< 9))
204  corners.push_back_fast(x + 9, y);
205  if(possible & (1<<10))
206  corners.push_back_fast(x +10, y);
207  if(possible & (1<<11))
208  corners.push_back_fast(x +11, y);
209  if(possible & (1<<12))
210  corners.push_back_fast(x +12, y);
211  if(possible & (1<<13))
212  corners.push_back_fast(x +13, y);
213  if(possible & (1<<14))
214  corners.push_back_fast(x +14, y);
215  if(possible & (1<<15))
216  corners.push_back_fast(x +15, y);
217  }
218  }
219 
220  for(int x=xend; x < I->width - 3; x++)
221  if(is_corner_10<Less>((const uint8_t*)I->imageData+I->widthStep*y+x, I->widthStep, barrier) || is_corner_10<Greater>((const uint8_t*)I->imageData+I->widthStep*y+x, I->widthStep, barrier))
222  corners.push_back_fast(x, y);
223  }
224 
225  // 3 last rows have no features:
226  if (ptr_feat_index_by_row) {
227  *ptr_feat_index_by_row++ = corners.size();
228  *ptr_feat_index_by_row++ = corners.size();
229  *ptr_feat_index_by_row++ = corners.size();
230  }
231 
232 }
233 
234 #endif // MRPT_HAS_SSE2 && MRPT_HAS_OPENCV
235 
236 
237 #if MRPT_HAS_OPENCV
238 
239 void fast_corner_detect_10(const IplImage* I, mrpt::vision::TSimpleFeatureList & corners, int barrier, uint8_t octave, std::vector<size_t> * out_feats_index_by_row)
240 {
241  if (I->width < 22)
242  {
243  fast_corner_detect_plain_10(I,corners,barrier, octave,out_feats_index_by_row);
244  return;
245  }
246  else if (I->width < 22 || I->height < 7)
247  return;
248 
249 #if MRPT_HAS_SSE2
250  if (mrpt::system::is_aligned<16>(I->imageData) && mrpt::system::is_aligned<16>(I->imageData+I->widthStep))
251  faster_corner_detect_10<true>(I, corners, barrier, octave,out_feats_index_by_row);
252  else
253  faster_corner_detect_10<false>(I, corners, barrier, octave,out_feats_index_by_row);
254 #else
255  fast_corner_detect_plain_10(I,corners,barrier, octave,out_feats_index_by_row);
256 #endif
257 }
258 #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.
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
utils_defs.h
fast_corner_detect_plain_10
void fast_corner_detect_plain_10(const IplImage *i, TSimpleFeatureList &corners, int b, uint8_t octave, std::vector< size_t > *out_feats_index_by_row)
Definition: faster_corner_10_plain.cpp:28
faster_corner_utilities.h
MRPT_UNUSED_PARAM
#define MRPT_UNUSED_PARAM(a)
Can be used to avoid "not used parameters" warnings from the compiler.
Definition: maps/CColouredPointsMap.h:307
c
const GLubyte * c
Definition: glext.h:5590
faster_corner_detect_10
void faster_corner_detect_10(const IplImage *I, mrpt::vision::TSimpleFeatureList &corners, int barrier, uint8_t octave, std::vector< size_t > *out_feats_index_by_row)
Definition: faster_corner_10.cpp:30
stride
GLsizei stride
Definition: glext.h:3702
g
GLubyte g
Definition: glext.h:5575
corner_10.h
mrpt
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
Definition: CParticleFilter.h:16
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
fast_corner_detect_10
void fast_corner_detect_10(const IplImage *I, mrpt::vision::TSimpleFeatureList &corners, int barrier, uint8_t octave, std::vector< size_t > *out_feats_index_by_row)
Definition: faster_corner_10.cpp:239


Page generated by Doxygen 1.8.14 for MRPT 1.5.7 Git: 5902e14cc Wed Apr 24 15:04:01 2019 +0200 at lun oct 28 01:39:17 CET 2019