40 #ifndef PCL_FEATURES_IMPL_BRISK_2D_HPP_
41 #define PCL_FEATURES_IMPL_BRISK_2D_HPP_
44 template <
typename Po
intInT,
typename Po
intOutT,
typename Keypo
intT,
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)
54 , strings_ (0), d_max_ (0.0f), d_min_ (0.0f), short_pairs_ (), long_pairs_ ()
55 , no_short_pairs_ (0), no_long_pairs_ (0)
57 , name_ (
"BRISK2Destimation")
61 std::vector<float> r_list;
62 std::vector<int> n_list;
67 const float f = 0.85f * pattern_scale_;
73 r_list[4] = f * 10.8f;
81 generateKernel (r_list, n_list, 5.85f * pattern_scale_, 8.2f * pattern_scale_);
85 template <
typename Po
intInT,
typename Po
intOutT,
typename Keypo
intT,
typename IntensityT>
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_;
96 template <
typename Po
intInT,
typename Po
intOutT,
typename Keypo
intT,
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)
106 const auto rings = radius_list.size ();
107 assert (radius_list.size () != 0 && radius_list.size () == number_list.size ());
109 for (
const auto number: number_list)
113 pattern_points_ =
new BriskPatternPoint[points_*scales_*n_rot_];
114 BriskPatternPoint* pattern_iterator = pattern_points_;
117 static const float lb_scale = std::log (scalerange_) / std::log (2.0);
118 static const float lb_scale_step = lb_scale / (float (scales_));
120 scale_list_ =
new float[scales_];
121 size_list_ =
new unsigned int[scales_];
123 const float sigma_scale = 1.3f;
125 for (
unsigned int scale = 0; scale < scales_; ++scale)
127 scale_list_[scale] = static_cast<float> (pow (
double (2.0), static_cast<double> (
float (scale) * lb_scale_step)));
128 size_list_[scale] = 0;
131 for (std::size_t rot = 0; rot < n_rot_; ++rot)
134 double theta = double (rot) * 2 * M_PI / double (n_rot_);
135 for (
int ring = 0; ring < rings; ++ring)
137 for (
int num = 0; num < number_list[ring]; ++num)
140 double alpha = double (num) * 2 * M_PI / double (number_list[ring]);
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));
147 pattern_iterator->sigma = sigma_scale * scale_list_[scale] * 0.5f;
149 pattern_iterator->sigma = static_cast<float> (sigma_scale * scale_list_[scale] * (
double (radius_list[ring])) * sin (M_PI /
double (number_list[ring])));
152 const unsigned int size = static_cast<unsigned int> (std::ceil (((scale_list_[scale] * radius_list[ring]) + pattern_iterator->sigma)) + 1);
154 if (size_list_[scale] < size)
155 size_list_[scale] = size;
165 short_pairs_ =
new BriskShortPair[points_ * (points_ - 1) / 2];
166 long_pairs_ =
new BriskLongPair[points_ * (points_ - 1) / 2];
171 if (index_change.empty ())
173 index_change.resize (points_ * (points_ - 1) / 2);
175 std::iota(index_change.begin (), index_change.end (), 0);
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++)
181 for (
unsigned int j = 0; j < i; j++)
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)
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);
197 else if (norm_sq < d_max_sq)
200 assert (no_short_pairs_ < index_change.size ());
201 BriskShortPair& shortPair = short_pairs_[index_change[no_short_pairs_]];
210 strings_ = int (std::ceil ((
float (no_short_pairs_)) / 128.0)) * 4 * 4;
214 template <
typename Po
intInT,
typename Po
intOutT,
typename Keypo
intT,
typename IntensityT>
inline int
216 const std::vector<unsigned char> &image,
217 int image_width,
int,
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
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;
232 const float sigma_half = brisk_point.sigma;
233 const float area = 4.0f * sigma_half * sigma_half;
239 if (sigma_half < 0.5)
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);
248 const unsigned char* ptr = static_cast<const unsigned char*>(&image[0]) + x + y * imagecols;
251 ret_val = (r_x_1 * r_y_1 * int (*ptr));
256 ret_val += (r_x * r_y_1 * int (*ptr));
261 ret_val += (r_x * r_y * int (*ptr));
266 ret_val += (r_x_1 * r_y * int (*ptr));
267 return (ret_val + 512) / 1024;
273 const int scaling = static_cast<int> (4194304.0f / area);
274 const int scaling2 = static_cast<int> (
float (scaling) * area / 1024.0f);
277 const int integralcols = imagecols + 1;
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;
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);
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));
311 const unsigned char* ptr = static_cast<const unsigned char*>(&image[0]) + x_left + imagecols * y_top;
314 ret_val = A * int (*ptr);
319 ret_val +=
B * int (*ptr);
322 ptr += dy * imagecols + 1;
324 ret_val += C * int (*ptr);
329 ret_val += D * int (*ptr);
333 const int* ptr_integral = static_cast<const int*> (&integral_image[0]) + x_left + integralcols * y_top + 1;
336 const int tmp1 = (*ptr_integral);
338 const int tmp2 = (*ptr_integral);
339 ptr_integral += integralcols;
340 const int tmp3 = (*ptr_integral);
342 const int tmp4 = (*ptr_integral);
343 ptr_integral += dy * integralcols;
344 const int tmp5 = (*ptr_integral);
346 const int tmp6 = (*ptr_integral);
347 ptr_integral += integralcols;
348 const int tmp7 = (*ptr_integral);
350 const int tmp8 = (*ptr_integral);
351 ptr_integral -= integralcols;
352 const int tmp9 = (*ptr_integral);
354 const int tmp10 = (*ptr_integral);
355 ptr_integral -= dy * integralcols;
356 const int tmp11 = (*ptr_integral);
358 const int tmp12 = (*ptr_integral);
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;
367 return (ret_val + upper + middle + left + right + bottom + scaling2 / 2) / scaling2;
373 const unsigned char* ptr = static_cast<const unsigned char*>(&image[0]) + x_left + imagecols * y_top;
376 ret_val = A * int (*ptr);
382 const unsigned char* end1 = ptr + dx;
385 for (; ptr < end1; ptr++)
386 ret_val += r_y_1_i *
int (*ptr);
387 ret_val +=
B * int (*ptr);
391 ptr += imagecols - dx - 1;
394 const unsigned char* end_j = ptr + dy * imagecols;
397 for (; ptr < end_j; ptr += imagecols - dx - 1)
399 ret_val += r_x_1_i * int (*ptr);
405 const unsigned char* end2 = ptr + dx;
408 for (; ptr < end2; ptr++)
409 ret_val +=
int (*ptr) * scaling;
411 ret_val += r_x1_i * int (*ptr);
414 ret_val += D * int (*ptr);
420 const unsigned char* end3 = ptr + dx;
423 for (; ptr<end3; ptr++)
424 ret_val += r_y1_i *
int (*ptr);
426 ret_val += C * int (*ptr);
428 return (ret_val + scaling2 / 2) / scaling2;
433 template <
typename Po
intInT,
typename Po
intOutT,
typename Keypo
intT,
typename IntensityT>
bool
435 const float min_x,
const float min_y,
436 const float max_x,
const float max_y,
const KeypointT& pt)
438 return ((pt.x < min_x) || (pt.x >= max_x) || (pt.y < min_y) || (pt.y >= max_y));
442 template <
typename Po
intInT,
typename Po
intOutT,
typename Keypo
intT,
typename IntensityT>
void
446 if (!input_cloud_->isOrganized ())
448 PCL_ERROR (
"[pcl::%s::initCompute] %s doesn't support non organized clouds!\n", name_.c_str ());
453 const int width = int (input_cloud_->width);
454 const int height = int (input_cloud_->height);
457 std::vector<unsigned char> image_data (width*height);
459 for (std::size_t i = 0; i < image_data.size (); ++i)
460 image_data[i] = static_cast<unsigned char> (intensity_ ((*input_cloud_)[i]));
463 std::size_t ksize = keypoints_->points.size ();
464 std::vector<int> kscales;
465 kscales.resize (ksize);
468 static const float lb_scalerange = std::log2 (scalerange_);
470 typename std::vector<KeypointT, Eigen::aligned_allocator<KeypointT> >::iterator beginning = keypoints_->points.begin ();
471 std::vector<int>::iterator beginningkscales = kscales.begin ();
473 static const float basic_size_06 = basic_size_ * 0.6f;
474 unsigned int basicscale = 0;
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);
479 for (std::size_t k = 0; k < ksize; k++)
482 if (scale_invariance_enabled_)
484 scale = std::max (static_cast<int> (
float (scales_) / lb_scalerange * (std::log2 (keypoints_->points[k].size / (basic_size_06))) + 0.5f), 0);
486 if (scale >= scales_) scale = scales_ - 1;
495 const int border = size_list_[scale];
496 const int border_x = width - border;
497 const int border_y = height - border;
499 if (RoiPredicate (
float (border),
float (border),
float (border_x),
float (border_y), keypoints_->points[k]))
502 keypoints_->points.erase (beginning + k);
503 kscales.erase (beginningkscales + k);
506 beginning = keypoints_->points.begin ();
507 beginningkscales = kscales.begin ();
514 keypoints_->width = std::uint32_t (keypoints_->size ());
515 keypoints_->height = 1;
519 std::vector<int> integral ((width+1)*(height+1), 0);
521 for (std::size_t row_index = 1; row_index < height; ++row_index)
523 for (std::size_t col_index = 1; col_index < width; ++col_index)
525 const std::size_t index = row_index*width+col_index;
526 const std::size_t index2 = (row_index)*(width+1)+(col_index);
528 integral[index2] = static_cast<int> (image_data[index])
529 - integral[index2-1-(width+1)]
530 + integral[index2-(width+1)]
531 + integral[index2-1];
535 int* values =
new int[points_];
553 for (std::size_t k = 0; k < ksize; k++)
555 unsigned char* ptr = &output.
points[k].descriptor[0];
558 KeypointT &kp = keypoints_->points[k];
559 const int& scale = kscales[k];
561 int* pvalues = values;
562 const float& x = float (kp.x);
563 const float& y = float (kp.y);
566 if (!rotation_invariance_enabled_)
572 for (
unsigned int i = 0; i < points_; i++)
573 *(pvalues++) = smoothedIntensity (image_data, width, height, integral, x, y, scale, 0, i);
578 const BriskLongPair* max = long_pairs_ + no_long_pairs_;
580 for (BriskLongPair* iter = long_pairs_; iter < max; ++iter)
582 t1 = *(values + iter->i);
583 t2 = *(values + iter->j);
584 const int delta_t = (t1 - t2);
587 const int tmp0 = delta_t * (iter->weighted_dx) / 1024;
588 const int tmp1 = delta_t * (iter->weighted_dy) / 1024;
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);
596 if (theta >=
int (n_rot_))
604 if (!rotation_invariance_enabled_)
608 theta = static_cast<int> (n_rot_ * (kp.angle / (360.0)) + 0.5);
611 if (theta >=
int (n_rot_))
623 for (
unsigned int i = 0; i < points_; i++)
624 *(pvalues++) = smoothedIntensity (image_data, width, height, integral, x, y, scale, theta, i);
627 using UINT32_ALIAS = std::uint32_t;
631 #define UCHAR_ALIAS std::uint32_t //__declspec(noalias)
632 #define UINT32_ALIAS std::uint32_t //__declspec(noalias)
636 UINT32_ALIAS* ptr2 = reinterpret_cast<UINT32_ALIAS*> (ptr);
637 const BriskShortPair* max = short_pairs_ + no_short_pairs_;
639 for (BriskShortPair* iter = short_pairs_; iter < max; ++iter)
641 t1 = *(values + iter->i);
642 t2 = *(values + iter->j);
645 *ptr2 |= ((1) << shifter);
675 #endif //#ifndef PCL_FEATURES_IMPL_BRISK_2D_HPP_