Point Cloud Library (PCL) 1.14.0
Loading...
Searching...
No Matches
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
45namespace pcl
46{
47
48template <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
82template <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
93template <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[points_*scales_*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
211template <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 int& 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 int integralcols = imagecols + 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
429template <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
438template <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 (width*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 ((width+1)*(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).
void resize(std::size_t count)
Resizes the container to contain count elements.
std::uint32_t width
The point cloud width (if organized as an image-structure).
std::uint32_t height
The point cloud height (if organized as an image-structure).
std::size_t size() const
@ 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