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