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