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
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>;
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.
MultiscaleFeaturePersistence()
Empty constructor.
typename pcl::PointCloud< PointFeature >::Ptr FeatureCloudPtr
PointCloudConstPtr input_
The input point cloud dataset.
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).
float selectNorm(FloatVectorT a, FloatVectorT b, int dim, NormType norm_type)
Method that calculates any norm type available, based on the norm_type variable.
shared_ptr< Indices > IndicesPtr