41 #ifndef PCL_FEATURES_IMPL_BRISK_2D_HPP_
42 #define PCL_FEATURES_IMPL_BRISK_2D_HPP_
48 template <
typename Po
intInT,
typename Po
intOutT,
typename Keypo
intT,
typename IntensityT>
50 : rotation_invariance_enabled_ (true)
51 , scale_invariance_enabled_ (true)
52 , pattern_scale_ (1.0f)
53 , input_cloud_ (), keypoints_ (), scale_range_ (), pattern_points_ (), points_ ()
54 , n_rot_ (1024), scale_list_ (nullptr), size_list_ (nullptr)
58 , strings_ (0), d_max_ (0.0f), d_min_ (0.0f), short_pairs_ (), long_pairs_ ()
59 , no_short_pairs_ (0), no_long_pairs_ (0)
61 , name_ (
"BRISK2Destimation")
65 std::vector<float> r_list;
66 std::vector<int> n_list;
71 const float f = 0.85f * pattern_scale_;
77 r_list[4] = f * 10.8f;
85 generateKernel (r_list, n_list, 5.85f * pattern_scale_, 8.2f * pattern_scale_);
89 template <
typename Po
intInT,
typename Po
intOutT,
typename Keypo
intT,
typename IntensityT>
92 delete [] pattern_points_;
93 delete [] short_pairs_;
94 delete [] long_pairs_;
95 delete [] scale_list_;
100 template <
typename Po
intInT,
typename Po
intOutT,
typename Keypo
intT,
typename IntensityT>
void
102 std::vector<float> &radius_list,
103 std::vector<int> &number_list,
float d_max,
float d_min,
104 std::vector<int> index_change)
110 const auto rings = radius_list.size ();
111 assert (radius_list.size () != 0 && radius_list.size () == number_list.size ());
113 for (
const auto number: number_list)
117 pattern_points_ =
new BriskPatternPoint[points_*scales_*n_rot_];
118 BriskPatternPoint* pattern_iterator = pattern_points_;
121 static const float lb_scale = std::log (scalerange_) / std::log (2.0);
122 static const float lb_scale_step = lb_scale / (
static_cast<float>(scales_));
124 scale_list_ =
new float[scales_];
125 size_list_ =
new unsigned int[scales_];
127 constexpr
float sigma_scale = 1.3f;
129 for (
unsigned int scale = 0; scale < scales_; ++scale)
131 scale_list_[scale] =
static_cast<float> (pow ((2.0),
static_cast<double> (
static_cast<float>(scale) * lb_scale_step)));
132 size_list_[scale] = 0;
135 for (std::size_t rot = 0; rot < n_rot_; ++rot)
138 double theta =
static_cast<double>(rot) * 2 *
M_PI /
static_cast<double>(n_rot_);
139 for (
int ring = 0; ring < static_cast<int>(rings); ++ring)
141 for (
int num = 0; num < number_list[ring]; ++num)
144 double alpha =
static_cast<double>(num) * 2 *
M_PI /
static_cast<double>(number_list[ring]);
147 pattern_iterator->x = scale_list_[scale] * radius_list[ring] *
static_cast<float> (std::cos (alpha + theta));
148 pattern_iterator->y = scale_list_[scale] * radius_list[ring] *
static_cast<float> (sin (alpha + theta));
151 pattern_iterator->sigma = sigma_scale * scale_list_[scale] * 0.5f;
153 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])));
156 const auto size =
static_cast<unsigned int> (std::ceil (((scale_list_[scale] * radius_list[ring]) + pattern_iterator->sigma)) + 1);
158 if (size_list_[scale] < size)
159 size_list_[scale] = size;
169 short_pairs_ =
new BriskShortPair[points_ * (points_ - 1) / 2];
170 long_pairs_ =
new BriskLongPair[points_ * (points_ - 1) / 2];
175 if (index_change.empty ())
177 index_change.resize (points_ * (points_ - 1) / 2);
179 std::iota(index_change.begin (), index_change.end (), 0);
181 const float d_min_sq = d_min_ * d_min_;
182 const float d_max_sq = d_max_ * d_max_;
183 for (
unsigned int i = 1; i < points_; i++)
185 for (
unsigned int j = 0; j < i; j++)
188 const float dx = pattern_points_[j].x - pattern_points_[i].x;
189 const float dy = pattern_points_[j].y - pattern_points_[i].y;
190 const float norm_sq = (dx*dx+dy*dy);
191 if (norm_sq > d_min_sq)
194 BriskLongPair& longPair = long_pairs_[no_long_pairs_];
195 longPair.weighted_dx =
static_cast<int>((dx / (norm_sq)) * 2048.0 + 0.5);
196 longPair.weighted_dy =
static_cast<int>((dy / (norm_sq)) * 2048.0 + 0.5);
201 else if (norm_sq < d_max_sq)
204 assert (no_short_pairs_ < index_change.size ());
205 BriskShortPair& shortPair = short_pairs_[index_change[no_short_pairs_]];
214 strings_ =
static_cast<int>(std::ceil ((
static_cast<float>(no_short_pairs_)) / 128.0)) * 4 * 4;
218 template <
typename Po
intInT,
typename Po
intOutT,
typename Keypo
intT,
typename IntensityT>
inline int
220 const std::vector<unsigned char> &image,
221 int image_width,
int,
223 const std::vector<int> &integral_image,
224 const float key_x,
const float key_y,
const unsigned int scale,
225 const unsigned int rot,
const unsigned int point)
const
228 const BriskPatternPoint& brisk_point = pattern_points_[scale * n_rot_*points_ + rot * points_ + point];
229 const float xf = brisk_point.x + key_x;
230 const float yf = brisk_point.y + key_y;
231 const int x =
static_cast<int>(xf);
232 const int y =
static_cast<int>(yf);
233 const int& imagecols = image_width;
236 const float sigma_half = brisk_point.sigma;
237 const float area = 4.0f * sigma_half * sigma_half;
243 if (sigma_half < 0.5)
246 const int r_x =
static_cast<int> ((xf -
static_cast<float>(x)) * 1024);
247 const int r_y =
static_cast<int> ((yf -
static_cast<float>(y)) * 1024);
248 const int r_x_1 = (1024 - r_x);
249 const int r_y_1 = (1024 - r_y);
252 const unsigned char* ptr =
static_cast<const unsigned char*
>(image.data()) + x + y * imagecols;
255 ret_val = (r_x_1 * r_y_1 *
static_cast<int>(*ptr));
260 ret_val += (r_x * r_y_1 *
static_cast<int>(*ptr));
265 ret_val += (r_x * r_y *
static_cast<int>(*ptr));
270 ret_val += (r_x_1 * r_y *
static_cast<int>(*ptr));
271 return (ret_val + 512) / 1024;
277 const int scaling =
static_cast<int> (4194304.0f / area);
278 const int scaling2 =
static_cast<int> (
static_cast<float>(scaling) * area / 1024.0f);
281 const int integralcols = imagecols + 1;
284 const float x_1 = xf - sigma_half;
285 const float x1 = xf + sigma_half;
286 const float y_1 = yf - sigma_half;
287 const float y1 = yf + sigma_half;
289 const int x_left =
static_cast<int>(x_1 + 0.5);
290 const int y_top =
static_cast<int>(y_1 + 0.5);
291 const int x_right =
static_cast<int>(x1 + 0.5);
292 const int y_bottom =
static_cast<int>(y1 + 0.5);
295 const float r_x_1 =
static_cast<float>(x_left) - x_1 + 0.5f;
296 const float r_y_1 =
static_cast<float>(y_top) - y_1 + 0.5f;
297 const float r_x1 = x1 -
static_cast<float>(x_right) + 0.5f;
298 const float r_y1 = y1 -
static_cast<float>(y_bottom) + 0.5f;
299 const int dx = x_right - x_left - 1;
300 const int dy = y_bottom - y_top - 1;
301 const int A =
static_cast<int> ((r_x_1 * r_y_1) *
static_cast<float>(scaling));
302 const int B =
static_cast<int> ((r_x1 * r_y_1) *
static_cast<float>(scaling));
303 const int C =
static_cast<int> ((r_x1 * r_y1) *
static_cast<float>(scaling));
304 const int D =
static_cast<int> ((r_x_1 * r_y1) *
static_cast<float>(scaling));
305 const int r_x_1_i =
static_cast<int> (r_x_1 *
static_cast<float>(scaling));
306 const int r_y_1_i =
static_cast<int> (r_y_1 *
static_cast<float>(scaling));
307 const int r_x1_i =
static_cast<int> (r_x1 *
static_cast<float>(scaling));
308 const int r_y1_i =
static_cast<int> (r_y1 *
static_cast<float>(scaling));
315 const unsigned char* ptr =
static_cast<const unsigned char*
>(image.data()) + x_left + imagecols * y_top;
318 ret_val = A *
static_cast<int>(*ptr);
323 ret_val +=
B *
static_cast<int>(*ptr);
326 ptr += dy * imagecols + 1;
328 ret_val += C *
static_cast<int>(*ptr);
333 ret_val += D *
static_cast<int>(*ptr);
337 const int* ptr_integral =
static_cast<const int*
> (integral_image.data()) + x_left + integralcols * y_top + 1;
340 const int tmp1 = (*ptr_integral);
342 const int tmp2 = (*ptr_integral);
343 ptr_integral += integralcols;
344 const int tmp3 = (*ptr_integral);
346 const int tmp4 = (*ptr_integral);
347 ptr_integral += dy * integralcols;
348 const int tmp5 = (*ptr_integral);
350 const int tmp6 = (*ptr_integral);
351 ptr_integral += integralcols;
352 const int tmp7 = (*ptr_integral);
354 const int tmp8 = (*ptr_integral);
355 ptr_integral -= integralcols;
356 const int tmp9 = (*ptr_integral);
358 const int tmp10 = (*ptr_integral);
359 ptr_integral -= dy * integralcols;
360 const int tmp11 = (*ptr_integral);
362 const int tmp12 = (*ptr_integral);
365 const int upper = (tmp3 -tmp2 +tmp1 -tmp12) * r_y_1_i;
366 const int middle = (tmp6 -tmp3 +tmp12 -tmp9) * scaling;
367 const int left = (tmp9 -tmp12 +tmp11 -tmp10) * r_x_1_i;
368 const int right = (tmp5 -tmp4 +tmp3 -tmp6) * r_x1_i;
369 const int bottom = (tmp7 -tmp6 +tmp9 -tmp8) * r_y1_i;
371 return (ret_val + upper + middle + left + right + bottom + scaling2 / 2) / scaling2;
377 const unsigned char* ptr =
static_cast<const unsigned char*
>(image.data()) + x_left + imagecols * y_top;
380 ret_val = A *
static_cast<int>(*ptr);
386 const unsigned char* end1 = ptr + dx;
389 for (; ptr < end1; ptr++)
390 ret_val += r_y_1_i *
static_cast<int>(*ptr);
391 ret_val +=
B *
static_cast<int>(*ptr);
395 ptr += imagecols - dx - 1;
398 const unsigned char* end_j = ptr + dy * imagecols;
401 for (; ptr < end_j; ptr += imagecols - dx - 1)
403 ret_val += r_x_1_i *
static_cast<int>(*ptr);
409 const unsigned char* end2 = ptr + dx;
412 for (; ptr < end2; ptr++)
413 ret_val +=
static_cast<int>(*ptr) * scaling;
415 ret_val += r_x1_i *
static_cast<int>(*ptr);
418 ret_val += D *
static_cast<int>(*ptr);
424 const unsigned char* end3 = ptr + dx;
427 for (; ptr<end3; ptr++)
428 ret_val += r_y1_i *
static_cast<int>(*ptr);
430 ret_val += C *
static_cast<int>(*ptr);
432 return (ret_val + scaling2 / 2) / scaling2;
436 template <
typename Po
intInT,
typename Po
intOutT,
typename Keypo
intT,
typename IntensityT>
bool
438 const float min_x,
const float min_y,
439 const float max_x,
const float max_y,
const KeypointT& pt)
441 return ((pt.x < min_x) || (pt.x >= max_x) || (pt.y < min_y) || (pt.y >= max_y));
445 template <
typename Po
intInT,
typename Po
intOutT,
typename Keypo
intT,
typename IntensityT>
void
449 if (!input_cloud_->isOrganized ())
451 PCL_ERROR (
"[pcl::%s::initCompute] %s doesn't support non organized clouds!\n", name_.c_str ());
456 const auto width =
static_cast<index_t>(input_cloud_->width);
457 const auto height =
static_cast<index_t>(input_cloud_->height);
460 std::vector<unsigned char> image_data (width*height);
462 for (std::size_t i = 0; i < image_data.size (); ++i)
463 image_data[i] =
static_cast<unsigned char> (intensity_ ((*input_cloud_)[i]));
466 auto ksize = keypoints_->size ();
467 std::vector<int> kscales;
468 kscales.resize (ksize);
471 static const float lb_scalerange = std::log2 (scalerange_);
473 auto beginning = keypoints_->points.begin ();
474 auto beginningkscales = kscales.begin ();
476 static const float basic_size_06 = basic_size_ * 0.6f;
477 unsigned int basicscale = 0;
479 if (!scale_invariance_enabled_)
480 basicscale = std::max (
static_cast<int> (
static_cast<float>(scales_) / lb_scalerange * (std::log2 (1.45f * basic_size_ / (basic_size_06))) + 0.5f), 0);
482 for (std::size_t k = 0; k < ksize; k++)
485 if (scale_invariance_enabled_)
487 scale = std::max (
static_cast<int> (
static_cast<float>(scales_) / lb_scalerange * (std::log2 ((*keypoints_)[k].size / (basic_size_06))) + 0.5f), 0);
489 if (scale >= scales_) scale = scales_ - 1;
498 const int border = size_list_[scale];
499 const int border_x = width - border;
500 const int border_y = height - border;
502 if (RoiPredicate (
static_cast<float>(border),
static_cast<float>(border),
static_cast<float>(border_x),
static_cast<float>(border_y), (*keypoints_)[k]))
505 keypoints_->points.erase (beginning + k);
506 kscales.erase (beginningkscales + k);
509 beginning = keypoints_->points.begin ();
510 beginningkscales = kscales.begin ();
517 keypoints_->width = keypoints_->size ();
518 keypoints_->height = 1;
522 std::vector<int> integral ((width+1)*(height+1), 0);
524 for (
index_t row_index = 1; row_index < height; ++row_index)
526 for (
index_t col_index = 1; col_index < width; ++col_index)
528 const std::size_t index = row_index*width+col_index;
529 const std::size_t index2 = (row_index)*(width+1)+(col_index);
531 integral[index2] =
static_cast<int> (image_data[index])
532 - integral[index2-1-(width+1)]
533 + integral[index2-(width+1)]
534 + integral[index2-1];
538 int* values =
new int[points_];
556 for (std::size_t k = 0; k < ksize; k++)
558 unsigned char* ptr = &output[k].descriptor[0];
561 KeypointT &kp = (*keypoints_)[k];
562 const int& scale = kscales[k];
564 int* pvalues = values;
565 const float& x = (kp.x);
566 const float& y = (kp.y);
570 if (!rotation_invariance_enabled_)
576 for (
unsigned int i = 0; i < points_; i++)
577 *(pvalues++) = smoothedIntensity (image_data, width, height, integral, x, y, scale, 0, i);
582 const BriskLongPair* max = long_pairs_ + no_long_pairs_;
584 for (BriskLongPair* iter = long_pairs_; iter < max; ++iter)
586 t1 = *(values + iter->i);
587 t2 = *(values + iter->j);
588 const int delta_t = (t1 - t2);
591 const int tmp0 = delta_t * (iter->weighted_dx) / 1024;
592 const int tmp1 = delta_t * (iter->weighted_dy) / 1024;
596 kp.angle = std::atan2 (
static_cast<float>(direction1),
static_cast<float>(direction0)) /
static_cast<float>(
M_PI) * 180.0f;
597 theta =
static_cast<int> ((
static_cast<float>(n_rot_) * kp.angle) / (360.0f) + 0.5f);
600 if (theta >=
static_cast<int>(n_rot_))
608 if (!rotation_invariance_enabled_)
612 theta =
static_cast<int> (n_rot_ * (kp.angle / (360.0)) + 0.5);
615 if (theta >=
static_cast<int>(n_rot_))
627 for (
unsigned int i = 0; i < points_; i++)
628 *(pvalues++) = smoothedIntensity (image_data, width, height, integral, x, y, scale, theta, i);
631 using UINT32_ALIAS = std::uint32_t;
635 #define UCHAR_ALIAS std::uint32_t
636 #define UINT32_ALIAS std::uint32_t
640 auto* ptr2 =
reinterpret_cast<UINT32_ALIAS*
> (ptr);
641 const BriskShortPair* max = short_pairs_ + no_short_pairs_;
643 for (BriskShortPair* iter = short_pairs_; iter < max; ++iter)
645 t1 = *(values + iter->i);
646 t2 = *(values + iter->j);
649 *ptr2 |= ((1) << shifter);
Implementation of the BRISK-descriptor, based on the original code and paper reference by.
BRISK2DEstimation()
Constructor.
virtual ~BRISK2DEstimation()
Destructor.
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...
void compute(PointCloudOutT &output)
Computes the descriptors for the previously specified points and input data.
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.
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).
detail::int_type_t< detail::index_type_size, detail::index_type_signed > index_t
Type used for an index in PCL.