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;
255 output_features.
width =
static_cast<std::uint32_t
> (output_features.
points.size ());
256 output_features.
height = 1;
260 #define PCL_INSTANTIATE_MultiscaleFeaturePersistence(InT, Feature) template class PCL_EXPORTS pcl::MultiscaleFeaturePersistence<InT, Feature>;
void determinePersistentFeatures(FeatureCloud &output_features, shared_ptr< std::vector< int > > &output_indices)
Central function that computes the persistent features.
std::uint32_t width
The point cloud width (if organized as an image-structure).
MultiscaleFeaturePersistence()
Empty constructor.
void computeFeaturesAtAllScales()
Method that calls computeFeatureAtScale () for each scale parameter.
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
Generic class for extracting the persistent features from an input point cloud It can be given any Fe...
float selectNorm(FloatVectorT a, FloatVectorT b, int dim, NormType norm_type)
Method that calculates any norm type available, based on the norm_type variable.
DefaultPointRepresentation extends PointRepresentation to define default behavior for common point ty...
typename pcl::PointCloud< PointFeature >::Ptr FeatureCloudPtr
std::uint32_t height
The point cloud height (if organized as an image-structure).
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)...
pcl::PCLHeader header
The point cloud header.