Point Cloud Library (PCL)  1.9.1-dev
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 #pragma once
39 
40 #include <pcl/recognition/quantizable_modality.h>
41 #include <pcl/recognition/distance_map.h>
42 
43 #include <pcl/pcl_base.h>
44 #include <pcl/point_cloud.h>
45 #include <pcl/point_types.h>
46 #include <pcl/recognition/point_types.h>
47 
48 
49 namespace pcl
50 {
51 
52  // --------------------------------------------------------------------------
53 
54  template <typename PointInT>
56  : public QuantizableModality, public PCLBase<PointInT>
57  {
58  protected:
60 
61  struct Candidate
62  {
63  float distance;
64 
65  unsigned char bin_index;
66 
67  std::size_t x;
68  std::size_t y;
69 
70  bool
71  operator< (const Candidate & rhs)
72  {
73  return (distance > rhs.distance);
74  }
75  };
76 
77  public:
79 
80  ColorModality ();
81 
82  virtual ~ColorModality ();
83 
84  inline QuantizedMap &
86  {
87  return (filtered_quantized_colors_);
88  }
89 
90  inline QuantizedMap &
92  {
93  return (spreaded_filtered_quantized_colors_);
94  }
95 
96  void
97  extractFeatures (const MaskMap & mask, std::size_t nr_features, std::size_t modalityIndex,
98  std::vector<QuantizedMultiModFeature> & features) const;
99 
100  /** \brief Provide a pointer to the input dataset (overwrites the PCLBase::setInputCloud method)
101  * \param cloud the const boost shared pointer to a PointCloud message
102  */
103  virtual void
104  setInputCloud (const typename PointCloudIn::ConstPtr & cloud)
105  {
106  input_ = cloud;
107  }
108 
109  virtual void
110  processInputData ();
111 
112  protected:
113 
114  void
115  quantizeColors ();
116 
117  void
119 
120  static inline int
121  quantizeColorOnRGBExtrema (const float r,
122  const float g,
123  const float b);
124 
125  void
126  computeDistanceMap (const MaskMap & input, DistanceMap & output) const;
127 
128  private:
129  float feature_distance_threshold_;
130 
131  pcl::QuantizedMap quantized_colors_;
132  pcl::QuantizedMap filtered_quantized_colors_;
133  pcl::QuantizedMap spreaded_filtered_quantized_colors_;
134 
135  };
136 
137 }
138 
139 //////////////////////////////////////////////////////////////////////////////////////////////
140 template <typename PointInT>
142  : feature_distance_threshold_ (1.0f), quantized_colors_ (), filtered_quantized_colors_ (), spreaded_filtered_quantized_colors_ ()
143 {
144 }
145 
146 //////////////////////////////////////////////////////////////////////////////////////////////
147 template <typename PointInT>
149 {
150 }
151 
152 //////////////////////////////////////////////////////////////////////////////////////////////
153 template <typename PointInT>
154 void
156 {
157  // quantize gradients
158  quantizeColors ();
159 
160  // filter quantized gradients to get only dominants one + thresholding
162 
163  // spread filtered quantized gradients
164  //spreadFilteredQunatizedColorGradients ();
165  const int spreading_size = 8;
166  pcl::QuantizedMap::spreadQuantizedMap (filtered_quantized_colors_,
167  spreaded_filtered_quantized_colors_, spreading_size);
168 }
169 
170 //////////////////////////////////////////////////////////////////////////////////////////////
171 template <typename PointInT>
173  const std::size_t nr_features,
174  const std::size_t modality_index,
175  std::vector<QuantizedMultiModFeature> & features) const
176 {
177  const std::size_t width = mask.getWidth ();
178  const std::size_t height = mask.getHeight ();
179 
180  MaskMap mask_maps[8];
181  for (std::size_t map_index = 0; map_index < 8; ++map_index)
182  mask_maps[map_index].resize (width, height);
183 
184  unsigned char map[255];
185  memset(map, 0, 255);
186 
187  map[0x1<<0] = 0;
188  map[0x1<<1] = 1;
189  map[0x1<<2] = 2;
190  map[0x1<<3] = 3;
191  map[0x1<<4] = 4;
192  map[0x1<<5] = 5;
193  map[0x1<<6] = 6;
194  map[0x1<<7] = 7;
195 
196  QuantizedMap distance_map_indices (width, height);
197  //memset (distance_map_indices.data, 0, sizeof (distance_map_indices.data[0])*width*height);
198 
199  for (std::size_t row_index = 0; row_index < height; ++row_index)
200  {
201  for (std::size_t col_index = 0; col_index < width; ++col_index)
202  {
203  if (mask (col_index, row_index) != 0)
204  {
205  //const unsigned char quantized_value = quantized_surface_normals_ (row_index, col_index);
206  const unsigned char quantized_value = filtered_quantized_colors_ (col_index, row_index);
207 
208  if (quantized_value == 0)
209  continue;
210  const int dist_map_index = map[quantized_value];
211 
212  distance_map_indices (col_index, row_index) = dist_map_index;
213  //distance_maps[dist_map_index].at<unsigned char>(row_index, col_index) = 255;
214  mask_maps[dist_map_index] (col_index, row_index) = 255;
215  }
216  }
217  }
218 
219  DistanceMap distance_maps[8];
220  for (int map_index = 0; map_index < 8; ++map_index)
221  computeDistanceMap (mask_maps[map_index], distance_maps[map_index]);
222 
223  std::list<Candidate> list1;
224  std::list<Candidate> list2;
225 
226  float weights[8] = {0,0,0,0,0,0,0,0};
227 
228  const std::size_t off = 4;
229  for (std::size_t row_index = off; row_index < height-off; ++row_index)
230  {
231  for (std::size_t col_index = off; col_index < width-off; ++col_index)
232  {
233  if (mask (col_index, row_index) != 0)
234  {
235  //const unsigned char quantized_value = quantized_surface_normals_ (row_index, col_index);
236  const unsigned char quantized_value = filtered_quantized_colors_ (col_index, row_index);
237 
238  //const float nx = surface_normals_ (col_index, row_index).normal_x;
239  //const float ny = surface_normals_ (col_index, row_index).normal_y;
240  //const float nz = surface_normals_ (col_index, row_index).normal_z;
241 
242  if (quantized_value != 0)
243  {
244  const int distance_map_index = map[quantized_value];
245 
246  //const float distance = distance_maps[distance_map_index].at<float> (row_index, col_index);
247  const float distance = distance_maps[distance_map_index] (col_index, row_index);
248 
249  if (distance >= feature_distance_threshold_)
250  {
251  Candidate candidate;
252 
253  candidate.distance = distance;
254  candidate.x = col_index;
255  candidate.y = row_index;
256  candidate.bin_index = distance_map_index;
257 
258  list1.push_back (candidate);
259 
260  ++weights[distance_map_index];
261  }
262  }
263  }
264  }
265  }
266 
267  for (typename std::list<Candidate>::iterator iter = list1.begin (); iter != list1.end (); ++iter)
268  iter->distance *= 1.0f / weights[iter->bin_index];
269 
270  list1.sort ();
271 
272  if (list1.size () <= nr_features)
273  {
274  features.reserve (list1.size ());
275  for (typename std::list<Candidate>::iterator iter = list1.begin (); iter != list1.end (); ++iter)
276  {
277  QuantizedMultiModFeature feature;
278 
279  feature.x = static_cast<int> (iter->x);
280  feature.y = static_cast<int> (iter->y);
281  feature.modality_index = modality_index;
282  feature.quantized_value = filtered_quantized_colors_ (iter->x, iter->y);
283 
284  features.push_back (feature);
285  }
286 
287  return;
288  }
289 
290  int distance = static_cast<int> (list1.size () / nr_features + 1); // ??? @todo:!:!:!:!:!:!
291  while (list2.size () != nr_features)
292  {
293  const int sqr_distance = distance*distance;
294  for (typename std::list<Candidate>::iterator iter1 = list1.begin (); iter1 != list1.end (); ++iter1)
295  {
296  bool candidate_accepted = true;
297 
298  for (typename std::list<Candidate>::iterator iter2 = list2.begin (); iter2 != list2.end (); ++iter2)
299  {
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;
303 
304  if (tmp_distance < sqr_distance)
305  {
306  candidate_accepted = false;
307  break;
308  }
309  }
310 
311  if (candidate_accepted)
312  list2.push_back (*iter1);
313 
314  if (list2.size () == nr_features) break;
315  }
316  --distance;
317  }
318 
319  for (typename std::list<Candidate>::iterator iter2 = list2.begin (); iter2 != list2.end (); ++iter2)
320  {
321  QuantizedMultiModFeature feature;
322 
323  feature.x = static_cast<int> (iter2->x);
324  feature.y = static_cast<int> (iter2->y);
325  feature.modality_index = modality_index;
326  feature.quantized_value = filtered_quantized_colors_ (iter2->x, iter2->y);
327 
328  features.push_back (feature);
329  }
330 }
331 
332 //////////////////////////////////////////////////////////////////////////////////////////////
333 template <typename PointInT>
334 void
336 {
337  const std::size_t width = input_->width;
338  const std::size_t height = input_->height;
339 
340  quantized_colors_.resize (width, height);
341 
342  for (std::size_t row_index = 0; row_index < height; ++row_index)
343  {
344  for (std::size_t col_index = 0; col_index < width; ++col_index)
345  {
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);
349 
350  quantized_colors_ (col_index, row_index) = quantizeColorOnRGBExtrema (r, g, b);
351  }
352  }
353 }
354 
355 //////////////////////////////////////////////////////////////////////////////////////////////
356 template <typename PointInT>
357 void
359 {
360  const std::size_t width = input_->width;
361  const std::size_t height = input_->height;
362 
363  filtered_quantized_colors_.resize (width, height);
364 
365  // filter data
366  for (std::size_t row_index = 1; row_index < height-1; ++row_index)
367  {
368  for (std::size_t col_index = 1; col_index < width-1; ++col_index)
369  {
370  unsigned char histogram[8] = {0,0,0,0,0,0,0,0};
371 
372  {
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]];
380  }
381  {
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]];
389  }
390  {
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]];
398  }
399 
400  unsigned char max_hist_value = 0;
401  int max_hist_index = -1;
402 
403  // for (int i = 0; i < 8; ++i)
404  // {
405  // if (max_hist_value < histogram[i+1])
406  // {
407  // max_hist_index = i;
408  // max_hist_value = histogram[i+1]
409  // }
410  // }
411  // Unrolled for performance optimization:
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];}
420 
421  //if (max_hist_index != -1 && max_hist_value >= 5)
422  filtered_quantized_colors_ (col_index, row_index) = 0x1 << max_hist_index;
423  //else
424  // filtered_quantized_color_gradients_ (col_index, row_index) = 0;
425 
426  }
427  }
428 }
429 
430 //////////////////////////////////////////////////////////////////////////////////////////////
431 template <typename PointInT>
432 int
434  const float g,
435  const float b)
436 {
437  const float r_inv = 255.0f-r;
438  const float g_inv = 255.0f-g;
439  const float b_inv = 255.0f-b;
440 
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;
449 
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)));
451 
452  if (min_dist == dist_0)
453  {
454  return 0;
455  }
456  if (min_dist == dist_1)
457  {
458  return 1;
459  }
460  if (min_dist == dist_2)
461  {
462  return 2;
463  }
464  if (min_dist == dist_3)
465  {
466  return 3;
467  }
468  if (min_dist == dist_4)
469  {
470  return 4;
471  }
472  if (min_dist == dist_5)
473  {
474  return 5;
475  }
476  if (min_dist == dist_6)
477  {
478  return 6;
479  }
480  return 7;
481 }
482 
483 //////////////////////////////////////////////////////////////////////////////////////////////
484 template <typename PointInT> void
486  DistanceMap & output) const
487 {
488  const std::size_t width = input.getWidth ();
489  const std::size_t height = input.getHeight ();
490 
491  output.resize (width, height);
492 
493  // compute distance map
494  //float *distance_map = new float[input_->points.size ()];
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)
498  {
499  if (mask_map[index] == 0)
500  distance_map[index] = 0.0f;
501  else
502  distance_map[index] = static_cast<float> (width + height);
503  }
504 
505  // first pass
506  float * previous_row = distance_map;
507  float * current_row = previous_row + width;
508  for (std::size_t ri = 1; ri < height; ++ri)
509  {
510  for (std::size_t ci = 1; ci < width; ++ci)
511  {
512  const float up_left = previous_row [ci - 1] + 1.4f; //distance_map[(ri-1)*input_->width + ci-1] + 1.4f;
513  const float up = previous_row [ci] + 1.0f; //distance_map[(ri-1)*input_->width + ci] + 1.0f;
514  const float up_right = previous_row [ci + 1] + 1.4f; //distance_map[(ri-1)*input_->width + ci+1] + 1.4f;
515  const float left = current_row [ci - 1] + 1.0f; //distance_map[ri*input_->width + ci-1] + 1.0f;
516  const float center = current_row [ci]; //distance_map[ri*input_->width + ci];
517 
518  const float min_value = std::min (std::min (up_left, up), std::min (left, up_right));
519 
520  if (min_value < center)
521  current_row[ci] = min_value; //distance_map[ri * input_->width + ci] = min_value;
522  }
523  previous_row = current_row;
524  current_row += width;
525  }
526 
527  // second pass
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)
531  {
532  for (int ci = static_cast<int> (width)-2; ci >= 0; --ci)
533  {
534  const float lower_left = next_row [ci - 1] + 1.4f; //distance_map[(ri+1)*input_->width + ci-1] + 1.4f;
535  const float lower = next_row [ci] + 1.0f; //distance_map[(ri+1)*input_->width + ci] + 1.0f;
536  const float lower_right = next_row [ci + 1] + 1.4f; //distance_map[(ri+1)*input_->width + ci+1] + 1.4f;
537  const float right = current_row [ci + 1] + 1.0f; //distance_map[ri*input_->width + ci+1] + 1.0f;
538  const float center = current_row [ci]; //distance_map[ri*input_->width + ci];
539 
540  const float min_value = std::min (std::min (lower_left, lower), std::min (right, lower_right));
541 
542  if (min_value < center)
543  current_row[ci] = min_value; //distance_map[ri*input_->width + ci] = min_value;
544  }
545  next_row = current_row;
546  current_row -= width;
547  }
548 }
virtual void processInputData()
static void spreadQuantizedMap(const QuantizedMap &input_map, QuantizedMap &output_map, std::size_t spreading_size)
QuantizedMap & getQuantizedMap()
Returns a reference to the internally computed quantized map.
void resize(std::size_t width, std::size_t height)
std::size_t getHeight() const
Definition: mask_map.h:64
This file defines compatibility wrappers for low level I/O functions.
Definition: convolution.h:45
Feature that defines a position and quantized value in a specific modality.
std::size_t modality_index
the index of the corresponding modality.
unsigned char quantized_value
the quantized value attached to the feature.
Represents a distance map obtained from a distance transformation.
Definition: distance_map.h:46
unsigned char * getData()
Definition: mask_map.h:70
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) ...
void resize(const std::size_t width, const std::size_t height)
Resizes the map to the specified size.
Definition: distance_map.h:80
PCL base class.
Definition: pcl_base.h:69
void computeDistanceMap(const MaskMap &input, DistanceMap &output) const
float distance(const PointT &p1, const PointT &p2)
Definition: geometry.h:60
Interface for a quantizable modality.
static int quantizeColorOnRGBExtrema(const float r, const float g, const float b)
void extractFeatures(const MaskMap &mask, std::size_t nr_features, std::size_t modalityIndex, std::vector< QuantizedMultiModFeature > &features) const
Extracts features from this modality within the specified mask.
std::size_t getWidth() const
Definition: mask_map.h:58
unsigned char * getData()
Definition: quantized_map.h:62
boost::shared_ptr< const PointCloud< PointInT > > ConstPtr
Definition: point_cloud.h:412
bool operator<(const Candidate &rhs)
PointCloudConstPtr input_
The input point cloud dataset.
Definition: pcl_base.h:151
float * getData()
Returns a pointer to the beginning of map.
Definition: distance_map.h:70
QuantizedMap & getSpreadedQuantizedMap()
Returns a reference to the internally computed spread quantized map.
virtual ~ColorModality()