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