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