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) 2011, Alexandru-Eugen Ichim 00006 * Copyright (c) 2012-, Open Perception, Inc. 00007 * 00008 * All rights reserved. 00009 * 00010 * Redistribution and use in source and binary forms, with or without 00011 * modification, are permitted provided that the following conditions 00012 * are met: 00013 * 00014 * * Redistributions of source code must retain the above copyright 00015 * notice, this list of conditions and the following disclaimer. 00016 * * Redistributions in binary form must reproduce the above 00017 * copyright notice, this list of conditions and the following 00018 * disclaimer in the documentation and/or other materials provided 00019 * with the distribution. 00020 * * Neither the name of the copyright holder(s) nor the names of its 00021 * contributors may be used to endorse or promote products derived 00022 * from this software without specific prior written permission. 00023 * 00024 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 00025 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 00026 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 00027 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 00028 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 00029 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 00030 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 00031 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 00032 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 00033 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 00034 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00035 * POSSIBILITY OF SUCH DAMAGE. 00036 * 00037 * $Id$ 00038 */ 00039 00040 #ifndef PCL_FEATURES_IMPL_MULTISCALE_FEATURE_PERSISTENCE_H_ 00041 #define PCL_FEATURES_IMPL_MULTISCALE_FEATURE_PERSISTENCE_H_ 00042 00043 #include <pcl/features/multiscale_feature_persistence.h> 00044 00045 ////////////////////////////////////////////////////////////////////////////////////////////// 00046 template <typename PointSource, typename PointFeature> 00047 pcl::MultiscaleFeaturePersistence<PointSource, PointFeature>::MultiscaleFeaturePersistence () : 00048 scale_values_ (), 00049 alpha_ (0), 00050 distance_metric_ (L1), 00051 feature_estimator_ (), 00052 features_at_scale_ (), 00053 features_at_scale_vectorized_ (), 00054 mean_feature_ (), 00055 feature_representation_ (), 00056 unique_features_indices_ (), 00057 unique_features_table_ () 00058 { 00059 feature_representation_.reset (new DefaultPointRepresentation<PointFeature>); 00060 // No input is needed, hack around the initCompute () check from PCLBase 00061 input_.reset (new pcl::PointCloud<PointSource> ()); 00062 } 00063 00064 00065 ////////////////////////////////////////////////////////////////////////////////////////////// 00066 template <typename PointSource, typename PointFeature> bool 00067 pcl::MultiscaleFeaturePersistence<PointSource, PointFeature>::initCompute () 00068 { 00069 if (!PCLBase<PointSource>::initCompute ()) 00070 { 00071 PCL_ERROR ("[pcl::MultiscaleFeaturePersistence::initCompute] PCLBase::initCompute () failed - no input cloud was given.\n"); 00072 return false; 00073 } 00074 if (!feature_estimator_) 00075 { 00076 PCL_ERROR ("[pcl::MultiscaleFeaturePersistence::initCompute] No feature estimator was set\n"); 00077 return false; 00078 } 00079 if (scale_values_.empty ()) 00080 { 00081 PCL_ERROR ("[pcl::MultiscaleFeaturePersistence::initCompute] No scale values were given\n"); 00082 return false; 00083 } 00084 00085 mean_feature_.resize (feature_representation_->getNumberOfDimensions ()); 00086 00087 return true; 00088 } 00089 00090 00091 ////////////////////////////////////////////////////////////////////////////////////////////// 00092 template <typename PointSource, typename PointFeature> void 00093 pcl::MultiscaleFeaturePersistence<PointSource, PointFeature>::computeFeaturesAtAllScales () 00094 { 00095 features_at_scale_.resize (scale_values_.size ()); 00096 features_at_scale_vectorized_.resize (scale_values_.size ()); 00097 for (size_t scale_i = 0; scale_i < scale_values_.size (); ++scale_i) 00098 { 00099 FeatureCloudPtr feature_cloud (new FeatureCloud ()); 00100 computeFeatureAtScale (scale_values_[scale_i], feature_cloud); 00101 features_at_scale_[scale_i] = feature_cloud; 00102 00103 // Vectorize each feature and insert it into the vectorized feature storage 00104 std::vector<std::vector<float> > feature_cloud_vectorized (feature_cloud->points.size ()); 00105 for (size_t feature_i = 0; feature_i < feature_cloud->points.size (); ++feature_i) 00106 { 00107 std::vector<float> feature_vectorized (feature_representation_->getNumberOfDimensions ()); 00108 feature_representation_->vectorize (feature_cloud->points[feature_i], feature_vectorized); 00109 feature_cloud_vectorized[feature_i] = feature_vectorized; 00110 } 00111 features_at_scale_vectorized_[scale_i] = feature_cloud_vectorized; 00112 } 00113 } 00114 00115 00116 ////////////////////////////////////////////////////////////////////////////////////////////// 00117 template <typename PointSource, typename PointFeature> void 00118 pcl::MultiscaleFeaturePersistence<PointSource, PointFeature>::computeFeatureAtScale (float &scale, 00119 FeatureCloudPtr &features) 00120 { 00121 feature_estimator_->setRadiusSearch (scale); 00122 feature_estimator_->compute (*features); 00123 } 00124 00125 00126 ////////////////////////////////////////////////////////////////////////////////////////////// 00127 template <typename PointSource, typename PointFeature> float 00128 pcl::MultiscaleFeaturePersistence<PointSource, PointFeature>::distanceBetweenFeatures (const std::vector<float> &a, 00129 const std::vector<float> &b) 00130 { 00131 return (pcl::selectNorm<std::vector<float> > (a, b, static_cast<int> (a.size ()), distance_metric_)); 00132 } 00133 00134 00135 ////////////////////////////////////////////////////////////////////////////////////////////// 00136 template <typename PointSource, typename PointFeature> void 00137 pcl::MultiscaleFeaturePersistence<PointSource, PointFeature>::calculateMeanFeature () 00138 { 00139 // Reset mean feature 00140 for (int i = 0; i < feature_representation_->getNumberOfDimensions (); ++i) 00141 mean_feature_[i] = 0.0f; 00142 00143 float normalization_factor = 0.0f; 00144 for (std::vector<std::vector<std::vector<float> > >::iterator scale_it = features_at_scale_vectorized_.begin (); scale_it != features_at_scale_vectorized_.end(); ++scale_it) { 00145 normalization_factor += static_cast<float> (scale_it->size ()); 00146 for (std::vector<std::vector<float> >::iterator feature_it = scale_it->begin (); feature_it != scale_it->end (); ++feature_it) 00147 for (int dim_i = 0; dim_i < feature_representation_->getNumberOfDimensions (); ++dim_i) 00148 mean_feature_[dim_i] += (*feature_it)[dim_i]; 00149 } 00150 00151 for (int dim_i = 0; dim_i < feature_representation_->getNumberOfDimensions (); ++dim_i) 00152 mean_feature_[dim_i] /= normalization_factor; 00153 } 00154 00155 00156 ////////////////////////////////////////////////////////////////////////////////////////////// 00157 template <typename PointSource, typename PointFeature> void 00158 pcl::MultiscaleFeaturePersistence<PointSource, PointFeature>::extractUniqueFeatures () 00159 { 00160 unique_features_indices_.resize (scale_values_.size ()); 00161 unique_features_table_.resize (scale_values_.size ()); 00162 for (size_t scale_i = 0; scale_i < features_at_scale_vectorized_.size (); ++scale_i) 00163 { 00164 // Calculate standard deviation within the scale 00165 float standard_dev = 0.0; 00166 std::vector<float> diff_vector (features_at_scale_vectorized_[scale_i].size ()); 00167 for (size_t point_i = 0; point_i < features_at_scale_vectorized_[scale_i].size (); ++point_i) 00168 { 00169 float diff = distanceBetweenFeatures (features_at_scale_vectorized_[scale_i][point_i], mean_feature_); 00170 standard_dev += diff * diff; 00171 diff_vector[point_i] = diff; 00172 } 00173 standard_dev = sqrtf (standard_dev / static_cast<float> (features_at_scale_vectorized_[scale_i].size ())); 00174 PCL_DEBUG ("[pcl::MultiscaleFeaturePersistence::extractUniqueFeatures] Standard deviation for scale %f is %f\n", scale_values_[scale_i], standard_dev); 00175 00176 // Select only points outside (mean +/- alpha * standard_dev) 00177 std::list<size_t> indices_per_scale; 00178 std::vector<bool> indices_table_per_scale (features_at_scale_[scale_i]->points.size (), false); 00179 for (size_t point_i = 0; point_i < features_at_scale_[scale_i]->points.size (); ++point_i) 00180 { 00181 if (diff_vector[point_i] > alpha_ * standard_dev) 00182 { 00183 indices_per_scale.push_back (point_i); 00184 indices_table_per_scale[point_i] = true; 00185 } 00186 } 00187 unique_features_indices_[scale_i] = indices_per_scale; 00188 unique_features_table_[scale_i] = indices_table_per_scale; 00189 } 00190 } 00191 00192 00193 ////////////////////////////////////////////////////////////////////////////////////////////// 00194 template <typename PointSource, typename PointFeature> void 00195 pcl::MultiscaleFeaturePersistence<PointSource, PointFeature>::determinePersistentFeatures (FeatureCloud &output_features, 00196 boost::shared_ptr<std::vector<int> > &output_indices) 00197 { 00198 if (!initCompute ()) 00199 return; 00200 00201 // Compute the features for all scales with the given feature estimator 00202 PCL_DEBUG ("[pcl::MultiscaleFeaturePersistence::determinePersistentFeatures] Computing features ...\n"); 00203 computeFeaturesAtAllScales (); 00204 00205 // Compute mean feature 00206 PCL_DEBUG ("[pcl::MultiscaleFeaturePersistence::determinePersistentFeatures] Calculating mean feature ...\n"); 00207 calculateMeanFeature (); 00208 00209 // Get the 'unique' features at each scale 00210 PCL_DEBUG ("[pcl::MultiscaleFeaturePersistence::determinePersistentFeatures] Extracting unique features ...\n"); 00211 extractUniqueFeatures (); 00212 00213 PCL_DEBUG ("[pcl::MultiscaleFeaturePersistence::determinePersistentFeatures] Determining persistent features between scales ...\n"); 00214 // Determine persistent features between scales 00215 00216 /* 00217 // Method 1: a feature is considered persistent if it is 'unique' in at least 2 different scales 00218 for (size_t scale_i = 0; scale_i < features_at_scale_vectorized_.size () - 1; ++scale_i) 00219 for (std::list<size_t>::iterator feature_it = unique_features_indices_[scale_i].begin (); feature_it != unique_features_indices_[scale_i].end (); ++feature_it) 00220 { 00221 if (unique_features_table_[scale_i][*feature_it] == true) 00222 { 00223 output_features.points.push_back (features_at_scale[scale_i]->points[*feature_it]); 00224 output_indices->push_back (feature_estimator_->getIndices ()->at (*feature_it)); 00225 } 00226 } 00227 */ 00228 // Method 2: a feature is considered persistent if it is 'unique' in all the scales 00229 for (std::list<size_t>::iterator feature_it = unique_features_indices_.front ().begin (); feature_it != unique_features_indices_.front ().end (); ++feature_it) 00230 { 00231 bool present_in_all = true; 00232 for (size_t scale_i = 0; scale_i < features_at_scale_.size (); ++scale_i) 00233 present_in_all = present_in_all && unique_features_table_[scale_i][*feature_it]; 00234 00235 if (present_in_all) 00236 { 00237 output_features.points.push_back (features_at_scale_.front ()->points[*feature_it]); 00238 output_indices->push_back (feature_estimator_->getIndices ()->at (*feature_it)); 00239 } 00240 } 00241 00242 // Consider that output cloud is unorganized 00243 output_features.header = feature_estimator_->getInputCloud ()->header; 00244 output_features.is_dense = feature_estimator_->getInputCloud ()->is_dense; 00245 output_features.width = static_cast<uint32_t> (output_features.points.size ()); 00246 output_features.height = 1; 00247 } 00248 00249 00250 #define PCL_INSTANTIATE_MultiscaleFeaturePersistence(InT, Feature) template class PCL_EXPORTS pcl::MultiscaleFeaturePersistence<InT, Feature>; 00251 00252 #endif /* PCL_FEATURES_IMPL_MULTISCALE_FEATURE_PERSISTENCE_H_ */