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