Point Cloud Library (PCL)  1.8.1
color_gradient_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_GRADIENT_MODALITY
39 #define PCL_RECOGNITION_COLOR_GRADIENT_MODALITY
40 
41 #include <pcl/recognition/quantizable_modality.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 #include <pcl/filters/convolution.h>
48 
49 #include <list>
50 
51 namespace pcl
52 {
53 
54  /** \brief Modality based on max-RGB gradients.
55  * \author Stefan Holzer
56  */
57  template <typename PointInT>
59  : public QuantizableModality, public PCLBase<PointInT>
60  {
61  protected:
63 
64  /** \brief Candidate for a feature (used in feature extraction methods). */
65  struct Candidate
66  {
67  /** \brief The gradient. */
69 
70  /** \brief The x-position. */
71  int x;
72  /** \brief The y-position. */
73  int y;
74 
75  /** \brief Operator for comparing to candidates (by magnitude of the gradient).
76  * \param[in] rhs the candidate to compare with.
77  */
78  bool operator< (const Candidate & rhs) const
79  {
80  return (gradient.magnitude > rhs.gradient.magnitude);
81  }
82  };
83 
84  public:
86 
87  /** \brief Different methods for feature selection/extraction. */
89  {
91  MASK_BORDER_EQUALLY, // this gives templates most equally to the OpenCV implementation
93  };
94 
95  /** \brief Constructor. */
97  /** \brief Destructor. */
98  virtual ~ColorGradientModality ();
99 
100  /** \brief Sets the threshold for the gradient magnitude which is used when quantizing the data.
101  * Gradients with a smaller magnitude are ignored.
102  * \param[in] threshold the new gradient magnitude threshold.
103  */
104  inline void
105  setGradientMagnitudeThreshold (const float threshold)
106  {
107  gradient_magnitude_threshold_ = threshold;
108  }
109 
110  /** \brief Sets the threshold for the gradient magnitude which is used for feature extraction.
111  * Gradients with a smaller magnitude are ignored.
112  * \param[in] threshold the new gradient magnitude threshold.
113  */
114  inline void
116  {
117  gradient_magnitude_threshold_feature_extraction_ = threshold;
118  }
119 
120  /** \brief Sets the feature selection method.
121  * \param[in] method the feature selection method.
122  */
123  inline void
125  {
126  feature_selection_method_ = method;
127  }
128 
129  /** \brief Sets the spreading size for spreading the quantized data. */
130  inline void
131  setSpreadingSize (const size_t spreading_size)
132  {
133  spreading_size_ = spreading_size;
134  }
135 
136  /** \brief Sets whether variable feature numbers for feature extraction is enabled.
137  * \param[in] enabled enables/disables variable feature numbers for feature extraction.
138  */
139  inline void
140  setVariableFeatureNr (const bool enabled)
141  {
142  variable_feature_nr_ = enabled;
143  }
144 
145  /** \brief Returns a reference to the internally computed quantized map. */
146  inline QuantizedMap &
148  {
149  return (filtered_quantized_color_gradients_);
150  }
151 
152  /** \brief Returns a reference to the internally computed spreaded quantized map. */
153  inline QuantizedMap &
155  {
156  return (spreaded_filtered_quantized_color_gradients_);
157  }
158 
159  /** \brief Returns a point cloud containing the max-RGB gradients. */
162  {
163  return (color_gradients_);
164  }
165 
166  /** \brief Extracts features from this modality within the specified mask.
167  * \param[in] mask defines the areas where features are searched in.
168  * \param[in] nr_features defines the number of features to be extracted
169  * (might be less if not sufficient information is present in the modality).
170  * \param[in] modalityIndex the index which is stored in the extracted features.
171  * \param[out] features the destination for the extracted features.
172  */
173  void
174  extractFeatures (const MaskMap & mask, size_t nr_features, size_t modalityIndex,
175  std::vector<QuantizedMultiModFeature> & features) const;
176 
177  /** \brief Extracts all possible features from the modality within the specified mask.
178  * \param[in] mask defines the areas where features are searched in.
179  * \param[in] nr_features IGNORED (TODO: remove this parameter).
180  * \param[in] modalityIndex the index which is stored in the extracted features.
181  * \param[out] features the destination for the extracted features.
182  */
183  void
184  extractAllFeatures (const MaskMap & mask, size_t nr_features, size_t modalityIndex,
185  std::vector<QuantizedMultiModFeature> & features) const;
186 
187  /** \brief Provide a pointer to the input dataset (overwrites the PCLBase::setInputCloud method)
188  * \param cloud the const boost shared pointer to a PointCloud message
189  */
190  virtual void
191  setInputCloud (const typename PointCloudIn::ConstPtr & cloud)
192  {
193  input_ = cloud;
194  }
195 
196  /** \brief Processes the input data (smoothing, computing gradients, quantizing, filtering, spreading). */
197  virtual void
198  processInputData ();
199 
200  /** \brief Processes the input data assuming that everything up to filtering is already done/available
201  * (so only spreading is performed). */
202  virtual void
204 
205  protected:
206 
207  /** \brief Computes the Gaussian kernel used for smoothing.
208  * \param[in] kernel_size the size of the Gaussian kernel.
209  * \param[in] sigma the sigma.
210  * \param[out] kernel_values the destination for the values of the kernel. */
211  void
212  computeGaussianKernel (const size_t kernel_size, const float sigma, std::vector <float> & kernel_values);
213 
214  /** \brief Computes the max-RGB gradients for the specified cloud.
215  * \param[in] cloud the cloud for which the gradients are computed.
216  */
217  void
219 
220  /** \brief Computes the max-RGB gradients for the specified cloud using sobel.
221  * \param[in] cloud the cloud for which the gradients are computed.
222  */
223  void
225 
226  /** \brief Quantizes the color gradients. */
227  void
229 
230  /** \brief Filters the quantized gradients. */
231  void
233 
234  /** \brief Erodes a mask.
235  * \param[in] mask_in the mask which will be eroded.
236  * \param[out] mask_out the destination for the eroded mask.
237  */
238  static void
239  erode (const pcl::MaskMap & mask_in, pcl::MaskMap & mask_out);
240 
241  private:
242 
243  /** \brief Determines whether variable numbers of features are extracted or not. */
244  bool variable_feature_nr_;
245 
246  /** \brief Stores a smoothed verion of the input cloud. */
247  pcl::PointCloud<pcl::RGB>::Ptr smoothed_input_;
248 
249  /** \brief Defines which feature selection method is used. */
250  FeatureSelectionMethod feature_selection_method_;
251 
252  /** \brief The threshold applied on the gradient magnitudes (for quantization). */
253  float gradient_magnitude_threshold_;
254  /** \brief The threshold applied on the gradient magnitudes for feature extraction. */
255  float gradient_magnitude_threshold_feature_extraction_;
256 
257  /** \brief The point cloud which holds the max-RGB gradients. */
258  pcl::PointCloud<pcl::GradientXY> color_gradients_;
259 
260  /** \brief The spreading size. */
261  size_t spreading_size_;
262 
263  /** \brief The map which holds the quantized max-RGB gradients. */
264  pcl::QuantizedMap quantized_color_gradients_;
265  /** \brief The map which holds the filtered quantized data. */
266  pcl::QuantizedMap filtered_quantized_color_gradients_;
267  /** \brief The map which holds the spreaded quantized data. */
268  pcl::QuantizedMap spreaded_filtered_quantized_color_gradients_;
269 
270  };
271 
272 }
273 
274 //////////////////////////////////////////////////////////////////////////////////////////////
275 template <typename PointInT>
278  : variable_feature_nr_ (false)
279  , smoothed_input_ (new pcl::PointCloud<pcl::RGB> ())
280  , feature_selection_method_ (DISTANCE_MAGNITUDE_SCORE)
281  , gradient_magnitude_threshold_ (10.0f)
282  , gradient_magnitude_threshold_feature_extraction_ (55.0f)
283  , color_gradients_ ()
284  , spreading_size_ (8)
285  , quantized_color_gradients_ ()
286  , filtered_quantized_color_gradients_ ()
287  , spreaded_filtered_quantized_color_gradients_ ()
288 {
289 }
290 
291 //////////////////////////////////////////////////////////////////////////////////////////////
292 template <typename PointInT>
295 {
296 }
297 
298 //////////////////////////////////////////////////////////////////////////////////////////////
299 template <typename PointInT> void
301 computeGaussianKernel (const size_t kernel_size, const float sigma, std::vector <float> & kernel_values)
302 {
303  // code taken from OpenCV
304  const int n = int (kernel_size);
305  const int SMALL_GAUSSIAN_SIZE = 7;
306  static const float small_gaussian_tab[][SMALL_GAUSSIAN_SIZE] =
307  {
308  {1.f},
309  {0.25f, 0.5f, 0.25f},
310  {0.0625f, 0.25f, 0.375f, 0.25f, 0.0625f},
311  {0.03125f, 0.109375f, 0.21875f, 0.28125f, 0.21875f, 0.109375f, 0.03125f}
312  };
313 
314  const float* fixed_kernel = n % 2 == 1 && n <= SMALL_GAUSSIAN_SIZE && sigma <= 0 ?
315  small_gaussian_tab[n>>1] : 0;
316 
317  //CV_Assert( ktype == CV_32F || ktype == CV_64F );
318  /*Mat kernel(n, 1, ktype);*/
319  kernel_values.resize (n);
320  float* cf = &(kernel_values[0]);
321  //double* cd = (double*)kernel.data;
322 
323  double sigmaX = sigma > 0 ? sigma : ((n-1)*0.5 - 1)*0.3 + 0.8;
324  double scale2X = -0.5/(sigmaX*sigmaX);
325  double sum = 0;
326 
327  int i;
328  for( i = 0; i < n; i++ )
329  {
330  double x = i - (n-1)*0.5;
331  double t = fixed_kernel ? double (fixed_kernel[i]) : std::exp (scale2X*x*x);
332 
333  cf[i] = float (t);
334  sum += cf[i];
335  }
336 
337  sum = 1./sum;
338  for (i = 0; i < n; i++ )
339  {
340  cf[i] = float (cf[i]*sum);
341  }
342 }
343 
344 //////////////////////////////////////////////////////////////////////////////////////////////
345 template <typename PointInT>
346 void
349 {
350  // compute gaussian kernel values
351  const size_t kernel_size = 7;
352  std::vector<float> kernel_values;
353  computeGaussianKernel (kernel_size, 0.0f, kernel_values);
354 
355  // smooth input
357  Eigen::ArrayXf gaussian_kernel(kernel_size);
358  //gaussian_kernel << 1.f/16, 1.f/8, 3.f/16, 2.f/8, 3.f/16, 1.f/8, 1.f/16;
359  //gaussian_kernel << 16.f/1600.f, 32.f/1600.f, 64.f/1600.f, 128.f/1600.f, 256.f/1600.f, 128.f/1600.f, 64.f/1600.f, 32.f/1600.f, 16.f/1600.f;
360  gaussian_kernel << kernel_values[0], kernel_values[1], kernel_values[2], kernel_values[3], kernel_values[4], kernel_values[5], kernel_values[6];
361 
363 
364  const uint32_t width = input_->width;
365  const uint32_t height = input_->height;
366 
367  rgb_input_->resize (width*height);
368  rgb_input_->width = width;
369  rgb_input_->height = height;
370  rgb_input_->is_dense = input_->is_dense;
371  for (size_t row_index = 0; row_index < height; ++row_index)
372  {
373  for (size_t col_index = 0; col_index < width; ++col_index)
374  {
375  (*rgb_input_) (col_index, row_index).r = (*input_) (col_index, row_index).r;
376  (*rgb_input_) (col_index, row_index).g = (*input_) (col_index, row_index).g;
377  (*rgb_input_) (col_index, row_index).b = (*input_) (col_index, row_index).b;
378  }
379  }
380 
381  convolution.setInputCloud (rgb_input_);
382  convolution.setKernel (gaussian_kernel);
383 
384  convolution.convolve (*smoothed_input_);
385 
386  // extract color gradients
387  computeMaxColorGradientsSobel (smoothed_input_);
388 
389  // quantize gradients
390  quantizeColorGradients ();
391 
392  // filter quantized gradients to get only dominants one + thresholding
393  filterQuantizedColorGradients ();
394 
395  // spread filtered quantized gradients
396  //spreadFilteredQunatizedColorGradients ();
397  pcl::QuantizedMap::spreadQuantizedMap (filtered_quantized_color_gradients_,
398  spreaded_filtered_quantized_color_gradients_,
399  spreading_size_);
400 }
401 
402 //////////////////////////////////////////////////////////////////////////////////////////////
403 template <typename PointInT>
404 void
407 {
408  // spread filtered quantized gradients
409  //spreadFilteredQunatizedColorGradients ();
410  pcl::QuantizedMap::spreadQuantizedMap (filtered_quantized_color_gradients_,
411  spreaded_filtered_quantized_color_gradients_,
412  spreading_size_);
413 }
414 
415 //////////////////////////////////////////////////////////////////////////////////////////////
416 template <typename PointInT>
418 extractFeatures (const MaskMap & mask, const size_t nr_features, const size_t modality_index,
419  std::vector<QuantizedMultiModFeature> & features) const
420 {
421  const size_t width = mask.getWidth ();
422  const size_t height = mask.getHeight ();
423 
424  std::list<Candidate> list1;
425  std::list<Candidate> list2;
426 
427 
428  if (feature_selection_method_ == DISTANCE_MAGNITUDE_SCORE)
429  {
430  for (size_t row_index = 0; row_index < height; ++row_index)
431  {
432  for (size_t col_index = 0; col_index < width; ++col_index)
433  {
434  if (mask (col_index, row_index) != 0)
435  {
436  const GradientXY & gradient = color_gradients_ (col_index, row_index);
437  if (gradient.magnitude > gradient_magnitude_threshold_feature_extraction_
438  && filtered_quantized_color_gradients_ (col_index, row_index) != 0)
439  {
440  Candidate candidate;
441  candidate.gradient = gradient;
442  candidate.x = static_cast<int> (col_index);
443  candidate.y = static_cast<int> (row_index);
444 
445  list1.push_back (candidate);
446  }
447  }
448  }
449  }
450 
451  list1.sort();
452 
453  if (variable_feature_nr_)
454  {
455  list2.push_back (*(list1.begin ()));
456  //while (list2.size () != nr_features)
457  bool feature_selection_finished = false;
458  while (!feature_selection_finished)
459  {
460  float best_score = 0.0f;
461  typename std::list<Candidate>::iterator best_iter = list1.end ();
462  for (typename std::list<Candidate>::iterator iter1 = list1.begin (); iter1 != list1.end (); ++iter1)
463  {
464  // find smallest distance
465  float smallest_distance = std::numeric_limits<float>::max ();
466  for (typename std::list<Candidate>::iterator iter2 = list2.begin (); iter2 != list2.end (); ++iter2)
467  {
468  const float dx = static_cast<float> (iter1->x) - static_cast<float> (iter2->x);
469  const float dy = static_cast<float> (iter1->y) - static_cast<float> (iter2->y);
470 
471  const float distance = dx*dx + dy*dy;
472 
473  if (distance < smallest_distance)
474  {
475  smallest_distance = distance;
476  }
477  }
478 
479  const float score = smallest_distance * iter1->gradient.magnitude;
480 
481  if (score > best_score)
482  {
483  best_score = score;
484  best_iter = iter1;
485  }
486  }
487 
488 
489  float min_min_sqr_distance = std::numeric_limits<float>::max ();
490  float max_min_sqr_distance = 0;
491  for (typename std::list<Candidate>::iterator iter2 = list2.begin (); iter2 != list2.end (); ++iter2)
492  {
493  float min_sqr_distance = std::numeric_limits<float>::max ();
494  for (typename std::list<Candidate>::iterator iter3 = list2.begin (); iter3 != list2.end (); ++iter3)
495  {
496  if (iter2 == iter3)
497  continue;
498 
499  const float dx = static_cast<float> (iter2->x) - static_cast<float> (iter3->x);
500  const float dy = static_cast<float> (iter2->y) - static_cast<float> (iter3->y);
501 
502  const float sqr_distance = dx*dx + dy*dy;
503 
504  if (sqr_distance < min_sqr_distance)
505  {
506  min_sqr_distance = sqr_distance;
507  }
508 
509  //std::cerr << min_sqr_distance;
510  }
511  //std::cerr << std::endl;
512 
513  // check current feature
514  {
515  const float dx = static_cast<float> (iter2->x) - static_cast<float> (best_iter->x);
516  const float dy = static_cast<float> (iter2->y) - static_cast<float> (best_iter->y);
517 
518  const float sqr_distance = dx*dx + dy*dy;
519 
520  if (sqr_distance < min_sqr_distance)
521  {
522  min_sqr_distance = sqr_distance;
523  }
524  }
525 
526  if (min_sqr_distance < min_min_sqr_distance)
527  min_min_sqr_distance = min_sqr_distance;
528  if (min_sqr_distance > max_min_sqr_distance)
529  max_min_sqr_distance = min_sqr_distance;
530 
531  //std::cerr << min_sqr_distance << ", " << min_min_sqr_distance << ", " << max_min_sqr_distance << std::endl;
532  }
533 
534  if (best_iter != list1.end ())
535  {
536  //std::cerr << "feature_index: " << list2.size () << std::endl;
537  //std::cerr << "min_min_sqr_distance: " << min_min_sqr_distance << std::endl;
538  //std::cerr << "max_min_sqr_distance: " << max_min_sqr_distance << std::endl;
539 
540  if (min_min_sqr_distance < 50)
541  {
542  feature_selection_finished = true;
543  break;
544  }
545 
546  list2.push_back (*best_iter);
547  }
548  }
549  }
550  else
551  {
552  if (list1.size () <= nr_features)
553  {
554  for (typename std::list<Candidate>::iterator iter1 = list1.begin (); iter1 != list1.end (); ++iter1)
555  {
556  QuantizedMultiModFeature feature;
557 
558  feature.x = iter1->x;
559  feature.y = iter1->y;
560  feature.modality_index = modality_index;
561  feature.quantized_value = filtered_quantized_color_gradients_ (iter1->x, iter1->y);
562 
563  features.push_back (feature);
564  }
565  return;
566  }
567 
568  list2.push_back (*(list1.begin ()));
569  while (list2.size () != nr_features)
570  {
571  float best_score = 0.0f;
572  typename std::list<Candidate>::iterator best_iter = list1.end ();
573  for (typename std::list<Candidate>::iterator iter1 = list1.begin (); iter1 != list1.end (); ++iter1)
574  {
575  // find smallest distance
576  float smallest_distance = std::numeric_limits<float>::max ();
577  for (typename std::list<Candidate>::iterator iter2 = list2.begin (); iter2 != list2.end (); ++iter2)
578  {
579  const float dx = static_cast<float> (iter1->x) - static_cast<float> (iter2->x);
580  const float dy = static_cast<float> (iter1->y) - static_cast<float> (iter2->y);
581 
582  const float distance = dx*dx + dy*dy;
583 
584  if (distance < smallest_distance)
585  {
586  smallest_distance = distance;
587  }
588  }
589 
590  const float score = smallest_distance * iter1->gradient.magnitude;
591 
592  if (score > best_score)
593  {
594  best_score = score;
595  best_iter = iter1;
596  }
597  }
598 
599  if (best_iter != list1.end ())
600  {
601  list2.push_back (*best_iter);
602  }
603  else
604  {
605  break;
606  }
607  }
608  }
609  }
610  else if (feature_selection_method_ == MASK_BORDER_HIGH_GRADIENTS || feature_selection_method_ == MASK_BORDER_EQUALLY)
611  {
612  MaskMap eroded_mask;
613  erode (mask, eroded_mask);
614 
615  MaskMap diff_mask;
616  MaskMap::getDifferenceMask (mask, eroded_mask, diff_mask);
617 
618  for (size_t row_index = 0; row_index < height; ++row_index)
619  {
620  for (size_t col_index = 0; col_index < width; ++col_index)
621  {
622  if (diff_mask (col_index, row_index) != 0)
623  {
624  const GradientXY & gradient = color_gradients_ (col_index, row_index);
625  if ((feature_selection_method_ == MASK_BORDER_EQUALLY || gradient.magnitude > gradient_magnitude_threshold_feature_extraction_)
626  && filtered_quantized_color_gradients_ (col_index, row_index) != 0)
627  {
628  Candidate candidate;
629  candidate.gradient = gradient;
630  candidate.x = static_cast<int> (col_index);
631  candidate.y = static_cast<int> (row_index);
632 
633  list1.push_back (candidate);
634  }
635  }
636  }
637  }
638 
639  list1.sort();
640 
641  if (list1.size () <= nr_features)
642  {
643  for (typename std::list<Candidate>::iterator iter1 = list1.begin (); iter1 != list1.end (); ++iter1)
644  {
645  QuantizedMultiModFeature feature;
646 
647  feature.x = iter1->x;
648  feature.y = iter1->y;
649  feature.modality_index = modality_index;
650  feature.quantized_value = filtered_quantized_color_gradients_ (iter1->x, iter1->y);
651 
652  features.push_back (feature);
653  }
654  return;
655  }
656 
657  size_t distance = list1.size () / nr_features + 1; // ???
658  while (list2.size () != nr_features)
659  {
660  const size_t sqr_distance = distance*distance;
661  for (typename std::list<Candidate>::iterator iter1 = list1.begin (); iter1 != list1.end (); ++iter1)
662  {
663  bool candidate_accepted = true;
664 
665  for (typename std::list<Candidate>::iterator iter2 = list2.begin (); iter2 != list2.end (); ++iter2)
666  {
667  const int dx = iter1->x - iter2->x;
668  const int dy = iter1->y - iter2->y;
669  const unsigned int tmp_distance = dx*dx + dy*dy;
670 
671  //if (tmp_distance < distance)
672  if (tmp_distance < sqr_distance)
673  {
674  candidate_accepted = false;
675  break;
676  }
677  }
678 
679  if (candidate_accepted)
680  list2.push_back (*iter1);
681 
682  if (list2.size () == nr_features)
683  break;
684  }
685  --distance;
686  }
687  }
688 
689  for (typename std::list<Candidate>::iterator iter2 = list2.begin (); iter2 != list2.end (); ++iter2)
690  {
691  QuantizedMultiModFeature feature;
692 
693  feature.x = iter2->x;
694  feature.y = iter2->y;
695  feature.modality_index = modality_index;
696  feature.quantized_value = filtered_quantized_color_gradients_ (iter2->x, iter2->y);
697 
698  features.push_back (feature);
699  }
700 }
701 
702 //////////////////////////////////////////////////////////////////////////////////////////////
703 template <typename PointInT> void
705 extractAllFeatures (const MaskMap & mask, const size_t, const size_t modality_index,
706  std::vector<QuantizedMultiModFeature> & features) const
707 {
708  const size_t width = mask.getWidth ();
709  const size_t height = mask.getHeight ();
710 
711  std::list<Candidate> list1;
712  std::list<Candidate> list2;
713 
714 
715  for (size_t row_index = 0; row_index < height; ++row_index)
716  {
717  for (size_t col_index = 0; col_index < width; ++col_index)
718  {
719  if (mask (col_index, row_index) != 0)
720  {
721  const GradientXY & gradient = color_gradients_ (col_index, row_index);
722  if (gradient.magnitude > gradient_magnitude_threshold_feature_extraction_
723  && filtered_quantized_color_gradients_ (col_index, row_index) != 0)
724  {
725  Candidate candidate;
726  candidate.gradient = gradient;
727  candidate.x = static_cast<int> (col_index);
728  candidate.y = static_cast<int> (row_index);
729 
730  list1.push_back (candidate);
731  }
732  }
733  }
734  }
735 
736  list1.sort();
737 
738  for (typename std::list<Candidate>::iterator iter1 = list1.begin (); iter1 != list1.end (); ++iter1)
739  {
740  QuantizedMultiModFeature feature;
741 
742  feature.x = iter1->x;
743  feature.y = iter1->y;
744  feature.modality_index = modality_index;
745  feature.quantized_value = filtered_quantized_color_gradients_ (iter1->x, iter1->y);
746 
747  features.push_back (feature);
748  }
749 }
750 
751 //////////////////////////////////////////////////////////////////////////////////////////////
752 template <typename PointInT>
753 void
756 {
757  const int width = cloud->width;
758  const int height = cloud->height;
759 
760  color_gradients_.points.resize (width*height);
761  color_gradients_.width = width;
762  color_gradients_.height = height;
763 
764  const float pi = tan (1.0f) * 2;
765  for (int row_index = 0; row_index < height-2; ++row_index)
766  {
767  for (int col_index = 0; col_index < width-2; ++col_index)
768  {
769  const int index0 = row_index*width+col_index;
770  const int index_c = row_index*width+col_index+2;
771  const int index_r = (row_index+2)*width+col_index;
772 
773  //const int index_d = (row_index+1)*width+col_index+1;
774 
775  const unsigned char r0 = cloud->points[index0].r;
776  const unsigned char g0 = cloud->points[index0].g;
777  const unsigned char b0 = cloud->points[index0].b;
778 
779  const unsigned char r_c = cloud->points[index_c].r;
780  const unsigned char g_c = cloud->points[index_c].g;
781  const unsigned char b_c = cloud->points[index_c].b;
782 
783  const unsigned char r_r = cloud->points[index_r].r;
784  const unsigned char g_r = cloud->points[index_r].g;
785  const unsigned char b_r = cloud->points[index_r].b;
786 
787  const float r_dx = static_cast<float> (r_c) - static_cast<float> (r0);
788  const float g_dx = static_cast<float> (g_c) - static_cast<float> (g0);
789  const float b_dx = static_cast<float> (b_c) - static_cast<float> (b0);
790 
791  const float r_dy = static_cast<float> (r_r) - static_cast<float> (r0);
792  const float g_dy = static_cast<float> (g_r) - static_cast<float> (g0);
793  const float b_dy = static_cast<float> (b_r) - static_cast<float> (b0);
794 
795  const float sqr_mag_r = r_dx*r_dx + r_dy*r_dy;
796  const float sqr_mag_g = g_dx*g_dx + g_dy*g_dy;
797  const float sqr_mag_b = b_dx*b_dx + b_dy*b_dy;
798 
799  if (sqr_mag_r > sqr_mag_g && sqr_mag_r > sqr_mag_b)
800  {
801  GradientXY gradient;
802  gradient.magnitude = sqrt (sqr_mag_r);
803  gradient.angle = atan2 (r_dy, r_dx) * 180.0f / pi;
804  gradient.x = static_cast<float> (col_index);
805  gradient.y = static_cast<float> (row_index);
806 
807  color_gradients_ (col_index+1, row_index+1) = gradient;
808  }
809  else if (sqr_mag_g > sqr_mag_b)
810  {
811  GradientXY gradient;
812  gradient.magnitude = sqrt (sqr_mag_g);
813  gradient.angle = atan2 (g_dy, g_dx) * 180.0f / pi;
814  gradient.x = static_cast<float> (col_index);
815  gradient.y = static_cast<float> (row_index);
816 
817  color_gradients_ (col_index+1, row_index+1) = gradient;
818  }
819  else
820  {
821  GradientXY gradient;
822  gradient.magnitude = sqrt (sqr_mag_b);
823  gradient.angle = atan2 (b_dy, b_dx) * 180.0f / pi;
824  gradient.x = static_cast<float> (col_index);
825  gradient.y = static_cast<float> (row_index);
826 
827  color_gradients_ (col_index+1, row_index+1) = gradient;
828  }
829 
830  assert (color_gradients_ (col_index+1, row_index+1).angle >= -180 &&
831  color_gradients_ (col_index+1, row_index+1).angle <= 180);
832  }
833  }
834 
835  return;
836 }
837 
838 //////////////////////////////////////////////////////////////////////////////////////////////
839 template <typename PointInT>
840 void
843 {
844  const int width = cloud->width;
845  const int height = cloud->height;
846 
847  color_gradients_.points.resize (width*height);
848  color_gradients_.width = width;
849  color_gradients_.height = height;
850 
851  const float pi = tanf (1.0f) * 2.0f;
852  for (int row_index = 1; row_index < height-1; ++row_index)
853  {
854  for (int col_index = 1; col_index < width-1; ++col_index)
855  {
856  const int r7 = static_cast<int> (cloud->points[(row_index-1)*width + (col_index-1)].r);
857  const int g7 = static_cast<int> (cloud->points[(row_index-1)*width + (col_index-1)].g);
858  const int b7 = static_cast<int> (cloud->points[(row_index-1)*width + (col_index-1)].b);
859  const int r8 = static_cast<int> (cloud->points[(row_index-1)*width + (col_index)].r);
860  const int g8 = static_cast<int> (cloud->points[(row_index-1)*width + (col_index)].g);
861  const int b8 = static_cast<int> (cloud->points[(row_index-1)*width + (col_index)].b);
862  const int r9 = static_cast<int> (cloud->points[(row_index-1)*width + (col_index+1)].r);
863  const int g9 = static_cast<int> (cloud->points[(row_index-1)*width + (col_index+1)].g);
864  const int b9 = static_cast<int> (cloud->points[(row_index-1)*width + (col_index+1)].b);
865  const int r4 = static_cast<int> (cloud->points[(row_index)*width + (col_index-1)].r);
866  const int g4 = static_cast<int> (cloud->points[(row_index)*width + (col_index-1)].g);
867  const int b4 = static_cast<int> (cloud->points[(row_index)*width + (col_index-1)].b);
868  const int r6 = static_cast<int> (cloud->points[(row_index)*width + (col_index+1)].r);
869  const int g6 = static_cast<int> (cloud->points[(row_index)*width + (col_index+1)].g);
870  const int b6 = static_cast<int> (cloud->points[(row_index)*width + (col_index+1)].b);
871  const int r1 = static_cast<int> (cloud->points[(row_index+1)*width + (col_index-1)].r);
872  const int g1 = static_cast<int> (cloud->points[(row_index+1)*width + (col_index-1)].g);
873  const int b1 = static_cast<int> (cloud->points[(row_index+1)*width + (col_index-1)].b);
874  const int r2 = static_cast<int> (cloud->points[(row_index+1)*width + (col_index)].r);
875  const int g2 = static_cast<int> (cloud->points[(row_index+1)*width + (col_index)].g);
876  const int b2 = static_cast<int> (cloud->points[(row_index+1)*width + (col_index)].b);
877  const int r3 = static_cast<int> (cloud->points[(row_index+1)*width + (col_index+1)].r);
878  const int g3 = static_cast<int> (cloud->points[(row_index+1)*width + (col_index+1)].g);
879  const int b3 = static_cast<int> (cloud->points[(row_index+1)*width + (col_index+1)].b);
880 
881  //const int r_tmp1 = - r7 + r3;
882  //const int r_tmp2 = - r1 + r9;
883  //const int g_tmp1 = - g7 + g3;
884  //const int g_tmp2 = - g1 + g9;
885  //const int b_tmp1 = - b7 + b3;
886  //const int b_tmp2 = - b1 + b9;
887  ////const int gx = - r7 - (r4<<2) - r1 + r3 + (r6<<2) + r9;
888  ////const int gy = - r7 - (r8<<2) - r9 + r1 + (r2<<2) + r3;
889  //const int r_dx = r_tmp1 + r_tmp2 - (r4<<2) + (r6<<2);
890  //const int r_dy = r_tmp1 - r_tmp2 - (r8<<2) + (r2<<2);
891  //const int g_dx = g_tmp1 + g_tmp2 - (g4<<2) + (g6<<2);
892  //const int g_dy = g_tmp1 - g_tmp2 - (g8<<2) + (g2<<2);
893  //const int b_dx = b_tmp1 + b_tmp2 - (b4<<2) + (b6<<2);
894  //const int b_dy = b_tmp1 - b_tmp2 - (b8<<2) + (b2<<2);
895 
896  //const int r_tmp1 = - r7 + r3;
897  //const int r_tmp2 = - r1 + r9;
898  //const int g_tmp1 = - g7 + g3;
899  //const int g_tmp2 = - g1 + g9;
900  //const int b_tmp1 = - b7 + b3;
901  //const int b_tmp2 = - b1 + b9;
902  //const int gx = - r7 - (r4<<2) - r1 + r3 + (r6<<2) + r9;
903  //const int gy = - r7 - (r8<<2) - r9 + r1 + (r2<<2) + r3;
904  const int r_dx = r9 + 2*r6 + r3 - (r7 + 2*r4 + r1);
905  const int r_dy = r1 + 2*r2 + r3 - (r7 + 2*r8 + r9);
906  const int g_dx = g9 + 2*g6 + g3 - (g7 + 2*g4 + g1);
907  const int g_dy = g1 + 2*g2 + g3 - (g7 + 2*g8 + g9);
908  const int b_dx = b9 + 2*b6 + b3 - (b7 + 2*b4 + b1);
909  const int b_dy = b1 + 2*b2 + b3 - (b7 + 2*b8 + b9);
910 
911  const int sqr_mag_r = r_dx*r_dx + r_dy*r_dy;
912  const int sqr_mag_g = g_dx*g_dx + g_dy*g_dy;
913  const int sqr_mag_b = b_dx*b_dx + b_dy*b_dy;
914 
915  if (sqr_mag_r > sqr_mag_g && sqr_mag_r > sqr_mag_b)
916  {
917  GradientXY gradient;
918  gradient.magnitude = std::sqrt (static_cast<float> (sqr_mag_r));
919  gradient.angle = atan2f (static_cast<float> (r_dy), static_cast<float> (r_dx)) * 180.0f / pi;
920  if (gradient.angle < -180.0f) gradient.angle += 360.0f;
921  if (gradient.angle >= 180.0f) gradient.angle -= 360.0f;
922  gradient.x = static_cast<float> (col_index);
923  gradient.y = static_cast<float> (row_index);
924 
925  color_gradients_ (col_index, row_index) = gradient;
926  }
927  else if (sqr_mag_g > sqr_mag_b)
928  {
929  GradientXY gradient;
930  gradient.magnitude = std::sqrt (static_cast<float> (sqr_mag_g));
931  gradient.angle = atan2f (static_cast<float> (g_dy), static_cast<float> (g_dx)) * 180.0f / pi;
932  if (gradient.angle < -180.0f) gradient.angle += 360.0f;
933  if (gradient.angle >= 180.0f) gradient.angle -= 360.0f;
934  gradient.x = static_cast<float> (col_index);
935  gradient.y = static_cast<float> (row_index);
936 
937  color_gradients_ (col_index, row_index) = gradient;
938  }
939  else
940  {
941  GradientXY gradient;
942  gradient.magnitude = std::sqrt (static_cast<float> (sqr_mag_b));
943  gradient.angle = atan2f (static_cast<float> (b_dy), static_cast<float> (b_dx)) * 180.0f / pi;
944  if (gradient.angle < -180.0f) gradient.angle += 360.0f;
945  if (gradient.angle >= 180.0f) gradient.angle -= 360.0f;
946  gradient.x = static_cast<float> (col_index);
947  gradient.y = static_cast<float> (row_index);
948 
949  color_gradients_ (col_index, row_index) = gradient;
950  }
951 
952  assert (color_gradients_ (col_index, row_index).angle >= -180 &&
953  color_gradients_ (col_index, row_index).angle <= 180);
954  }
955  }
956 
957  return;
958 }
959 
960 //////////////////////////////////////////////////////////////////////////////////////////////
961 template <typename PointInT>
962 void
965 {
966  //std::cerr << "quantize this, bastard!!!" << std::endl;
967 
968  //unsigned char quantization_map[16] = {0,1,2,3,4,5,6,7,0,1,2,3,4,5,6,7};
969  //unsigned char quantization_map[16] = {1,2,3,4,5,6,7,8,1,2,3,4,5,6,7,8};
970 
971  //for (float angle = 0.0f; angle < 360.0f; angle += 1.0f)
972  //{
973  // const int quantized_value = quantization_map[static_cast<int> (angle * angleScale)];
974  // std::cerr << angle << ": " << quantized_value << std::endl;
975  //}
976 
977 
978  const size_t width = input_->width;
979  const size_t height = input_->height;
980 
981  quantized_color_gradients_.resize (width, height);
982 
983  const float angleScale = 16.0f/360.0f;
984 
985  //float min_angle = std::numeric_limits<float>::max ();
986  //float max_angle = -std::numeric_limits<float>::max ();
987  for (size_t row_index = 0; row_index < height; ++row_index)
988  {
989  for (size_t col_index = 0; col_index < width; ++col_index)
990  {
991  if (color_gradients_ (col_index, row_index).magnitude < gradient_magnitude_threshold_)
992  {
993  quantized_color_gradients_ (col_index, row_index) = 0;
994  continue;
995  }
996 
997  const float angle = 11.25f + color_gradients_ (col_index, row_index).angle + 180.0f;
998  const int quantized_value = (static_cast<int> (angle * angleScale)) & 7;
999  quantized_color_gradients_ (col_index, row_index) = static_cast<unsigned char> (quantized_value + 1);
1000 
1001  //const float angle = color_gradients_ (col_index, row_index).angle + 180.0f;
1002 
1003  //min_angle = std::min (min_angle, angle);
1004  //max_angle = std::max (max_angle, angle);
1005 
1006  //if (angle < 0.0f || angle >= 360.0f)
1007  //{
1008  // std::cerr << "angle shitty: " << angle << std::endl;
1009  //}
1010 
1011  //const int quantized_value = quantization_map[static_cast<int> (angle * angleScale)];
1012  //quantized_color_gradients_ (col_index, row_index) = static_cast<unsigned char> (quantized_value);
1013 
1014  //assert (0 <= quantized_value && quantized_value < 16);
1015  //quantized_color_gradients_ (col_index, row_index) = quantization_map[quantized_value];
1016  //quantized_color_gradients_ (col_index, row_index) = static_cast<unsigned char> ((quantized_value & 7) + 1); // = (quantized_value % 8) + 1
1017  }
1018  }
1019 
1020  //std::cerr << ">>>>> " << min_angle << ", " << max_angle << std::endl;
1021 }
1022 
1023 //////////////////////////////////////////////////////////////////////////////////////////////
1024 template <typename PointInT>
1025 void
1028 {
1029  const size_t width = input_->width;
1030  const size_t height = input_->height;
1031 
1032  filtered_quantized_color_gradients_.resize (width, height);
1033 
1034  // filter data
1035  for (size_t row_index = 1; row_index < height-1; ++row_index)
1036  {
1037  for (size_t col_index = 1; col_index < width-1; ++col_index)
1038  {
1039  unsigned char histogram[9] = {0,0,0,0,0,0,0,0,0};
1040 
1041  {
1042  const unsigned char * data_ptr = quantized_color_gradients_.getData () + (row_index-1)*width+col_index-1;
1043  assert (data_ptr[0] < 9 && data_ptr[1] < 9 && data_ptr[2] < 9);
1044  ++histogram[data_ptr[0]];
1045  ++histogram[data_ptr[1]];
1046  ++histogram[data_ptr[2]];
1047  }
1048  {
1049  const unsigned char * data_ptr = quantized_color_gradients_.getData () + row_index*width+col_index-1;
1050  assert (data_ptr[0] < 9 && data_ptr[1] < 9 && data_ptr[2] < 9);
1051  ++histogram[data_ptr[0]];
1052  ++histogram[data_ptr[1]];
1053  ++histogram[data_ptr[2]];
1054  }
1055  {
1056  const unsigned char * data_ptr = quantized_color_gradients_.getData () + (row_index+1)*width+col_index-1;
1057  assert (data_ptr[0] < 9 && data_ptr[1] < 9 && data_ptr[2] < 9);
1058  ++histogram[data_ptr[0]];
1059  ++histogram[data_ptr[1]];
1060  ++histogram[data_ptr[2]];
1061  }
1062 
1063  unsigned char max_hist_value = 0;
1064  int max_hist_index = -1;
1065 
1066  // for (int i = 0; i < 8; ++i)
1067  // {
1068  // if (max_hist_value < histogram[i+1])
1069  // {
1070  // max_hist_index = i;
1071  // max_hist_value = histogram[i+1]
1072  // }
1073  // }
1074  // Unrolled for performance optimization:
1075  if (max_hist_value < histogram[1]) {max_hist_index = 0; max_hist_value = histogram[1];}
1076  if (max_hist_value < histogram[2]) {max_hist_index = 1; max_hist_value = histogram[2];}
1077  if (max_hist_value < histogram[3]) {max_hist_index = 2; max_hist_value = histogram[3];}
1078  if (max_hist_value < histogram[4]) {max_hist_index = 3; max_hist_value = histogram[4];}
1079  if (max_hist_value < histogram[5]) {max_hist_index = 4; max_hist_value = histogram[5];}
1080  if (max_hist_value < histogram[6]) {max_hist_index = 5; max_hist_value = histogram[6];}
1081  if (max_hist_value < histogram[7]) {max_hist_index = 6; max_hist_value = histogram[7];}
1082  if (max_hist_value < histogram[8]) {max_hist_index = 7; max_hist_value = histogram[8];}
1083 
1084  if (max_hist_index != -1 && max_hist_value >= 5)
1085  filtered_quantized_color_gradients_ (col_index, row_index) = static_cast<unsigned char> (0x1 << max_hist_index);
1086  else
1087  filtered_quantized_color_gradients_ (col_index, row_index) = 0;
1088 
1089  }
1090  }
1091 }
1092 
1093 //////////////////////////////////////////////////////////////////////////////////////////////
1094 template <typename PointInT>
1095 void
1097 erode (const pcl::MaskMap & mask_in,
1098  pcl::MaskMap & mask_out)
1099 {
1100  const size_t width = mask_in.getWidth ();
1101  const size_t height = mask_in.getHeight ();
1102 
1103  mask_out.resize (width, height);
1104 
1105  for (size_t row_index = 1; row_index < height-1; ++row_index)
1106  {
1107  for (size_t col_index = 1; col_index < width-1; ++col_index)
1108  {
1109  if (mask_in (col_index, row_index-1) == 0 ||
1110  mask_in (col_index-1, row_index) == 0 ||
1111  mask_in (col_index+1, row_index) == 0 ||
1112  mask_in (col_index, row_index+1) == 0)
1113  {
1114  mask_out (col_index, row_index) = 0;
1115  }
1116  else
1117  {
1118  mask_out (col_index, row_index) = 255;
1119  }
1120  }
1121  }
1122 }
1123 
1124 #endif
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
Definition: point_cloud.h:410
Feature that defines a position and quantized value in a specific modality.
void computeMaxColorGradientsSobel(const typename pcl::PointCloud< pcl::RGB >::ConstPtr &cloud)
Computes the max-RGB gradients for the specified cloud using sobel.
void convolve(const Eigen::ArrayXf &h_kernel, const Eigen::ArrayXf &v_kernel, PointCloudOut &output)
Convolve point cloud with an horizontal kernel along rows then vertical kernel along columns : convol...
void quantizeColorGradients()
Quantizes the color gradients.
void setFeatureSelectionMethod(const FeatureSelectionMethod method)
Sets the feature selection method.
bool operator<(const Candidate &rhs) const
Operator for comparing to candidates (by magnitude of the gradient).
unsigned char quantized_value
the quantized value attached to the feature.
uint32_t height
The point cloud height (if organized as an image-structure).
Definition: point_cloud.h:415
void resize(size_t width, size_t height)
void setKernel(const Eigen::ArrayXf &kernel)
Set convolving kernel.
Definition: convolution.h:109
A point structure representing Euclidean xyz coordinates, and the intensity value.
Definition: point_types.h:51
virtual void setInputCloud(const typename PointCloudIn::ConstPtr &cloud)
Provide a pointer to the input dataset (overwrites the PCLBase::setInputCloud method)
boost::shared_ptr< PointCloud< PointT > > Ptr
Definition: point_cloud.h:428
A structure representing RGB color information.
QuantizedMap & getQuantizedMap()
Returns a reference to the internally computed quantized map.
uint32_t width
The point cloud width (if organized as an image-structure).
Definition: point_cloud.h:413
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.
QuantizedMap & getSpreadedQuantizedMap()
Returns a reference to the internally computed spreaded quantized map.
Convolution is a mathematical operation on two functions f and g, producing a third function that is ...
Definition: convolution.h:77
Defines all the PCL implemented PointT point type structures.
void computeGaussianKernel(const size_t kernel_size, const float sigma, std::vector< float > &kernel_values)
Computes the Gaussian kernel used for smoothing.
void setInputCloud(const PointCloudInConstPtr &cloud)
Provide a pointer to the input dataset.
Definition: convolution.h:104
PCL base class.
Definition: pcl_base.h:68
void extractAllFeatures(const MaskMap &mask, size_t nr_features, size_t modalityIndex, std::vector< QuantizedMultiModFeature > &features) const
Extracts all possible features from the modality within the specified mask.
void setGradientMagnitudeThresholdForFeatureExtraction(const float threshold)
Sets the threshold for the gradient magnitude which is used for feature extraction.
void computeMaxColorGradients(const typename pcl::PointCloud< pcl::RGB >::ConstPtr &cloud)
Computes the max-RGB gradients for the specified cloud.
float distance(const PointT &p1, const PointT &p2)
Definition: geometry.h:60
size_t getHeight() const
Definition: mask_map.h:61
boost::shared_ptr< const PointCloud< PointRGBT > > ConstPtr
Definition: point_cloud.h:429
Interface for a quantizable modality.
size_t getWidth() const
Definition: mask_map.h:58
virtual void processInputDataFromFiltered()
Processes the input data assuming that everything up to filtering is already done/available (so only ...
pcl::PointCloud< PointInT > PointCloudIn
Modality based on max-RGB gradients.
void filterQuantizedColorGradients()
Filters the quantized gradients.
virtual ~ColorGradientModality()
Destructor.
FeatureSelectionMethod
Different methods for feature selection/extraction.
bool is_dense
True if no points are invalid (e.g., have NaN or Inf values).
Definition: point_cloud.h:418
void setSpreadingSize(const size_t spreading_size)
Sets the spreading size for spreading the quantized data.
virtual void processInputData()
Processes the input data (smoothing, computing gradients, quantizing, filtering, spreading).
PointCloudConstPtr input_
The input point cloud dataset.
Definition: pcl_base.h:150
void resize(size_t n)
Resize the cloud.
Definition: point_cloud.h:455
static void erode(const pcl::MaskMap &mask_in, pcl::MaskMap &mask_out)
Erodes a mask.
static void spreadQuantizedMap(const QuantizedMap &input_map, QuantizedMap &output_map, size_t spreading_size)
void setGradientMagnitudeThreshold(const float threshold)
Sets the threshold for the gradient magnitude which is used when quantizing the data.
void setVariableFeatureNr(const bool enabled)
Sets whether variable feature numbers for feature extraction is enabled.
pcl::PointCloud< pcl::GradientXY > & getMaxColorGradients()
Returns a point cloud containing the max-RGB gradients.
size_t modality_index
the index of the corresponding modality.
Candidate for a feature (used in feature extraction methods).