40 #ifndef PCL_FEATURES_IMPL_MULTISCALE_FEATURE_PERSISTENCE_H_
41 #define PCL_FEATURES_IMPL_MULTISCALE_FEATURE_PERSISTENCE_H_
44 #include <pcl/features/multiscale_feature_persistence.h>
47 template <
typename Po
intSource,
typename Po
intFeature>
50 distance_metric_ (
L1),
51 feature_estimator_ (),
52 features_at_scale_ (),
53 feature_representation_ ()
62 template <
typename Po
intSource,
typename Po
intFeature>
bool
67 PCL_ERROR (
"[pcl::MultiscaleFeaturePersistence::initCompute] PCLBase::initCompute () failed - no input cloud was given.\n");
70 if (!feature_estimator_)
72 PCL_ERROR (
"[pcl::MultiscaleFeaturePersistence::initCompute] No feature estimator was set\n");
75 if (scale_values_.empty ())
77 PCL_ERROR (
"[pcl::MultiscaleFeaturePersistence::initCompute] No scale values were given\n");
81 mean_feature_.resize (feature_representation_->getNumberOfDimensions ());
88 template <
typename Po
intSource,
typename Po
intFeature>
void
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)
98 computeFeatureAtScale (scale_values_[scale_i], feature_cloud);
99 features_at_scale_[scale_i] = feature_cloud;
102 std::vector<std::vector<float> > feature_cloud_vectorized;
103 feature_cloud_vectorized.reserve (feature_cloud->points.size ());
105 for (
const auto& feature: feature_cloud->points)
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));
111 features_at_scale_vectorized_.emplace_back (std::move(feature_cloud_vectorized));
117 template <
typename Po
intSource,
typename Po
intFeature>
void
119 FeatureCloudPtr &features)
121 feature_estimator_->setRadiusSearch (scale);
122 feature_estimator_->compute (*features);
127 template <
typename Po
intSource,
typename Po
intFeature>
float
129 const std::vector<float> &b)
131 return (
pcl::selectNorm<std::vector<float> > (a, b, a.size (), distance_metric_));
136 template <
typename Po
intSource,
typename Po
intFeature>
void
140 std::fill_n(mean_feature_.begin (), mean_feature_.size (), 0.f);
142 std::size_t normalization_factor = 0;
143 for (
const auto& scale: features_at_scale_vectorized_)
145 normalization_factor += scale.size ();
146 for (
const auto &feature : scale)
147 std::transform(mean_feature_.cbegin (), mean_feature_.cend (),
148 feature.cbegin (), mean_feature_.begin (), std::plus<>{});
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;
162 template <
typename Po
intSource,
typename Po
intFeature>
void
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 ());
170 for (std::size_t scale_i = 0; scale_i < features_at_scale_vectorized_.size (); ++scale_i)
173 float standard_dev = 0.0;
174 std::vector<float> diff_vector (features_at_scale_vectorized_[scale_i].size ());
177 for (
const auto& feature: features_at_scale_vectorized_[scale_i])
179 float diff = distanceBetweenFeatures (feature, mean_feature_);
180 standard_dev += diff * diff;
181 diff_vector.emplace_back (diff);
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);
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)
191 if (diff_vector[point_i] > alpha_ * standard_dev)
193 indices_per_scale.emplace_back (point_i);
194 indices_table_per_scale[point_i] =
true;
197 unique_features_indices_.emplace_back (std::move(indices_per_scale));
198 unique_features_table_.emplace_back (std::move(indices_table_per_scale));
204 template <
typename Po
intSource,
typename Po
intFeature>
void
206 shared_ptr<std::vector<int> > &output_indices)
212 PCL_DEBUG (
"[pcl::MultiscaleFeaturePersistence::determinePersistentFeatures] Computing features ...\n");
213 computeFeaturesAtAllScales ();
216 PCL_DEBUG (
"[pcl::MultiscaleFeaturePersistence::determinePersistentFeatures] Calculating mean feature ...\n");
217 calculateMeanFeature ();
220 PCL_DEBUG (
"[pcl::MultiscaleFeaturePersistence::determinePersistentFeatures] Extracting unique features ...\n");
221 extractUniqueFeatures ();
223 PCL_DEBUG (
"[pcl::MultiscaleFeaturePersistence::determinePersistentFeatures] Determining persistent features between scales ...\n");
239 for (
const auto& feature: unique_features_indices_.front ())
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];
247 output_features.
points.emplace_back (features_at_scale_.front ()->points[feature]);
248 output_indices->emplace_back (feature_estimator_->getIndices ()->at (feature));
253 output_features.
header = feature_estimator_->getInputCloud ()->header;
254 output_features.
is_dense = feature_estimator_->getInputCloud ()->is_dense;
256 output_features.
height = 1;
260 #define PCL_INSTANTIATE_MultiscaleFeaturePersistence(InT, Feature) template class PCL_EXPORTS pcl::MultiscaleFeaturePersistence<InT, Feature>;