Point Cloud Library (PCL)  1.10.1
brisk_2d.hpp
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Point Cloud Library (PCL) - www.pointclouds.org
5  * Copyright (c) 2012-, Open Perception , Inc.
6  * Copyright (C) 2011 The Autonomous Systems Lab (ASL), ETH Zurich,
7  * Stefan Leutenegger, Simon Lynen and Margarita Chli.
8  *
9  * All rights reserved.
10  *
11  * Redistribution and use in source and binary forms, with or without
12  * modification, are permitted provided that the following conditions
13  * are met:
14  *
15  * * Redistributions of source code must retain the above copyright
16  * notice, this list of conditions and the following disclaimer.
17  * * Redistributions in binary form must reproduce the above
18  * copyright notice, this list of conditions and the following
19  * disclaimer in the documentation and/or other materials provided
20  * with the distribution.
21  * * Neither the name of the copyright holder(s) nor the names of its
22  * contributors may be used to endorse or promote products derived
23  * from this software without specific prior written permission.
24  *
25  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
26  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
27  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
28  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
29  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
30  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
31  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
32  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
33  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
34  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
35  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
36  * POSSIBILITY OF SUCH DAMAGE.
37  *
38  */
39 
40 #ifndef PCL_FEATURES_IMPL_BRISK_2D_HPP_
41 #define PCL_FEATURES_IMPL_BRISK_2D_HPP_
42 
43 ///////////////////////////////////////////////////////////////////////////////////////////
44 template <typename PointInT, typename PointOutT, typename KeypointT, typename IntensityT>
46  : rotation_invariance_enabled_ (true)
47  , scale_invariance_enabled_ (true)
48  , pattern_scale_ (1.0f)
49  , input_cloud_ (), keypoints_ (), scale_range_ (), pattern_points_ (), points_ ()
50  , n_rot_ (1024), scale_list_ (nullptr), size_list_ (nullptr)
51  , scales_ (64)
52  , scalerange_ (30)
53  , basic_size_ (12.0)
54  , strings_ (0), d_max_ (0.0f), d_min_ (0.0f), short_pairs_ (), long_pairs_ ()
55  , no_short_pairs_ (0), no_long_pairs_ (0)
56  , intensity_ ()
57  , name_ ("BRISK2Destimation")
58 {
59  // Since we do not assume pattern_scale_ should be changed by the user, we
60  // can initialize the kernel in the constructor
61  std::vector<float> r_list;
62  std::vector<int> n_list;
63 
64  // this is the standard pattern found to be suitable also
65  r_list.resize (5);
66  n_list.resize (5);
67  const float f = 0.85f * pattern_scale_;
68 
69  r_list[0] = f * 0.0f;
70  r_list[1] = f * 2.9f;
71  r_list[2] = f * 4.9f;
72  r_list[3] = f * 7.4f;
73  r_list[4] = f * 10.8f;
74 
75  n_list[0] = 1;
76  n_list[1] = 10;
77  n_list[2] = 14;
78  n_list[3] = 15;
79  n_list[4] = 20;
80 
81  generateKernel (r_list, n_list, 5.85f * pattern_scale_, 8.2f * pattern_scale_);
82 }
83 
84 ///////////////////////////////////////////////////////////////////////////////////////////
85 template <typename PointInT, typename PointOutT, typename KeypointT, typename IntensityT>
87 {
88  if (pattern_points_) delete [] pattern_points_;
89  if (short_pairs_) delete [] short_pairs_;
90  if (long_pairs_) delete [] long_pairs_;
91  if (scale_list_) delete [] scale_list_;
92  if (size_list_) delete [] size_list_;
93 }
94 
95 ///////////////////////////////////////////////////////////////////////////////////////////
96 template <typename PointInT, typename PointOutT, typename KeypointT, typename IntensityT> void
98  std::vector<float> &radius_list,
99  std::vector<int> &number_list, float d_max, float d_min,
100  std::vector<int> index_change)
101 {
102  d_max_ = d_max;
103  d_min_ = d_min;
104 
105  // get the total number of points
106  const auto rings = radius_list.size ();
107  assert (radius_list.size () != 0 && radius_list.size () == number_list.size ());
108  points_ = 0; // remember the total number of points
109  for (const auto number: number_list)
110  points_ += number;
111 
112  // set up the patterns
113  pattern_points_ = new BriskPatternPoint[points_*scales_*n_rot_];
114  BriskPatternPoint* pattern_iterator = pattern_points_;
115 
116  // define the scale discretization:
117  static const float lb_scale = std::log (scalerange_) / std::log (2.0);
118  static const float lb_scale_step = lb_scale / (float (scales_));
119 
120  scale_list_ = new float[scales_];
121  size_list_ = new unsigned int[scales_];
122 
123  const float sigma_scale = 1.3f;
124 
125  for (unsigned int scale = 0; scale < scales_; ++scale)
126  {
127  scale_list_[scale] = static_cast<float> (pow (double (2.0), static_cast<double> (float (scale) * lb_scale_step)));
128  size_list_[scale] = 0;
129 
130  // generate the pattern points look-up
131  for (std::size_t rot = 0; rot < n_rot_; ++rot)
132  {
133  // this is the rotation of the feature
134  double theta = double (rot) * 2 * M_PI / double (n_rot_);
135  for (int ring = 0; ring < rings; ++ring)
136  {
137  for (int num = 0; num < number_list[ring]; ++num)
138  {
139  // the actual coordinates on the circle
140  double alpha = double (num) * 2 * M_PI / double (number_list[ring]);
141 
142  // feature rotation plus angle of the point
143  pattern_iterator->x = scale_list_[scale] * radius_list[ring] * static_cast<float> (std::cos (alpha + theta));
144  pattern_iterator->y = scale_list_[scale] * radius_list[ring] * static_cast<float> (sin (alpha + theta));
145  // and the gaussian kernel sigma
146  if (ring == 0)
147  pattern_iterator->sigma = sigma_scale * scale_list_[scale] * 0.5f;
148  else
149  pattern_iterator->sigma = static_cast<float> (sigma_scale * scale_list_[scale] * (double (radius_list[ring])) * sin (M_PI / double (number_list[ring])));
150 
151  // adapt the sizeList if necessary
152  const unsigned int size = static_cast<unsigned int> (std::ceil (((scale_list_[scale] * radius_list[ring]) + pattern_iterator->sigma)) + 1);
153 
154  if (size_list_[scale] < size)
155  size_list_[scale] = size;
156 
157  // increment the iterator
158  ++pattern_iterator;
159  }
160  }
161  }
162  }
163 
164  // now also generate pairings
165  short_pairs_ = new BriskShortPair[points_ * (points_ - 1) / 2];
166  long_pairs_ = new BriskLongPair[points_ * (points_ - 1) / 2];
167  no_short_pairs_ = 0;
168  no_long_pairs_ = 0;
169 
170  // fill index_change with 0..n if empty
171  if (index_change.empty ())
172  {
173  index_change.resize (points_ * (points_ - 1) / 2);
174  }
175  std::iota(index_change.begin (), index_change.end (), 0);
176 
177  const float d_min_sq = d_min_ * d_min_;
178  const float d_max_sq = d_max_ * d_max_;
179  for (unsigned int i = 1; i < points_; i++)
180  {
181  for (unsigned int j = 0; j < i; j++)
182  { //(find all the pairs)
183  // point pair distance:
184  const float dx = pattern_points_[j].x - pattern_points_[i].x;
185  const float dy = pattern_points_[j].y - pattern_points_[i].y;
186  const float norm_sq = (dx*dx+dy*dy);
187  if (norm_sq > d_min_sq)
188  {
189  // save to long pairs
190  BriskLongPair& longPair = long_pairs_[no_long_pairs_];
191  longPair.weighted_dx = int ((dx / (norm_sq)) * 2048.0 + 0.5);
192  longPair.weighted_dy = int ((dy / (norm_sq)) * 2048.0 + 0.5);
193  longPair.i = i;
194  longPair.j = j;
195  ++no_long_pairs_;
196  }
197  else if (norm_sq < d_max_sq)
198  {
199  // save to short pairs
200  assert (no_short_pairs_ < index_change.size ()); // make sure the user passes something sensible
201  BriskShortPair& shortPair = short_pairs_[index_change[no_short_pairs_]];
202  shortPair.j = j;
203  shortPair.i = i;
204  ++no_short_pairs_;
205  }
206  }
207  }
208 
209  // no bits:
210  strings_ = int (std::ceil ((float (no_short_pairs_)) / 128.0)) * 4 * 4;
211 }
212 
213 ///////////////////////////////////////////////////////////////////////////////////////////
214 template <typename PointInT, typename PointOutT, typename KeypointT, typename IntensityT> inline int
216  const std::vector<unsigned char> &image,
217  int image_width, int,
218  //const Stefan& integral,
219  const std::vector<int> &integral_image,
220  const float key_x, const float key_y, const unsigned int scale,
221  const unsigned int rot, const unsigned int point) const
222 {
223  // get the float position
224  const BriskPatternPoint& brisk_point = pattern_points_[scale * n_rot_*points_ + rot * points_ + point];
225  const float xf = brisk_point.x + key_x;
226  const float yf = brisk_point.y + key_y;
227  const int x = int (xf);
228  const int y = int (yf);
229  const int& imagecols = image_width;
230 
231  // get the sigma:
232  const float sigma_half = brisk_point.sigma;
233  const float area = 4.0f * sigma_half * sigma_half;
234 
235  // Get the point step
236 
237  // calculate output:
238  int ret_val;
239  if (sigma_half < 0.5)
240  {
241  // interpolation multipliers:
242  const int r_x = static_cast<int> ((xf - float (x)) * 1024);
243  const int r_y = static_cast<int> ((yf - float (y)) * 1024);
244  const int r_x_1 = (1024 - r_x);
245  const int r_y_1 = (1024 - r_y);
246 
247  //+const unsigned char* ptr = static_cast<const unsigned char*> (&image.points[0].r) + x + y * imagecols;
248  const unsigned char* ptr = static_cast<const unsigned char*>(&image[0]) + x + y * imagecols;
249 
250  // just interpolate:
251  ret_val = (r_x_1 * r_y_1 * int (*ptr));
252 
253  //+ptr += sizeof (PointInT);
254  ptr++;
255 
256  ret_val += (r_x * r_y_1 * int (*ptr));
257 
258  //+ptr += (imagecols * sizeof (PointInT));
259  ptr += imagecols;
260 
261  ret_val += (r_x * r_y * int (*ptr));
262 
263  //+ptr -= sizeof (PointInT);
264  ptr--;
265 
266  ret_val += (r_x_1 * r_y * int (*ptr));
267  return (ret_val + 512) / 1024;
268  }
269 
270  // this is the standard case (simple, not speed optimized yet):
271 
272  // scaling:
273  const int scaling = static_cast<int> (4194304.0f / area);
274  const int scaling2 = static_cast<int> (float (scaling) * area / 1024.0f);
275 
276  // the integral image is larger:
277  const int integralcols = imagecols + 1;
278 
279  // calculate borders
280  const float x_1 = xf - sigma_half;
281  const float x1 = xf + sigma_half;
282  const float y_1 = yf - sigma_half;
283  const float y1 = yf + sigma_half;
284 
285  const int x_left = int (x_1 + 0.5);
286  const int y_top = int (y_1 + 0.5);
287  const int x_right = int (x1 + 0.5);
288  const int y_bottom = int (y1 + 0.5);
289 
290  // overlap area - multiplication factors:
291  const float r_x_1 = float (x_left) - x_1 + 0.5f;
292  const float r_y_1 = float (y_top) - y_1 + 0.5f;
293  const float r_x1 = x1 - float (x_right) + 0.5f;
294  const float r_y1 = y1 - float (y_bottom) + 0.5f;
295  const int dx = x_right - x_left - 1;
296  const int dy = y_bottom - y_top - 1;
297  const int A = static_cast<int> ((r_x_1 * r_y_1) * float (scaling));
298  const int B = static_cast<int> ((r_x1 * r_y_1) * float (scaling));
299  const int C = static_cast<int> ((r_x1 * r_y1) * float (scaling));
300  const int D = static_cast<int> ((r_x_1 * r_y1) * float (scaling));
301  const int r_x_1_i = static_cast<int> (r_x_1 * float (scaling));
302  const int r_y_1_i = static_cast<int> (r_y_1 * float (scaling));
303  const int r_x1_i = static_cast<int> (r_x1 * float (scaling));
304  const int r_y1_i = static_cast<int> (r_y1 * float (scaling));
305 
306  if (dx + dy > 2)
307  {
308  // now the calculation:
309 
310  //+const unsigned char* ptr = static_cast<const unsigned char*> (&image.points[0].r) + x_left + imagecols * y_top;
311  const unsigned char* ptr = static_cast<const unsigned char*>(&image[0]) + x_left + imagecols * y_top;
312 
313  // first the corners:
314  ret_val = A * int (*ptr);
315 
316  //+ptr += (dx + 1) * sizeof (PointInT);
317  ptr += dx + 1;
318 
319  ret_val += B * int (*ptr);
320 
321  //+ptr += (dy * imagecols + 1) * sizeof (PointInT);
322  ptr += dy * imagecols + 1;
323 
324  ret_val += C * int (*ptr);
325 
326  //+ptr -= (dx + 1) * sizeof (PointInT);
327  ptr -= dx + 1;
328 
329  ret_val += D * int (*ptr);
330 
331  // next the edges:
332  //+int* ptr_integral;// = static_cast<int*> (integral.data) + x_left + integralcols * y_top + 1;
333  const int* ptr_integral = static_cast<const int*> (&integral_image[0]) + x_left + integralcols * y_top + 1;
334 
335  // find a simple path through the different surface corners
336  const int tmp1 = (*ptr_integral);
337  ptr_integral += dx;
338  const int tmp2 = (*ptr_integral);
339  ptr_integral += integralcols;
340  const int tmp3 = (*ptr_integral);
341  ptr_integral++;
342  const int tmp4 = (*ptr_integral);
343  ptr_integral += dy * integralcols;
344  const int tmp5 = (*ptr_integral);
345  ptr_integral--;
346  const int tmp6 = (*ptr_integral);
347  ptr_integral += integralcols;
348  const int tmp7 = (*ptr_integral);
349  ptr_integral -= dx;
350  const int tmp8 = (*ptr_integral);
351  ptr_integral -= integralcols;
352  const int tmp9 = (*ptr_integral);
353  ptr_integral--;
354  const int tmp10 = (*ptr_integral);
355  ptr_integral -= dy * integralcols;
356  const int tmp11 = (*ptr_integral);
357  ptr_integral++;
358  const int tmp12 = (*ptr_integral);
359 
360  // assign the weighted surface integrals:
361  const int upper = (tmp3 -tmp2 +tmp1 -tmp12) * r_y_1_i;
362  const int middle = (tmp6 -tmp3 +tmp12 -tmp9) * scaling;
363  const int left = (tmp9 -tmp12 +tmp11 -tmp10) * r_x_1_i;
364  const int right = (tmp5 -tmp4 +tmp3 -tmp6) * r_x1_i;
365  const int bottom = (tmp7 -tmp6 +tmp9 -tmp8) * r_y1_i;
366 
367  return (ret_val + upper + middle + left + right + bottom + scaling2 / 2) / scaling2;
368  }
369 
370  // now the calculation:
371 
372  //const unsigned char* ptr = static_cast<const unsigned char*> (&image.points[0].r) + x_left + imagecols * y_top;
373  const unsigned char* ptr = static_cast<const unsigned char*>(&image[0]) + x_left + imagecols * y_top;
374 
375  // first row:
376  ret_val = A * int (*ptr);
377 
378  //+ptr += sizeof (PointInT);
379  ptr++;
380 
381  //+const unsigned char* end1 = ptr + (dx * sizeof (PointInT));
382  const unsigned char* end1 = ptr + dx;
383 
384  //+for (; ptr < end1; ptr += sizeof (PointInT))
385  for (; ptr < end1; ptr++)
386  ret_val += r_y_1_i * int (*ptr);
387  ret_val += B * int (*ptr);
388 
389  // middle ones:
390  //+ptr += (imagecols - dx - 1) * sizeof (PointInT);
391  ptr += imagecols - dx - 1;
392 
393  //+const unsigned char* end_j = ptr + (dy * imagecols) * sizeof (PointInT);
394  const unsigned char* end_j = ptr + dy * imagecols;
395 
396  //+for (; ptr < end_j; ptr += (imagecols - dx - 1) * sizeof (PointInT))
397  for (; ptr < end_j; ptr += imagecols - dx - 1)
398  {
399  ret_val += r_x_1_i * int (*ptr);
400 
401  //+ptr += sizeof (PointInT);
402  ptr++;
403 
404  //+const unsigned char* end2 = ptr + (dx * sizeof (PointInT));
405  const unsigned char* end2 = ptr + dx;
406 
407  //+for (; ptr < end2; ptr += sizeof (PointInT))
408  for (; ptr < end2; ptr++)
409  ret_val += int (*ptr) * scaling;
410 
411  ret_val += r_x1_i * int (*ptr);
412  }
413  // last row:
414  ret_val += D * int (*ptr);
415 
416  //+ptr += sizeof (PointInT);
417  ptr++;
418 
419  //+const unsigned char* end3 = ptr + (dx * sizeof (PointInT));
420  const unsigned char* end3 = ptr + dx;
421 
422  //+for (; ptr<end3; ptr += sizeof (PointInT))
423  for (; ptr<end3; ptr++)
424  ret_val += r_y1_i * int (*ptr);
425 
426  ret_val += C * int (*ptr);
427 
428  return (ret_val + scaling2 / 2) / scaling2;
429 }
430 
431 
432 //////////////////////////////////////////////////////////////////////////////
433 template <typename PointInT, typename PointOutT, typename KeypointT, typename IntensityT> bool
435  const float min_x, const float min_y,
436  const float max_x, const float max_y, const KeypointT& pt)
437 {
438  return ((pt.x < min_x) || (pt.x >= max_x) || (pt.y < min_y) || (pt.y >= max_y));
439 }
440 
441 ///////////////////////////////////////////////////////////////////////////////////////////
442 template <typename PointInT, typename PointOutT, typename KeypointT, typename IntensityT> void
444  PointCloudOutT &output)
445 {
446  if (!input_cloud_->isOrganized ())
447  {
448  PCL_ERROR ("[pcl::%s::initCompute] %s doesn't support non organized clouds!\n", name_.c_str ());
449  return;
450  }
451 
452  // image size
453  const int width = int (input_cloud_->width);
454  const int height = int (input_cloud_->height);
455 
456  // destination for intensity data; will be forwarded to BRISK
457  std::vector<unsigned char> image_data (width*height);
458 
459  for (std::size_t i = 0; i < image_data.size (); ++i)
460  image_data[i] = static_cast<unsigned char> (intensity_ ((*input_cloud_)[i]));
461 
462  // Remove keypoints very close to the border
463  std::size_t ksize = keypoints_->points.size ();
464  std::vector<int> kscales; // remember the scale per keypoint
465  kscales.resize (ksize);
466 
467  // initialize constants
468  static const float lb_scalerange = std::log2 (scalerange_);
469 
470  typename std::vector<KeypointT, Eigen::aligned_allocator<KeypointT> >::iterator beginning = keypoints_->points.begin ();
471  std::vector<int>::iterator beginningkscales = kscales.begin ();
472 
473  static const float basic_size_06 = basic_size_ * 0.6f;
474  unsigned int basicscale = 0;
475 
476  if (!scale_invariance_enabled_)
477  basicscale = std::max (static_cast<int> (float (scales_) / lb_scalerange * (std::log2 (1.45f * basic_size_ / (basic_size_06))) + 0.5f), 0);
478 
479  for (std::size_t k = 0; k < ksize; k++)
480  {
481  unsigned int scale;
482  if (scale_invariance_enabled_)
483  {
484  scale = std::max (static_cast<int> (float (scales_) / lb_scalerange * (std::log2 (keypoints_->points[k].size / (basic_size_06))) + 0.5f), 0);
485  // saturate
486  if (scale >= scales_) scale = scales_ - 1;
487  kscales[k] = scale;
488  }
489  else
490  {
491  scale = basicscale;
492  kscales[k] = scale;
493  }
494 
495  const int border = size_list_[scale];
496  const int border_x = width - border;
497  const int border_y = height - border;
498 
499  if (RoiPredicate (float (border), float (border), float (border_x), float (border_y), keypoints_->points[k]))
500  {
501  //std::cerr << "remove keypoint" << std::endl;
502  keypoints_->points.erase (beginning + k);
503  kscales.erase (beginningkscales + k);
504  if (k == 0)
505  {
506  beginning = keypoints_->points.begin ();
507  beginningkscales = kscales.begin ();
508  }
509  ksize--;
510  k--;
511  }
512  }
513 
514  keypoints_->width = std::uint32_t (keypoints_->size ());
515  keypoints_->height = 1;
516 
517  // first, calculate the integral image over the whole image:
518  // current integral image
519  std::vector<int> integral ((width+1)*(height+1), 0); // the integral image
520 
521  for (std::size_t row_index = 1; row_index < height; ++row_index)
522  {
523  for (std::size_t col_index = 1; col_index < width; ++col_index)
524  {
525  const std::size_t index = row_index*width+col_index;
526  const std::size_t index2 = (row_index)*(width+1)+(col_index);
527 
528  integral[index2] = static_cast<int> (image_data[index])
529  - integral[index2-1-(width+1)]
530  + integral[index2-(width+1)]
531  + integral[index2-1];
532  }
533  }
534 
535  int* values = new int[points_]; // for temporary use
536 
537  // resize the descriptors:
538  //output = zeros (ksize, strings_);
539 
540  // now do the extraction for all keypoints:
541 
542  // temporary variables containing gray values at sample points:
543  int t1;
544  int t2;
545 
546  // the feature orientation
547  int direction0;
548  int direction1;
549 
550  output.resize (ksize);
551  //output.width = ksize;
552  //output.height = 1;
553  for (std::size_t k = 0; k < ksize; k++)
554  {
555  unsigned char* ptr = &output.points[k].descriptor[0];
556 
557  int theta;
558  KeypointT &kp = keypoints_->points[k];
559  const int& scale = kscales[k];
560  int shifter = 0;
561  int* pvalues = values;
562  const float& x = float (kp.x);
563  const float& y = float (kp.y);
564  if (true) // kp.angle==-1
565  {
566  if (!rotation_invariance_enabled_)
567  // don't compute the gradient direction, just assign a rotation of 0 degree
568  theta = 0;
569  else
570  {
571  // get the gray values in the unrotated pattern
572  for (unsigned int i = 0; i < points_; i++)
573  *(pvalues++) = smoothedIntensity (image_data, width, height, integral, x, y, scale, 0, i);
574 
575  direction0 = 0;
576  direction1 = 0;
577  // now iterate through the long pairings
578  const BriskLongPair* max = long_pairs_ + no_long_pairs_;
579 
580  for (BriskLongPair* iter = long_pairs_; iter < max; ++iter)
581  {
582  t1 = *(values + iter->i);
583  t2 = *(values + iter->j);
584  const int delta_t = (t1 - t2);
585 
586  // update the direction:
587  const int tmp0 = delta_t * (iter->weighted_dx) / 1024;
588  const int tmp1 = delta_t * (iter->weighted_dy) / 1024;
589  direction0 += tmp0;
590  direction1 += tmp1;
591  }
592  kp.angle = std::atan2 (float (direction1), float (direction0)) / float (M_PI) * 180.0f;
593  theta = static_cast<int> ((float (n_rot_) * kp.angle) / (360.0f) + 0.5f);
594  if (theta < 0)
595  theta += n_rot_;
596  if (theta >= int (n_rot_))
597  theta -= n_rot_;
598  }
599  }
600  else
601  {
602  // figure out the direction:
603  //int theta=rotationInvariance*round((_n_rot*std::atan2(direction.at<int>(0,0),direction.at<int>(1,0)))/(2*M_PI));
604  if (!rotation_invariance_enabled_)
605  theta = 0;
606  else
607  {
608  theta = static_cast<int> (n_rot_ * (kp.angle / (360.0)) + 0.5);
609  if (theta < 0)
610  theta += n_rot_;
611  if (theta >= int (n_rot_))
612  theta -= n_rot_;
613  }
614  }
615 
616  // now also extract the stuff for the actual direction:
617  // let us compute the smoothed values
618  shifter = 0;
619 
620  //unsigned int mean=0;
621  pvalues = values;
622  // get the gray values in the rotated pattern
623  for (unsigned int i = 0; i < points_; i++)
624  *(pvalues++) = smoothedIntensity (image_data, width, height, integral, x, y, scale, theta, i);
625 
626 #ifdef __GNUC__
627  using UINT32_ALIAS = std::uint32_t;
628 #endif
629 #ifdef _MSC_VER
630  // Todo: find the equivalent to may_alias
631  #define UCHAR_ALIAS std::uint32_t //__declspec(noalias)
632  #define UINT32_ALIAS std::uint32_t //__declspec(noalias)
633 #endif
634 
635  // now iterate through all the pairings
636  UINT32_ALIAS* ptr2 = reinterpret_cast<UINT32_ALIAS*> (ptr);
637  const BriskShortPair* max = short_pairs_ + no_short_pairs_;
638 
639  for (BriskShortPair* iter = short_pairs_; iter < max; ++iter)
640  {
641  t1 = *(values + iter->i);
642  t2 = *(values + iter->j);
643 
644  if (t1 > t2)
645  *ptr2 |= ((1) << shifter);
646 
647  // else already initialized with zero
648  // take care of the iterators:
649  ++shifter;
650 
651  if (shifter == 32)
652  {
653  shifter = 0;
654  ++ptr2;
655  }
656  }
657 
658  //ptr += strings_;
659 
660  //// Account for the scale + orientation;
661  //ptr += sizeof (output.points[0].scale);
662  //ptr += sizeof (output.points[0].orientation);
663  }
664 
665  // we do not change the denseness
666  output.width = int (output.points.size ());
667  output.height = 1;
668  output.is_dense = true;
669 
670  // clean-up
671  delete [] values;
672 }
673 
674 
675 #endif //#ifndef PCL_FEATURES_IMPL_BRISK_2D_HPP_
676 
pcl::PointCloud::height
std::uint32_t height
The point cloud height (if organized as an image-structure).
Definition: point_cloud.h:402
pcl::PointCloud::points
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
Definition: point_cloud.h:397
pcl::BRISK2DEstimation::BRISK2DEstimation
BRISK2DEstimation()
Constructor.
Definition: brisk_2d.hpp:45
pcl::PointCloud::resize
void resize(std::size_t n)
Resize the cloud.
Definition: point_cloud.h:442
pcl::PointCloud< PointOutT >
pcl::BRISK2DEstimation::compute
void compute(PointCloudOutT &output)
Computes the descriptors for the previously specified points and input data.
Definition: brisk_2d.hpp:443
pcl::BRISK2DEstimation::~BRISK2DEstimation
virtual ~BRISK2DEstimation()
Destructor.
Definition: brisk_2d.hpp:86
pcl::PointCloud::width
std::uint32_t width
The point cloud width (if organized as an image-structure).
Definition: point_cloud.h:400
pcl::PointCloud::is_dense
bool is_dense
True if no points are invalid (e.g., have NaN or Inf values in any of their floating point fields).
Definition: point_cloud.h:405
pcl::BRISK2DEstimation::smoothedIntensity
int smoothedIntensity(const std::vector< unsigned char > &image, int image_width, int image_height, const std::vector< int > &integral_image, const float key_x, const float key_y, const unsigned int scale, const unsigned int rot, const unsigned int point) const
Compute the smoothed intensity for a given x/y position in the image.
Definition: brisk_2d.hpp:215
pcl::BRISK2DEstimation::generateKernel
void generateKernel(std::vector< float > &radius_list, std::vector< int > &number_list, float d_max=5.85f, float d_min=8.2f, std::vector< int > index_change=std::vector< int >())
Call this to generate the kernel: circle of radius r (pixels), with n points; short pairings with dMa...
Definition: brisk_2d.hpp:97
pcl::B
Definition: norms.h:54
pcl::BRISK2DEstimation
Implementation of the BRISK-descriptor, based on the original code and paper reference by.
Definition: brisk_2d.h:67