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