Point Cloud Library (PCL)  1.7.1
multiscale_feature_persistence.hpp
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Point Cloud Library (PCL) - www.pointclouds.org
5  * Copyright (c) 2011, Alexandru-Eugen Ichim
6  * Copyright (c) 2012-, Open Perception, Inc.
7  *
8  * All rights reserved.
9  *
10  * Redistribution and use in source and binary forms, with or without
11  * modification, are permitted provided that the following conditions
12  * are met:
13  *
14  * * Redistributions of source code must retain the above copyright
15  * notice, this list of conditions and the following disclaimer.
16  * * Redistributions in binary form must reproduce the above
17  * copyright notice, this list of conditions and the following
18  * disclaimer in the documentation and/or other materials provided
19  * with the distribution.
20  * * Neither the name of the copyright holder(s) nor the names of its
21  * contributors may be used to endorse or promote products derived
22  * from this software without specific prior written permission.
23  *
24  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
25  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
26  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
27  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
28  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
29  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
30  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
31  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
32  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
33  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
34  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
35  * POSSIBILITY OF SUCH DAMAGE.
36  *
37  * $Id$
38  */
39 
40 #ifndef PCL_FEATURES_IMPL_MULTISCALE_FEATURE_PERSISTENCE_H_
41 #define PCL_FEATURES_IMPL_MULTISCALE_FEATURE_PERSISTENCE_H_
42 
43 #include <pcl/features/multiscale_feature_persistence.h>
44 
45 //////////////////////////////////////////////////////////////////////////////////////////////
46 template <typename PointSource, typename PointFeature>
48  scale_values_ (),
49  alpha_ (0),
50  distance_metric_ (L1),
51  feature_estimator_ (),
52  features_at_scale_ (),
53  features_at_scale_vectorized_ (),
54  mean_feature_ (),
55  feature_representation_ (),
56  unique_features_indices_ (),
57  unique_features_table_ ()
58 {
59  feature_representation_.reset (new DefaultPointRepresentation<PointFeature>);
60  // No input is needed, hack around the initCompute () check from PCLBase
61  input_.reset (new pcl::PointCloud<PointSource> ());
62 }
63 
64 
65 //////////////////////////////////////////////////////////////////////////////////////////////
66 template <typename PointSource, typename PointFeature> bool
68 {
70  {
71  PCL_ERROR ("[pcl::MultiscaleFeaturePersistence::initCompute] PCLBase::initCompute () failed - no input cloud was given.\n");
72  return false;
73  }
74  if (!feature_estimator_)
75  {
76  PCL_ERROR ("[pcl::MultiscaleFeaturePersistence::initCompute] No feature estimator was set\n");
77  return false;
78  }
79  if (scale_values_.empty ())
80  {
81  PCL_ERROR ("[pcl::MultiscaleFeaturePersistence::initCompute] No scale values were given\n");
82  return false;
83  }
84 
85  mean_feature_.resize (feature_representation_->getNumberOfDimensions ());
86 
87  return true;
88 }
89 
90 
91 //////////////////////////////////////////////////////////////////////////////////////////////
92 template <typename PointSource, typename PointFeature> void
94 {
95  features_at_scale_.resize (scale_values_.size ());
96  features_at_scale_vectorized_.resize (scale_values_.size ());
97  for (size_t scale_i = 0; scale_i < scale_values_.size (); ++scale_i)
98  {
99  FeatureCloudPtr feature_cloud (new FeatureCloud ());
100  computeFeatureAtScale (scale_values_[scale_i], feature_cloud);
101  features_at_scale_[scale_i] = feature_cloud;
102 
103  // Vectorize each feature and insert it into the vectorized feature storage
104  std::vector<std::vector<float> > feature_cloud_vectorized (feature_cloud->points.size ());
105  for (size_t feature_i = 0; feature_i < feature_cloud->points.size (); ++feature_i)
106  {
107  std::vector<float> feature_vectorized (feature_representation_->getNumberOfDimensions ());
108  feature_representation_->vectorize (feature_cloud->points[feature_i], feature_vectorized);
109  feature_cloud_vectorized[feature_i] = feature_vectorized;
110  }
111  features_at_scale_vectorized_[scale_i] = feature_cloud_vectorized;
112  }
113 }
114 
115 
116 //////////////////////////////////////////////////////////////////////////////////////////////
117 template <typename PointSource, typename PointFeature> void
119  FeatureCloudPtr &features)
120 {
121  feature_estimator_->setRadiusSearch (scale);
122  feature_estimator_->compute (*features);
123 }
124 
125 
126 //////////////////////////////////////////////////////////////////////////////////////////////
127 template <typename PointSource, typename PointFeature> float
129  const std::vector<float> &b)
130 {
131  return (pcl::selectNorm<std::vector<float> > (a, b, static_cast<int> (a.size ()), distance_metric_));
132 }
133 
134 
135 //////////////////////////////////////////////////////////////////////////////////////////////
136 template <typename PointSource, typename PointFeature> void
138 {
139  // Reset mean feature
140  for (int i = 0; i < feature_representation_->getNumberOfDimensions (); ++i)
141  mean_feature_[i] = 0.0f;
142 
143  float normalization_factor = 0.0f;
144  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) {
145  normalization_factor += static_cast<float> (scale_it->size ());
146  for (std::vector<std::vector<float> >::iterator feature_it = scale_it->begin (); feature_it != scale_it->end (); ++feature_it)
147  for (int dim_i = 0; dim_i < feature_representation_->getNumberOfDimensions (); ++dim_i)
148  mean_feature_[dim_i] += (*feature_it)[dim_i];
149  }
150 
151  for (int dim_i = 0; dim_i < feature_representation_->getNumberOfDimensions (); ++dim_i)
152  mean_feature_[dim_i] /= normalization_factor;
153 }
154 
155 
156 //////////////////////////////////////////////////////////////////////////////////////////////
157 template <typename PointSource, typename PointFeature> void
159 {
160  unique_features_indices_.resize (scale_values_.size ());
161  unique_features_table_.resize (scale_values_.size ());
162  for (size_t scale_i = 0; scale_i < features_at_scale_vectorized_.size (); ++scale_i)
163  {
164  // Calculate standard deviation within the scale
165  float standard_dev = 0.0;
166  std::vector<float> diff_vector (features_at_scale_vectorized_[scale_i].size ());
167  for (size_t point_i = 0; point_i < features_at_scale_vectorized_[scale_i].size (); ++point_i)
168  {
169  float diff = distanceBetweenFeatures (features_at_scale_vectorized_[scale_i][point_i], mean_feature_);
170  standard_dev += diff * diff;
171  diff_vector[point_i] = diff;
172  }
173  standard_dev = sqrtf (standard_dev / static_cast<float> (features_at_scale_vectorized_[scale_i].size ()));
174  PCL_DEBUG ("[pcl::MultiscaleFeaturePersistence::extractUniqueFeatures] Standard deviation for scale %f is %f\n", scale_values_[scale_i], standard_dev);
175 
176  // Select only points outside (mean +/- alpha * standard_dev)
177  std::list<size_t> indices_per_scale;
178  std::vector<bool> indices_table_per_scale (features_at_scale_[scale_i]->points.size (), false);
179  for (size_t point_i = 0; point_i < features_at_scale_[scale_i]->points.size (); ++point_i)
180  {
181  if (diff_vector[point_i] > alpha_ * standard_dev)
182  {
183  indices_per_scale.push_back (point_i);
184  indices_table_per_scale[point_i] = true;
185  }
186  }
187  unique_features_indices_[scale_i] = indices_per_scale;
188  unique_features_table_[scale_i] = indices_table_per_scale;
189  }
190 }
191 
192 
193 //////////////////////////////////////////////////////////////////////////////////////////////
194 template <typename PointSource, typename PointFeature> void
196  boost::shared_ptr<std::vector<int> > &output_indices)
197 {
198  if (!initCompute ())
199  return;
200 
201  // Compute the features for all scales with the given feature estimator
202  PCL_DEBUG ("[pcl::MultiscaleFeaturePersistence::determinePersistentFeatures] Computing features ...\n");
203  computeFeaturesAtAllScales ();
204 
205  // Compute mean feature
206  PCL_DEBUG ("[pcl::MultiscaleFeaturePersistence::determinePersistentFeatures] Calculating mean feature ...\n");
207  calculateMeanFeature ();
208 
209  // Get the 'unique' features at each scale
210  PCL_DEBUG ("[pcl::MultiscaleFeaturePersistence::determinePersistentFeatures] Extracting unique features ...\n");
211  extractUniqueFeatures ();
212 
213  PCL_DEBUG ("[pcl::MultiscaleFeaturePersistence::determinePersistentFeatures] Determining persistent features between scales ...\n");
214  // Determine persistent features between scales
215 
216 /*
217  // Method 1: a feature is considered persistent if it is 'unique' in at least 2 different scales
218  for (size_t scale_i = 0; scale_i < features_at_scale_vectorized_.size () - 1; ++scale_i)
219  for (std::list<size_t>::iterator feature_it = unique_features_indices_[scale_i].begin (); feature_it != unique_features_indices_[scale_i].end (); ++feature_it)
220  {
221  if (unique_features_table_[scale_i][*feature_it] == true)
222  {
223  output_features.points.push_back (features_at_scale[scale_i]->points[*feature_it]);
224  output_indices->push_back (feature_estimator_->getIndices ()->at (*feature_it));
225  }
226  }
227 */
228  // Method 2: a feature is considered persistent if it is 'unique' in all the scales
229  for (std::list<size_t>::iterator feature_it = unique_features_indices_.front ().begin (); feature_it != unique_features_indices_.front ().end (); ++feature_it)
230  {
231  bool present_in_all = true;
232  for (size_t scale_i = 0; scale_i < features_at_scale_.size (); ++scale_i)
233  present_in_all = present_in_all && unique_features_table_[scale_i][*feature_it];
234 
235  if (present_in_all)
236  {
237  output_features.points.push_back (features_at_scale_.front ()->points[*feature_it]);
238  output_indices->push_back (feature_estimator_->getIndices ()->at (*feature_it));
239  }
240  }
241 
242  // Consider that output cloud is unorganized
243  output_features.header = feature_estimator_->getInputCloud ()->header;
244  output_features.is_dense = feature_estimator_->getInputCloud ()->is_dense;
245  output_features.width = static_cast<uint32_t> (output_features.points.size ());
246  output_features.height = 1;
247 }
248 
249 
250 #define PCL_INSTANTIATE_MultiscaleFeaturePersistence(InT, Feature) template class PCL_EXPORTS pcl::MultiscaleFeaturePersistence<InT, Feature>;
251 
252 #endif /* PCL_FEATURES_IMPL_MULTISCALE_FEATURE_PERSISTENCE_H_ */
pcl::PointCloud< PointFeature >::Ptr FeatureCloudPtr
pcl::PCLHeader header
The point cloud header.
Definition: point_cloud.h:407
void determinePersistentFeatures(FeatureCloud &output_features, boost::shared_ptr< std::vector< int > > &output_indices)
Central function that computes the persistent features.
uint32_t width
The point cloud width (if organized as an image-structure).
Definition: point_cloud.h:413
Generic class for extracting the persistent features from an input point cloud It can be given any Fe...
DefaultPointRepresentation extends PointRepresentation to define default behavior for common point ty...
PCL base class.
Definition: pcl_base.h:68
uint32_t height
The point cloud height (if organized as an image-structure).
Definition: point_cloud.h:415
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
Definition: point_cloud.h:410
bool is_dense
True if no points are invalid (e.g., have NaN or Inf values).
Definition: point_cloud.h:418
Definition: norms.h:55
PointCloudConstPtr input_
The input point cloud dataset.
Definition: pcl_base.h:146
float selectNorm(FloatVectorT a, FloatVectorT b, int dim, NormType norm_type)
Method that calculates any norm type available, based on the norm_type variable.
Definition: norms.hpp:49
void computeFeaturesAtAllScales()
Method that calls computeFeatureAtScale () for each scale parameter.