38 #ifndef PCL_RECOGNITION_COLOR_MODALITY 39 #define PCL_RECOGNITION_COLOR_MODALITY 41 #include <pcl/recognition/quantizable_modality.h> 42 #include <pcl/recognition/distance_map.h> 44 #include <pcl/pcl_base.h> 45 #include <pcl/point_cloud.h> 47 #include <pcl/recognition/point_types.h> 55 template <
typename Po
intInT>
88 return (filtered_quantized_colors_);
94 return (spreaded_filtered_quantized_colors_);
99 std::vector<QuantizedMultiModFeature> & features)
const;
130 float feature_distance_threshold_;
141 template <
typename Po
intInT>
143 : feature_distance_threshold_ (1.0f), quantized_colors_ (), filtered_quantized_colors_ (), spreaded_filtered_quantized_colors_ ()
148 template <
typename Po
intInT>
154 template <
typename Po
intInT>
162 filterQuantizedColors ();
166 const int spreading_size = 8;
168 spreaded_filtered_quantized_colors_, spreading_size);
172 template <
typename Po
intInT>
174 const size_t nr_features,
175 const size_t modality_index,
176 std::vector<QuantizedMultiModFeature> & features)
const 178 const size_t width = mask.
getWidth ();
182 for (
size_t map_index = 0; map_index < 8; ++map_index)
183 mask_maps[map_index].resize (width, height);
185 unsigned char map[255];
200 for (
size_t row_index = 0; row_index < height; ++row_index)
202 for (
size_t col_index = 0; col_index < width; ++col_index)
204 if (mask (col_index, row_index) != 0)
207 const unsigned char quantized_value = filtered_quantized_colors_ (col_index, row_index);
209 if (quantized_value == 0)
211 const int dist_map_index = map[quantized_value];
213 distance_map_indices (col_index, row_index) = dist_map_index;
215 mask_maps[dist_map_index] (col_index, row_index) = 255;
221 for (
int map_index = 0; map_index < 8; ++map_index)
222 computeDistanceMap (mask_maps[map_index], distance_maps[map_index]);
224 std::list<Candidate> list1;
225 std::list<Candidate> list2;
227 float weights[8] = {0,0,0,0,0,0,0,0};
229 const size_t off = 4;
230 for (
size_t row_index = off; row_index < height-off; ++row_index)
232 for (
size_t col_index = off; col_index < width-off; ++col_index)
234 if (mask (col_index, row_index) != 0)
237 const unsigned char quantized_value = filtered_quantized_colors_ (col_index, row_index);
243 if (quantized_value != 0)
245 const int distance_map_index = map[quantized_value];
248 const float distance = distance_maps[distance_map_index] (col_index, row_index);
250 if (
distance >= feature_distance_threshold_)
255 candidate.
x = col_index;
256 candidate.
y = row_index;
257 candidate.
bin_index = distance_map_index;
259 list1.push_back (candidate);
261 ++weights[distance_map_index];
268 for (
typename std::list<Candidate>::iterator iter = list1.begin (); iter != list1.end (); ++iter)
269 iter->distance *= 1.0f / weights[iter->bin_index];
273 if (list1.size () <= nr_features)
275 features.reserve (list1.size ());
276 for (
typename std::list<Candidate>::iterator iter = list1.begin (); iter != list1.end (); ++iter)
280 feature.
x = static_cast<int> (iter->x);
281 feature.
y = static_cast<int> (iter->y);
283 feature.
quantized_value = filtered_quantized_colors_ (iter->x, iter->y);
285 features.push_back (feature);
291 int distance = static_cast<int> (list1.size () / nr_features + 1);
292 while (list2.size () != nr_features)
295 for (
typename std::list<Candidate>::iterator iter1 = list1.begin (); iter1 != list1.end (); ++iter1)
297 bool candidate_accepted =
true;
299 for (
typename std::list<Candidate>::iterator iter2 = list2.begin (); iter2 != list2.end (); ++iter2)
301 const int dx = static_cast<int> (iter1->x) - static_cast<int> (iter2->x);
302 const int dy = static_cast<int> (iter1->y) - static_cast<int> (iter2->y);
303 const int tmp_distance = dx*dx + dy*dy;
305 if (tmp_distance < sqr_distance)
307 candidate_accepted =
false;
312 if (candidate_accepted)
313 list2.push_back (*iter1);
315 if (list2.size () == nr_features)
break;
320 for (
typename std::list<Candidate>::iterator iter2 = list2.begin (); iter2 != list2.end (); ++iter2)
324 feature.
x = static_cast<int> (iter2->x);
325 feature.
y = static_cast<int> (iter2->y);
327 feature.
quantized_value = filtered_quantized_colors_ (iter2->x, iter2->y);
329 features.push_back (feature);
334 template <
typename Po
intInT>
338 const size_t width = input_->width;
339 const size_t height = input_->height;
341 quantized_colors_.resize (width, height);
343 for (
size_t row_index = 0; row_index < height; ++row_index)
345 for (
size_t col_index = 0; col_index < width; ++col_index)
347 const float r = static_cast<float> ((*input_) (col_index, row_index).r);
348 const float g = static_cast<float> ((*input_) (col_index, row_index).g);
349 const float b = static_cast<float> ((*input_) (col_index, row_index).b);
351 quantized_colors_ (col_index, row_index) = quantizeColorOnRGBExtrema (r, g, b);
357 template <
typename Po
intInT>
361 const size_t width = input_->width;
362 const size_t height = input_->height;
364 filtered_quantized_colors_.resize (width, height);
367 for (
size_t row_index = 1; row_index < height-1; ++row_index)
369 for (
size_t col_index = 1; col_index < width-1; ++col_index)
371 unsigned char histogram[8] = {0,0,0,0,0,0,0,0};
374 const unsigned char * data_ptr = quantized_colors_.getData () + (row_index-1)*width+col_index-1;
375 assert (0 <= data_ptr[0] && data_ptr[0] < 9 &&
376 0 <= data_ptr[1] && data_ptr[1] < 9 &&
377 0 <= data_ptr[2] && data_ptr[2] < 9);
378 ++histogram[data_ptr[0]];
379 ++histogram[data_ptr[1]];
380 ++histogram[data_ptr[2]];
383 const unsigned char * data_ptr = quantized_colors_.getData () + row_index*width+col_index-1;
384 assert (0 <= data_ptr[0] && data_ptr[0] < 9 &&
385 0 <= data_ptr[1] && data_ptr[1] < 9 &&
386 0 <= data_ptr[2] && data_ptr[2] < 9);
387 ++histogram[data_ptr[0]];
388 ++histogram[data_ptr[1]];
389 ++histogram[data_ptr[2]];
392 const unsigned char * data_ptr = quantized_colors_.getData () + (row_index+1)*width+col_index-1;
393 assert (0 <= data_ptr[0] && data_ptr[0] < 9 &&
394 0 <= data_ptr[1] && data_ptr[1] < 9 &&
395 0 <= data_ptr[2] && data_ptr[2] < 9);
396 ++histogram[data_ptr[0]];
397 ++histogram[data_ptr[1]];
398 ++histogram[data_ptr[2]];
401 unsigned char max_hist_value = 0;
402 int max_hist_index = -1;
413 if (max_hist_value < histogram[0]) {max_hist_index = 0; max_hist_value = histogram[0];}
414 if (max_hist_value < histogram[1]) {max_hist_index = 1; max_hist_value = histogram[1];}
415 if (max_hist_value < histogram[2]) {max_hist_index = 2; max_hist_value = histogram[2];}
416 if (max_hist_value < histogram[3]) {max_hist_index = 3; max_hist_value = histogram[3];}
417 if (max_hist_value < histogram[4]) {max_hist_index = 4; max_hist_value = histogram[4];}
418 if (max_hist_value < histogram[5]) {max_hist_index = 5; max_hist_value = histogram[5];}
419 if (max_hist_value < histogram[6]) {max_hist_index = 6; max_hist_value = histogram[6];}
420 if (max_hist_value < histogram[7]) {max_hist_index = 7; max_hist_value = histogram[7];}
423 filtered_quantized_colors_ (col_index, row_index) = 0x1 << max_hist_index;
432 template <
typename Po
intInT>
438 const float r_inv = 255.0f-r;
439 const float g_inv = 255.0f-g;
440 const float b_inv = 255.0f-b;
442 const float dist_0 = (r*r + g*g + b*b)*2.0f;
443 const float dist_1 = r*r + g*g + b_inv*b_inv;
444 const float dist_2 = r*r + g_inv*g_inv+ b*b;
445 const float dist_3 = r*r + g_inv*g_inv + b_inv*b_inv;
446 const float dist_4 = r_inv*r_inv + g*g + b*b;
447 const float dist_5 = r_inv*r_inv + g*g + b_inv*b_inv;
448 const float dist_6 = r_inv*r_inv + g_inv*g_inv+ b*b;
449 const float dist_7 = (r_inv*r_inv + g_inv*g_inv + b_inv*b_inv)*1.5f;
451 const float min_dist = std::min (std::min (std::min (dist_0, dist_1), std::min (dist_2, dist_3)), std::min (std::min (dist_4, dist_5), std::min (dist_6, dist_7)));
453 if (min_dist == dist_0)
457 if (min_dist == dist_1)
461 if (min_dist == dist_2)
465 if (min_dist == dist_3)
469 if (min_dist == dist_4)
473 if (min_dist == dist_5)
477 if (min_dist == dist_6)
485 template <
typename Po
intInT>
void 489 const size_t width = input.
getWidth ();
490 const size_t height = input.
getHeight ();
492 output.
resize (width, height);
496 const unsigned char * mask_map = input.
getData ();
497 float * distance_map = output.
getData ();
498 for (
size_t index = 0; index < width*height; ++index)
500 if (mask_map[index] == 0)
501 distance_map[index] = 0.0f;
503 distance_map[index] = static_cast<float> (width + height);
507 float * previous_row = distance_map;
508 float * current_row = previous_row + width;
509 for (
size_t ri = 1; ri < height; ++ri)
511 for (
size_t ci = 1; ci < width; ++ci)
513 const float up_left = previous_row [ci - 1] + 1.4f;
514 const float up = previous_row [ci] + 1.0f;
515 const float up_right = previous_row [ci + 1] + 1.4f;
516 const float left = current_row [ci - 1] + 1.0f;
517 const float center = current_row [ci];
519 const float min_value = std::min (std::min (up_left, up), std::min (left, up_right));
521 if (min_value < center)
522 current_row[ci] = min_value;
524 previous_row = current_row;
525 current_row += width;
529 float * next_row = distance_map + width * (height - 1);
530 current_row = next_row - width;
531 for (
int ri = static_cast<int> (height)-2; ri >= 0; --ri)
533 for (
int ci = static_cast<int> (width)-2; ci >= 0; --ci)
535 const float lower_left = next_row [ci - 1] + 1.4f;
536 const float lower = next_row [ci] + 1.0f;
537 const float lower_right = next_row [ci + 1] + 1.4f;
538 const float right = current_row [ci + 1] + 1.0f;
539 const float center = current_row [ci];
541 const float min_value = std::min (std::min (lower_left, lower), std::min (right, lower_right));
543 if (min_value < center)
544 current_row[ci] = min_value;
546 next_row = current_row;
547 current_row -= width;
virtual void processInputData()
QuantizedMap & getQuantizedMap()
Returns a reference to the internally computed quantized map.
This file defines compatibility wrappers for low level I/O functions.
Feature that defines a position and quantized value in a specific modality.
void filterQuantizedColors()
void resize(const size_t width, const size_t height)
Resizes the map to the specified size.
unsigned char quantized_value
the quantized value attached to the feature.
Represents a distance map obtained from a distance transformation.
void extractFeatures(const MaskMap &mask, size_t nr_features, size_t modalityIndex, std::vector< QuantizedMultiModFeature > &features) const
Extracts features from this modality within the specified mask.
unsigned char * getData()
Defines all the PCL implemented PointT point type structures.
virtual void setInputCloud(const typename PointCloudIn::ConstPtr &cloud)
Provide a pointer to the input dataset (overwrites the PCLBase::setInputCloud method)
pcl::PointCloud< PointInT > PointCloudIn
void computeDistanceMap(const MaskMap &input, DistanceMap &output) const
float distance(const PointT &p1, const PointT &p2)
boost::shared_ptr< const PointCloud< PointInT > > ConstPtr
Interface for a quantizable modality.
static int quantizeColorOnRGBExtrema(const float r, const float g, const float b)
bool operator<(const Candidate &rhs)
PointCloudConstPtr input_
The input point cloud dataset.
static void spreadQuantizedMap(const QuantizedMap &input_map, QuantizedMap &output_map, size_t spreading_size)
float * getData()
Returns a pointer to the beginning of map.
QuantizedMap & getSpreadedQuantizedMap()
Returns a reference to the internally computed spread quantized map.
size_t modality_index
the index of the corresponding modality.