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