Point Cloud Library (PCL) 1.12.0
Loading...
Searching...
No Matches
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
50namespace 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
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
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
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//////////////////////////////////////////////////////////////////////////////////////////////
274template <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//////////////////////////////////////////////////////////////////////////////////////////////
287template <typename PointInT>
292
293//////////////////////////////////////////////////////////////////////////////////////////////
294template <typename PointInT> void
296computeGaussianKernel (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//////////////////////////////////////////////////////////////////////////////////////////////
339template <typename PointInT>
340void
343{
344 // compute gaussian kernel values
345 const std::size_t kernel_size = 7;
346 std::vector<float> kernel_values;
347 computeGaussianKernel (kernel_size, 0.0f, kernel_values);
348
349 // smooth input
351 Eigen::ArrayXf gaussian_kernel(kernel_size);
352 //gaussian_kernel << 1.f/16, 1.f/8, 3.f/16, 2.f/8, 3.f/16, 1.f/8, 1.f/16;
353 //gaussian_kernel << 16.f/1600.f, 32.f/1600.f, 64.f/1600.f, 128.f/1600.f, 256.f/1600.f, 128.f/1600.f, 64.f/1600.f, 32.f/1600.f, 16.f/1600.f;
355
357
358 const std::uint32_t width = input_->width;
359 const std::uint32_t height = input_->height;
360
361 rgb_input_->resize (width*height);
362 rgb_input_->width = width;
363 rgb_input_->height = height;
364 rgb_input_->is_dense = input_->is_dense;
365 for (std::size_t row_index = 0; row_index < height; ++row_index)
366 {
367 for (std::size_t col_index = 0; col_index < width; ++col_index)
368 {
369 (*rgb_input_) (col_index, row_index).r = (*input_) (col_index, row_index).r;
370 (*rgb_input_) (col_index, row_index).g = (*input_) (col_index, row_index).g;
371 (*rgb_input_) (col_index, row_index).b = (*input_) (col_index, row_index).b;
372 }
373 }
374
375 convolution.setInputCloud (rgb_input_);
376 convolution.setKernel (gaussian_kernel);
377
378 convolution.convolve (*smoothed_input_);
379
380 // extract color gradients
381 computeMaxColorGradientsSobel (smoothed_input_);
382
383 // quantize gradients
384 quantizeColorGradients ();
385
386 // filter quantized gradients to get only dominants one + thresholding
387 filterQuantizedColorGradients ();
388
389 // spread filtered quantized gradients
390 //spreadFilteredQunatizedColorGradients ();
391 pcl::QuantizedMap::spreadQuantizedMap (filtered_quantized_color_gradients_,
392 spreaded_filtered_quantized_color_gradients_,
393 spreading_size_);
394}
395
396//////////////////////////////////////////////////////////////////////////////////////////////
397template <typename PointInT>
398void
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//////////////////////////////////////////////////////////////////////////////////////////////
410template <typename PointInT>
412extractFeatures (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 {
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;
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;
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
499 {
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
515 {
517 }
518 }
519
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 {
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 {
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;
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 {
607 erode (mask, eroded_mask);
608
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 {
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 {
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)
666 {
667 candidate_accepted = false;
668 break;
669 }
670 }
671
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 {
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//////////////////////////////////////////////////////////////////////////////////////////////
696template <typename PointInT> void
698extractAllFeatures (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 {
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 {
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//////////////////////////////////////////////////////////////////////////////////////////////
745template <typename PointInT>
746void
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
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//////////////////////////////////////////////////////////////////////////////////////////////
832template <typename PointInT>
833void
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
909 {
910 GradientXY gradient;
911 gradient.magnitude = std::sqrt (static_cast<float> (sqr_mag_r));
912 gradient.angle = std::atan2 (static_cast<float> (r_dy), static_cast<float> (r_dx)) * 180.0f / pi;
913 if (gradient.angle < -180.0f) gradient.angle += 360.0f;
914 if (gradient.angle >= 180.0f) gradient.angle -= 360.0f;
915 gradient.x = static_cast<float> (col_index);
916 gradient.y = static_cast<float> (row_index);
917
918 color_gradients_ (col_index, row_index) = gradient;
919 }
920 else if (sqr_mag_g > sqr_mag_b)
921 {
922 GradientXY gradient;
923 gradient.magnitude = std::sqrt (static_cast<float> (sqr_mag_g));
924 gradient.angle = std::atan2 (static_cast<float> (g_dy), static_cast<float> (g_dx)) * 180.0f / pi;
925 if (gradient.angle < -180.0f) gradient.angle += 360.0f;
926 if (gradient.angle >= 180.0f) gradient.angle -= 360.0f;
927 gradient.x = static_cast<float> (col_index);
928 gradient.y = static_cast<float> (row_index);
929
930 color_gradients_ (col_index, row_index) = gradient;
931 }
932 else
933 {
934 GradientXY gradient;
935 gradient.magnitude = std::sqrt (static_cast<float> (sqr_mag_b));
936 gradient.angle = std::atan2 (static_cast<float> (b_dy), static_cast<float> (b_dx)) * 180.0f / pi;
937 if (gradient.angle < -180.0f) gradient.angle += 360.0f;
938 if (gradient.angle >= 180.0f) gradient.angle -= 360.0f;
939 gradient.x = static_cast<float> (col_index);
940 gradient.y = static_cast<float> (row_index);
941
942 color_gradients_ (col_index, row_index) = gradient;
943 }
944
945 assert (color_gradients_ (col_index, row_index).angle >= -180 &&
946 color_gradients_ (col_index, row_index).angle <= 180);
947 }
948 }
949
950 return;
951}
952
953//////////////////////////////////////////////////////////////////////////////////////////////
954template <typename PointInT>
955void
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//////////////////////////////////////////////////////////////////////////////////////////////
1017template <typename PointInT>
1018void
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//////////////////////////////////////////////////////////////////////////////////////////////
1087template <typename PointInT>
1088void
1090erode (const pcl::MaskMap & mask_in,
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 {
1108 }
1109 else
1110 {
1111 mask_out (col_index, row_index) = 255;
1112 }
1113 }
1114 }
1115}
Modality based on max-RGB gradients.
void filterQuantizedColorGradients()
Filters the quantized 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.
QuantizedMap & getQuantizedMap() override
Returns a reference to the internally computed quantized map.
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.
pcl::PointCloud< pcl::GradientXY > & getMaxColorGradients()
Returns a point cloud containing the max-RGB gradients.
void extractFeatures(const MaskMap &mask, std::size_t nr_features, std::size_t modalityIndex, std::vector< QuantizedMultiModFeature > &features) const override
Extracts features from this modality within the specified mask.
void computeGaussianKernel(const std::size_t kernel_size, const float sigma, std::vector< float > &kernel_values)
Computes the Gaussian kernel used for smoothing.
void setGradientMagnitudeThreshold(const float threshold)
Sets the threshold for the gradient magnitude which is used when quantizing the data.
Iterator class for point clouds with or without given indices.
std::size_t size() const
Size of the range the iterator is going through.
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
shared_ptr< PointCloud< PointT > > Ptr
shared_ptr< const PointCloud< PointInT > > ConstPtr
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
Defines all the PCL implemented PointT point type structures.
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.