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_ */