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>
MultiscaleFeaturePersistence()47 pcl::MultiscaleFeaturePersistence<PointSource, PointFeature>::MultiscaleFeaturePersistence () :
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
initCompute()62 pcl::MultiscaleFeaturePersistence<PointSource, PointFeature>::initCompute ()
63 {
64 if (!PCLBase<PointSource>::initCompute ())
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
computeFeaturesAtAllScales()88 pcl::MultiscaleFeaturePersistence<PointSource, PointFeature>::computeFeaturesAtAllScales ()
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
computeFeatureAtScale(float & scale,FeatureCloudPtr & features)117 pcl::MultiscaleFeaturePersistence<PointSource, PointFeature>::computeFeatureAtScale (float &scale,
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
distanceBetweenFeatures(const std::vector<float> & a,const std::vector<float> & b)127 pcl::MultiscaleFeaturePersistence<PointSource, PointFeature>::distanceBetweenFeatures (const std::vector<float> &a,
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
calculateMeanFeature()136 pcl::MultiscaleFeaturePersistence<PointSource, PointFeature>::calculateMeanFeature ()
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
extractUniqueFeatures()162 pcl::MultiscaleFeaturePersistence<PointSource, PointFeature>::extractUniqueFeatures ()
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 = ::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
determinePersistentFeatures(FeatureCloud & output_features,pcl::IndicesPtr & output_indices)204 pcl::MultiscaleFeaturePersistence<PointSource, PointFeature>::determinePersistentFeatures (FeatureCloud &output_features,
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_ */
262