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.
pcl::PCLHeader header
The point cloud header.
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...
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.
uint32_t height
The point cloud height (if organized as an image-structure).