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_.push_back(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::max<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 std::vector<float> diff_vector;
170 std::size_t size = 0;
171 for (
const auto& feature : features_at_scale_vectorized_)
173 size = std::max(size, feature.size());
175 diff_vector.reserve(size);
176 for (std::size_t scale_i = 0; scale_i < features_at_scale_vectorized_.size (); ++scale_i)
179 float standard_dev = 0.0;
182 for (
const auto& feature: features_at_scale_vectorized_[scale_i])
184 float diff = distanceBetweenFeatures (feature, mean_feature_);
185 standard_dev += diff * diff;
186 diff_vector.emplace_back (diff);
188 standard_dev = std::sqrt (standard_dev /
static_cast<float> (features_at_scale_vectorized_[scale_i].size ()));
189 PCL_DEBUG (
"[pcl::MultiscaleFeaturePersistence::extractUniqueFeatures] Standard deviation for scale %f is %f\n", scale_values_[scale_i], standard_dev);
192 std::list<std::size_t> indices_per_scale;
193 std::vector<bool> indices_table_per_scale (features_at_scale_vectorized_[scale_i].size (),
false);
194 for (std::size_t point_i = 0; point_i < features_at_scale_vectorized_[scale_i].size (); ++point_i)
196 if (diff_vector[point_i] > alpha_ * standard_dev)
198 indices_per_scale.emplace_back (point_i);
199 indices_table_per_scale[point_i] =
true;
202 unique_features_indices_.emplace_back (std::move(indices_per_scale));
203 unique_features_table_.emplace_back (std::move(indices_table_per_scale));
209 template <
typename Po
intSource,
typename Po
intFeature>
void
217 PCL_DEBUG (
"[pcl::MultiscaleFeaturePersistence::determinePersistentFeatures] Computing features ...\n");
218 computeFeaturesAtAllScales ();
221 PCL_DEBUG (
"[pcl::MultiscaleFeaturePersistence::determinePersistentFeatures] Calculating mean feature ...\n");
222 calculateMeanFeature ();
225 PCL_DEBUG (
"[pcl::MultiscaleFeaturePersistence::determinePersistentFeatures] Extracting unique features ...\n");
226 extractUniqueFeatures ();
228 PCL_DEBUG (
"[pcl::MultiscaleFeaturePersistence::determinePersistentFeatures] Determining persistent features between scales ...\n");
244 for (
const auto& feature: unique_features_indices_.front ())
246 bool present_in_all =
true;
247 for (std::size_t scale_i = 0; scale_i < features_at_scale_.size (); ++scale_i)
248 present_in_all = present_in_all && unique_features_table_[scale_i][feature];
252 output_features.
emplace_back ((*features_at_scale_.front ())[feature]);
253 output_indices->emplace_back (feature_estimator_->getIndices ()->at (feature));
258 output_features.
header = feature_estimator_->getInputCloud ()->header;
259 output_features.
is_dense = feature_estimator_->getInputCloud ()->is_dense;
260 output_features.
width = output_features.
size ();
261 output_features.
height = 1;
265 #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