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