Point Cloud Library (PCL)  1.10.1-dev
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 <numeric>
44 #include <pcl/features/multiscale_feature_persistence.h>
45 
46 //////////////////////////////////////////////////////////////////////////////////////////////
47 template <typename PointSource, typename PointFeature>
49  alpha_ (0),
50  distance_metric_ (L1),
51  feature_estimator_ (),
52  features_at_scale_ (),
53  feature_representation_ ()
54 {
55  feature_representation_.reset (new DefaultPointRepresentation<PointFeature>);
56  // No input is needed, hack around the initCompute () check from PCLBase
57  input_.reset (new pcl::PointCloud<PointSource> ());
58 }
59 
60 
61 //////////////////////////////////////////////////////////////////////////////////////////////
62 template <typename PointSource, typename PointFeature> bool
64 {
66  {
67  PCL_ERROR ("[pcl::MultiscaleFeaturePersistence::initCompute] PCLBase::initCompute () failed - no input cloud was given.\n");
68  return false;
69  }
70  if (!feature_estimator_)
71  {
72  PCL_ERROR ("[pcl::MultiscaleFeaturePersistence::initCompute] No feature estimator was set\n");
73  return false;
74  }
75  if (scale_values_.empty ())
76  {
77  PCL_ERROR ("[pcl::MultiscaleFeaturePersistence::initCompute] No scale values were given\n");
78  return false;
79  }
80 
81  mean_feature_.resize (feature_representation_->getNumberOfDimensions ());
82 
83  return true;
84 }
85 
86 
87 //////////////////////////////////////////////////////////////////////////////////////////////
88 template <typename PointSource, typename PointFeature> void
90 {
91  features_at_scale_.clear ();
92  features_at_scale_.reserve (scale_values_.size ());
93  features_at_scale_vectorized_.clear ();
94  features_at_scale_vectorized_.reserve (scale_values_.size ());
95  for (std::size_t scale_i = 0; scale_i < scale_values_.size (); ++scale_i)
96  {
97  FeatureCloudPtr feature_cloud (new FeatureCloud ());
98  computeFeatureAtScale (scale_values_[scale_i], feature_cloud);
99  features_at_scale_[scale_i] = feature_cloud;
100 
101  // Vectorize each feature and insert it into the vectorized feature storage
102  std::vector<std::vector<float> > feature_cloud_vectorized;
103  feature_cloud_vectorized.reserve (feature_cloud->points.size ());
104 
105  for (const auto& feature: feature_cloud->points)
106  {
107  std::vector<float> feature_vectorized (feature_representation_->getNumberOfDimensions ());
108  feature_representation_->vectorize (feature, feature_vectorized);
109  feature_cloud_vectorized.emplace_back (std::move(feature_vectorized));
110  }
111  features_at_scale_vectorized_.emplace_back (std::move(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, a.size (), distance_metric_));
132 }
133 
134 
135 //////////////////////////////////////////////////////////////////////////////////////////////
136 template <typename PointSource, typename PointFeature> void
138 {
139  // Reset mean feature
140  std::fill_n(mean_feature_.begin (), mean_feature_.size (), 0.f);
141 
142  std::size_t normalization_factor = 0;
143  for (const auto& scale: features_at_scale_vectorized_)
144  {
145  normalization_factor += scale.size (); // not using accumulate for cache efficiency
146  for (const auto &feature : scale)
147  std::transform(mean_feature_.cbegin (), mean_feature_.cend (),
148  feature.cbegin (), mean_feature_.begin (), std::plus<>{});
149  }
150 
151  const float factor = std::min<float>(1, normalization_factor);
152  std::transform(mean_feature_.cbegin(),
153  mean_feature_.cend(),
154  mean_feature_.begin(),
155  [factor](const auto& mean) {
156  return mean / factor;
157  });
158 }
159 
160 
161 //////////////////////////////////////////////////////////////////////////////////////////////
162 template <typename PointSource, typename PointFeature> void
164 {
165  unique_features_indices_.clear ();
166  unique_features_table_.clear ();
167  unique_features_indices_.reserve (scale_values_.size ());
168  unique_features_table_.reserve (scale_values_.size ());
169 
170  for (std::size_t scale_i = 0; scale_i < features_at_scale_vectorized_.size (); ++scale_i)
171  {
172  // Calculate standard deviation within the scale
173  float standard_dev = 0.0;
174  std::vector<float> diff_vector (features_at_scale_vectorized_[scale_i].size ());
175  diff_vector.clear();
176 
177  for (const auto& feature: features_at_scale_vectorized_[scale_i])
178  {
179  float diff = distanceBetweenFeatures (feature, mean_feature_);
180  standard_dev += diff * diff;
181  diff_vector.emplace_back (diff);
182  }
183  standard_dev = std::sqrt (standard_dev / static_cast<float> (features_at_scale_vectorized_[scale_i].size ()));
184  PCL_DEBUG ("[pcl::MultiscaleFeaturePersistence::extractUniqueFeatures] Standard deviation for scale %f is %f\n", scale_values_[scale_i], standard_dev);
185 
186  // Select only points outside (mean +/- alpha * standard_dev)
187  std::list<std::size_t> indices_per_scale;
188  std::vector<bool> indices_table_per_scale (features_at_scale_[scale_i]->points.size (), false);
189  for (std::size_t point_i = 0; point_i < features_at_scale_[scale_i]->points.size (); ++point_i)
190  {
191  if (diff_vector[point_i] > alpha_ * standard_dev)
192  {
193  indices_per_scale.emplace_back (point_i);
194  indices_table_per_scale[point_i] = true;
195  }
196  }
197  unique_features_indices_.emplace_back (std::move(indices_per_scale));
198  unique_features_table_.emplace_back (std::move(indices_table_per_scale));
199  }
200 }
201 
202 
203 //////////////////////////////////////////////////////////////////////////////////////////////
204 template <typename PointSource, typename PointFeature> void
206  shared_ptr<std::vector<int> > &output_indices)
207 {
208  if (!initCompute ())
209  return;
210 
211  // Compute the features for all scales with the given feature estimator
212  PCL_DEBUG ("[pcl::MultiscaleFeaturePersistence::determinePersistentFeatures] Computing features ...\n");
214 
215  // Compute mean feature
216  PCL_DEBUG ("[pcl::MultiscaleFeaturePersistence::determinePersistentFeatures] Calculating mean feature ...\n");
217  calculateMeanFeature ();
218 
219  // Get the 'unique' features at each scale
220  PCL_DEBUG ("[pcl::MultiscaleFeaturePersistence::determinePersistentFeatures] Extracting unique features ...\n");
221  extractUniqueFeatures ();
222 
223  PCL_DEBUG ("[pcl::MultiscaleFeaturePersistence::determinePersistentFeatures] Determining persistent features between scales ...\n");
224  // Determine persistent features between scales
225 
226 /*
227  // Method 1: a feature is considered persistent if it is 'unique' in at least 2 different scales
228  for (std::size_t scale_i = 0; scale_i < features_at_scale_vectorized_.size () - 1; ++scale_i)
229  for (std::list<std::size_t>::iterator feature_it = unique_features_indices_[scale_i].begin (); feature_it != unique_features_indices_[scale_i].end (); ++feature_it)
230  {
231  if (unique_features_table_[scale_i][*feature_it] == true)
232  {
233  output_features.points.push_back (features_at_scale[scale_i]->points[*feature_it]);
234  output_indices->push_back (feature_estimator_->getIndices ()->at (*feature_it));
235  }
236  }
237 */
238  // Method 2: a feature is considered persistent if it is 'unique' in all the scales
239  for (const auto& feature: unique_features_indices_.front ())
240  {
241  bool present_in_all = true;
242  for (std::size_t scale_i = 0; scale_i < features_at_scale_.size (); ++scale_i)
243  present_in_all = present_in_all && unique_features_table_[scale_i][feature];
244 
245  if (present_in_all)
246  {
247  output_features.points.emplace_back (features_at_scale_.front ()->points[feature]);
248  output_indices->emplace_back (feature_estimator_->getIndices ()->at (feature));
249  }
250  }
251 
252  // Consider that output cloud is unorganized
253  output_features.header = feature_estimator_->getInputCloud ()->header;
254  output_features.is_dense = feature_estimator_->getInputCloud ()->is_dense;
255  output_features.width = static_cast<std::uint32_t> (output_features.points.size ());
256  output_features.height = 1;
257 }
258 
259 
260 #define PCL_INSTANTIATE_MultiscaleFeaturePersistence(InT, Feature) template class PCL_EXPORTS pcl::MultiscaleFeaturePersistence<InT, Feature>;
261 
262 #endif /* PCL_FEATURES_IMPL_MULTISCALE_FEATURE_PERSISTENCE_H_ */
void determinePersistentFeatures(FeatureCloud &output_features, shared_ptr< std::vector< int > > &output_indices)
Central function that computes the persistent features.
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
Definition: point_cloud.h:396
void computeFeaturesAtAllScales()
Method that calls computeFeatureAtScale () for each scale parameter.
std::uint32_t width
The point cloud width (if organized as an image-structure).
Definition: point_cloud.h:399
Definition: norms.h:54
PCL base class.
Definition: pcl_base.h:70
std::uint32_t height
The point cloud height (if organized as an image-structure).
Definition: point_cloud.h:401
pcl::PCLHeader header
The point cloud header.
Definition: point_cloud.h:393
Generic class for extracting the persistent features from an input point cloud It can be given any Fe...
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
DefaultPointRepresentation extends PointRepresentation to define default behavior for common point ty...
typename pcl::PointCloud< PointFeature >::Ptr FeatureCloudPtr
bool is_dense
True if no points are invalid (e.g., have NaN or Inf values in any of their floating point fields)...
Definition: point_cloud.h:404
PointCloudConstPtr input_
The input point cloud dataset.
Definition: pcl_base.h:148
boost::shared_ptr< T > shared_ptr
Alias for boost::shared_ptr.
Definition: memory.h:81