Point Cloud Library (PCL)  1.7.0
color_modality.h
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Point Cloud Library (PCL) - www.pointclouds.org
5  * Copyright (c) 2010-2011, Willow Garage, Inc.
6  *
7  * All rights reserved.
8  *
9  * Redistribution and use in source and binary forms, with or without
10  * modification, are permitted provided that the following conditions
11  * are met:
12  *
13  * * Redistributions of source code must retain the above copyright
14  * notice, this list of conditions and the following disclaimer.
15  * * Redistributions in binary form must reproduce the above
16  * copyright notice, this list of conditions and the following
17  * disclaimer in the documentation and/or other materials provided
18  * with the distribution.
19  * * Neither the name of Willow Garage, Inc. nor the names of its
20  * contributors may be used to endorse or promote products derived
21  * from this software without specific prior written permission.
22  *
23  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
24  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
25  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
26  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
27  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
28  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
29  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
30  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
31  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
32  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
33  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
34  * POSSIBILITY OF SUCH DAMAGE.
35  *
36  */
37 
38 #ifndef PCL_RECOGNITION_COLOR_MODALITY
39 #define PCL_RECOGNITION_COLOR_MODALITY
40 
41 #include <pcl/recognition/quantizable_modality.h>
42 #include <pcl/recognition/distance_map.h>
43 
44 #include <pcl/pcl_base.h>
45 #include <pcl/point_cloud.h>
46 #include <pcl/point_types.h>
47 #include <pcl/recognition/point_types.h>
48 
49 
50 namespace pcl
51 {
52 
53  // --------------------------------------------------------------------------
54 
55  template <typename PointInT>
57  : public QuantizableModality, public PCLBase<PointInT>
58  {
59  protected:
61 
62  struct Candidate
63  {
64  float distance;
65 
66  unsigned char bin_index;
67 
68  size_t x;
69  size_t y;
70 
71  bool
72  operator< (const Candidate & rhs)
73  {
74  return (distance > rhs.distance);
75  }
76  };
77 
78  public:
80 
81  ColorModality ();
82 
83  virtual ~ColorModality ();
84 
85  inline QuantizedMap &
87  {
88  return (filtered_quantized_colors_);
89  }
90 
91  inline QuantizedMap &
93  {
94  return (spreaded_filtered_quantized_colors_);
95  }
96 
97  void
98  extractFeatures (const MaskMap & mask, size_t nr_features, size_t modalityIndex,
99  std::vector<QuantizedMultiModFeature> & features) const;
100 
101  /** \brief Provide a pointer to the input dataset (overwrites the PCLBase::setInputCloud method)
102  * \param cloud the const boost shared pointer to a PointCloud message
103  */
104  virtual void
105  setInputCloud (const typename PointCloudIn::ConstPtr & cloud)
106  {
107  input_ = cloud;
108  }
109 
110  virtual void
111  processInputData ();
112 
113  protected:
114 
115  void
116  quantizeColors ();
117 
118  void
120 
121  static inline int
122  quantizeColorOnRGBExtrema (const float r,
123  const float g,
124  const float b);
125 
126  void
127  computeDistanceMap (const MaskMap & input, DistanceMap & output) const;
128 
129  private:
130  float feature_distance_threshold_;
131 
132  pcl::QuantizedMap quantized_colors_;
133  pcl::QuantizedMap filtered_quantized_colors_;
134  pcl::QuantizedMap spreaded_filtered_quantized_colors_;
135 
136  };
137 
138 }
139 
140 //////////////////////////////////////////////////////////////////////////////////////////////
141 template <typename PointInT>
143  : feature_distance_threshold_ (1.0f), quantized_colors_ (), filtered_quantized_colors_ (), spreaded_filtered_quantized_colors_ ()
144 {
145 }
146 
147 //////////////////////////////////////////////////////////////////////////////////////////////
148 template <typename PointInT>
150 {
151 }
152 
153 //////////////////////////////////////////////////////////////////////////////////////////////
154 template <typename PointInT>
155 void
157 {
158  // quantize gradients
159  quantizeColors ();
160 
161  // filter quantized gradients to get only dominants one + thresholding
162  filterQuantizedColors ();
163 
164  // spread filtered quantized gradients
165  //spreadFilteredQunatizedColorGradients ();
166  const int spreading_size = 8;
167  pcl::QuantizedMap::spreadQuantizedMap (filtered_quantized_colors_,
168  spreaded_filtered_quantized_colors_, spreading_size);
169 }
170 
171 //////////////////////////////////////////////////////////////////////////////////////////////
172 template <typename PointInT>
174  const size_t nr_features,
175  const size_t modality_index,
176  std::vector<QuantizedMultiModFeature> & features) const
177 {
178  const size_t width = mask.getWidth ();
179  const size_t height = mask.getHeight ();
180 
181  MaskMap mask_maps[8];
182  for (size_t map_index = 0; map_index < 8; ++map_index)
183  mask_maps[map_index].resize (width, height);
184 
185  unsigned char map[255];
186  memset(map, 0, 255);
187 
188  map[0x1<<0] = 0;
189  map[0x1<<1] = 1;
190  map[0x1<<2] = 2;
191  map[0x1<<3] = 3;
192  map[0x1<<4] = 4;
193  map[0x1<<5] = 5;
194  map[0x1<<6] = 6;
195  map[0x1<<7] = 7;
196 
197  QuantizedMap distance_map_indices (width, height);
198  //memset (distance_map_indices.data, 0, sizeof (distance_map_indices.data[0])*width*height);
199 
200  for (size_t row_index = 0; row_index < height; ++row_index)
201  {
202  for (size_t col_index = 0; col_index < width; ++col_index)
203  {
204  if (mask (col_index, row_index) != 0)
205  {
206  //const unsigned char quantized_value = quantized_surface_normals_ (row_index, col_index);
207  const unsigned char quantized_value = filtered_quantized_colors_ (col_index, row_index);
208 
209  if (quantized_value == 0)
210  continue;
211  const int dist_map_index = map[quantized_value];
212 
213  distance_map_indices (col_index, row_index) = dist_map_index;
214  //distance_maps[dist_map_index].at<unsigned char>(row_index, col_index) = 255;
215  mask_maps[dist_map_index] (col_index, row_index) = 255;
216  }
217  }
218  }
219 
220  DistanceMap distance_maps[8];
221  for (int map_index = 0; map_index < 8; ++map_index)
222  computeDistanceMap (mask_maps[map_index], distance_maps[map_index]);
223 
224  std::list<Candidate> list1;
225  std::list<Candidate> list2;
226 
227  float weights[8] = {0,0,0,0,0,0,0,0};
228 
229  const size_t off = 4;
230  for (size_t row_index = off; row_index < height-off; ++row_index)
231  {
232  for (size_t col_index = off; col_index < width-off; ++col_index)
233  {
234  if (mask (col_index, row_index) != 0)
235  {
236  //const unsigned char quantized_value = quantized_surface_normals_ (row_index, col_index);
237  const unsigned char quantized_value = filtered_quantized_colors_ (col_index, row_index);
238 
239  //const float nx = surface_normals_ (col_index, row_index).normal_x;
240  //const float ny = surface_normals_ (col_index, row_index).normal_y;
241  //const float nz = surface_normals_ (col_index, row_index).normal_z;
242 
243  if (quantized_value != 0)
244  {
245  const int distance_map_index = map[quantized_value];
246 
247  //const float distance = distance_maps[distance_map_index].at<float> (row_index, col_index);
248  const float distance = distance_maps[distance_map_index] (col_index, row_index);
249 
250  if (distance >= feature_distance_threshold_)
251  {
252  Candidate candidate;
253 
254  candidate.distance = distance;
255  candidate.x = col_index;
256  candidate.y = row_index;
257  candidate.bin_index = distance_map_index;
258 
259  list1.push_back (candidate);
260 
261  ++weights[distance_map_index];
262  }
263  }
264  }
265  }
266  }
267 
268  for (typename std::list<Candidate>::iterator iter = list1.begin (); iter != list1.end (); ++iter)
269  iter->distance *= 1.0f / weights[iter->bin_index];
270 
271  list1.sort ();
272 
273  if (list1.size () <= nr_features)
274  {
275  features.reserve (list1.size ());
276  for (typename std::list<Candidate>::iterator iter = list1.begin (); iter != list1.end (); ++iter)
277  {
278  QuantizedMultiModFeature feature;
279 
280  feature.x = static_cast<int> (iter->x);
281  feature.y = static_cast<int> (iter->y);
282  feature.modality_index = modality_index;
283  feature.quantized_value = filtered_quantized_colors_ (iter->x, iter->y);
284 
285  features.push_back (feature);
286  }
287 
288  return;
289  }
290 
291  int distance = static_cast<int> (list1.size () / nr_features + 1); // ??? @todo:!:!:!:!:!:!
292  while (list2.size () != nr_features)
293  {
294  const int sqr_distance = distance*distance;
295  for (typename std::list<Candidate>::iterator iter1 = list1.begin (); iter1 != list1.end (); ++iter1)
296  {
297  bool candidate_accepted = true;
298 
299  for (typename std::list<Candidate>::iterator iter2 = list2.begin (); iter2 != list2.end (); ++iter2)
300  {
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;
304 
305  if (tmp_distance < sqr_distance)
306  {
307  candidate_accepted = false;
308  break;
309  }
310  }
311 
312  if (candidate_accepted)
313  list2.push_back (*iter1);
314 
315  if (list2.size () == nr_features) break;
316  }
317  --distance;
318  }
319 
320  for (typename std::list<Candidate>::iterator iter2 = list2.begin (); iter2 != list2.end (); ++iter2)
321  {
322  QuantizedMultiModFeature feature;
323 
324  feature.x = static_cast<int> (iter2->x);
325  feature.y = static_cast<int> (iter2->y);
326  feature.modality_index = modality_index;
327  feature.quantized_value = filtered_quantized_colors_ (iter2->x, iter2->y);
328 
329  features.push_back (feature);
330  }
331 }
332 
333 //////////////////////////////////////////////////////////////////////////////////////////////
334 template <typename PointInT>
335 void
337 {
338  const size_t width = input_->width;
339  const size_t height = input_->height;
340 
341  quantized_colors_.resize (width, height);
342 
343  for (size_t row_index = 0; row_index < height; ++row_index)
344  {
345  for (size_t col_index = 0; col_index < width; ++col_index)
346  {
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);
350 
351  quantized_colors_ (col_index, row_index) = quantizeColorOnRGBExtrema (r, g, b);
352  }
353  }
354 }
355 
356 //////////////////////////////////////////////////////////////////////////////////////////////
357 template <typename PointInT>
358 void
360 {
361  const size_t width = input_->width;
362  const size_t height = input_->height;
363 
364  filtered_quantized_colors_.resize (width, height);
365 
366  // filter data
367  for (size_t row_index = 1; row_index < height-1; ++row_index)
368  {
369  for (size_t col_index = 1; col_index < width-1; ++col_index)
370  {
371  unsigned char histogram[8] = {0,0,0,0,0,0,0,0};
372 
373  {
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]];
381  }
382  {
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]];
390  }
391  {
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]];
399  }
400 
401  unsigned char max_hist_value = 0;
402  int max_hist_index = -1;
403 
404  // for (int i = 0; i < 8; ++i)
405  // {
406  // if (max_hist_value < histogram[i+1])
407  // {
408  // max_hist_index = i;
409  // max_hist_value = histogram[i+1]
410  // }
411  // }
412  // Unrolled for performance optimization:
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];}
421 
422  //if (max_hist_index != -1 && max_hist_value >= 5)
423  filtered_quantized_colors_ (col_index, row_index) = 0x1 << max_hist_index;
424  //else
425  // filtered_quantized_color_gradients_ (col_index, row_index) = 0;
426 
427  }
428  }
429 }
430 
431 //////////////////////////////////////////////////////////////////////////////////////////////
432 template <typename PointInT>
433 int
435  const float g,
436  const float b)
437 {
438  const float r_inv = 255.0f-r;
439  const float g_inv = 255.0f-g;
440  const float b_inv = 255.0f-b;
441 
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;
450 
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)));
452 
453  if (min_dist == dist_0)
454  {
455  return 0;
456  }
457  if (min_dist == dist_1)
458  {
459  return 1;
460  }
461  if (min_dist == dist_2)
462  {
463  return 2;
464  }
465  if (min_dist == dist_3)
466  {
467  return 3;
468  }
469  if (min_dist == dist_4)
470  {
471  return 4;
472  }
473  if (min_dist == dist_5)
474  {
475  return 5;
476  }
477  if (min_dist == dist_6)
478  {
479  return 6;
480  }
481  return 7;
482 }
483 
484 //////////////////////////////////////////////////////////////////////////////////////////////
485 template <typename PointInT> void
487  DistanceMap & output) const
488 {
489  const size_t width = input.getWidth ();
490  const size_t height = input.getHeight ();
491 
492  output.resize (width, height);
493 
494  // compute distance map
495  //float *distance_map = new float[input_->points.size ()];
496  const unsigned char * mask_map = input.getData ();
497  float * distance_map = output.getData ();
498  for (size_t index = 0; index < width*height; ++index)
499  {
500  if (mask_map[index] == 0)
501  distance_map[index] = 0.0f;
502  else
503  distance_map[index] = static_cast<float> (width + height);
504  }
505 
506  // first pass
507  float * previous_row = distance_map;
508  float * current_row = previous_row + width;
509  for (size_t ri = 1; ri < height; ++ri)
510  {
511  for (size_t ci = 1; ci < width; ++ci)
512  {
513  const float up_left = previous_row [ci - 1] + 1.4f; //distance_map[(ri-1)*input_->width + ci-1] + 1.4f;
514  const float up = previous_row [ci] + 1.0f; //distance_map[(ri-1)*input_->width + ci] + 1.0f;
515  const float up_right = previous_row [ci + 1] + 1.4f; //distance_map[(ri-1)*input_->width + ci+1] + 1.4f;
516  const float left = current_row [ci - 1] + 1.0f; //distance_map[ri*input_->width + ci-1] + 1.0f;
517  const float center = current_row [ci]; //distance_map[ri*input_->width + ci];
518 
519  const float min_value = std::min (std::min (up_left, up), std::min (left, up_right));
520 
521  if (min_value < center)
522  current_row[ci] = min_value; //distance_map[ri * input_->width + ci] = min_value;
523  }
524  previous_row = current_row;
525  current_row += width;
526  }
527 
528  // second pass
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)
532  {
533  for (int ci = static_cast<int> (width)-2; ci >= 0; --ci)
534  {
535  const float lower_left = next_row [ci - 1] + 1.4f; //distance_map[(ri+1)*input_->width + ci-1] + 1.4f;
536  const float lower = next_row [ci] + 1.0f; //distance_map[(ri+1)*input_->width + ci] + 1.0f;
537  const float lower_right = next_row [ci + 1] + 1.4f; //distance_map[(ri+1)*input_->width + ci+1] + 1.4f;
538  const float right = current_row [ci + 1] + 1.0f; //distance_map[ri*input_->width + ci+1] + 1.0f;
539  const float center = current_row [ci]; //distance_map[ri*input_->width + ci];
540 
541  const float min_value = std::min (std::min (lower_left, lower), std::min (right, lower_right));
542 
543  if (min_value < center)
544  current_row[ci] = min_value; //distance_map[ri*input_->width + ci] = min_value;
545  }
546  next_row = current_row;
547  current_row -= width;
548  }
549 }
550 
551 
552 #endif