Point Cloud Library (PCL)
1.7.0
|
00001 /* 00002 * Software License Agreement (BSD License) 00003 * 00004 * Point Cloud Library (PCL) - www.pointclouds.org 00005 * Copyright (c) 2010-2011, Willow Garage, Inc. 00006 * 00007 * All rights reserved. 00008 * 00009 * Redistribution and use in source and binary forms, with or without 00010 * modification, are permitted provided that the following conditions 00011 * are met: 00012 * 00013 * * Redistributions of source code must retain the above copyright 00014 * notice, this list of conditions and the following disclaimer. 00015 * * Redistributions in binary form must reproduce the above 00016 * copyright notice, this list of conditions and the following 00017 * disclaimer in the documentation and/or other materials provided 00018 * with the distribution. 00019 * * Neither the name of Willow Garage, Inc. nor the names of its 00020 * contributors may be used to endorse or promote products derived 00021 * from this software without specific prior written permission. 00022 * 00023 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 00024 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 00025 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 00026 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 00027 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 00028 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 00029 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 00030 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 00031 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 00032 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 00033 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00034 * POSSIBILITY OF SUCH DAMAGE. 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 00054 /** \brief Modality based on max-RGB gradients. 00055 * \author Stefan Holzer 00056 */ 00057 template <typename PointInT> 00058 class ColorGradientModality 00059 : public QuantizableModality, public PCLBase<PointInT> 00060 { 00061 protected: 00062 using PCLBase<PointInT>::input_; 00063 00064 /** \brief Candidate for a feature (used in feature extraction methods). */ 00065 struct Candidate 00066 { 00067 /** \brief The gradient. */ 00068 GradientXY gradient; 00069 00070 /** \brief The x-position. */ 00071 int x; 00072 /** \brief The y-position. */ 00073 int y; 00074 00075 /** \brief Operator for comparing to candidates (by magnitude of the gradient). 00076 * \param[in] rhs the candidate to compare with. 00077 */ 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 00087 /** \brief Different methods for feature selection/extraction. */ 00088 enum FeatureSelectionMethod 00089 { 00090 MASK_BORDER_HIGH_GRADIENTS, 00091 MASK_BORDER_EQUALLY, // this gives templates most equally to the OpenCV implementation 00092 DISTANCE_MAGNITUDE_SCORE 00093 }; 00094 00095 /** \brief Constructor. */ 00096 ColorGradientModality (); 00097 /** \brief Destructor. */ 00098 virtual ~ColorGradientModality (); 00099 00100 /** \brief Sets the threshold for the gradient magnitude which is used when quantizing the data. 00101 * Gradients with a smaller magnitude are ignored. 00102 * \param[in] threshold the new gradient magnitude threshold. 00103 */ 00104 inline void 00105 setGradientMagnitudeThreshold (const float threshold) 00106 { 00107 gradient_magnitude_threshold_ = threshold; 00108 } 00109 00110 /** \brief Sets the threshold for the gradient magnitude which is used for feature extraction. 00111 * Gradients with a smaller magnitude are ignored. 00112 * \param[in] threshold the new gradient magnitude threshold. 00113 */ 00114 inline void 00115 setGradientMagnitudeThresholdForFeatureExtraction (const float threshold) 00116 { 00117 gradient_magnitude_threshold_feature_extraction_ = threshold; 00118 } 00119 00120 /** \brief Sets the feature selection method. 00121 * \param[in] method the feature selection method. 00122 */ 00123 inline void 00124 setFeatureSelectionMethod (const FeatureSelectionMethod method) 00125 { 00126 feature_selection_method_ = method; 00127 } 00128 00129 /** \brief Sets the spreading size for spreading the quantized data. */ 00130 inline void 00131 setSpreadingSize (const size_t spreading_size) 00132 { 00133 spreading_size_ = spreading_size; 00134 } 00135 00136 /** \brief Sets whether variable feature numbers for feature extraction is enabled. 00137 * \param[in] enabled enables/disables variable feature numbers for feature extraction. 00138 */ 00139 inline void 00140 setVariableFeatureNr (const bool enabled) 00141 { 00142 variable_feature_nr_ = enabled; 00143 } 00144 00145 /** \brief Returns a reference to the internally computed quantized map. */ 00146 inline QuantizedMap & 00147 getQuantizedMap () 00148 { 00149 return (filtered_quantized_color_gradients_); 00150 } 00151 00152 /** \brief Returns a reference to the internally computed spreaded quantized map. */ 00153 inline QuantizedMap & 00154 getSpreadedQuantizedMap () 00155 { 00156 return (spreaded_filtered_quantized_color_gradients_); 00157 } 00158 00159 /** \brief Returns a point cloud containing the max-RGB gradients. */ 00160 inline pcl::PointCloud<pcl::GradientXY> & 00161 getMaxColorGradients () 00162 { 00163 return (color_gradients_); 00164 } 00165 00166 /** \brief Extracts features from this modality within the specified mask. 00167 * \param[in] mask defines the areas where features are searched in. 00168 * \param[in] nr_features defines the number of features to be extracted 00169 * (might be less if not sufficient information is present in the modality). 00170 * \param[in] modality_index the index which is stored in the extracted features. 00171 * \param[out] features the destination for the extracted features. 00172 */ 00173 void 00174 extractFeatures (const MaskMap & mask, size_t nr_features, size_t modalityIndex, 00175 std::vector<QuantizedMultiModFeature> & features) const; 00176 00177 /** \brief Extracts all possible features from the modality within the specified mask. 00178 * \param[in] mask defines the areas where features are searched in. 00179 * \param[in] nr_features IGNORED (TODO: remove this parameter). 00180 * \param[in] modality_index the index which is stored in the extracted features. 00181 * \param[out] features the destination for the extracted features. 00182 */ 00183 void 00184 extractAllFeatures (const MaskMap & mask, size_t nr_features, size_t modalityIndex, 00185 std::vector<QuantizedMultiModFeature> & features) const; 00186 00187 /** \brief Provide a pointer to the input dataset (overwrites the PCLBase::setInputCloud method) 00188 * \param cloud the const boost shared pointer to a PointCloud message 00189 */ 00190 virtual void 00191 setInputCloud (const typename PointCloudIn::ConstPtr & cloud) 00192 { 00193 input_ = cloud; 00194 } 00195 00196 /** \brief Processes the input data (smoothing, computing gradients, quantizing, filtering, spreading). */ 00197 virtual void 00198 processInputData (); 00199 00200 /** \brief Processes the input data assuming that everything up to filtering is already done/available 00201 * (so only spreading is performed). */ 00202 virtual void 00203 processInputDataFromFiltered (); 00204 00205 protected: 00206 00207 /** \brief Computes the Gaussian kernel used for smoothing. 00208 * \param[in] kernel_size the size of the Gaussian kernel. 00209 * \param[in] sigma the sigma. 00210 * \param[out] kernel_values the destination for the values of the kernel. */ 00211 void 00212 computeGaussianKernel (const size_t kernel_size, const float sigma, std::vector <float> & kernel_values); 00213 00214 /** \brief Computes the max-RGB gradients for the specified cloud. 00215 * \param[in] cloud the cloud for which the gradients are computed. 00216 */ 00217 void 00218 computeMaxColorGradients (const typename pcl::PointCloud<pcl::RGB>::ConstPtr & cloud); 00219 00220 /** \brief Computes the max-RGB gradients for the specified cloud using sobel. 00221 * \param[in] cloud the cloud for which the gradients are computed. 00222 */ 00223 void 00224 computeMaxColorGradientsSobel (const typename pcl::PointCloud<pcl::RGB>::ConstPtr & cloud); 00225 00226 /** \brief Quantizes the color gradients. */ 00227 void 00228 quantizeColorGradients (); 00229 00230 /** \brief Filters the quantized gradients. */ 00231 void 00232 filterQuantizedColorGradients (); 00233 00234 /** \brief Erodes a mask. 00235 * \param[in] mask_in the mask which will be eroded. 00236 * \param[out] mask_out the destination for the eroded mask. 00237 */ 00238 static void 00239 erode (const pcl::MaskMap & mask_in, pcl::MaskMap & mask_out); 00240 00241 private: 00242 00243 /** \brief Determines whether variable numbers of features are extracted or not. */ 00244 bool variable_feature_nr_; 00245 00246 /** \brief Stores a smoothed verion of the input cloud. */ 00247 pcl::PointCloud<pcl::RGB>::Ptr smoothed_input_; 00248 00249 /** \brief Defines which feature selection method is used. */ 00250 FeatureSelectionMethod feature_selection_method_; 00251 00252 /** \brief The threshold applied on the gradient magnitudes (for quantization). */ 00253 float gradient_magnitude_threshold_; 00254 /** \brief The threshold applied on the gradient magnitudes for feature extraction. */ 00255 float gradient_magnitude_threshold_feature_extraction_; 00256 00257 /** \brief The point cloud which holds the max-RGB gradients. */ 00258 pcl::PointCloud<pcl::GradientXY> color_gradients_; 00259 00260 /** \brief The spreading size. */ 00261 size_t spreading_size_; 00262 00263 /** \brief The map which holds the quantized max-RGB gradients. */ 00264 pcl::QuantizedMap quantized_color_gradients_; 00265 /** \brief The map which holds the filtered quantized data. */ 00266 pcl::QuantizedMap filtered_quantized_color_gradients_; 00267 /** \brief The map which holds the spreaded quantized data. */ 00268 pcl::QuantizedMap spreaded_filtered_quantized_color_gradients_; 00269 00270 }; 00271 00272 } 00273 00274 ////////////////////////////////////////////////////////////////////////////////////////////// 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 00291 ////////////////////////////////////////////////////////////////////////////////////////////// 00292 template <typename PointInT> 00293 pcl::ColorGradientModality<PointInT>:: 00294 ~ColorGradientModality () 00295 { 00296 } 00297 00298 ////////////////////////////////////////////////////////////////////////////////////////////// 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 // code taken from OpenCV 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 //CV_Assert( ktype == CV_32F || ktype == CV_64F ); 00318 /*Mat kernel(n, 1, ktype);*/ 00319 kernel_values.resize (n); 00320 float* cf = &(kernel_values[0]); 00321 //double* cd = (double*)kernel.data; 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 00344 ////////////////////////////////////////////////////////////////////////////////////////////// 00345 template <typename PointInT> 00346 void 00347 pcl::ColorGradientModality<PointInT>:: 00348 processInputData () 00349 { 00350 // compute gaussian kernel values 00351 const size_t kernel_size = 7; 00352 std::vector<float> kernel_values; 00353 computeGaussianKernel (kernel_size, 0.0f, kernel_values); 00354 00355 // smooth input 00356 pcl::filters::Convolution<pcl::RGB, pcl::RGB> convolution; 00357 Eigen::ArrayXf gaussian_kernel(kernel_size); 00358 //gaussian_kernel << 1.f/16, 1.f/8, 3.f/16, 2.f/8, 3.f/16, 1.f/8, 1.f/16; 00359 //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; 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 // extract color gradients 00387 computeMaxColorGradientsSobel (smoothed_input_); 00388 00389 // quantize gradients 00390 quantizeColorGradients (); 00391 00392 // filter quantized gradients to get only dominants one + thresholding 00393 filterQuantizedColorGradients (); 00394 00395 // spread filtered quantized gradients 00396 //spreadFilteredQunatizedColorGradients (); 00397 pcl::QuantizedMap::spreadQuantizedMap (filtered_quantized_color_gradients_, 00398 spreaded_filtered_quantized_color_gradients_, 00399 spreading_size_); 00400 } 00401 00402 ////////////////////////////////////////////////////////////////////////////////////////////// 00403 template <typename PointInT> 00404 void 00405 pcl::ColorGradientModality<PointInT>:: 00406 processInputDataFromFiltered () 00407 { 00408 // spread filtered quantized gradients 00409 //spreadFilteredQunatizedColorGradients (); 00410 pcl::QuantizedMap::spreadQuantizedMap (filtered_quantized_color_gradients_, 00411 spreaded_filtered_quantized_color_gradients_, 00412 spreading_size_); 00413 } 00414 00415 ////////////////////////////////////////////////////////////////////////////////////////////// 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 //while (list2.size () != nr_features) 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 // find smallest distance 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 //std::cerr << min_sqr_distance; 00510 } 00511 //std::cerr << std::endl; 00512 00513 // check current feature 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 //std::cerr << min_sqr_distance << ", " << min_min_sqr_distance << ", " << max_min_sqr_distance << std::endl; 00532 } 00533 00534 if (best_iter != list1.end ()) 00535 { 00536 //std::cerr << "feature_index: " << list2.size () << std::endl; 00537 //std::cerr << "min_min_sqr_distance: " << min_min_sqr_distance << std::endl; 00538 //std::cerr << "max_min_sqr_distance: " << max_min_sqr_distance << std::endl; 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 // find smallest distance 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 //if (tmp_distance < distance) 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 00702 ////////////////////////////////////////////////////////////////////////////////////////////// 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 00751 ////////////////////////////////////////////////////////////////////////////////////////////// 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 //const int index_d = (row_index+1)*width+col_index+1; 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 00838 ////////////////////////////////////////////////////////////////////////////////////////////// 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 //const int r_tmp1 = - r7 + r3; 00882 //const int r_tmp2 = - r1 + r9; 00883 //const int g_tmp1 = - g7 + g3; 00884 //const int g_tmp2 = - g1 + g9; 00885 //const int b_tmp1 = - b7 + b3; 00886 //const int b_tmp2 = - b1 + b9; 00887 ////const int gx = - r7 - (r4<<2) - r1 + r3 + (r6<<2) + r9; 00888 ////const int gy = - r7 - (r8<<2) - r9 + r1 + (r2<<2) + r3; 00889 //const int r_dx = r_tmp1 + r_tmp2 - (r4<<2) + (r6<<2); 00890 //const int r_dy = r_tmp1 - r_tmp2 - (r8<<2) + (r2<<2); 00891 //const int g_dx = g_tmp1 + g_tmp2 - (g4<<2) + (g6<<2); 00892 //const int g_dy = g_tmp1 - g_tmp2 - (g8<<2) + (g2<<2); 00893 //const int b_dx = b_tmp1 + b_tmp2 - (b4<<2) + (b6<<2); 00894 //const int b_dy = b_tmp1 - b_tmp2 - (b8<<2) + (b2<<2); 00895 00896 //const int r_tmp1 = - r7 + r3; 00897 //const int r_tmp2 = - r1 + r9; 00898 //const int g_tmp1 = - g7 + g3; 00899 //const int g_tmp2 = - g1 + g9; 00900 //const int b_tmp1 = - b7 + b3; 00901 //const int b_tmp2 = - b1 + b9; 00902 //const int gx = - r7 - (r4<<2) - r1 + r3 + (r6<<2) + r9; 00903 //const int gy = - r7 - (r8<<2) - r9 + r1 + (r2<<2) + r3; 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 00960 ////////////////////////////////////////////////////////////////////////////////////////////// 00961 template <typename PointInT> 00962 void 00963 pcl::ColorGradientModality<PointInT>:: 00964 quantizeColorGradients () 00965 { 00966 //std::cerr << "quantize this, bastard!!!" << std::endl; 00967 00968 //unsigned char quantization_map[16] = {0,1,2,3,4,5,6,7,0,1,2,3,4,5,6,7}; 00969 //unsigned char quantization_map[16] = {1,2,3,4,5,6,7,8,1,2,3,4,5,6,7,8}; 00970 00971 //for (float angle = 0.0f; angle < 360.0f; angle += 1.0f) 00972 //{ 00973 // const int quantized_value = quantization_map[static_cast<int> (angle * angleScale)]; 00974 // std::cerr << angle << ": " << quantized_value << std::endl; 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 //float min_angle = std::numeric_limits<float>::max (); 00986 //float max_angle = -std::numeric_limits<float>::max (); 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 //const float angle = color_gradients_ (col_index, row_index).angle + 180.0f; 01002 01003 //min_angle = std::min (min_angle, angle); 01004 //max_angle = std::max (max_angle, angle); 01005 01006 //if (angle < 0.0f || angle >= 360.0f) 01007 //{ 01008 // std::cerr << "angle shitty: " << angle << std::endl; 01009 //} 01010 01011 //const int quantized_value = quantization_map[static_cast<int> (angle * angleScale)]; 01012 //quantized_color_gradients_ (col_index, row_index) = static_cast<unsigned char> (quantized_value); 01013 01014 //assert (0 <= quantized_value && quantized_value < 16); 01015 //quantized_color_gradients_ (col_index, row_index) = quantization_map[quantized_value]; 01016 //quantized_color_gradients_ (col_index, row_index) = static_cast<unsigned char> ((quantized_value & 7) + 1); // = (quantized_value % 8) + 1 01017 } 01018 } 01019 01020 //std::cerr << ">>>>> " << min_angle << ", " << max_angle << std::endl; 01021 } 01022 01023 ////////////////////////////////////////////////////////////////////////////////////////////// 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 // filter data 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 // for (int i = 0; i < 8; ++i) 01067 // { 01068 // if (max_hist_value < histogram[i+1]) 01069 // { 01070 // max_hist_index = i; 01071 // max_hist_value = histogram[i+1] 01072 // } 01073 // } 01074 // Unrolled for performance optimization: 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 01093 ////////////////////////////////////////////////////////////////////////////////////////////// 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