40 #ifndef PCL_FEATURES_IMPL_MULTISCALE_FEATURE_PERSISTENCE_H_
41 #define PCL_FEATURES_IMPL_MULTISCALE_FEATURE_PERSISTENCE_H_
43 #include <pcl/features/multiscale_feature_persistence.h>
46 template <
typename Po
intSource,
typename Po
intFeature>
49 distance_metric_ (
L1),
50 feature_estimator_ (),
51 features_at_scale_ (),
52 feature_representation_ ()
61 template <
typename Po
intSource,
typename Po
intFeature>
bool
66 PCL_ERROR (
"[pcl::MultiscaleFeaturePersistence::initCompute] PCLBase::initCompute () failed - no input cloud was given.\n");
69 if (!feature_estimator_)
71 PCL_ERROR (
"[pcl::MultiscaleFeaturePersistence::initCompute] No feature estimator was set\n");
74 if (scale_values_.empty ())
76 PCL_ERROR (
"[pcl::MultiscaleFeaturePersistence::initCompute] No scale values were given\n");
80 mean_feature_.resize (feature_representation_->getNumberOfDimensions ());
87 template <
typename Po
intSource,
typename Po
intFeature>
void
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)
97 computeFeatureAtScale (scale_values_[scale_i], feature_cloud);
98 features_at_scale_[scale_i] = feature_cloud;
101 std::vector<std::vector<float> > feature_cloud_vectorized;
102 feature_cloud_vectorized.reserve (feature_cloud->size ());
104 for (
const auto& feature: feature_cloud->points)
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));
110 features_at_scale_vectorized_.emplace_back (std::move(feature_cloud_vectorized));
116 template <
typename Po
intSource,
typename Po
intFeature>
void
118 FeatureCloudPtr &features)
120 feature_estimator_->setRadiusSearch (scale);
121 feature_estimator_->compute (*features);
126 template <
typename Po
intSource,
typename Po
intFeature>
float
128 const std::vector<float> &b)
130 return (
pcl::selectNorm<std::vector<float> > (a, b, a.size (), distance_metric_));
135 template <
typename Po
intSource,
typename Po
intFeature>
void
139 std::fill_n(mean_feature_.begin (), mean_feature_.size (), 0.f);
141 std::size_t normalization_factor = 0;
142 for (
const auto& scale: features_at_scale_vectorized_)
144 normalization_factor += scale.size ();
145 for (
const auto &feature : scale)
146 std::transform(mean_feature_.cbegin (), mean_feature_.cend (),
147 feature.cbegin (), mean_feature_.begin (), std::plus<>{});
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;
161 template <
typename Po
intSource,
typename Po
intFeature>
void
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 ());
169 for (std::size_t scale_i = 0; scale_i < features_at_scale_vectorized_.size (); ++scale_i)
172 float standard_dev = 0.0;
173 std::vector<float> diff_vector (features_at_scale_vectorized_[scale_i].size ());
176 for (
const auto& feature: features_at_scale_vectorized_[scale_i])
178 float diff = distanceBetweenFeatures (feature, mean_feature_);
179 standard_dev += diff * diff;
180 diff_vector.emplace_back (diff);
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);
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)
190 if (diff_vector[point_i] > alpha_ * standard_dev)
192 indices_per_scale.emplace_back (point_i);
193 indices_table_per_scale[point_i] =
true;
196 unique_features_indices_.emplace_back (std::move(indices_per_scale));
197 unique_features_table_.emplace_back (std::move(indices_table_per_scale));
203 template <
typename Po
intSource,
typename Po
intFeature>
void
205 shared_ptr<std::vector<int> > &output_indices)
211 PCL_DEBUG (
"[pcl::MultiscaleFeaturePersistence::determinePersistentFeatures] Computing features ...\n");
212 computeFeaturesAtAllScales ();
215 PCL_DEBUG (
"[pcl::MultiscaleFeaturePersistence::determinePersistentFeatures] Calculating mean feature ...\n");
216 calculateMeanFeature ();
219 PCL_DEBUG (
"[pcl::MultiscaleFeaturePersistence::determinePersistentFeatures] Extracting unique features ...\n");
220 extractUniqueFeatures ();
222 PCL_DEBUG (
"[pcl::MultiscaleFeaturePersistence::determinePersistentFeatures] Determining persistent features between scales ...\n");
238 for (
const auto& feature: unique_features_indices_.front ())
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];
246 output_features.
emplace_back ((*features_at_scale_.front ())[feature]);
247 output_indices->emplace_back (feature_estimator_->getIndices ()->at (feature));
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;
259 #define PCL_INSTANTIATE_MultiscaleFeaturePersistence(InT, Feature) template class PCL_EXPORTS pcl::MultiscaleFeaturePersistence<InT, Feature>;