Point Cloud Library (PCL) 1.14.0
Loading...
Searching...
No Matches
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//////////////////////////////////////////////////////////////////////////////////////////////
46template <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
56}
57
58
59//////////////////////////////////////////////////////////////////////////////////////////////
60template <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//////////////////////////////////////////////////////////////////////////////////////////////
86template <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//////////////////////////////////////////////////////////////////////////////////////////////
115template <typename PointSource, typename PointFeature> void
117 FeatureCloudPtr &features)
118{
119 feature_estimator_->setRadiusSearch (scale);
120 feature_estimator_->compute (*features);
121}
122
123
124//////////////////////////////////////////////////////////////////////////////////////////////
125template <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//////////////////////////////////////////////////////////////////////////////////////////////
134template <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//////////////////////////////////////////////////////////////////////////////////////////////
160template <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//////////////////////////////////////////////////////////////////////////////////////////////
208template <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
PointCloud represents the base class in PCL for storing collections of 3D points.
bool is_dense
True if no points are invalid (e.g., have NaN or Inf values in any of their floating point fields).
std::uint32_t width
The point cloud width (if organized as an image-structure).
reference emplace_back(Args &&...args)
Emplace a new point in the cloud, at the end of the container.
pcl::PCLHeader header
The point cloud header.
std::uint32_t height
The point cloud height (if organized as an image-structure).
std::size_t size() const
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
void transform(const T t[12], const T p[3], T out[3])
The first 9 elements of 't' are treated as a 3x3 matrix (row major order) and the last 3 as a transla...
Definition auxiliary.h:304
std::enable_if_t<!std::is_array< T >::value > plus(T &l, const T &r)
shared_ptr< Indices > IndicesPtr
Definition pcl_base.h:58