38 #ifndef PCL_FEATURES_COLOR_GRADIENT_DOT_MODALITY
39 #define PCL_FEATURES_COLOR_GRADIENT_DOT_MODALITY
41 #include <pcl/pcl_base.h>
42 #include <pcl/point_cloud.h>
43 #include <pcl/point_types.h>
45 #include <pcl/recognition/dot_modality.h>
46 #include <pcl/recognition/quantized_map.h>
76 inline PointRGB (
const uint8_t b,
const uint8_t g,
const uint8_t r)
77 : b (b), g (g), r (r), _unused (0)
80 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
100 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
109 os <<
"(" << p.
x <<
"," << p.
y <<
" - " << p.
magnitude <<
")";
115 template <
typename Po
intInT>
145 gradient_magnitude_threshold_ = threshold;
157 return (dominant_quantized_color_gradients_);
191 float gradient_magnitude_threshold_;
202 template <
typename Po
intInT>
205 : bin_size_ (bin_size), gradient_magnitude_threshold_ (80.0f), color_gradients_ (), dominant_quantized_color_gradients_ ()
210 template <
typename Po
intInT>
217 template <
typename Po
intInT>
223 computeMaxColorGradients ();
226 computeDominantQuantizedGradients ();
233 template <
typename Po
intInT>
238 const int width = input_->width;
239 const int height = input_->height;
241 color_gradients_.points.resize (width*height);
242 color_gradients_.width = width;
243 color_gradients_.height = height;
245 const float pi = tan(1.0f)*4;
246 for (
int row_index = 0; row_index < height-2; ++row_index)
248 for (
int col_index = 0; col_index < width-2; ++col_index)
250 const int index0 = row_index*width+col_index;
251 const int index_c = row_index*width+col_index+2;
252 const int index_r = (row_index+2)*width+col_index;
256 const unsigned char r0 = input_->points[index0].r;
257 const unsigned char g0 = input_->points[index0].g;
258 const unsigned char b0 = input_->points[index0].b;
260 const unsigned char r_c = input_->points[index_c].r;
261 const unsigned char g_c = input_->points[index_c].g;
262 const unsigned char b_c = input_->points[index_c].b;
264 const unsigned char r_r = input_->points[index_r].r;
265 const unsigned char g_r = input_->points[index_r].g;
266 const unsigned char b_r = input_->points[index_r].b;
268 const float r_dx =
static_cast<float> (r_c) - static_cast<float> (r0);
269 const float g_dx =
static_cast<float> (g_c) - static_cast<float> (g0);
270 const float b_dx =
static_cast<float> (b_c) - static_cast<float> (b0);
272 const float r_dy =
static_cast<float> (r_r) - static_cast<float> (r0);
273 const float g_dy =
static_cast<float> (g_r) - static_cast<float> (g0);
274 const float b_dy =
static_cast<float> (b_r) - static_cast<float> (b0);
276 const float sqr_mag_r = r_dx*r_dx + r_dy*r_dy;
277 const float sqr_mag_g = g_dx*g_dx + g_dy*g_dy;
278 const float sqr_mag_b = b_dx*b_dx + b_dy*b_dy;
281 gradient.
x = col_index;
282 gradient.
y = row_index;
283 if (sqr_mag_r > sqr_mag_g && sqr_mag_r > sqr_mag_b)
286 gradient.
angle = atan2 (r_dy, r_dx) * 180.0f / pi;
288 else if (sqr_mag_g > sqr_mag_b)
292 gradient.
angle = atan2 (g_dy, g_dx) * 180.0f / pi;
302 gradient.
angle = atan2 (b_dy, b_dx) * 180.0f / pi;
309 assert (color_gradients_ (col_index+1, row_index+1).angle >= -180 &&
310 color_gradients_ (col_index+1, row_index+1).angle <= 180);
312 color_gradients_ (col_index+1, row_index+1) = gradient;
482 template <
typename Po
intInT>
487 const size_t input_width = input_->width;
488 const size_t input_height = input_->height;
490 const size_t output_width = input_width / bin_size_;
491 const size_t output_height = input_height / bin_size_;
493 dominant_quantized_color_gradients_.resize (output_width, output_height);
498 const size_t num_gradient_bins = 7;
499 const size_t max_num_of_gradients = 1;
501 const float divisor = 180.0f / (num_gradient_bins - 1.0f);
503 float global_max_gradient = 0.0f;
504 float local_max_gradient = 0.0f;
506 unsigned char * peak_pointer = dominant_quantized_color_gradients_.getData ();
507 memset (peak_pointer, 0, output_width*output_height);
510 for (
size_t row_bin_index = 0; row_bin_index < output_height; ++row_bin_index)
512 for (
size_t col_bin_index = 0; col_bin_index < output_width; ++col_bin_index)
514 const size_t x_position = col_bin_index * bin_size_;
515 const size_t y_position = row_bin_index * bin_size_;
525 size_t max_gradient_pos_x;
526 size_t max_gradient_pos_y;
531 for (
size_t row_sub_index = 0; row_sub_index < bin_size_; ++row_sub_index)
533 for (
size_t col_sub_index = 0; col_sub_index < bin_size_; ++col_sub_index)
535 const float magnitude = color_gradients_ (col_sub_index + x_position, row_sub_index + y_position).magnitude;
537 if (magnitude > max_gradient)
539 max_gradient = magnitude;
540 max_gradient_pos_x = col_sub_index;
541 max_gradient_pos_y = row_sub_index;
547 if (max_gradient >= gradient_magnitude_threshold_)
549 const size_t angle =
static_cast<size_t> (180 + color_gradients_ (max_gradient_pos_x + x_position, max_gradient_pos_y + y_position).angle + 0.5f);
550 const size_t bin_index =
static_cast<size_t> ((angle >= 180 ? angle-180 : angle)/divisor);
552 *peak_pointer |= 1 << bin_index;
571 if (*peak_pointer == 0)
573 *peak_pointer |= 1 << 7;
593 template <
typename Po
intInT>
599 const size_t input_width = input_->width;
600 const size_t input_height = input_->height;
602 const size_t output_width = input_width / bin_size_;
603 const size_t output_height = input_height / bin_size_;
605 const size_t sub_start_x = region.
x / bin_size_;
606 const size_t sub_start_y = region.
y / bin_size_;
607 const size_t sub_width = region.
width / bin_size_;
608 const size_t sub_height = region.
height / bin_size_;
611 map.
resize (sub_width, sub_height);
616 const size_t num_gradient_bins = 7;
617 const size_t max_num_of_gradients = 7;
619 const float divisor = 180.0f / (num_gradient_bins - 1.0f);
621 float global_max_gradient = 0.0f;
622 float local_max_gradient = 0.0f;
624 unsigned char * peak_pointer = map.
getData ();
627 for (
size_t row_bin_index = 0; row_bin_index < sub_height; ++row_bin_index)
629 for (
size_t col_bin_index = 0; col_bin_index < sub_width; ++col_bin_index)
631 std::vector<size_t> x_coordinates;
632 std::vector<size_t> y_coordinates;
633 std::vector<float> values;
635 for (
int row_pixel_index = -static_cast<int> (bin_size_)/2;
636 row_pixel_index <= static_cast<int> (bin_size_)/2;
637 row_pixel_index +=
static_cast<int> (bin_size_)/2)
639 const size_t y_position = row_pixel_index + (sub_start_y + row_bin_index)*bin_size_;
641 if (y_position < 0 || y_position >= input_height)
644 for (
int col_pixel_index = -static_cast<int> (bin_size_)/2;
645 col_pixel_index <= static_cast<int> (bin_size_)/2;
646 col_pixel_index +=
static_cast<int> (bin_size_)/2)
648 const size_t x_position = col_pixel_index + (sub_start_x + col_bin_index)*bin_size_;
651 if (x_position < 0 || x_position >= input_width)
656 local_max_gradient = 0.0f;
657 for (
size_t row_sub_index = 0; row_sub_index < bin_size_; ++row_sub_index)
659 for (
size_t col_sub_index = 0; col_sub_index < bin_size_; ++col_sub_index)
661 const float magnitude = color_gradients_ (col_sub_index + x_position, row_sub_index + y_position).magnitude;
663 if (magnitude > local_max_gradient)
664 local_max_gradient = magnitude;
671 if (local_max_gradient > global_max_gradient)
673 global_max_gradient = local_max_gradient;
680 size_t max_gradient_pos_x;
681 size_t max_gradient_pos_y;
686 for (
size_t row_sub_index = 0; row_sub_index < bin_size_; ++row_sub_index)
688 for (
size_t col_sub_index = 0; col_sub_index < bin_size_; ++col_sub_index)
690 const float magnitude = color_gradients_ (col_sub_index + x_position, row_sub_index + y_position).magnitude;
692 if (magnitude > max_gradient)
694 max_gradient = magnitude;
695 max_gradient_pos_x = col_sub_index;
696 max_gradient_pos_y = row_sub_index;
703 if (local_max_gradient < gradient_magnitude_threshold_)
711 counter >= max_num_of_gradients)
718 const size_t angle =
static_cast<size_t> (180 + color_gradients_ (max_gradient_pos_x + x_position, max_gradient_pos_y + y_position).angle + 0.5f);
719 const size_t bin_index =
static_cast<size_t> ((angle >= 180 ? angle-180 : angle)/divisor);
721 *peak_pointer |= 1 << bin_index;
723 x_coordinates.push_back (max_gradient_pos_x + x_position);
724 y_coordinates.push_back (max_gradient_pos_y + y_position);
725 values.push_back (max_gradient);
727 color_gradients_ (max_gradient_pos_x + x_position, max_gradient_pos_y + y_position).magnitude = -1.0f;
731 for (
size_t value_index = 0; value_index < values.size (); ++value_index)
733 color_gradients_ (x_coordinates[value_index], y_coordinates[value_index]).magnitude = values[value_index];
736 x_coordinates.clear ();
737 y_coordinates.clear ();
742 if (*peak_pointer == 0)
744 *peak_pointer |= 1 << 7;