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>
50 distance_metric_ (
L1),
51 feature_estimator_ (),
52 features_at_scale_ (),
53 features_at_scale_vectorized_ (),
55 feature_representation_ (),
56 unique_features_indices_ (),
57 unique_features_table_ ()
66 template <
typename Po
intSource,
typename Po
intFeature>
bool
71 PCL_ERROR (
"[pcl::MultiscaleFeaturePersistence::initCompute] PCLBase::initCompute () failed - no input cloud was given.\n");
74 if (!feature_estimator_)
76 PCL_ERROR (
"[pcl::MultiscaleFeaturePersistence::initCompute] No feature estimator was set\n");
79 if (scale_values_.empty ())
81 PCL_ERROR (
"[pcl::MultiscaleFeaturePersistence::initCompute] No scale values were given\n");
85 mean_feature_.resize (feature_representation_->getNumberOfDimensions ());
92 template <
typename Po
intSource,
typename Po
intFeature>
void
95 features_at_scale_.resize (scale_values_.size ());
96 features_at_scale_vectorized_.resize (scale_values_.size ());
97 for (
size_t scale_i = 0; scale_i < scale_values_.size (); ++scale_i)
100 computeFeatureAtScale (scale_values_[scale_i], feature_cloud);
101 features_at_scale_[scale_i] = feature_cloud;
104 std::vector<std::vector<float> > feature_cloud_vectorized (feature_cloud->points.size ());
105 for (
size_t feature_i = 0; feature_i < feature_cloud->points.size (); ++feature_i)
107 std::vector<float> feature_vectorized (feature_representation_->getNumberOfDimensions ());
108 feature_representation_->vectorize (feature_cloud->points[feature_i], feature_vectorized);
109 feature_cloud_vectorized[feature_i] = feature_vectorized;
111 features_at_scale_vectorized_[scale_i] = 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, static_cast<int> (a.size ()), distance_metric_));
136 template <
typename Po
intSource,
typename Po
intFeature>
void
140 for (
int i = 0; i < feature_representation_->getNumberOfDimensions (); ++i)
141 mean_feature_[i] = 0.0f;
143 float normalization_factor = 0.0f;
144 for (std::vector<std::vector<std::vector<float> > >::iterator scale_it = features_at_scale_vectorized_.begin (); scale_it != features_at_scale_vectorized_.end(); ++scale_it) {
145 normalization_factor +=
static_cast<float> (scale_it->size ());
146 for (std::vector<std::vector<float> >::iterator feature_it = scale_it->begin (); feature_it != scale_it->end (); ++feature_it)
147 for (
int dim_i = 0; dim_i < feature_representation_->getNumberOfDimensions (); ++dim_i)
148 mean_feature_[dim_i] += (*feature_it)[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_.resize (scale_values_.size ());
161 unique_features_table_.resize (scale_values_.size ());
162 for (
size_t scale_i = 0; scale_i < features_at_scale_vectorized_.size (); ++scale_i)
165 float standard_dev = 0.0;
166 std::vector<float> diff_vector (features_at_scale_vectorized_[scale_i].size ());
167 for (
size_t point_i = 0; point_i < features_at_scale_vectorized_[scale_i].size (); ++point_i)
169 float diff = distanceBetweenFeatures (features_at_scale_vectorized_[scale_i][point_i], mean_feature_);
170 standard_dev += diff * diff;
171 diff_vector[point_i] = diff;
173 standard_dev = sqrtf (standard_dev / static_cast<float> (features_at_scale_vectorized_[scale_i].size ()));
174 PCL_DEBUG (
"[pcl::MultiscaleFeaturePersistence::extractUniqueFeatures] Standard deviation for scale %f is %f\n", scale_values_[scale_i], standard_dev);
177 std::list<size_t> indices_per_scale;
178 std::vector<bool> indices_table_per_scale (features_at_scale_[scale_i]->points.size (),
false);
179 for (
size_t point_i = 0; point_i < features_at_scale_[scale_i]->points.size (); ++point_i)
181 if (diff_vector[point_i] > alpha_ * standard_dev)
183 indices_per_scale.push_back (point_i);
184 indices_table_per_scale[point_i] =
true;
187 unique_features_indices_[scale_i] = indices_per_scale;
188 unique_features_table_[scale_i] = indices_table_per_scale;
194 template <
typename Po
intSource,
typename Po
intFeature>
void
196 boost::shared_ptr<std::vector<int> > &output_indices)
202 PCL_DEBUG (
"[pcl::MultiscaleFeaturePersistence::determinePersistentFeatures] Computing features ...\n");
203 computeFeaturesAtAllScales ();
206 PCL_DEBUG (
"[pcl::MultiscaleFeaturePersistence::determinePersistentFeatures] Calculating mean feature ...\n");
207 calculateMeanFeature ();
210 PCL_DEBUG (
"[pcl::MultiscaleFeaturePersistence::determinePersistentFeatures] Extracting unique features ...\n");
211 extractUniqueFeatures ();
213 PCL_DEBUG (
"[pcl::MultiscaleFeaturePersistence::determinePersistentFeatures] Determining persistent features between scales ...\n");
229 for (std::list<size_t>::iterator feature_it = unique_features_indices_.front ().begin (); feature_it != unique_features_indices_.front ().end (); ++feature_it)
231 bool present_in_all =
true;
232 for (
size_t scale_i = 0; scale_i < features_at_scale_.size (); ++scale_i)
233 present_in_all = present_in_all && unique_features_table_[scale_i][*feature_it];
237 output_features.
points.push_back (features_at_scale_.front ()->points[*feature_it]);
238 output_indices->push_back (feature_estimator_->getIndices ()->at (*feature_it));
243 output_features.
header = feature_estimator_->getInputCloud ()->header;
244 output_features.
is_dense = feature_estimator_->getInputCloud ()->is_dense;
245 output_features.
width =
static_cast<uint32_t
> (output_features.
points.size ());
246 output_features.
height = 1;
250 #define PCL_INSTANTIATE_MultiscaleFeaturePersistence(InT, Feature) template class PCL_EXPORTS pcl::MultiscaleFeaturePersistence<InT, Feature>;
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.
uint32_t height
The point cloud height (if organized as an image-structure).
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...
PointCloudConstPtr input_
The input point cloud dataset.
pcl::PointCloud< PointFeature >::Ptr FeatureCloudPtr
bool is_dense
True if no points are invalid (e.g., have NaN or Inf values).
void determinePersistentFeatures(FeatureCloud &output_features, boost::shared_ptr< std::vector< int > > &output_indices)
Central function that computes the persistent features.
pcl::PCLHeader header
The point cloud header.