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->points.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 for (
int i = 0; i < feature_representation_->getNumberOfDimensions (); ++i)
140 mean_feature_[i] = 0.0f;
142 float normalization_factor = 0.0f;
143 for (
const auto& scale: features_at_scale_vectorized_)
145 normalization_factor +=
static_cast<float> (scale.size ());
146 for (
const auto &feature : scale)
147 for (
int dim_i = 0; dim_i < feature_representation_->getNumberOfDimensions (); ++dim_i)
148 mean_feature_[dim_i] += feature[dim_i];
151 for (
int dim_i = 0; dim_i < feature_representation_->getNumberOfDimensions (); ++dim_i)
152 mean_feature_[dim_i] /= normalization_factor;
157 template <
typename Po
intSource,
typename Po
intFeature>
void
160 unique_features_indices_.clear ();
161 unique_features_table_.clear ();
162 unique_features_indices_.reserve (scale_values_.size ());
163 unique_features_table_.reserve (scale_values_.size ());
165 for (std::size_t scale_i = 0; scale_i < features_at_scale_vectorized_.size (); ++scale_i)
168 float standard_dev = 0.0;
169 std::vector<float> diff_vector (features_at_scale_vectorized_[scale_i].size ());
172 for (
const auto& feature: features_at_scale_vectorized_[scale_i])
174 float diff = distanceBetweenFeatures (feature, mean_feature_);
175 standard_dev += diff * diff;
176 diff_vector.emplace_back (diff);
178 standard_dev = std::sqrt (standard_dev /
static_cast<float> (features_at_scale_vectorized_[scale_i].size ()));
179 PCL_DEBUG (
"[pcl::MultiscaleFeaturePersistence::extractUniqueFeatures] Standard deviation for scale %f is %f\n", scale_values_[scale_i], standard_dev);
182 std::list<std::size_t> indices_per_scale;
183 std::vector<bool> indices_table_per_scale (features_at_scale_[scale_i]->points.size (),
false);
184 for (std::size_t point_i = 0; point_i < features_at_scale_[scale_i]->points.size (); ++point_i)
186 if (diff_vector[point_i] > alpha_ * standard_dev)
188 indices_per_scale.emplace_back (point_i);
189 indices_table_per_scale[point_i] =
true;
192 unique_features_indices_.emplace_back (std::move(indices_per_scale));
193 unique_features_table_.emplace_back (std::move(indices_table_per_scale));
199 template <
typename Po
intSource,
typename Po
intFeature>
void
201 shared_ptr<std::vector<int> > &output_indices)
207 PCL_DEBUG (
"[pcl::MultiscaleFeaturePersistence::determinePersistentFeatures] Computing features ...\n");
208 computeFeaturesAtAllScales ();
211 PCL_DEBUG (
"[pcl::MultiscaleFeaturePersistence::determinePersistentFeatures] Calculating mean feature ...\n");
212 calculateMeanFeature ();
215 PCL_DEBUG (
"[pcl::MultiscaleFeaturePersistence::determinePersistentFeatures] Extracting unique features ...\n");
216 extractUniqueFeatures ();
218 PCL_DEBUG (
"[pcl::MultiscaleFeaturePersistence::determinePersistentFeatures] Determining persistent features between scales ...\n");
234 for (
const auto& feature: unique_features_indices_.front ())
236 bool present_in_all =
true;
237 for (std::size_t scale_i = 0; scale_i < features_at_scale_.size (); ++scale_i)
238 present_in_all = present_in_all && unique_features_table_[scale_i][feature];
242 output_features.
points.emplace_back (features_at_scale_.front ()->points[feature]);
243 output_indices->emplace_back (feature_estimator_->getIndices ()->at (feature));
248 output_features.
header = feature_estimator_->getInputCloud ()->header;
249 output_features.
is_dense = feature_estimator_->getInputCloud ()->is_dense;
251 output_features.
height = 1;
255 #define PCL_INSTANTIATE_MultiscaleFeaturePersistence(InT, Feature) template class PCL_EXPORTS pcl::MultiscaleFeaturePersistence<InT, Feature>;