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