00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037
00038 #ifndef PCL_RECOGNITION_COLOR_GRADIENT_MODALITY
00039 #define PCL_RECOGNITION_COLOR_GRADIENT_MODALITY
00040
00041 #include <pcl/recognition/quantizable_modality.h>
00042
00043 #include <pcl/pcl_base.h>
00044 #include <pcl/point_cloud.h>
00045 #include <pcl/point_types.h>
00046 #include <pcl/recognition/point_types.h>
00047 #include <pcl/filters/convolution.h>
00048
00049 #include <list>
00050
00051 namespace pcl
00052 {
00053
00057 template <typename PointInT>
00058 class ColorGradientModality
00059 : public QuantizableModality, public PCLBase<PointInT>
00060 {
00061 protected:
00062 using PCLBase<PointInT>::input_;
00063
00065 struct Candidate
00066 {
00068 GradientXY gradient;
00069
00071 int x;
00073 int y;
00074
00078 bool operator< (const Candidate & rhs)
00079 {
00080 return (gradient.magnitude > rhs.gradient.magnitude);
00081 }
00082 };
00083
00084 public:
00085 typedef typename pcl::PointCloud<PointInT> PointCloudIn;
00086
00088 enum FeatureSelectionMethod
00089 {
00090 MASK_BORDER_HIGH_GRADIENTS,
00091 MASK_BORDER_EQUALLY,
00092 DISTANCE_MAGNITUDE_SCORE
00093 };
00094
00096 ColorGradientModality ();
00098 virtual ~ColorGradientModality ();
00099
00104 inline void
00105 setGradientMagnitudeThreshold (const float threshold)
00106 {
00107 gradient_magnitude_threshold_ = threshold;
00108 }
00109
00114 inline void
00115 setGradientMagnitudeThresholdForFeatureExtraction (const float threshold)
00116 {
00117 gradient_magnitude_threshold_feature_extraction_ = threshold;
00118 }
00119
00123 inline void
00124 setFeatureSelectionMethod (const FeatureSelectionMethod method)
00125 {
00126 feature_selection_method_ = method;
00127 }
00128
00130 inline void
00131 setSpreadingSize (const size_t spreading_size)
00132 {
00133 spreading_size_ = spreading_size;
00134 }
00135
00139 inline void
00140 setVariableFeatureNr (const bool enabled)
00141 {
00142 variable_feature_nr_ = enabled;
00143 }
00144
00146 inline QuantizedMap &
00147 getQuantizedMap ()
00148 {
00149 return (filtered_quantized_color_gradients_);
00150 }
00151
00153 inline QuantizedMap &
00154 getSpreadedQuantizedMap ()
00155 {
00156 return (spreaded_filtered_quantized_color_gradients_);
00157 }
00158
00160 inline pcl::PointCloud<pcl::GradientXY> &
00161 getMaxColorGradients ()
00162 {
00163 return (color_gradients_);
00164 }
00165
00173 void
00174 extractFeatures (const MaskMap & mask, size_t nr_features, size_t modalityIndex,
00175 std::vector<QuantizedMultiModFeature> & features) const;
00176
00183 void
00184 extractAllFeatures (const MaskMap & mask, size_t nr_features, size_t modalityIndex,
00185 std::vector<QuantizedMultiModFeature> & features) const;
00186
00190 virtual void
00191 setInputCloud (const typename PointCloudIn::ConstPtr & cloud)
00192 {
00193 input_ = cloud;
00194 }
00195
00197 virtual void
00198 processInputData ();
00199
00202 virtual void
00203 processInputDataFromFiltered ();
00204
00205 protected:
00206
00211 void
00212 computeGaussianKernel (const size_t kernel_size, const float sigma, std::vector <float> & kernel_values);
00213
00217 void
00218 computeMaxColorGradients (const typename pcl::PointCloud<pcl::RGB>::ConstPtr & cloud);
00219
00223 void
00224 computeMaxColorGradientsSobel (const typename pcl::PointCloud<pcl::RGB>::ConstPtr & cloud);
00225
00227 void
00228 quantizeColorGradients ();
00229
00231 void
00232 filterQuantizedColorGradients ();
00233
00238 static void
00239 erode (const pcl::MaskMap & mask_in, pcl::MaskMap & mask_out);
00240
00241 private:
00242
00244 bool variable_feature_nr_;
00245
00247 pcl::PointCloud<pcl::RGB>::Ptr smoothed_input_;
00248
00250 FeatureSelectionMethod feature_selection_method_;
00251
00253 float gradient_magnitude_threshold_;
00255 float gradient_magnitude_threshold_feature_extraction_;
00256
00258 pcl::PointCloud<pcl::GradientXY> color_gradients_;
00259
00261 size_t spreading_size_;
00262
00264 pcl::QuantizedMap quantized_color_gradients_;
00266 pcl::QuantizedMap filtered_quantized_color_gradients_;
00268 pcl::QuantizedMap spreaded_filtered_quantized_color_gradients_;
00269
00270 };
00271
00272 }
00273
00275 template <typename PointInT>
00276 pcl::ColorGradientModality<PointInT>::
00277 ColorGradientModality ()
00278 : variable_feature_nr_ (false)
00279 , smoothed_input_ (new pcl::PointCloud<pcl::RGB> ())
00280 , feature_selection_method_ (DISTANCE_MAGNITUDE_SCORE)
00281 , gradient_magnitude_threshold_ (10.0f)
00282 , gradient_magnitude_threshold_feature_extraction_ (55.0f)
00283 , color_gradients_ ()
00284 , spreading_size_ (8)
00285 , quantized_color_gradients_ ()
00286 , filtered_quantized_color_gradients_ ()
00287 , spreaded_filtered_quantized_color_gradients_ ()
00288 {
00289 }
00290
00292 template <typename PointInT>
00293 pcl::ColorGradientModality<PointInT>::
00294 ~ColorGradientModality ()
00295 {
00296 }
00297
00299 template <typename PointInT> void
00300 pcl::ColorGradientModality<PointInT>::
00301 computeGaussianKernel (const size_t kernel_size, const float sigma, std::vector <float> & kernel_values)
00302 {
00303
00304 const int n = int (kernel_size);
00305 const int SMALL_GAUSSIAN_SIZE = 7;
00306 static const float small_gaussian_tab[][SMALL_GAUSSIAN_SIZE] =
00307 {
00308 {1.f},
00309 {0.25f, 0.5f, 0.25f},
00310 {0.0625f, 0.25f, 0.375f, 0.25f, 0.0625f},
00311 {0.03125f, 0.109375f, 0.21875f, 0.28125f, 0.21875f, 0.109375f, 0.03125f}
00312 };
00313
00314 const float* fixed_kernel = n % 2 == 1 && n <= SMALL_GAUSSIAN_SIZE && sigma <= 0 ?
00315 small_gaussian_tab[n>>1] : 0;
00316
00317
00318
00319 kernel_values.resize (n);
00320 float* cf = &(kernel_values[0]);
00321
00322
00323 double sigmaX = sigma > 0 ? sigma : ((n-1)*0.5 - 1)*0.3 + 0.8;
00324 double scale2X = -0.5/(sigmaX*sigmaX);
00325 double sum = 0;
00326
00327 int i;
00328 for( i = 0; i < n; i++ )
00329 {
00330 double x = i - (n-1)*0.5;
00331 double t = fixed_kernel ? double (fixed_kernel[i]) : std::exp (scale2X*x*x);
00332
00333 cf[i] = float (t);
00334 sum += cf[i];
00335 }
00336
00337 sum = 1./sum;
00338 for (i = 0; i < n; i++ )
00339 {
00340 cf[i] = float (cf[i]*sum);
00341 }
00342 }
00343
00345 template <typename PointInT>
00346 void
00347 pcl::ColorGradientModality<PointInT>::
00348 processInputData ()
00349 {
00350
00351 const size_t kernel_size = 7;
00352 std::vector<float> kernel_values;
00353 computeGaussianKernel (kernel_size, 0.0f, kernel_values);
00354
00355
00356 pcl::filters::Convolution<pcl::RGB, pcl::RGB> convolution;
00357 Eigen::ArrayXf gaussian_kernel(kernel_size);
00358
00359
00360 gaussian_kernel << kernel_values[0], kernel_values[1], kernel_values[2], kernel_values[3], kernel_values[4], kernel_values[5], kernel_values[6];
00361
00362 pcl::PointCloud<pcl::RGB>::Ptr rgb_input_ (new pcl::PointCloud<pcl::RGB>());
00363
00364 const uint32_t width = input_->width;
00365 const uint32_t height = input_->height;
00366
00367 rgb_input_->resize (width*height);
00368 rgb_input_->width = width;
00369 rgb_input_->height = height;
00370 rgb_input_->is_dense = input_->is_dense;
00371 for (size_t row_index = 0; row_index < height; ++row_index)
00372 {
00373 for (size_t col_index = 0; col_index < width; ++col_index)
00374 {
00375 (*rgb_input_) (col_index, row_index).r = (*input_) (col_index, row_index).r;
00376 (*rgb_input_) (col_index, row_index).g = (*input_) (col_index, row_index).g;
00377 (*rgb_input_) (col_index, row_index).b = (*input_) (col_index, row_index).b;
00378 }
00379 }
00380
00381 convolution.setInputCloud (rgb_input_);
00382 convolution.setKernel (gaussian_kernel);
00383
00384 convolution.convolve (*smoothed_input_);
00385
00386
00387 computeMaxColorGradientsSobel (smoothed_input_);
00388
00389
00390 quantizeColorGradients ();
00391
00392
00393 filterQuantizedColorGradients ();
00394
00395
00396
00397 pcl::QuantizedMap::spreadQuantizedMap (filtered_quantized_color_gradients_,
00398 spreaded_filtered_quantized_color_gradients_,
00399 spreading_size_);
00400 }
00401
00403 template <typename PointInT>
00404 void
00405 pcl::ColorGradientModality<PointInT>::
00406 processInputDataFromFiltered ()
00407 {
00408
00409
00410 pcl::QuantizedMap::spreadQuantizedMap (filtered_quantized_color_gradients_,
00411 spreaded_filtered_quantized_color_gradients_,
00412 spreading_size_);
00413 }
00414
00416 template <typename PointInT>
00417 void pcl::ColorGradientModality<PointInT>::
00418 extractFeatures (const MaskMap & mask, const size_t nr_features, const size_t modality_index,
00419 std::vector<QuantizedMultiModFeature> & features) const
00420 {
00421 const size_t width = mask.getWidth ();
00422 const size_t height = mask.getHeight ();
00423
00424 std::list<Candidate> list1;
00425 std::list<Candidate> list2;
00426
00427
00428 if (feature_selection_method_ == DISTANCE_MAGNITUDE_SCORE)
00429 {
00430 for (size_t row_index = 0; row_index < height; ++row_index)
00431 {
00432 for (size_t col_index = 0; col_index < width; ++col_index)
00433 {
00434 if (mask (col_index, row_index) != 0)
00435 {
00436 const GradientXY & gradient = color_gradients_ (col_index, row_index);
00437 if (gradient.magnitude > gradient_magnitude_threshold_feature_extraction_
00438 && filtered_quantized_color_gradients_ (col_index, row_index) != 0)
00439 {
00440 Candidate candidate;
00441 candidate.gradient = gradient;
00442 candidate.x = static_cast<int> (col_index);
00443 candidate.y = static_cast<int> (row_index);
00444
00445 list1.push_back (candidate);
00446 }
00447 }
00448 }
00449 }
00450
00451 list1.sort();
00452
00453 if (variable_feature_nr_)
00454 {
00455 list2.push_back (*(list1.begin ()));
00456
00457 bool feature_selection_finished = false;
00458 while (!feature_selection_finished)
00459 {
00460 float best_score = 0.0f;
00461 typename std::list<Candidate>::iterator best_iter = list1.end ();
00462 for (typename std::list<Candidate>::iterator iter1 = list1.begin (); iter1 != list1.end (); ++iter1)
00463 {
00464
00465 float smallest_distance = std::numeric_limits<float>::max ();
00466 for (typename std::list<Candidate>::iterator iter2 = list2.begin (); iter2 != list2.end (); ++iter2)
00467 {
00468 const float dx = static_cast<float> (iter1->x) - static_cast<float> (iter2->x);
00469 const float dy = static_cast<float> (iter1->y) - static_cast<float> (iter2->y);
00470
00471 const float distance = dx*dx + dy*dy;
00472
00473 if (distance < smallest_distance)
00474 {
00475 smallest_distance = distance;
00476 }
00477 }
00478
00479 const float score = smallest_distance * iter1->gradient.magnitude;
00480
00481 if (score > best_score)
00482 {
00483 best_score = score;
00484 best_iter = iter1;
00485 }
00486 }
00487
00488
00489 float min_min_sqr_distance = std::numeric_limits<float>::max ();
00490 float max_min_sqr_distance = 0;
00491 for (typename std::list<Candidate>::iterator iter2 = list2.begin (); iter2 != list2.end (); ++iter2)
00492 {
00493 float min_sqr_distance = std::numeric_limits<float>::max ();
00494 for (typename std::list<Candidate>::iterator iter3 = list2.begin (); iter3 != list2.end (); ++iter3)
00495 {
00496 if (iter2 == iter3)
00497 continue;
00498
00499 const float dx = static_cast<float> (iter2->x) - static_cast<float> (iter3->x);
00500 const float dy = static_cast<float> (iter2->y) - static_cast<float> (iter3->y);
00501
00502 const float sqr_distance = dx*dx + dy*dy;
00503
00504 if (sqr_distance < min_sqr_distance)
00505 {
00506 min_sqr_distance = sqr_distance;
00507 }
00508
00509
00510 }
00511
00512
00513
00514 {
00515 const float dx = static_cast<float> (iter2->x) - static_cast<float> (best_iter->x);
00516 const float dy = static_cast<float> (iter2->y) - static_cast<float> (best_iter->y);
00517
00518 const float sqr_distance = dx*dx + dy*dy;
00519
00520 if (sqr_distance < min_sqr_distance)
00521 {
00522 min_sqr_distance = sqr_distance;
00523 }
00524 }
00525
00526 if (min_sqr_distance < min_min_sqr_distance)
00527 min_min_sqr_distance = min_sqr_distance;
00528 if (min_sqr_distance > max_min_sqr_distance)
00529 max_min_sqr_distance = min_sqr_distance;
00530
00531
00532 }
00533
00534 if (best_iter != list1.end ())
00535 {
00536
00537
00538
00539
00540 if (min_min_sqr_distance < 50)
00541 {
00542 feature_selection_finished = true;
00543 break;
00544 }
00545
00546 list2.push_back (*best_iter);
00547 }
00548 }
00549 }
00550 else
00551 {
00552 if (list1.size () <= nr_features)
00553 {
00554 for (typename std::list<Candidate>::iterator iter1 = list1.begin (); iter1 != list1.end (); ++iter1)
00555 {
00556 QuantizedMultiModFeature feature;
00557
00558 feature.x = iter1->x;
00559 feature.y = iter1->y;
00560 feature.modality_index = modality_index;
00561 feature.quantized_value = filtered_quantized_color_gradients_ (iter1->x, iter1->y);
00562
00563 features.push_back (feature);
00564 }
00565 return;
00566 }
00567
00568 list2.push_back (*(list1.begin ()));
00569 while (list2.size () != nr_features)
00570 {
00571 float best_score = 0.0f;
00572 typename std::list<Candidate>::iterator best_iter = list1.end ();
00573 for (typename std::list<Candidate>::iterator iter1 = list1.begin (); iter1 != list1.end (); ++iter1)
00574 {
00575
00576 float smallest_distance = std::numeric_limits<float>::max ();
00577 for (typename std::list<Candidate>::iterator iter2 = list2.begin (); iter2 != list2.end (); ++iter2)
00578 {
00579 const float dx = static_cast<float> (iter1->x) - static_cast<float> (iter2->x);
00580 const float dy = static_cast<float> (iter1->y) - static_cast<float> (iter2->y);
00581
00582 const float distance = dx*dx + dy*dy;
00583
00584 if (distance < smallest_distance)
00585 {
00586 smallest_distance = distance;
00587 }
00588 }
00589
00590 const float score = smallest_distance * iter1->gradient.magnitude;
00591
00592 if (score > best_score)
00593 {
00594 best_score = score;
00595 best_iter = iter1;
00596 }
00597 }
00598
00599 if (best_iter != list1.end ())
00600 {
00601 list2.push_back (*best_iter);
00602 }
00603 else
00604 {
00605 break;
00606 }
00607 }
00608 }
00609 }
00610 else if (feature_selection_method_ == MASK_BORDER_HIGH_GRADIENTS || feature_selection_method_ == MASK_BORDER_EQUALLY)
00611 {
00612 MaskMap eroded_mask;
00613 erode (mask, eroded_mask);
00614
00615 MaskMap diff_mask;
00616 MaskMap::getDifferenceMask (mask, eroded_mask, diff_mask);
00617
00618 for (size_t row_index = 0; row_index < height; ++row_index)
00619 {
00620 for (size_t col_index = 0; col_index < width; ++col_index)
00621 {
00622 if (diff_mask (col_index, row_index) != 0)
00623 {
00624 const GradientXY & gradient = color_gradients_ (col_index, row_index);
00625 if ((feature_selection_method_ == MASK_BORDER_EQUALLY || gradient.magnitude > gradient_magnitude_threshold_feature_extraction_)
00626 && filtered_quantized_color_gradients_ (col_index, row_index) != 0)
00627 {
00628 Candidate candidate;
00629 candidate.gradient = gradient;
00630 candidate.x = static_cast<int> (col_index);
00631 candidate.y = static_cast<int> (row_index);
00632
00633 list1.push_back (candidate);
00634 }
00635 }
00636 }
00637 }
00638
00639 list1.sort();
00640
00641 if (list1.size () <= nr_features)
00642 {
00643 for (typename std::list<Candidate>::iterator iter1 = list1.begin (); iter1 != list1.end (); ++iter1)
00644 {
00645 QuantizedMultiModFeature feature;
00646
00647 feature.x = iter1->x;
00648 feature.y = iter1->y;
00649 feature.modality_index = modality_index;
00650 feature.quantized_value = filtered_quantized_color_gradients_ (iter1->x, iter1->y);
00651
00652 features.push_back (feature);
00653 }
00654 return;
00655 }
00656
00657 size_t distance = list1.size () / nr_features + 1;
00658 while (list2.size () != nr_features)
00659 {
00660 const size_t sqr_distance = distance*distance;
00661 for (typename std::list<Candidate>::iterator iter1 = list1.begin (); iter1 != list1.end (); ++iter1)
00662 {
00663 bool candidate_accepted = true;
00664
00665 for (typename std::list<Candidate>::iterator iter2 = list2.begin (); iter2 != list2.end (); ++iter2)
00666 {
00667 const int dx = iter1->x - iter2->x;
00668 const int dy = iter1->y - iter2->y;
00669 const unsigned int tmp_distance = dx*dx + dy*dy;
00670
00671
00672 if (tmp_distance < sqr_distance)
00673 {
00674 candidate_accepted = false;
00675 break;
00676 }
00677 }
00678
00679 if (candidate_accepted)
00680 list2.push_back (*iter1);
00681
00682 if (list2.size () == nr_features)
00683 break;
00684 }
00685 --distance;
00686 }
00687 }
00688
00689 for (typename std::list<Candidate>::iterator iter2 = list2.begin (); iter2 != list2.end (); ++iter2)
00690 {
00691 QuantizedMultiModFeature feature;
00692
00693 feature.x = iter2->x;
00694 feature.y = iter2->y;
00695 feature.modality_index = modality_index;
00696 feature.quantized_value = filtered_quantized_color_gradients_ (iter2->x, iter2->y);
00697
00698 features.push_back (feature);
00699 }
00700 }
00701
00703 template <typename PointInT> void
00704 pcl::ColorGradientModality<PointInT>::
00705 extractAllFeatures (const MaskMap & mask, const size_t, const size_t modality_index,
00706 std::vector<QuantizedMultiModFeature> & features) const
00707 {
00708 const size_t width = mask.getWidth ();
00709 const size_t height = mask.getHeight ();
00710
00711 std::list<Candidate> list1;
00712 std::list<Candidate> list2;
00713
00714
00715 for (size_t row_index = 0; row_index < height; ++row_index)
00716 {
00717 for (size_t col_index = 0; col_index < width; ++col_index)
00718 {
00719 if (mask (col_index, row_index) != 0)
00720 {
00721 const GradientXY & gradient = color_gradients_ (col_index, row_index);
00722 if (gradient.magnitude > gradient_magnitude_threshold_feature_extraction_
00723 && filtered_quantized_color_gradients_ (col_index, row_index) != 0)
00724 {
00725 Candidate candidate;
00726 candidate.gradient = gradient;
00727 candidate.x = static_cast<int> (col_index);
00728 candidate.y = static_cast<int> (row_index);
00729
00730 list1.push_back (candidate);
00731 }
00732 }
00733 }
00734 }
00735
00736 list1.sort();
00737
00738 for (typename std::list<Candidate>::iterator iter1 = list1.begin (); iter1 != list1.end (); ++iter1)
00739 {
00740 QuantizedMultiModFeature feature;
00741
00742 feature.x = iter1->x;
00743 feature.y = iter1->y;
00744 feature.modality_index = modality_index;
00745 feature.quantized_value = filtered_quantized_color_gradients_ (iter1->x, iter1->y);
00746
00747 features.push_back (feature);
00748 }
00749 }
00750
00752 template <typename PointInT>
00753 void
00754 pcl::ColorGradientModality<PointInT>::
00755 computeMaxColorGradients (const typename pcl::PointCloud<pcl::RGB>::ConstPtr & cloud)
00756 {
00757 const int width = cloud->width;
00758 const int height = cloud->height;
00759
00760 color_gradients_.points.resize (width*height);
00761 color_gradients_.width = width;
00762 color_gradients_.height = height;
00763
00764 const float pi = tan (1.0f) * 2;
00765 for (int row_index = 0; row_index < height-2; ++row_index)
00766 {
00767 for (int col_index = 0; col_index < width-2; ++col_index)
00768 {
00769 const int index0 = row_index*width+col_index;
00770 const int index_c = row_index*width+col_index+2;
00771 const int index_r = (row_index+2)*width+col_index;
00772
00773
00774
00775 const unsigned char r0 = cloud->points[index0].r;
00776 const unsigned char g0 = cloud->points[index0].g;
00777 const unsigned char b0 = cloud->points[index0].b;
00778
00779 const unsigned char r_c = cloud->points[index_c].r;
00780 const unsigned char g_c = cloud->points[index_c].g;
00781 const unsigned char b_c = cloud->points[index_c].b;
00782
00783 const unsigned char r_r = cloud->points[index_r].r;
00784 const unsigned char g_r = cloud->points[index_r].g;
00785 const unsigned char b_r = cloud->points[index_r].b;
00786
00787 const float r_dx = static_cast<float> (r_c) - static_cast<float> (r0);
00788 const float g_dx = static_cast<float> (g_c) - static_cast<float> (g0);
00789 const float b_dx = static_cast<float> (b_c) - static_cast<float> (b0);
00790
00791 const float r_dy = static_cast<float> (r_r) - static_cast<float> (r0);
00792 const float g_dy = static_cast<float> (g_r) - static_cast<float> (g0);
00793 const float b_dy = static_cast<float> (b_r) - static_cast<float> (b0);
00794
00795 const float sqr_mag_r = r_dx*r_dx + r_dy*r_dy;
00796 const float sqr_mag_g = g_dx*g_dx + g_dy*g_dy;
00797 const float sqr_mag_b = b_dx*b_dx + b_dy*b_dy;
00798
00799 if (sqr_mag_r > sqr_mag_g && sqr_mag_r > sqr_mag_b)
00800 {
00801 GradientXY gradient;
00802 gradient.magnitude = sqrt (sqr_mag_r);
00803 gradient.angle = atan2 (r_dy, r_dx) * 180.0f / pi;
00804 gradient.x = static_cast<float> (col_index);
00805 gradient.y = static_cast<float> (row_index);
00806
00807 color_gradients_ (col_index+1, row_index+1) = gradient;
00808 }
00809 else if (sqr_mag_g > sqr_mag_b)
00810 {
00811 GradientXY gradient;
00812 gradient.magnitude = sqrt (sqr_mag_g);
00813 gradient.angle = atan2 (g_dy, g_dx) * 180.0f / pi;
00814 gradient.x = static_cast<float> (col_index);
00815 gradient.y = static_cast<float> (row_index);
00816
00817 color_gradients_ (col_index+1, row_index+1) = gradient;
00818 }
00819 else
00820 {
00821 GradientXY gradient;
00822 gradient.magnitude = sqrt (sqr_mag_b);
00823 gradient.angle = atan2 (b_dy, b_dx) * 180.0f / pi;
00824 gradient.x = static_cast<float> (col_index);
00825 gradient.y = static_cast<float> (row_index);
00826
00827 color_gradients_ (col_index+1, row_index+1) = gradient;
00828 }
00829
00830 assert (color_gradients_ (col_index+1, row_index+1).angle >= -180 &&
00831 color_gradients_ (col_index+1, row_index+1).angle <= 180);
00832 }
00833 }
00834
00835 return;
00836 }
00837
00839 template <typename PointInT>
00840 void
00841 pcl::ColorGradientModality<PointInT>::
00842 computeMaxColorGradientsSobel (const typename pcl::PointCloud<pcl::RGB>::ConstPtr & cloud)
00843 {
00844 const int width = cloud->width;
00845 const int height = cloud->height;
00846
00847 color_gradients_.points.resize (width*height);
00848 color_gradients_.width = width;
00849 color_gradients_.height = height;
00850
00851 const float pi = tanf (1.0f) * 2.0f;
00852 for (int row_index = 1; row_index < height-1; ++row_index)
00853 {
00854 for (int col_index = 1; col_index < width-1; ++col_index)
00855 {
00856 const int r7 = static_cast<int> (cloud->points[(row_index-1)*width + (col_index-1)].r);
00857 const int g7 = static_cast<int> (cloud->points[(row_index-1)*width + (col_index-1)].g);
00858 const int b7 = static_cast<int> (cloud->points[(row_index-1)*width + (col_index-1)].b);
00859 const int r8 = static_cast<int> (cloud->points[(row_index-1)*width + (col_index)].r);
00860 const int g8 = static_cast<int> (cloud->points[(row_index-1)*width + (col_index)].g);
00861 const int b8 = static_cast<int> (cloud->points[(row_index-1)*width + (col_index)].b);
00862 const int r9 = static_cast<int> (cloud->points[(row_index-1)*width + (col_index+1)].r);
00863 const int g9 = static_cast<int> (cloud->points[(row_index-1)*width + (col_index+1)].g);
00864 const int b9 = static_cast<int> (cloud->points[(row_index-1)*width + (col_index+1)].b);
00865 const int r4 = static_cast<int> (cloud->points[(row_index)*width + (col_index-1)].r);
00866 const int g4 = static_cast<int> (cloud->points[(row_index)*width + (col_index-1)].g);
00867 const int b4 = static_cast<int> (cloud->points[(row_index)*width + (col_index-1)].b);
00868 const int r6 = static_cast<int> (cloud->points[(row_index)*width + (col_index+1)].r);
00869 const int g6 = static_cast<int> (cloud->points[(row_index)*width + (col_index+1)].g);
00870 const int b6 = static_cast<int> (cloud->points[(row_index)*width + (col_index+1)].b);
00871 const int r1 = static_cast<int> (cloud->points[(row_index+1)*width + (col_index-1)].r);
00872 const int g1 = static_cast<int> (cloud->points[(row_index+1)*width + (col_index-1)].g);
00873 const int b1 = static_cast<int> (cloud->points[(row_index+1)*width + (col_index-1)].b);
00874 const int r2 = static_cast<int> (cloud->points[(row_index+1)*width + (col_index)].r);
00875 const int g2 = static_cast<int> (cloud->points[(row_index+1)*width + (col_index)].g);
00876 const int b2 = static_cast<int> (cloud->points[(row_index+1)*width + (col_index)].b);
00877 const int r3 = static_cast<int> (cloud->points[(row_index+1)*width + (col_index+1)].r);
00878 const int g3 = static_cast<int> (cloud->points[(row_index+1)*width + (col_index+1)].g);
00879 const int b3 = static_cast<int> (cloud->points[(row_index+1)*width + (col_index+1)].b);
00880
00881
00882
00883
00884
00885
00886
00889
00890
00891
00892
00893
00894
00895
00896
00897
00898
00899
00900
00901
00902
00903
00904 const int r_dx = r9 + 2*r6 + r3 - (r7 + 2*r4 + r1);
00905 const int r_dy = r1 + 2*r2 + r3 - (r7 + 2*r8 + r9);
00906 const int g_dx = g9 + 2*g6 + g3 - (g7 + 2*g4 + g1);
00907 const int g_dy = g1 + 2*g2 + g3 - (g7 + 2*g8 + g9);
00908 const int b_dx = b9 + 2*b6 + b3 - (b7 + 2*b4 + b1);
00909 const int b_dy = b1 + 2*b2 + b3 - (b7 + 2*b8 + b9);
00910
00911 const int sqr_mag_r = r_dx*r_dx + r_dy*r_dy;
00912 const int sqr_mag_g = g_dx*g_dx + g_dy*g_dy;
00913 const int sqr_mag_b = b_dx*b_dx + b_dy*b_dy;
00914
00915 if (sqr_mag_r > sqr_mag_g && sqr_mag_r > sqr_mag_b)
00916 {
00917 GradientXY gradient;
00918 gradient.magnitude = sqrtf (static_cast<float> (sqr_mag_r));
00919 gradient.angle = atan2f (static_cast<float> (r_dy), static_cast<float> (r_dx)) * 180.0f / pi;
00920 if (gradient.angle < -180.0f) gradient.angle += 360.0f;
00921 if (gradient.angle >= 180.0f) gradient.angle -= 360.0f;
00922 gradient.x = static_cast<float> (col_index);
00923 gradient.y = static_cast<float> (row_index);
00924
00925 color_gradients_ (col_index, row_index) = gradient;
00926 }
00927 else if (sqr_mag_g > sqr_mag_b)
00928 {
00929 GradientXY gradient;
00930 gradient.magnitude = sqrtf (static_cast<float> (sqr_mag_g));
00931 gradient.angle = atan2f (static_cast<float> (g_dy), static_cast<float> (g_dx)) * 180.0f / pi;
00932 if (gradient.angle < -180.0f) gradient.angle += 360.0f;
00933 if (gradient.angle >= 180.0f) gradient.angle -= 360.0f;
00934 gradient.x = static_cast<float> (col_index);
00935 gradient.y = static_cast<float> (row_index);
00936
00937 color_gradients_ (col_index, row_index) = gradient;
00938 }
00939 else
00940 {
00941 GradientXY gradient;
00942 gradient.magnitude = sqrtf (static_cast<float> (sqr_mag_b));
00943 gradient.angle = atan2f (static_cast<float> (b_dy), static_cast<float> (b_dx)) * 180.0f / pi;
00944 if (gradient.angle < -180.0f) gradient.angle += 360.0f;
00945 if (gradient.angle >= 180.0f) gradient.angle -= 360.0f;
00946 gradient.x = static_cast<float> (col_index);
00947 gradient.y = static_cast<float> (row_index);
00948
00949 color_gradients_ (col_index, row_index) = gradient;
00950 }
00951
00952 assert (color_gradients_ (col_index, row_index).angle >= -180 &&
00953 color_gradients_ (col_index, row_index).angle <= 180);
00954 }
00955 }
00956
00957 return;
00958 }
00959
00961 template <typename PointInT>
00962 void
00963 pcl::ColorGradientModality<PointInT>::
00964 quantizeColorGradients ()
00965 {
00966
00967
00968
00969
00970
00971
00972
00973
00974
00975
00976
00977
00978 const size_t width = input_->width;
00979 const size_t height = input_->height;
00980
00981 quantized_color_gradients_.resize (width, height);
00982
00983 const float angleScale = 16.0f/360.0f;
00984
00985
00986
00987 for (size_t row_index = 0; row_index < height; ++row_index)
00988 {
00989 for (size_t col_index = 0; col_index < width; ++col_index)
00990 {
00991 if (color_gradients_ (col_index, row_index).magnitude < gradient_magnitude_threshold_)
00992 {
00993 quantized_color_gradients_ (col_index, row_index) = 0;
00994 continue;
00995 }
00996
00997 const float angle = 11.25f + color_gradients_ (col_index, row_index).angle + 180.0f;
00998 const int quantized_value = (static_cast<int> (angle * angleScale)) & 7;
00999 quantized_color_gradients_ (col_index, row_index) = static_cast<unsigned char> (quantized_value + 1);
01000
01001
01002
01003
01004
01005
01006
01007
01008
01009
01010
01011
01012
01013
01014
01015
01016
01017 }
01018 }
01019
01020
01021 }
01022
01024 template <typename PointInT>
01025 void
01026 pcl::ColorGradientModality<PointInT>::
01027 filterQuantizedColorGradients ()
01028 {
01029 const size_t width = input_->width;
01030 const size_t height = input_->height;
01031
01032 filtered_quantized_color_gradients_.resize (width, height);
01033
01034
01035 for (size_t row_index = 1; row_index < height-1; ++row_index)
01036 {
01037 for (size_t col_index = 1; col_index < width-1; ++col_index)
01038 {
01039 unsigned char histogram[9] = {0,0,0,0,0,0,0,0,0};
01040
01041 {
01042 const unsigned char * data_ptr = quantized_color_gradients_.getData () + (row_index-1)*width+col_index-1;
01043 assert (data_ptr[0] < 9 && data_ptr[1] < 9 && data_ptr[2] < 9);
01044 ++histogram[data_ptr[0]];
01045 ++histogram[data_ptr[1]];
01046 ++histogram[data_ptr[2]];
01047 }
01048 {
01049 const unsigned char * data_ptr = quantized_color_gradients_.getData () + row_index*width+col_index-1;
01050 assert (data_ptr[0] < 9 && data_ptr[1] < 9 && data_ptr[2] < 9);
01051 ++histogram[data_ptr[0]];
01052 ++histogram[data_ptr[1]];
01053 ++histogram[data_ptr[2]];
01054 }
01055 {
01056 const unsigned char * data_ptr = quantized_color_gradients_.getData () + (row_index+1)*width+col_index-1;
01057 assert (data_ptr[0] < 9 && data_ptr[1] < 9 && data_ptr[2] < 9);
01058 ++histogram[data_ptr[0]];
01059 ++histogram[data_ptr[1]];
01060 ++histogram[data_ptr[2]];
01061 }
01062
01063 unsigned char max_hist_value = 0;
01064 int max_hist_index = -1;
01065
01066
01067
01068
01069
01070
01071
01072
01073
01074
01075 if (max_hist_value < histogram[1]) {max_hist_index = 0; max_hist_value = histogram[1];}
01076 if (max_hist_value < histogram[2]) {max_hist_index = 1; max_hist_value = histogram[2];}
01077 if (max_hist_value < histogram[3]) {max_hist_index = 2; max_hist_value = histogram[3];}
01078 if (max_hist_value < histogram[4]) {max_hist_index = 3; max_hist_value = histogram[4];}
01079 if (max_hist_value < histogram[5]) {max_hist_index = 4; max_hist_value = histogram[5];}
01080 if (max_hist_value < histogram[6]) {max_hist_index = 5; max_hist_value = histogram[6];}
01081 if (max_hist_value < histogram[7]) {max_hist_index = 6; max_hist_value = histogram[7];}
01082 if (max_hist_value < histogram[8]) {max_hist_index = 7; max_hist_value = histogram[8];}
01083
01084 if (max_hist_index != -1 && max_hist_value >= 5)
01085 filtered_quantized_color_gradients_ (col_index, row_index) = static_cast<unsigned char> (0x1 << max_hist_index);
01086 else
01087 filtered_quantized_color_gradients_ (col_index, row_index) = 0;
01088
01089 }
01090 }
01091 }
01092
01094 template <typename PointInT>
01095 void
01096 pcl::ColorGradientModality<PointInT>::
01097 erode (const pcl::MaskMap & mask_in,
01098 pcl::MaskMap & mask_out)
01099 {
01100 const size_t width = mask_in.getWidth ();
01101 const size_t height = mask_in.getHeight ();
01102
01103 mask_out.resize (width, height);
01104
01105 for (size_t row_index = 1; row_index < height-1; ++row_index)
01106 {
01107 for (size_t col_index = 1; col_index < width-1; ++col_index)
01108 {
01109 if (mask_in (col_index, row_index-1) == 0 ||
01110 mask_in (col_index-1, row_index) == 0 ||
01111 mask_in (col_index+1, row_index) == 0 ||
01112 mask_in (col_index, row_index+1) == 0)
01113 {
01114 mask_out (col_index, row_index) = 0;
01115 }
01116 else
01117 {
01118 mask_out (col_index, row_index) = 255;
01119 }
01120 }
01121 }
01122 }
01123
01124 #endif