Point Cloud Library (PCL)  1.14.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
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_{false};
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_{10.0f};
253  /** \brief The threshold applied on the gradient magnitudes for feature extraction. */
254  float gradient_magnitude_threshold_feature_extraction_{55.0f};
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_{8};
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  : smoothed_input_ (new pcl::PointCloud<pcl::RGB> ())
278  , feature_selection_method_ (DISTANCE_MAGNITUDE_SCORE)
279 {
280 }
281 
282 //////////////////////////////////////////////////////////////////////////////////////////////
283 template <typename PointInT>
285 ~ColorGradientModality () = default;
286 
287 //////////////////////////////////////////////////////////////////////////////////////////////
288 template <typename PointInT> void
290 computeGaussianKernel (const std::size_t kernel_size, const float sigma, std::vector <float> & kernel_values)
291 {
292  // code taken from OpenCV
293  const int n = static_cast<int>(kernel_size);
294  constexpr int SMALL_GAUSSIAN_SIZE = 7;
295  static const float small_gaussian_tab[][SMALL_GAUSSIAN_SIZE] =
296  {
297  {1.f},
298  {0.25f, 0.5f, 0.25f},
299  {0.0625f, 0.25f, 0.375f, 0.25f, 0.0625f},
300  {0.03125f, 0.109375f, 0.21875f, 0.28125f, 0.21875f, 0.109375f, 0.03125f}
301  };
302 
303  const float* fixed_kernel = n % 2 == 1 && n <= SMALL_GAUSSIAN_SIZE && sigma <= 0 ?
304  small_gaussian_tab[n>>1] : nullptr;
305 
306  //CV_Assert( ktype == CV_32F || ktype == CV_64F );
307  /*Mat kernel(n, 1, ktype);*/
308  kernel_values.resize (n);
309  float* cf = kernel_values.data();
310  //double* cd = (double*)kernel.data;
311 
312  double sigmaX = sigma > 0 ? sigma : ((n-1)*0.5 - 1)*0.3 + 0.8;
313  double scale2X = -0.5/(sigmaX*sigmaX);
314  double sum = 0;
315 
316  for( int i = 0; i < n; i++ )
317  {
318  double x = i - (n-1)*0.5;
319  double t = fixed_kernel ? static_cast<double>(fixed_kernel[i]) : std::exp (scale2X*x*x);
320 
321  cf[i] = static_cast<float>(t);
322  sum += cf[i];
323  }
324 
325  sum = 1./sum;
326  for ( int i = 0; i < n; i++ )
327  {
328  cf[i] = static_cast<float>(cf[i]*sum);
329  }
330 }
331 
332 //////////////////////////////////////////////////////////////////////////////////////////////
333 template <typename PointInT>
334 void
337 {
338  // compute gaussian kernel values
339  constexpr std::size_t kernel_size = 7;
340  std::vector<float> kernel_values;
341  computeGaussianKernel (kernel_size, 0.0f, kernel_values);
342 
343  // smooth input
345  Eigen::ArrayXf gaussian_kernel(kernel_size);
346  //gaussian_kernel << 1.f/16, 1.f/8, 3.f/16, 2.f/8, 3.f/16, 1.f/8, 1.f/16;
347  //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;
348  gaussian_kernel << kernel_values[0], kernel_values[1], kernel_values[2], kernel_values[3], kernel_values[4], kernel_values[5], kernel_values[6];
349 
351 
352  const std::uint32_t width = input_->width;
353  const std::uint32_t height = input_->height;
354 
355  rgb_input_->resize (width*height);
356  rgb_input_->width = width;
357  rgb_input_->height = height;
358  rgb_input_->is_dense = input_->is_dense;
359  for (std::size_t row_index = 0; row_index < height; ++row_index)
360  {
361  for (std::size_t col_index = 0; col_index < width; ++col_index)
362  {
363  (*rgb_input_) (col_index, row_index).r = (*input_) (col_index, row_index).r;
364  (*rgb_input_) (col_index, row_index).g = (*input_) (col_index, row_index).g;
365  (*rgb_input_) (col_index, row_index).b = (*input_) (col_index, row_index).b;
366  }
367  }
368 
369  convolution.setInputCloud (rgb_input_);
370  convolution.setKernel (gaussian_kernel);
372  convolution.convolve (*smoothed_input_);
373 
374  // extract color gradients
375  computeMaxColorGradientsSobel (smoothed_input_);
376 
377  // quantize gradients
378  quantizeColorGradients ();
379 
380  // filter quantized gradients to get only dominants one + thresholding
381  filterQuantizedColorGradients ();
382 
383  // spread filtered quantized gradients
384  //spreadFilteredQunatizedColorGradients ();
385  pcl::QuantizedMap::spreadQuantizedMap (filtered_quantized_color_gradients_,
386  spreaded_filtered_quantized_color_gradients_,
387  spreading_size_);
388 }
389 
390 //////////////////////////////////////////////////////////////////////////////////////////////
391 template <typename PointInT>
392 void
395 {
396  // spread filtered quantized gradients
397  //spreadFilteredQunatizedColorGradients ();
398  pcl::QuantizedMap::spreadQuantizedMap (filtered_quantized_color_gradients_,
399  spreaded_filtered_quantized_color_gradients_,
400  spreading_size_);
401 }
402 
403 //////////////////////////////////////////////////////////////////////////////////////////////
404 template <typename PointInT>
406 extractFeatures (const MaskMap & mask, const std::size_t nr_features, const std::size_t modality_index,
407  std::vector<QuantizedMultiModFeature> & features) const
408 {
409  const std::size_t width = mask.getWidth ();
410  const std::size_t height = mask.getHeight ();
411 
412  std::list<Candidate> list1;
413  std::list<Candidate> list2;
414 
415 
416  if (feature_selection_method_ == DISTANCE_MAGNITUDE_SCORE)
417  {
418  for (std::size_t row_index = 0; row_index < height; ++row_index)
419  {
420  for (std::size_t col_index = 0; col_index < width; ++col_index)
421  {
422  if (mask (col_index, row_index) != 0)
423  {
424  const GradientXY & gradient = color_gradients_ (col_index, row_index);
425  if (gradient.magnitude > gradient_magnitude_threshold_feature_extraction_
426  && filtered_quantized_color_gradients_ (col_index, row_index) != 0)
427  {
428  Candidate candidate;
429  candidate.gradient = gradient;
430  candidate.x = static_cast<int> (col_index);
431  candidate.y = static_cast<int> (row_index);
432 
433  list1.push_back (candidate);
434  }
435  }
436  }
437  }
438 
439  list1.sort();
440 
441  if (variable_feature_nr_)
442  {
443  list2.push_back (*(list1.begin ()));
444  //while (list2.size () != nr_features)
445  bool feature_selection_finished = false;
446  while (!feature_selection_finished)
447  {
448  float best_score = 0.0f;
449  auto best_iter = list1.end ();
450  for (auto iter1 = list1.begin (); iter1 != list1.end (); ++iter1)
451  {
452  // find smallest distance
453  float smallest_distance = std::numeric_limits<float>::max ();
454  for (auto iter2 = list2.begin (); iter2 != list2.end (); ++iter2)
455  {
456  const float dx = static_cast<float> (iter1->x) - static_cast<float> (iter2->x);
457  const float dy = static_cast<float> (iter1->y) - static_cast<float> (iter2->y);
458 
459  const float distance = dx*dx + dy*dy;
460 
461  if (distance < smallest_distance)
462  {
463  smallest_distance = distance;
464  }
465  }
466 
467  const float score = smallest_distance * iter1->gradient.magnitude;
468 
469  if (score > best_score)
470  {
471  best_score = score;
472  best_iter = iter1;
473  }
474  }
475 
476 
477  float min_min_sqr_distance = std::numeric_limits<float>::max ();
478  float max_min_sqr_distance = 0;
479  for (auto iter2 = list2.begin (); iter2 != list2.end (); ++iter2)
480  {
481  float min_sqr_distance = std::numeric_limits<float>::max ();
482  for (auto iter3 = list2.begin (); iter3 != list2.end (); ++iter3)
483  {
484  if (iter2 == iter3)
485  continue;
486 
487  const float dx = static_cast<float> (iter2->x) - static_cast<float> (iter3->x);
488  const float dy = static_cast<float> (iter2->y) - static_cast<float> (iter3->y);
489 
490  const float sqr_distance = dx*dx + dy*dy;
491 
492  if (sqr_distance < min_sqr_distance)
493  {
494  min_sqr_distance = sqr_distance;
495  }
496 
497  //std::cerr << min_sqr_distance;
498  }
499  //std::cerr << std::endl;
500 
501  // check current feature
502  {
503  const float dx = static_cast<float> (iter2->x) - static_cast<float> (best_iter->x);
504  const float dy = static_cast<float> (iter2->y) - static_cast<float> (best_iter->y);
505 
506  const float sqr_distance = dx*dx + dy*dy;
507 
508  if (sqr_distance < min_sqr_distance)
509  {
510  min_sqr_distance = sqr_distance;
511  }
512  }
513 
514  if (min_sqr_distance < min_min_sqr_distance)
515  min_min_sqr_distance = min_sqr_distance;
516  if (min_sqr_distance > max_min_sqr_distance)
517  max_min_sqr_distance = min_sqr_distance;
518 
519  //std::cerr << min_sqr_distance << ", " << min_min_sqr_distance << ", " << max_min_sqr_distance << std::endl;
520  }
521 
522  if (best_iter != list1.end ())
523  {
524  //std::cerr << "feature_index: " << list2.size () << std::endl;
525  //std::cerr << "min_min_sqr_distance: " << min_min_sqr_distance << std::endl;
526  //std::cerr << "max_min_sqr_distance: " << max_min_sqr_distance << std::endl;
527 
528  if (min_min_sqr_distance < 50)
529  {
530  feature_selection_finished = true;
531  break;
532  }
533 
534  list2.push_back (*best_iter);
535  }
536  }
537  }
538  else
539  {
540  if (list1.size () <= nr_features)
541  {
542  for (auto iter1 = list1.begin (); iter1 != list1.end (); ++iter1)
543  {
544  QuantizedMultiModFeature feature;
545 
546  feature.x = iter1->x;
547  feature.y = iter1->y;
548  feature.modality_index = modality_index;
549  feature.quantized_value = filtered_quantized_color_gradients_ (iter1->x, iter1->y);
550 
551  features.push_back (feature);
552  }
553  return;
554  }
555 
556  list2.push_back (*(list1.begin ()));
557  while (list2.size () != nr_features)
558  {
559  float best_score = 0.0f;
560  auto best_iter = list1.end ();
561  for (auto iter1 = list1.begin (); iter1 != list1.end (); ++iter1)
562  {
563  // find smallest distance
564  float smallest_distance = std::numeric_limits<float>::max ();
565  for (auto iter2 = list2.begin (); iter2 != list2.end (); ++iter2)
566  {
567  const float dx = static_cast<float> (iter1->x) - static_cast<float> (iter2->x);
568  const float dy = static_cast<float> (iter1->y) - static_cast<float> (iter2->y);
569 
570  const float distance = dx*dx + dy*dy;
571 
572  if (distance < smallest_distance)
573  {
574  smallest_distance = distance;
575  }
576  }
577 
578  const float score = smallest_distance * iter1->gradient.magnitude;
579 
580  if (score > best_score)
581  {
582  best_score = score;
583  best_iter = iter1;
584  }
585  }
586 
587  if (best_iter != list1.end ())
588  {
589  list2.push_back (*best_iter);
590  }
591  else
592  {
593  break;
594  }
595  }
596  }
597  }
598  else if (feature_selection_method_ == MASK_BORDER_HIGH_GRADIENTS || feature_selection_method_ == MASK_BORDER_EQUALLY)
599  {
600  MaskMap eroded_mask;
601  erode (mask, eroded_mask);
602 
603  auto diff_mask = MaskMap::getDifferenceMask (mask, eroded_mask);
604 
605  for (std::size_t row_index = 0; row_index < height; ++row_index)
606  {
607  for (std::size_t col_index = 0; col_index < width; ++col_index)
608  {
609  if (diff_mask (col_index, row_index) != 0)
610  {
611  const GradientXY & gradient = color_gradients_ (col_index, row_index);
612  if ((feature_selection_method_ == MASK_BORDER_EQUALLY || gradient.magnitude > gradient_magnitude_threshold_feature_extraction_)
613  && filtered_quantized_color_gradients_ (col_index, row_index) != 0)
614  {
615  Candidate candidate;
616  candidate.gradient = gradient;
617  candidate.x = static_cast<int> (col_index);
618  candidate.y = static_cast<int> (row_index);
619 
620  list1.push_back (candidate);
621  }
622  }
623  }
624  }
625 
626  list1.sort();
627 
628  if (list1.size () <= nr_features)
629  {
630  for (auto iter1 = list1.begin (); iter1 != list1.end (); ++iter1)
631  {
632  QuantizedMultiModFeature feature;
633 
634  feature.x = iter1->x;
635  feature.y = iter1->y;
636  feature.modality_index = modality_index;
637  feature.quantized_value = filtered_quantized_color_gradients_ (iter1->x, iter1->y);
638 
639  features.push_back (feature);
640  }
641  return;
642  }
643 
644  std::size_t distance = list1.size () / nr_features + 1; // ???
645  while (list2.size () != nr_features)
646  {
647  const std::size_t sqr_distance = distance*distance;
648  for (auto iter1 = list1.begin (); iter1 != list1.end (); ++iter1)
649  {
650  bool candidate_accepted = true;
651 
652  for (auto iter2 = list2.begin (); iter2 != list2.end (); ++iter2)
653  {
654  const int dx = iter1->x - iter2->x;
655  const int dy = iter1->y - iter2->y;
656  const unsigned int tmp_distance = dx*dx + dy*dy;
657 
658  //if (tmp_distance < distance)
659  if (tmp_distance < sqr_distance)
660  {
661  candidate_accepted = false;
662  break;
663  }
664  }
665 
666  if (candidate_accepted)
667  list2.push_back (*iter1);
668 
669  if (list2.size () == nr_features)
670  break;
671  }
672  --distance;
673  }
674  }
675 
676  for (auto iter2 = list2.begin (); iter2 != list2.end (); ++iter2)
677  {
678  QuantizedMultiModFeature feature;
679 
680  feature.x = iter2->x;
681  feature.y = iter2->y;
682  feature.modality_index = modality_index;
683  feature.quantized_value = filtered_quantized_color_gradients_ (iter2->x, iter2->y);
684 
685  features.push_back (feature);
686  }
687 }
688 
689 //////////////////////////////////////////////////////////////////////////////////////////////
690 template <typename PointInT> void
692 extractAllFeatures (const MaskMap & mask, const std::size_t, const std::size_t modality_index,
693  std::vector<QuantizedMultiModFeature> & features) const
694 {
695  const std::size_t width = mask.getWidth ();
696  const std::size_t height = mask.getHeight ();
697 
698  std::list<Candidate> list1;
699  std::list<Candidate> list2;
700 
701 
702  for (std::size_t row_index = 0; row_index < height; ++row_index)
703  {
704  for (std::size_t col_index = 0; col_index < width; ++col_index)
705  {
706  if (mask (col_index, row_index) != 0)
707  {
708  const GradientXY & gradient = color_gradients_ (col_index, row_index);
709  if (gradient.magnitude > gradient_magnitude_threshold_feature_extraction_
710  && filtered_quantized_color_gradients_ (col_index, row_index) != 0)
711  {
712  Candidate candidate;
713  candidate.gradient = gradient;
714  candidate.x = static_cast<int> (col_index);
715  candidate.y = static_cast<int> (row_index);
716 
717  list1.push_back (candidate);
718  }
719  }
720  }
721  }
722 
723  list1.sort();
724 
725  for (auto iter1 = list1.begin (); iter1 != list1.end (); ++iter1)
726  {
727  QuantizedMultiModFeature feature;
728 
729  feature.x = iter1->x;
730  feature.y = iter1->y;
731  feature.modality_index = modality_index;
732  feature.quantized_value = filtered_quantized_color_gradients_ (iter1->x, iter1->y);
733 
734  features.push_back (feature);
735  }
736 }
737 
738 //////////////////////////////////////////////////////////////////////////////////////////////
739 template <typename PointInT>
740 void
743 {
744  const int width = cloud->width;
745  const int height = cloud->height;
746 
747  color_gradients_.resize (width*height);
748  color_gradients_.width = width;
749  color_gradients_.height = height;
750 
751  const float pi = std::tan (1.0f) * 2;
752  for (int row_index = 0; row_index < height-2; ++row_index)
753  {
754  for (int col_index = 0; col_index < width-2; ++col_index)
755  {
756  const int index0 = row_index*width+col_index;
757  const int index_c = row_index*width+col_index+2;
758  const int index_r = (row_index+2)*width+col_index;
759 
760  //const int index_d = (row_index+1)*width+col_index+1;
761 
762  const unsigned char r0 = (*cloud)[index0].r;
763  const unsigned char g0 = (*cloud)[index0].g;
764  const unsigned char b0 = (*cloud)[index0].b;
765 
766  const unsigned char r_c = (*cloud)[index_c].r;
767  const unsigned char g_c = (*cloud)[index_c].g;
768  const unsigned char b_c = (*cloud)[index_c].b;
769 
770  const unsigned char r_r = (*cloud)[index_r].r;
771  const unsigned char g_r = (*cloud)[index_r].g;
772  const unsigned char b_r = (*cloud)[index_r].b;
773 
774  const float r_dx = static_cast<float> (r_c) - static_cast<float> (r0);
775  const float g_dx = static_cast<float> (g_c) - static_cast<float> (g0);
776  const float b_dx = static_cast<float> (b_c) - static_cast<float> (b0);
777 
778  const float r_dy = static_cast<float> (r_r) - static_cast<float> (r0);
779  const float g_dy = static_cast<float> (g_r) - static_cast<float> (g0);
780  const float b_dy = static_cast<float> (b_r) - static_cast<float> (b0);
781 
782  const float sqr_mag_r = r_dx*r_dx + r_dy*r_dy;
783  const float sqr_mag_g = g_dx*g_dx + g_dy*g_dy;
784  const float sqr_mag_b = b_dx*b_dx + b_dy*b_dy;
785 
786  if (sqr_mag_r > sqr_mag_g && sqr_mag_r > sqr_mag_b)
787  {
788  GradientXY gradient;
789  gradient.magnitude = std::sqrt (sqr_mag_r);
790  gradient.angle = std::atan2 (r_dy, r_dx) * 180.0f / pi;
791  gradient.x = static_cast<float> (col_index);
792  gradient.y = static_cast<float> (row_index);
793 
794  color_gradients_ (col_index+1, row_index+1) = gradient;
795  }
796  else if (sqr_mag_g > sqr_mag_b)
797  {
798  GradientXY gradient;
799  gradient.magnitude = std::sqrt (sqr_mag_g);
800  gradient.angle = std::atan2 (g_dy, g_dx) * 180.0f / pi;
801  gradient.x = static_cast<float> (col_index);
802  gradient.y = static_cast<float> (row_index);
803 
804  color_gradients_ (col_index+1, row_index+1) = gradient;
805  }
806  else
807  {
808  GradientXY gradient;
809  gradient.magnitude = std::sqrt (sqr_mag_b);
810  gradient.angle = std::atan2 (b_dy, b_dx) * 180.0f / pi;
811  gradient.x = static_cast<float> (col_index);
812  gradient.y = static_cast<float> (row_index);
813 
814  color_gradients_ (col_index+1, row_index+1) = gradient;
815  }
816 
817  assert (color_gradients_ (col_index+1, row_index+1).angle >= -180 &&
818  color_gradients_ (col_index+1, row_index+1).angle <= 180);
819  }
820  }
821 
822  return;
823 }
824 
825 //////////////////////////////////////////////////////////////////////////////////////////////
826 template <typename PointInT>
827 void
830 {
831  const int width = cloud->width;
832  const int height = cloud->height;
833 
834  color_gradients_.resize (width*height);
835  color_gradients_.width = width;
836  color_gradients_.height = height;
837 
838  const float pi = tanf (1.0f) * 2.0f;
839  for (int row_index = 1; row_index < height-1; ++row_index)
840  {
841  for (int col_index = 1; col_index < width-1; ++col_index)
842  {
843  const int r7 = static_cast<int> ((*cloud)[(row_index-1)*width + (col_index-1)].r);
844  const int g7 = static_cast<int> ((*cloud)[(row_index-1)*width + (col_index-1)].g);
845  const int b7 = static_cast<int> ((*cloud)[(row_index-1)*width + (col_index-1)].b);
846  const int r8 = static_cast<int> ((*cloud)[(row_index-1)*width + (col_index)].r);
847  const int g8 = static_cast<int> ((*cloud)[(row_index-1)*width + (col_index)].g);
848  const int b8 = static_cast<int> ((*cloud)[(row_index-1)*width + (col_index)].b);
849  const int r9 = static_cast<int> ((*cloud)[(row_index-1)*width + (col_index+1)].r);
850  const int g9 = static_cast<int> ((*cloud)[(row_index-1)*width + (col_index+1)].g);
851  const int b9 = static_cast<int> ((*cloud)[(row_index-1)*width + (col_index+1)].b);
852  const int r4 = static_cast<int> ((*cloud)[(row_index)*width + (col_index-1)].r);
853  const int g4 = static_cast<int> ((*cloud)[(row_index)*width + (col_index-1)].g);
854  const int b4 = static_cast<int> ((*cloud)[(row_index)*width + (col_index-1)].b);
855  const int r6 = static_cast<int> ((*cloud)[(row_index)*width + (col_index+1)].r);
856  const int g6 = static_cast<int> ((*cloud)[(row_index)*width + (col_index+1)].g);
857  const int b6 = static_cast<int> ((*cloud)[(row_index)*width + (col_index+1)].b);
858  const int r1 = static_cast<int> ((*cloud)[(row_index+1)*width + (col_index-1)].r);
859  const int g1 = static_cast<int> ((*cloud)[(row_index+1)*width + (col_index-1)].g);
860  const int b1 = static_cast<int> ((*cloud)[(row_index+1)*width + (col_index-1)].b);
861  const int r2 = static_cast<int> ((*cloud)[(row_index+1)*width + (col_index)].r);
862  const int g2 = static_cast<int> ((*cloud)[(row_index+1)*width + (col_index)].g);
863  const int b2 = static_cast<int> ((*cloud)[(row_index+1)*width + (col_index)].b);
864  const int r3 = static_cast<int> ((*cloud)[(row_index+1)*width + (col_index+1)].r);
865  const int g3 = static_cast<int> ((*cloud)[(row_index+1)*width + (col_index+1)].g);
866  const int b3 = static_cast<int> ((*cloud)[(row_index+1)*width + (col_index+1)].b);
867 
868  //const int r_tmp1 = - r7 + r3;
869  //const int r_tmp2 = - r1 + r9;
870  //const int g_tmp1 = - g7 + g3;
871  //const int g_tmp2 = - g1 + g9;
872  //const int b_tmp1 = - b7 + b3;
873  //const int b_tmp2 = - b1 + b9;
874  ////const int gx = - r7 - (r4<<2) - r1 + r3 + (r6<<2) + r9;
875  ////const int gy = - r7 - (r8<<2) - r9 + r1 + (r2<<2) + r3;
876  //const int r_dx = r_tmp1 + r_tmp2 - (r4<<2) + (r6<<2);
877  //const int r_dy = r_tmp1 - r_tmp2 - (r8<<2) + (r2<<2);
878  //const int g_dx = g_tmp1 + g_tmp2 - (g4<<2) + (g6<<2);
879  //const int g_dy = g_tmp1 - g_tmp2 - (g8<<2) + (g2<<2);
880  //const int b_dx = b_tmp1 + b_tmp2 - (b4<<2) + (b6<<2);
881  //const int b_dy = b_tmp1 - b_tmp2 - (b8<<2) + (b2<<2);
882 
883  //const int r_tmp1 = - r7 + r3;
884  //const int r_tmp2 = - r1 + r9;
885  //const int g_tmp1 = - g7 + g3;
886  //const int g_tmp2 = - g1 + g9;
887  //const int b_tmp1 = - b7 + b3;
888  //const int b_tmp2 = - b1 + b9;
889  //const int gx = - r7 - (r4<<2) - r1 + r3 + (r6<<2) + r9;
890  //const int gy = - r7 - (r8<<2) - r9 + r1 + (r2<<2) + r3;
891  const int r_dx = r9 + 2*r6 + r3 - (r7 + 2*r4 + r1);
892  const int r_dy = r1 + 2*r2 + r3 - (r7 + 2*r8 + r9);
893  const int g_dx = g9 + 2*g6 + g3 - (g7 + 2*g4 + g1);
894  const int g_dy = g1 + 2*g2 + g3 - (g7 + 2*g8 + g9);
895  const int b_dx = b9 + 2*b6 + b3 - (b7 + 2*b4 + b1);
896  const int b_dy = b1 + 2*b2 + b3 - (b7 + 2*b8 + b9);
897 
898  const int sqr_mag_r = r_dx*r_dx + r_dy*r_dy;
899  const int sqr_mag_g = g_dx*g_dx + g_dy*g_dy;
900  const int sqr_mag_b = b_dx*b_dx + b_dy*b_dy;
901 
902  if (sqr_mag_r > sqr_mag_g && sqr_mag_r > sqr_mag_b)
903  {
904  GradientXY gradient;
905  gradient.magnitude = std::sqrt (static_cast<float> (sqr_mag_r));
906  gradient.angle = std::atan2 (static_cast<float> (r_dy), static_cast<float> (r_dx)) * 180.0f / pi;
907  if (gradient.angle < -180.0f) gradient.angle += 360.0f;
908  if (gradient.angle >= 180.0f) gradient.angle -= 360.0f;
909  gradient.x = static_cast<float> (col_index);
910  gradient.y = static_cast<float> (row_index);
911 
912  color_gradients_ (col_index, row_index) = gradient;
913  }
914  else if (sqr_mag_g > sqr_mag_b)
915  {
916  GradientXY gradient;
917  gradient.magnitude = std::sqrt (static_cast<float> (sqr_mag_g));
918  gradient.angle = std::atan2 (static_cast<float> (g_dy), static_cast<float> (g_dx)) * 180.0f / pi;
919  if (gradient.angle < -180.0f) gradient.angle += 360.0f;
920  if (gradient.angle >= 180.0f) gradient.angle -= 360.0f;
921  gradient.x = static_cast<float> (col_index);
922  gradient.y = static_cast<float> (row_index);
923 
924  color_gradients_ (col_index, row_index) = gradient;
925  }
926  else
927  {
928  GradientXY gradient;
929  gradient.magnitude = std::sqrt (static_cast<float> (sqr_mag_b));
930  gradient.angle = std::atan2 (static_cast<float> (b_dy), static_cast<float> (b_dx)) * 180.0f / pi;
931  if (gradient.angle < -180.0f) gradient.angle += 360.0f;
932  if (gradient.angle >= 180.0f) gradient.angle -= 360.0f;
933  gradient.x = static_cast<float> (col_index);
934  gradient.y = static_cast<float> (row_index);
935 
936  color_gradients_ (col_index, row_index) = gradient;
937  }
938 
939  assert (color_gradients_ (col_index, row_index).angle >= -180 &&
940  color_gradients_ (col_index, row_index).angle <= 180);
941  }
942  }
943 
944  return;
945 }
946 
947 //////////////////////////////////////////////////////////////////////////////////////////////
948 template <typename PointInT>
949 void
952 {
953  //std::cerr << "quantize this, bastard!!!" << std::endl;
954 
955  //unsigned char quantization_map[16] = {0,1,2,3,4,5,6,7,0,1,2,3,4,5,6,7};
956  //unsigned char quantization_map[16] = {1,2,3,4,5,6,7,8,1,2,3,4,5,6,7,8};
957 
958  //for (float angle = 0.0f; angle < 360.0f; angle += 1.0f)
959  //{
960  // const int quantized_value = quantization_map[static_cast<int> (angle * angleScale)];
961  // std::cerr << angle << ": " << quantized_value << std::endl;
962  //}
963 
964 
965  const std::size_t width = input_->width;
966  const std::size_t height = input_->height;
967 
968  quantized_color_gradients_.resize (width, height);
969 
970  constexpr float angleScale = 16.0f / 360.0f;
971 
972  //float min_angle = std::numeric_limits<float>::max ();
973  //float max_angle = -std::numeric_limits<float>::max ();
974  for (std::size_t row_index = 0; row_index < height; ++row_index)
975  {
976  for (std::size_t col_index = 0; col_index < width; ++col_index)
977  {
978  if (color_gradients_ (col_index, row_index).magnitude < gradient_magnitude_threshold_)
979  {
980  quantized_color_gradients_ (col_index, row_index) = 0;
981  continue;
982  }
983 
984  const float angle = 11.25f + color_gradients_ (col_index, row_index).angle + 180.0f;
985  const int quantized_value = (static_cast<int> (angle * angleScale)) & 7;
986  quantized_color_gradients_ (col_index, row_index) = static_cast<unsigned char> (quantized_value + 1);
987 
988  //const float angle = color_gradients_ (col_index, row_index).angle + 180.0f;
989 
990  //min_angle = std::min (min_angle, angle);
991  //max_angle = std::max (max_angle, angle);
992 
993  //if (angle < 0.0f || angle >= 360.0f)
994  //{
995  // std::cerr << "angle shitty: " << angle << std::endl;
996  //}
997 
998  //const int quantized_value = quantization_map[static_cast<int> (angle * angleScale)];
999  //quantized_color_gradients_ (col_index, row_index) = static_cast<unsigned char> (quantized_value);
1000 
1001  //assert (0 <= quantized_value && quantized_value < 16);
1002  //quantized_color_gradients_ (col_index, row_index) = quantization_map[quantized_value];
1003  //quantized_color_gradients_ (col_index, row_index) = static_cast<unsigned char> ((quantized_value & 7) + 1); // = (quantized_value % 8) + 1
1004  }
1005  }
1006 
1007  //std::cerr << ">>>>> " << min_angle << ", " << max_angle << std::endl;
1008 }
1009 
1010 //////////////////////////////////////////////////////////////////////////////////////////////
1011 template <typename PointInT>
1012 void
1015 {
1016  const std::size_t width = input_->width;
1017  const std::size_t height = input_->height;
1018 
1019  filtered_quantized_color_gradients_.resize (width, height);
1020 
1021  // filter data
1022  for (std::size_t row_index = 1; row_index < height-1; ++row_index)
1023  {
1024  for (std::size_t col_index = 1; col_index < width-1; ++col_index)
1025  {
1026  unsigned char histogram[9] = {0,0,0,0,0,0,0,0,0};
1027 
1028  {
1029  const unsigned char * data_ptr = quantized_color_gradients_.getData () + (row_index-1)*width+col_index-1;
1030  assert (data_ptr[0] < 9 && data_ptr[1] < 9 && data_ptr[2] < 9);
1031  ++histogram[data_ptr[0]];
1032  ++histogram[data_ptr[1]];
1033  ++histogram[data_ptr[2]];
1034  }
1035  {
1036  const unsigned char * data_ptr = quantized_color_gradients_.getData () + row_index*width+col_index-1;
1037  assert (data_ptr[0] < 9 && data_ptr[1] < 9 && data_ptr[2] < 9);
1038  ++histogram[data_ptr[0]];
1039  ++histogram[data_ptr[1]];
1040  ++histogram[data_ptr[2]];
1041  }
1042  {
1043  const unsigned char * data_ptr = quantized_color_gradients_.getData () + (row_index+1)*width+col_index-1;
1044  assert (data_ptr[0] < 9 && data_ptr[1] < 9 && data_ptr[2] < 9);
1045  ++histogram[data_ptr[0]];
1046  ++histogram[data_ptr[1]];
1047  ++histogram[data_ptr[2]];
1048  }
1049 
1050  unsigned char max_hist_value = 0;
1051  int max_hist_index = -1;
1052 
1053  // for (int i = 0; i < 8; ++i)
1054  // {
1055  // if (max_hist_value < histogram[i+1])
1056  // {
1057  // max_hist_index = i;
1058  // max_hist_value = histogram[i+1]
1059  // }
1060  // }
1061  // Unrolled for performance optimization:
1062  if (max_hist_value < histogram[1]) {max_hist_index = 0; max_hist_value = histogram[1];}
1063  if (max_hist_value < histogram[2]) {max_hist_index = 1; max_hist_value = histogram[2];}
1064  if (max_hist_value < histogram[3]) {max_hist_index = 2; max_hist_value = histogram[3];}
1065  if (max_hist_value < histogram[4]) {max_hist_index = 3; max_hist_value = histogram[4];}
1066  if (max_hist_value < histogram[5]) {max_hist_index = 4; max_hist_value = histogram[5];}
1067  if (max_hist_value < histogram[6]) {max_hist_index = 5; max_hist_value = histogram[6];}
1068  if (max_hist_value < histogram[7]) {max_hist_index = 6; max_hist_value = histogram[7];}
1069  if (max_hist_value < histogram[8]) {max_hist_index = 7; max_hist_value = histogram[8];}
1070 
1071  if (max_hist_index != -1 && max_hist_value >= 5)
1072  filtered_quantized_color_gradients_ (col_index, row_index) = static_cast<unsigned char> (0x1 << max_hist_index);
1073  else
1074  filtered_quantized_color_gradients_ (col_index, row_index) = 0;
1075 
1076  }
1077  }
1078 }
1079 
1080 //////////////////////////////////////////////////////////////////////////////////////////////
1081 template <typename PointInT>
1082 void
1084 erode (const pcl::MaskMap & mask_in,
1085  pcl::MaskMap & mask_out)
1086 {
1087  const std::size_t width = mask_in.getWidth ();
1088  const std::size_t height = mask_in.getHeight ();
1089 
1090  mask_out.resize (width, height);
1091 
1092  for (std::size_t row_index = 1; row_index < height-1; ++row_index)
1093  {
1094  for (std::size_t col_index = 1; col_index < width-1; ++col_index)
1095  {
1096  if (mask_in (col_index, row_index-1) == 0 ||
1097  mask_in (col_index-1, row_index) == 0 ||
1098  mask_in (col_index+1, row_index) == 0 ||
1099  mask_in (col_index, row_index+1) == 0)
1100  {
1101  mask_out (col_index, row_index) = 0;
1102  }
1103  else
1104  {
1105  mask_out (col_index, row_index) = 255;
1106  }
1107  }
1108  }
1109 }
Modality based on max-RGB gradients.
void filterQuantizedColorGradients()
Filters the quantized gradients.
pcl::PointCloud< pcl::GradientXY > & getMaxColorGradients()
Returns a point cloud containing the max-RGB gradients.
static void erode(const pcl::MaskMap &mask_in, pcl::MaskMap &mask_out)
Erodes a mask.
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.
FeatureSelectionMethod
Different methods for feature selection/extraction.
void setGradientMagnitudeThresholdForFeatureExtraction(const float threshold)
Sets the threshold for the gradient magnitude which is used for feature extraction.
void computeMaxColorGradientsSobel(const typename pcl::PointCloud< pcl::RGB >::ConstPtr &cloud)
Computes the max-RGB gradients for the specified cloud using sobel.
void setVariableFeatureNr(const bool enabled)
Sets whether variable feature numbers for feature extraction is enabled.
void setSpreadingSize(const std::size_t spreading_size)
Sets the spreading size for spreading the quantized data.
void computeMaxColorGradients(const typename pcl::PointCloud< pcl::RGB >::ConstPtr &cloud)
Computes the max-RGB gradients for the specified cloud.
void setFeatureSelectionMethod(const FeatureSelectionMethod method)
Sets the feature selection method.
virtual void processInputDataFromFiltered()
Processes the input data assuming that everything up to filtering is already done/available (so only ...
virtual void processInputData()
Processes the input data (smoothing, computing gradients, quantizing, filtering, spreading).
void quantizeColorGradients()
Quantizes the color gradients.
void setInputCloud(const typename PointCloudIn::ConstPtr &cloud) override
Provide a pointer to the input dataset (overwrites the PCLBase::setInputCloud method)
QuantizedMap & getSpreadedQuantizedMap() override
Returns a reference to the internally computed spread quantized map.
~ColorGradientModality() override
Destructor.
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 computeGaussianKernel(const std::size_t kernel_size, const float sigma, std::vector< float > &kernel_values)
Computes the Gaussian kernel used for smoothing.
QuantizedMap & getQuantizedMap() override
Returns a reference to the internally computed quantized map.
void setGradientMagnitudeThreshold(const float threshold)
Sets the threshold for the gradient magnitude which is used when quantizing the data.
std::size_t getWidth() const
Definition: mask_map.h:57
void resize(std::size_t width, std::size_t height)
std::size_t getHeight() const
Definition: mask_map.h:63
static PCL_NODISCARD MaskMap getDifferenceMask(const MaskMap &mask0, const MaskMap &mask1)
PCL base class.
Definition: pcl_base.h:70
PointCloudConstPtr input_
The input point cloud dataset.
Definition: pcl_base.h:147
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:403
void resize(std::size_t count)
Resizes the container to contain count elements.
Definition: point_cloud.h:462
std::uint32_t width
The point cloud width (if organized as an image-structure).
Definition: point_cloud.h:398
std::uint32_t height
The point cloud height (if organized as an image-structure).
Definition: point_cloud.h:400
shared_ptr< PointCloud< PointT > > Ptr
Definition: point_cloud.h:413
shared_ptr< const PointCloud< PointInT > > ConstPtr
Definition: point_cloud.h:414
Interface for a quantizable modality.
static void spreadQuantizedMap(const QuantizedMap &input_map, QuantizedMap &output_map, std::size_t spreading_size)
Convolution is a mathematical operation on two functions f and g, producing a third function that is ...
Definition: convolution.h:73
void setInputCloud(const PointCloudInConstPtr &cloud)
Provide a pointer to the input dataset.
Definition: convolution.h:99
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 setBordersPolicy(int policy)
Set the borders policy.
Definition: convolution.h:107
void setKernel(const Eigen::ArrayXf &kernel)
Set convolving kernel.
Definition: convolution.h:104
Defines all the PCL implemented PointT point type structures.
float distance(const PointT &p1, const PointT &p2)
Definition: geometry.h:60
Candidate for a feature (used in feature extraction methods).
bool operator<(const Candidate &rhs) const
Operator for comparing to candidates (by magnitude of the gradient).
A point structure representing Euclidean xyz coordinates, and the intensity value.
Definition: point_types.h:53
Feature that defines a position and quantized value in a specific modality.
std::size_t modality_index
the index of the corresponding modality.
unsigned char quantized_value
the quantized value attached to the feature.
A structure representing RGB color information.