Point Cloud Library (PCL)  1.3.1
multiscale_feature_persistence.hpp
Go to the documentation of this file.
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_ */
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Defines