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> 45 #include <pcl/recognition/dot_modality.h> 46 #include <pcl/recognition/point_types.h> 47 #include <pcl/recognition/quantized_map.h> 52 template <
typename Po
intInT>
82 gradient_magnitude_threshold_ = threshold;
94 return (dominant_quantized_color_gradients_);
128 float gradient_magnitude_threshold_;
139 template <
typename Po
intInT>
142 : bin_size_ (bin_size), gradient_magnitude_threshold_ (80.0f), color_gradients_ (), dominant_quantized_color_gradients_ ()
147 template <
typename Po
intInT>
154 template <
typename Po
intInT>
170 template <
typename Po
intInT>
175 const int width =
input_->width;
176 const int height =
input_->height;
178 color_gradients_.
points.resize (width*height);
179 color_gradients_.
width = width;
180 color_gradients_.
height = height;
182 const float pi = tan(1.0f)*4;
183 for (
int row_index = 0; row_index < height-2; ++row_index)
185 for (
int col_index = 0; col_index < width-2; ++col_index)
187 const int index0 = row_index*width+col_index;
188 const int index_c = row_index*width+col_index+2;
189 const int index_r = (row_index+2)*width+col_index;
193 const unsigned char r0 =
input_->points[index0].r;
194 const unsigned char g0 =
input_->points[index0].g;
195 const unsigned char b0 =
input_->points[index0].b;
197 const unsigned char r_c =
input_->points[index_c].r;
198 const unsigned char g_c =
input_->points[index_c].g;
199 const unsigned char b_c =
input_->points[index_c].b;
201 const unsigned char r_r =
input_->points[index_r].r;
202 const unsigned char g_r =
input_->points[index_r].g;
203 const unsigned char b_r =
input_->points[index_r].b;
205 const float r_dx =
static_cast<float> (r_c) - static_cast<float> (r0);
206 const float g_dx =
static_cast<float> (g_c) - static_cast<float> (g0);
207 const float b_dx =
static_cast<float> (b_c) - static_cast<float> (b0);
209 const float r_dy =
static_cast<float> (r_r) - static_cast<float> (r0);
210 const float g_dy =
static_cast<float> (g_r) - static_cast<float> (g0);
211 const float b_dy =
static_cast<float> (b_r) - static_cast<float> (b0);
213 const float sqr_mag_r = r_dx*r_dx + r_dy*r_dy;
214 const float sqr_mag_g = g_dx*g_dx + g_dy*g_dy;
215 const float sqr_mag_b = b_dx*b_dx + b_dy*b_dy;
218 gradient.
x = col_index;
219 gradient.
y = row_index;
220 if (sqr_mag_r > sqr_mag_g && sqr_mag_r > sqr_mag_b)
223 gradient.
angle = atan2 (r_dy, r_dx) * 180.0f / pi;
225 else if (sqr_mag_g > sqr_mag_b)
229 gradient.
angle = atan2 (g_dy, g_dx) * 180.0f / pi;
239 gradient.
angle = atan2 (b_dy, b_dx) * 180.0f / pi;
246 assert (color_gradients_ (col_index+1, row_index+1).angle >= -180 &&
247 color_gradients_ (col_index+1, row_index+1).angle <= 180);
249 color_gradients_ (col_index+1, row_index+1) = gradient;
419 template <
typename Po
intInT>
424 const size_t input_width =
input_->width;
425 const size_t input_height =
input_->height;
427 const size_t output_width = input_width / bin_size_;
428 const size_t output_height = input_height / bin_size_;
430 dominant_quantized_color_gradients_.
resize (output_width, output_height);
435 const size_t num_gradient_bins = 7;
436 const size_t max_num_of_gradients = 1;
438 const float divisor = 180.0f / (num_gradient_bins - 1.0f);
440 float global_max_gradient = 0.0f;
441 float local_max_gradient = 0.0f;
443 unsigned char * peak_pointer = dominant_quantized_color_gradients_.
getData ();
444 memset (peak_pointer, 0, output_width*output_height);
447 for (
size_t row_bin_index = 0; row_bin_index < output_height; ++row_bin_index)
449 for (
size_t col_bin_index = 0; col_bin_index < output_width; ++col_bin_index)
451 const size_t x_position = col_bin_index * bin_size_;
452 const size_t y_position = row_bin_index * bin_size_;
462 size_t max_gradient_pos_x;
463 size_t max_gradient_pos_y;
468 for (
size_t row_sub_index = 0; row_sub_index < bin_size_; ++row_sub_index)
470 for (
size_t col_sub_index = 0; col_sub_index < bin_size_; ++col_sub_index)
472 const float magnitude = color_gradients_ (col_sub_index + x_position, row_sub_index + y_position).magnitude;
474 if (magnitude > max_gradient)
476 max_gradient = magnitude;
477 max_gradient_pos_x = col_sub_index;
478 max_gradient_pos_y = row_sub_index;
484 if (max_gradient >= gradient_magnitude_threshold_)
486 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);
487 const size_t bin_index =
static_cast<size_t> ((angle >= 180 ? angle-180 : angle)/divisor);
489 *peak_pointer |= 1 << bin_index;
508 if (*peak_pointer == 0)
510 *peak_pointer |= 1 << 7;
530 template <
typename Po
intInT>
536 const size_t input_width =
input_->width;
537 const size_t input_height =
input_->height;
539 const size_t output_width = input_width / bin_size_;
540 const size_t output_height = input_height / bin_size_;
542 const size_t sub_start_x = region.
x / bin_size_;
543 const size_t sub_start_y = region.
y / bin_size_;
544 const size_t sub_width = region.
width / bin_size_;
545 const size_t sub_height = region.
height / bin_size_;
548 map.
resize (sub_width, sub_height);
553 const size_t num_gradient_bins = 7;
554 const size_t max_num_of_gradients = 7;
556 const float divisor = 180.0f / (num_gradient_bins - 1.0f);
558 float global_max_gradient = 0.0f;
559 float local_max_gradient = 0.0f;
561 unsigned char * peak_pointer = map.
getData ();
564 for (
size_t row_bin_index = 0; row_bin_index < sub_height; ++row_bin_index)
566 for (
size_t col_bin_index = 0; col_bin_index < sub_width; ++col_bin_index)
568 std::vector<size_t> x_coordinates;
569 std::vector<size_t> y_coordinates;
570 std::vector<float> values;
572 for (
int row_pixel_index = -static_cast<int> (bin_size_)/2;
573 row_pixel_index <= static_cast<int> (bin_size_)/2;
574 row_pixel_index +=
static_cast<int> (bin_size_)/2)
576 const size_t y_position = row_pixel_index + (sub_start_y + row_bin_index)*bin_size_;
578 if (y_position < 0 || y_position >= input_height)
581 for (
int col_pixel_index = -static_cast<int> (bin_size_)/2;
582 col_pixel_index <= static_cast<int> (bin_size_)/2;
583 col_pixel_index +=
static_cast<int> (bin_size_)/2)
585 const size_t x_position = col_pixel_index + (sub_start_x + col_bin_index)*bin_size_;
588 if (x_position < 0 || x_position >= input_width)
593 local_max_gradient = 0.0f;
594 for (
size_t row_sub_index = 0; row_sub_index < bin_size_; ++row_sub_index)
596 for (
size_t col_sub_index = 0; col_sub_index < bin_size_; ++col_sub_index)
598 const float magnitude = color_gradients_ (col_sub_index + x_position, row_sub_index + y_position).magnitude;
600 if (magnitude > local_max_gradient)
601 local_max_gradient = magnitude;
608 if (local_max_gradient > global_max_gradient)
610 global_max_gradient = local_max_gradient;
617 size_t max_gradient_pos_x;
618 size_t max_gradient_pos_y;
623 for (
size_t row_sub_index = 0; row_sub_index < bin_size_; ++row_sub_index)
625 for (
size_t col_sub_index = 0; col_sub_index < bin_size_; ++col_sub_index)
627 const float magnitude = color_gradients_ (col_sub_index + x_position, row_sub_index + y_position).magnitude;
629 if (magnitude > max_gradient)
631 max_gradient = magnitude;
632 max_gradient_pos_x = col_sub_index;
633 max_gradient_pos_y = row_sub_index;
640 if (local_max_gradient < gradient_magnitude_threshold_)
648 counter >= max_num_of_gradients)
655 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);
656 const size_t bin_index =
static_cast<size_t> ((angle >= 180 ? angle-180 : angle)/divisor);
658 *peak_pointer |= 1 << bin_index;
660 x_coordinates.
push_back (max_gradient_pos_x + x_position);
661 y_coordinates.push_back (max_gradient_pos_y + y_position);
662 values.push_back (max_gradient);
664 color_gradients_ (max_gradient_pos_x + x_position, max_gradient_pos_y + y_position).magnitude = -1.0f;
668 for (
size_t value_index = 0; value_index < values.size (); ++value_index)
670 color_gradients_ (x_coordinates[value_index], y_coordinates[value_index]).magnitude = values[value_index];
673 x_coordinates.
clear ();
674 y_coordinates.clear ();
679 if (*peak_pointer == 0)
681 *peak_pointer |= 1 << 7;
void computeMaxColorGradients()
QuantizedMap computeInvariantQuantizedMap(const MaskMap &mask, const RegionXY ®ion)
bool operator<(const Candidate &rhs)
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
This file defines compatibility wrappers for low level I/O functions.
void push_back(const PointT &pt)
Insert a new point in the cloud, at the end of the container.
virtual void processInputData()
Defines a region in XY-space.
uint32_t height
The point cloud height (if organized as an image-structure).
A point structure representing Euclidean xyz coordinates, and the intensity value.
int x
x-position of the region.
QuantizedMap & getDominantQuantizedMap()
int y
y-position of the region.
uint32_t width
The point cloud width (if organized as an image-structure).
void resize(size_t width, size_t height)
Defines all the PCL implemented PointT point type structures.
void computeDominantQuantizedGradients()
int height
height of the region.
boost::shared_ptr< const PointCloud< PointInT > > ConstPtr
void clear()
Removes all points in a cloud and sets the width and height to 0.
void setGradientMagnitudeThreshold(const float threshold)
unsigned char * getData()
ColorGradientDOTModality(size_t bin_size)
PointCloudConstPtr input_
The input point cloud dataset.
virtual void setInputCloud(const typename PointCloudIn::ConstPtr &cloud)
Provide a pointer to the input dataset (overwrites the PCLBase::setInputCloud method) ...
virtual ~ColorGradientDOTModality()
int width
width of the region.
pcl::PointCloud< PointInT > PointCloudIn