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 std::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, std::size_t nr_features, std::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, std::size_t nr_features, std::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 std::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  std::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 std::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 std::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 std::uint32_t width = input_->width;
359  const std::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 (std::size_t row_index = 0; row_index < height; ++row_index)
366  {
367  for (std::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 std::size_t nr_features, const std::size_t modality_index,
413  std::vector<QuantizedMultiModFeature> & features) const
414 {
415  const std::size_t width = mask.getWidth ();
416  const std::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 (std::size_t row_index = 0; row_index < height; ++row_index)
425  {
426  for (std::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 (std::size_t row_index = 0; row_index < height; ++row_index)
612  {
613  for (std::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  std::size_t distance = list1.size () / nr_features + 1; // ???
651  while (list2.size () != nr_features)
652  {
653  const std::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 std::size_t, const std::size_t modality_index,
699  std::vector<QuantizedMultiModFeature> & features) const
700 {
701  const std::size_t width = mask.getWidth ();
702  const std::size_t height = mask.getHeight ();
703 
704  std::list<Candidate> list1;
705  std::list<Candidate> list2;
706 
707 
708  for (std::size_t row_index = 0; row_index < height; ++row_index)
709  {
710  for (std::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 std::size_t width = input_->width;
972  const std::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 (std::size_t row_index = 0; row_index < height; ++row_index)
981  {
982  for (std::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 std::size_t width = input_->width;
1023  const std::size_t height = input_->height;
1024 
1025  filtered_quantized_color_gradients_.resize (width, height);
1026 
1027  // filter data
1028  for (std::size_t row_index = 1; row_index < height-1; ++row_index)
1029  {
1030  for (std::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 std::size_t width = mask_in.getWidth ();
1094  const std::size_t height = mask_in.getHeight ();
1095 
1096  mask_out.resize (width, height);
1097 
1098  for (std::size_t row_index = 1; row_index < height-1; ++row_index)
1099  {
1100  for (std::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.
static void spreadQuantizedMap(const QuantizedMap &input_map, QuantizedMap &output_map, std::size_t spreading_size)
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
Definition: point_cloud.h:397
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:442
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.
std::size_t modality_index
the index of the corresponding modality.
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.
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
void setSpreadingSize(const std::size_t spreading_size)
Sets the spreading size for spreading the quantized data.
std::uint32_t width
The point cloud width (if organized as an image-structure).
Definition: point_cloud.h:400
A structure representing RGB color information.
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 extractAllFeatures(const MaskMap &mask, std::size_t nr_features, std::size_t modalityIndex, std::vector< QuantizedMultiModFeature > &features) const override
Extracts all possible features from the modality within the specified mask.
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:415
std::uint32_t height
The point cloud height (if organized as an image-structure).
Definition: point_cloud.h:402
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 extractFeatures(const MaskMap &mask, std::size_t nr_features, std::size_t modalityIndex, std::vector< QuantizedMultiModFeature > &features) const override
Extracts features from this modality within the specified mask.
void filterQuantizedColorGradients()
Filters the quantized gradients.
boost::shared_ptr< const PointCloud< PointRGBT > > ConstPtr
Definition: point_cloud.h:416
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:405
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
static void erode(const pcl::MaskMap &mask_in, pcl::MaskMap &mask_out)
Erodes a mask.
void setGradientMagnitudeThreshold(const float threshold)
Sets the threshold for the gradient magnitude which is used when quantizing the data.
void computeGaussianKernel(const std::size_t kernel_size, const float sigma, std::vector< float > &kernel_values)
Computes the Gaussian kernel used for smoothing.
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.
Candidate for a feature (used in feature extraction methods).