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>
58 template <
typename Po
intInT>
91 return (filtered_quantized_colors_);
97 return (spreaded_filtered_quantized_colors_);
102 std::vector<QuantizedMultiModFeature> & features)
const;
133 float feature_distance_threshold_;
144 template <
typename Po
intInT>
146 : feature_distance_threshold_ (1.0f), quantized_colors_ (), filtered_quantized_colors_ (), spreaded_filtered_quantized_colors_ ()
151 template <
typename Po
intInT>
157 template <
typename Po
intInT>
165 filterQuantizedColors ();
169 const int spreading_size = 8;
171 spreaded_filtered_quantized_colors_, spreading_size);
175 template <
typename Po
intInT>
177 const std::size_t nr_features,
178 const std::size_t modality_index,
179 std::vector<QuantizedMultiModFeature> & features)
const
181 const std::size_t width = mask.
getWidth ();
182 const std::size_t height = mask.
getHeight ();
185 for (std::size_t map_index = 0; map_index < 8; ++map_index)
186 mask_maps[map_index].resize (width, height);
188 unsigned char map[255];
203 for (std::size_t row_index = 0; row_index < height; ++row_index)
205 for (std::size_t col_index = 0; col_index < width; ++col_index)
207 if (mask (col_index, row_index) != 0)
210 const unsigned char quantized_value = filtered_quantized_colors_ (col_index, row_index);
212 if (quantized_value == 0)
214 const int dist_map_index = map[quantized_value];
216 distance_map_indices (col_index, row_index) = dist_map_index;
218 mask_maps[dist_map_index] (col_index, row_index) = 255;
224 for (
int map_index = 0; map_index < 8; ++map_index)
225 computeDistanceMap (mask_maps[map_index], distance_maps[map_index]);
227 std::list<Candidate> list1;
228 std::list<Candidate> list2;
230 float weights[8] = {0,0,0,0,0,0,0,0};
232 const std::size_t off = 4;
233 for (std::size_t row_index = off; row_index < height-off; ++row_index)
235 for (std::size_t col_index = off; col_index < width-off; ++col_index)
237 if (mask (col_index, row_index) != 0)
240 const unsigned char quantized_value = filtered_quantized_colors_ (col_index, row_index);
246 if (quantized_value != 0)
248 const int distance_map_index = map[quantized_value];
251 const float distance = distance_maps[distance_map_index] (col_index, row_index);
253 if (
distance >= feature_distance_threshold_)
258 candidate.
x = col_index;
259 candidate.
y = row_index;
260 candidate.
bin_index = distance_map_index;
262 list1.push_back (candidate);
264 ++weights[distance_map_index];
271 for (
typename std::list<Candidate>::iterator iter = list1.begin (); iter != list1.end (); ++iter)
272 iter->distance *= 1.0f / weights[iter->bin_index];
276 if (list1.size () <= nr_features)
278 features.reserve (list1.size ());
279 for (
typename std::list<Candidate>::iterator iter = list1.begin (); iter != list1.end (); ++iter)
283 feature.
x =
static_cast<int> (iter->x);
284 feature.
y =
static_cast<int> (iter->y);
286 feature.
quantized_value = filtered_quantized_colors_ (iter->x, iter->y);
288 features.push_back (feature);
294 int distance =
static_cast<int> (list1.size () / nr_features + 1);
295 while (list2.size () != nr_features)
298 for (
typename std::list<Candidate>::iterator iter1 = list1.begin (); iter1 != list1.end (); ++iter1)
300 bool candidate_accepted =
true;
302 for (
typename std::list<Candidate>::iterator iter2 = list2.begin (); iter2 != list2.end (); ++iter2)
304 const int dx =
static_cast<int> (iter1->x) -
static_cast<int> (iter2->x);
305 const int dy =
static_cast<int> (iter1->y) -
static_cast<int> (iter2->y);
306 const int tmp_distance = dx*dx + dy*dy;
308 if (tmp_distance < sqr_distance)
310 candidate_accepted =
false;
315 if (candidate_accepted)
316 list2.push_back (*iter1);
318 if (list2.size () == nr_features)
break;
323 for (
typename std::list<Candidate>::iterator iter2 = list2.begin (); iter2 != list2.end (); ++iter2)
327 feature.
x =
static_cast<int> (iter2->x);
328 feature.
y =
static_cast<int> (iter2->y);
330 feature.
quantized_value = filtered_quantized_colors_ (iter2->x, iter2->y);
332 features.push_back (feature);
337 template <
typename Po
intInT>
341 const std::size_t width = input_->width;
342 const std::size_t height = input_->height;
344 quantized_colors_.resize (width, height);
346 for (std::size_t row_index = 0; row_index < height; ++row_index)
348 for (std::size_t col_index = 0; col_index < width; ++col_index)
350 const float r =
static_cast<float> ((*input_) (col_index, row_index).r);
351 const float g =
static_cast<float> ((*input_) (col_index, row_index).g);
352 const float b =
static_cast<float> ((*input_) (col_index, row_index).b);
354 quantized_colors_ (col_index, row_index) = quantizeColorOnRGBExtrema (r, g, b);
360 template <
typename Po
intInT>
364 const std::size_t width = input_->width;
365 const std::size_t height = input_->height;
367 filtered_quantized_colors_.resize (width, height);
370 for (std::size_t row_index = 1; row_index < height-1; ++row_index)
372 for (std::size_t col_index = 1; col_index < width-1; ++col_index)
374 unsigned char histogram[8] = {0,0,0,0,0,0,0,0};
377 const unsigned char * data_ptr = quantized_colors_.getData () + (row_index-1)*width+col_index-1;
378 assert (0 <= data_ptr[0] && data_ptr[0] < 9 &&
379 0 <= data_ptr[1] && data_ptr[1] < 9 &&
380 0 <= data_ptr[2] && data_ptr[2] < 9);
381 ++histogram[data_ptr[0]];
382 ++histogram[data_ptr[1]];
383 ++histogram[data_ptr[2]];
386 const unsigned char * data_ptr = quantized_colors_.getData () + row_index*width+col_index-1;
387 assert (0 <= data_ptr[0] && data_ptr[0] < 9 &&
388 0 <= data_ptr[1] && data_ptr[1] < 9 &&
389 0 <= data_ptr[2] && data_ptr[2] < 9);
390 ++histogram[data_ptr[0]];
391 ++histogram[data_ptr[1]];
392 ++histogram[data_ptr[2]];
395 const unsigned char * data_ptr = quantized_colors_.getData () + (row_index+1)*width+col_index-1;
396 assert (0 <= data_ptr[0] && data_ptr[0] < 9 &&
397 0 <= data_ptr[1] && data_ptr[1] < 9 &&
398 0 <= data_ptr[2] && data_ptr[2] < 9);
399 ++histogram[data_ptr[0]];
400 ++histogram[data_ptr[1]];
401 ++histogram[data_ptr[2]];
404 unsigned char max_hist_value = 0;
405 int max_hist_index = -1;
416 if (max_hist_value < histogram[0]) {max_hist_index = 0; max_hist_value = histogram[0];}
417 if (max_hist_value < histogram[1]) {max_hist_index = 1; max_hist_value = histogram[1];}
418 if (max_hist_value < histogram[2]) {max_hist_index = 2; max_hist_value = histogram[2];}
419 if (max_hist_value < histogram[3]) {max_hist_index = 3; max_hist_value = histogram[3];}
420 if (max_hist_value < histogram[4]) {max_hist_index = 4; max_hist_value = histogram[4];}
421 if (max_hist_value < histogram[5]) {max_hist_index = 5; max_hist_value = histogram[5];}
422 if (max_hist_value < histogram[6]) {max_hist_index = 6; max_hist_value = histogram[6];}
423 if (max_hist_value < histogram[7]) {max_hist_index = 7; max_hist_value = histogram[7];}
426 filtered_quantized_colors_ (col_index, row_index) = 0x1 << max_hist_index;
435 template <
typename Po
intInT>
441 const float r_inv = 255.0f-r;
442 const float g_inv = 255.0f-g;
443 const float b_inv = 255.0f-b;
445 const float dist_0 = (r*r + g*g + b*b)*2.0f;
446 const float dist_1 = r*r + g*g + b_inv*b_inv;
447 const float dist_2 = r*r + g_inv*g_inv+ b*b;
448 const float dist_3 = r*r + g_inv*g_inv + b_inv*b_inv;
449 const float dist_4 = r_inv*r_inv + g*g + b*b;
450 const float dist_5 = r_inv*r_inv + g*g + b_inv*b_inv;
451 const float dist_6 = r_inv*r_inv + g_inv*g_inv+ b*b;
452 const float dist_7 = (r_inv*r_inv + g_inv*g_inv + b_inv*b_inv)*1.5f;
454 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)));
456 if (min_dist == dist_0)
460 if (min_dist == dist_1)
464 if (min_dist == dist_2)
468 if (min_dist == dist_3)
472 if (min_dist == dist_4)
476 if (min_dist == dist_5)
480 if (min_dist == dist_6)
488 template <
typename Po
intInT>
void
492 const std::size_t width = input.
getWidth ();
493 const std::size_t height = input.
getHeight ();
495 output.
resize (width, height);
499 const unsigned char * mask_map = input.
getData ();
500 float * distance_map = output.
getData ();
501 for (std::size_t index = 0; index < width*height; ++index)
503 if (mask_map[index] == 0)
504 distance_map[index] = 0.0f;
506 distance_map[index] =
static_cast<float> (width + height);
510 float * previous_row = distance_map;
511 float * current_row = previous_row + width;
512 for (std::size_t ri = 1; ri < height; ++ri)
514 for (std::size_t ci = 1; ci < width; ++ci)
516 const float up_left = previous_row [ci - 1] + 1.4f;
517 const float up = previous_row [ci] + 1.0f;
518 const float up_right = previous_row [ci + 1] + 1.4f;
519 const float left = current_row [ci - 1] + 1.0f;
520 const float center = current_row [ci];
522 const float min_value = std::min (std::min (up_left, up), std::min (left, up_right));
524 if (min_value < center)
525 current_row[ci] = min_value;
527 previous_row = current_row;
528 current_row += width;
532 float * next_row = distance_map + width * (height - 1);
533 current_row = next_row - width;
534 for (
int ri =
static_cast<int> (height)-2; ri >= 0; --ri)
536 for (
int ci =
static_cast<int> (width)-2; ci >= 0; --ci)
538 const float lower_left = next_row [ci - 1] + 1.4f;
539 const float lower = next_row [ci] + 1.0f;
540 const float lower_right = next_row [ci + 1] + 1.4f;
541 const float right = current_row [ci + 1] + 1.0f;
542 const float center = current_row [ci];
544 const float min_value = std::min (std::min (lower_left, lower), std::min (right, lower_right));
546 if (min_value < center)
547 current_row[ci] = min_value;
549 next_row = current_row;
550 current_row -= width;