40 #include <pcl/recognition/quantizable_modality.h>
41 #include <pcl/recognition/distance_map.h>
43 #include <pcl/pcl_base.h>
44 #include <pcl/point_cloud.h>
46 #include <pcl/recognition/point_types.h>
54 template <
typename Po
intInT>
87 return (filtered_quantized_colors_);
93 return (spreaded_filtered_quantized_colors_);
98 std::vector<QuantizedMultiModFeature> & features)
const;
129 float feature_distance_threshold_;
140 template <
typename Po
intInT>
142 : feature_distance_threshold_ (1.0f), quantized_colors_ (), filtered_quantized_colors_ (), spreaded_filtered_quantized_colors_ ()
147 template <
typename Po
intInT>
153 template <
typename Po
intInT>
161 filterQuantizedColors ();
165 const int spreading_size = 8;
167 spreaded_filtered_quantized_colors_, spreading_size);
171 template <
typename Po
intInT>
173 const std::size_t nr_features,
174 const std::size_t modality_index,
175 std::vector<QuantizedMultiModFeature> & features)
const
177 const std::size_t width = mask.
getWidth ();
178 const std::size_t height = mask.
getHeight ();
181 for (std::size_t map_index = 0; map_index < 8; ++map_index)
182 mask_maps[map_index].resize (width, height);
184 unsigned char map[255];
199 for (std::size_t row_index = 0; row_index < height; ++row_index)
201 for (std::size_t col_index = 0; col_index < width; ++col_index)
203 if (mask (col_index, row_index) != 0)
206 const unsigned char quantized_value = filtered_quantized_colors_ (col_index, row_index);
208 if (quantized_value == 0)
210 const int dist_map_index = map[quantized_value];
212 distance_map_indices (col_index, row_index) = dist_map_index;
214 mask_maps[dist_map_index] (col_index, row_index) = 255;
220 for (
int map_index = 0; map_index < 8; ++map_index)
221 computeDistanceMap (mask_maps[map_index], distance_maps[map_index]);
223 std::list<Candidate> list1;
224 std::list<Candidate> list2;
226 float weights[8] = {0,0,0,0,0,0,0,0};
228 const std::size_t off = 4;
229 for (std::size_t row_index = off; row_index < height-off; ++row_index)
231 for (std::size_t col_index = off; col_index < width-off; ++col_index)
233 if (mask (col_index, row_index) != 0)
236 const unsigned char quantized_value = filtered_quantized_colors_ (col_index, row_index);
242 if (quantized_value != 0)
244 const int distance_map_index = map[quantized_value];
247 const float distance = distance_maps[distance_map_index] (col_index, row_index);
249 if (
distance >= feature_distance_threshold_)
254 candidate.
x = col_index;
255 candidate.
y = row_index;
256 candidate.
bin_index = distance_map_index;
258 list1.push_back (candidate);
260 ++weights[distance_map_index];
267 for (
typename std::list<Candidate>::iterator iter = list1.begin (); iter != list1.end (); ++iter)
268 iter->distance *= 1.0f / weights[iter->bin_index];
272 if (list1.size () <= nr_features)
274 features.reserve (list1.size ());
275 for (
typename std::list<Candidate>::iterator iter = list1.begin (); iter != list1.end (); ++iter)
279 feature.
x = static_cast<int> (iter->x);
280 feature.
y = static_cast<int> (iter->y);
282 feature.
quantized_value = filtered_quantized_colors_ (iter->x, iter->y);
284 features.push_back (feature);
290 int distance = static_cast<int> (list1.size () / nr_features + 1);
291 while (list2.size () != nr_features)
294 for (
typename std::list<Candidate>::iterator iter1 = list1.begin (); iter1 != list1.end (); ++iter1)
296 bool candidate_accepted =
true;
298 for (
typename std::list<Candidate>::iterator iter2 = list2.begin (); iter2 != list2.end (); ++iter2)
300 const int dx = static_cast<int> (iter1->x) - static_cast<int> (iter2->x);
301 const int dy = static_cast<int> (iter1->y) - static_cast<int> (iter2->y);
302 const int tmp_distance = dx*dx + dy*dy;
304 if (tmp_distance < sqr_distance)
306 candidate_accepted =
false;
311 if (candidate_accepted)
312 list2.push_back (*iter1);
314 if (list2.size () == nr_features)
break;
319 for (
typename std::list<Candidate>::iterator iter2 = list2.begin (); iter2 != list2.end (); ++iter2)
323 feature.
x = static_cast<int> (iter2->x);
324 feature.
y = static_cast<int> (iter2->y);
326 feature.
quantized_value = filtered_quantized_colors_ (iter2->x, iter2->y);
328 features.push_back (feature);
333 template <
typename Po
intInT>
337 const std::size_t width = input_->width;
338 const std::size_t height = input_->height;
340 quantized_colors_.resize (width, height);
342 for (std::size_t row_index = 0; row_index < height; ++row_index)
344 for (std::size_t col_index = 0; col_index < width; ++col_index)
346 const float r = static_cast<float> ((*input_) (col_index, row_index).r);
347 const float g = static_cast<float> ((*input_) (col_index, row_index).g);
348 const float b = static_cast<float> ((*input_) (col_index, row_index).b);
350 quantized_colors_ (col_index, row_index) = quantizeColorOnRGBExtrema (r, g, b);
356 template <
typename Po
intInT>
360 const std::size_t width = input_->width;
361 const std::size_t height = input_->height;
363 filtered_quantized_colors_.resize (width, height);
366 for (std::size_t row_index = 1; row_index < height-1; ++row_index)
368 for (std::size_t col_index = 1; col_index < width-1; ++col_index)
370 unsigned char histogram[8] = {0,0,0,0,0,0,0,0};
373 const unsigned char * data_ptr = quantized_colors_.getData () + (row_index-1)*width+col_index-1;
374 assert (0 <= data_ptr[0] && data_ptr[0] < 9 &&
375 0 <= data_ptr[1] && data_ptr[1] < 9 &&
376 0 <= data_ptr[2] && data_ptr[2] < 9);
377 ++histogram[data_ptr[0]];
378 ++histogram[data_ptr[1]];
379 ++histogram[data_ptr[2]];
382 const unsigned char * data_ptr = quantized_colors_.getData () + row_index*width+col_index-1;
383 assert (0 <= data_ptr[0] && data_ptr[0] < 9 &&
384 0 <= data_ptr[1] && data_ptr[1] < 9 &&
385 0 <= data_ptr[2] && data_ptr[2] < 9);
386 ++histogram[data_ptr[0]];
387 ++histogram[data_ptr[1]];
388 ++histogram[data_ptr[2]];
391 const unsigned char * data_ptr = quantized_colors_.getData () + (row_index+1)*width+col_index-1;
392 assert (0 <= data_ptr[0] && data_ptr[0] < 9 &&
393 0 <= data_ptr[1] && data_ptr[1] < 9 &&
394 0 <= data_ptr[2] && data_ptr[2] < 9);
395 ++histogram[data_ptr[0]];
396 ++histogram[data_ptr[1]];
397 ++histogram[data_ptr[2]];
400 unsigned char max_hist_value = 0;
401 int max_hist_index = -1;
412 if (max_hist_value < histogram[0]) {max_hist_index = 0; max_hist_value = histogram[0];}
413 if (max_hist_value < histogram[1]) {max_hist_index = 1; max_hist_value = histogram[1];}
414 if (max_hist_value < histogram[2]) {max_hist_index = 2; max_hist_value = histogram[2];}
415 if (max_hist_value < histogram[3]) {max_hist_index = 3; max_hist_value = histogram[3];}
416 if (max_hist_value < histogram[4]) {max_hist_index = 4; max_hist_value = histogram[4];}
417 if (max_hist_value < histogram[5]) {max_hist_index = 5; max_hist_value = histogram[5];}
418 if (max_hist_value < histogram[6]) {max_hist_index = 6; max_hist_value = histogram[6];}
419 if (max_hist_value < histogram[7]) {max_hist_index = 7; max_hist_value = histogram[7];}
422 filtered_quantized_colors_ (col_index, row_index) = 0x1 << max_hist_index;
431 template <
typename Po
intInT>
437 const float r_inv = 255.0f-r;
438 const float g_inv = 255.0f-g;
439 const float b_inv = 255.0f-b;
441 const float dist_0 = (r*r + g*g + b*b)*2.0f;
442 const float dist_1 = r*r + g*g + b_inv*b_inv;
443 const float dist_2 = r*r + g_inv*g_inv+ b*b;
444 const float dist_3 = r*r + g_inv*g_inv + b_inv*b_inv;
445 const float dist_4 = r_inv*r_inv + g*g + b*b;
446 const float dist_5 = r_inv*r_inv + g*g + b_inv*b_inv;
447 const float dist_6 = r_inv*r_inv + g_inv*g_inv+ b*b;
448 const float dist_7 = (r_inv*r_inv + g_inv*g_inv + b_inv*b_inv)*1.5f;
450 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)));
452 if (min_dist == dist_0)
456 if (min_dist == dist_1)
460 if (min_dist == dist_2)
464 if (min_dist == dist_3)
468 if (min_dist == dist_4)
472 if (min_dist == dist_5)
476 if (min_dist == dist_6)
484 template <
typename Po
intInT>
void
488 const std::size_t width = input.
getWidth ();
489 const std::size_t height = input.
getHeight ();
491 output.
resize (width, height);
495 const unsigned char * mask_map = input.
getData ();
496 float * distance_map = output.
getData ();
497 for (std::size_t index = 0; index < width*height; ++index)
499 if (mask_map[index] == 0)
500 distance_map[index] = 0.0f;
502 distance_map[index] = static_cast<float> (width + height);
506 float * previous_row = distance_map;
507 float * current_row = previous_row + width;
508 for (std::size_t ri = 1; ri < height; ++ri)
510 for (std::size_t ci = 1; ci < width; ++ci)
512 const float up_left = previous_row [ci - 1] + 1.4f;
513 const float up = previous_row [ci] + 1.0f;
514 const float up_right = previous_row [ci + 1] + 1.4f;
515 const float left = current_row [ci - 1] + 1.0f;
516 const float center = current_row [ci];
518 const float min_value = std::min (std::min (up_left, up), std::min (left, up_right));
520 if (min_value < center)
521 current_row[ci] = min_value;
523 previous_row = current_row;
524 current_row += width;
528 float * next_row = distance_map + width * (height - 1);
529 current_row = next_row - width;
530 for (
int ri = static_cast<int> (height)-2; ri >= 0; --ri)
532 for (
int ci = static_cast<int> (width)-2; ci >= 0; --ci)
534 const float lower_left = next_row [ci - 1] + 1.4f;
535 const float lower = next_row [ci] + 1.0f;
536 const float lower_right = next_row [ci + 1] + 1.4f;
537 const float right = current_row [ci + 1] + 1.0f;
538 const float center = current_row [ci];
540 const float min_value = std::min (std::min (lower_left, lower), std::min (right, lower_right));
542 if (min_value < center)
543 current_row[ci] = min_value;
545 next_row = current_row;
546 current_row -= width;