42 #ifndef PCL_REGISTRATION_IMPL_PYRAMID_FEATURE_MATCHING_H_ 43 #define PCL_REGISTRATION_IMPL_PYRAMID_FEATURE_MATCHING_H_ 46 #include <pcl/console/print.h> 52 template <
typename Po
intFeature>
float 57 if (pyramid_a->nr_dimensions != pyramid_b->nr_dimensions)
59 PCL_ERROR (
"[pcl::PyramidFeatureMatching::comparePyramidFeatureHistograms] The two given pyramids have different numbers of dimensions: %u vs %u\n", pyramid_a->nr_dimensions, pyramid_b->nr_dimensions);
62 if (pyramid_a->nr_levels != pyramid_b->nr_levels)
64 PCL_ERROR (
"[pcl::PyramidFeatureMatching::comparePyramidFeatureHistograms] The two given pyramids have different numbers of levels: %u vs %u\n", pyramid_a->nr_levels, pyramid_b->nr_levels);
70 if (pyramid_a->hist_levels[0].hist.size () != pyramid_b->hist_levels[0].hist.size ())
72 PCL_ERROR (
"[pcl::PyramidFeatureMatching::comparePyramidFeatureHistograms] The two given pyramids have different numbers of bins on level 0: %u vs %u\n", pyramid_a->hist_levels[0].hist.size (), pyramid_b->hist_levels[0].hist.size ());
75 float match_count_level = 0.0f;
76 for (std::size_t bin_i = 0; bin_i < pyramid_a->hist_levels[0].hist.size (); ++bin_i)
78 if (pyramid_a->hist_levels[0].hist[bin_i] < pyramid_b->hist_levels[0].hist[bin_i])
79 match_count_level +=
static_cast<float> (pyramid_a->hist_levels[0].hist[bin_i]);
81 match_count_level +=
static_cast<float> (pyramid_b->hist_levels[0].hist[bin_i]);
85 float match_count = match_count_level;
86 for (std::size_t level_i = 1; level_i < pyramid_a->nr_levels; ++level_i)
88 if (pyramid_a->hist_levels[level_i].hist.size () != pyramid_b->hist_levels[level_i].hist.size ())
90 PCL_ERROR (
"[pcl::PyramidFeatureMatching::comparePyramidFeatureHistograms] The two given pyramids have different numbers of bins on level %u: %u vs %u\n", level_i, pyramid_a->hist_levels[level_i].hist.size (), pyramid_b->hist_levels[level_i].hist.size ());
94 float match_count_prev_level = match_count_level;
95 match_count_level = 0.0f;
96 for (std::size_t bin_i = 0; bin_i < pyramid_a->hist_levels[level_i].hist.size (); ++bin_i)
98 if (pyramid_a->hist_levels[level_i].hist[bin_i] < pyramid_b->hist_levels[level_i].hist[bin_i])
99 match_count_level +=
static_cast<float> (pyramid_a->hist_levels[level_i].hist[bin_i]);
101 match_count_level +=
static_cast<float> (pyramid_b->hist_levels[level_i].hist[bin_i]);
104 float level_normalization_factor = powf (2.0f, static_cast<float> (level_i));
105 match_count += (match_count_level - match_count_prev_level) / level_normalization_factor;
110 float self_similarity_a =
static_cast<float> (pyramid_a->nr_features),
111 self_similarity_b = static_cast<float> (pyramid_b->nr_features);
112 PCL_DEBUG (
"[pcl::PyramidFeatureMatching::comparePyramidFeatureHistograms] Self similarity measures: %f, %f\n", self_similarity_a, self_similarity_b);
113 match_count /= std::sqrt (self_similarity_a * self_similarity_b);
119 template <
typename Po
intFeature>
121 nr_dimensions (0), nr_levels (0), nr_features (0),
123 is_computed_ (false),
129 template <
typename Po
intFeature>
void 132 std::size_t total_vector_size = 1;
133 for (std::vector<std::size_t>::iterator dim_it = bins_per_dimension.begin (); dim_it != bins_per_dimension.end (); ++dim_it)
134 total_vector_size *= *dim_it;
136 hist.resize (total_vector_size, 0);
140 template <
typename Po
intFeature>
bool 141 PyramidFeatureHistogram<PointFeature>::initializeHistogram ()
146 PCL_ERROR (
"[pcl::PyramidFeatureHistogram::initializeHistogram] PCLBase initCompute failed\n");
150 if (dimension_range_input_.empty ())
152 PCL_ERROR (
"[pcl::PyramidFeatureHistogram::initializeHistogram] Input dimension range was not set\n");
156 if (dimension_range_target_.empty ())
158 PCL_ERROR (
"[pcl::PyramidFeatureHistogram::initializeHistogram] Target dimension range was not set\n");
162 if (dimension_range_input_.size () != dimension_range_target_.size ())
164 PCL_ERROR (
"[pcl::PyramidFeatureHistogram::initializeHistogram] Input and target dimension ranges do not agree in size: %u vs %u\n",
165 dimension_range_input_.size (), dimension_range_target_.size ());
170 nr_dimensions = dimension_range_target_.size ();
171 nr_features = input_->size ();
173 for (std::vector<std::pair<float, float> >::iterator range_it = dimension_range_target_.begin (); range_it != dimension_range_target_.end (); ++range_it)
175 float aux = range_it->first - range_it->second;
179 nr_levels =
static_cast<std::size_t
> (std::ceil (std::log2(D)));
180 PCL_DEBUG (
"[pcl::PyramidFeatureHistogram::initializeHistogram] Pyramid will have %u levels with a hyper-parallelepiped diagonal size of %f\n", nr_levels, D);
183 hist_levels.resize (nr_levels);
184 for (std::size_t level_i = 0; level_i < nr_levels; ++level_i)
186 std::vector<std::size_t> bins_per_dimension (nr_dimensions);
187 std::vector<float> bin_step (nr_dimensions);
188 for (std::size_t dim_i = 0; dim_i < nr_dimensions; ++dim_i)
190 bins_per_dimension[dim_i] =
191 static_cast<std::size_t
> (std::ceil ((dimension_range_target_[dim_i].second - dimension_range_target_[dim_i].first) / (powf (2.0f, static_cast<float> (level_i)) * std::sqrt (static_cast<float> (nr_dimensions)))));
192 bin_step[dim_i] = powf (2.0f, static_cast<float> (level_i)) * std::sqrt (static_cast<float> (nr_dimensions));
194 hist_levels[level_i] = PyramidFeatureHistogramLevel (bins_per_dimension, bin_step);
196 PCL_DEBUG (
"[pcl::PyramidFeatureHistogram::initializeHistogram] Created vector of size %u at level %u\nwith #bins per dimension:", hist_levels.back ().hist.size (), level_i);
197 for (std::size_t dim_i = 0; dim_i < nr_dimensions; ++dim_i)
198 PCL_DEBUG (
"%u ", bins_per_dimension[dim_i]);
206 template <
typename Po
intFeature>
unsigned int&
207 PyramidFeatureHistogram<PointFeature>::at (std::vector<std::size_t> &access,
210 if (access.size () != nr_dimensions)
212 PCL_ERROR (
"[pcl::PyramidFeatureHistogram::at] Cannot access histogram position because the access point does not have the right number of dimensions\n");
213 return hist_levels.front ().hist.front ();
215 if (level >= hist_levels.size ())
217 PCL_ERROR (
"[pcl::PyramidFeatureHistogram::at] Trying to access a too large level\n");
218 return hist_levels.front ().hist.front ();
221 std::size_t vector_position = 0;
222 std::size_t dim_accumulator = 1;
224 for (
int i = static_cast<int> (access.size ()) - 1; i >= 0; --i)
226 vector_position += access[i] * dim_accumulator;
227 dim_accumulator *= hist_levels[level].bins_per_dimension[i];
230 return hist_levels[level].hist[vector_position];
234 template <
typename Po
intFeature>
unsigned int&
235 PyramidFeatureHistogram<PointFeature>::at (std::vector<float> &feature,
238 if (feature.size () != nr_dimensions)
240 PCL_ERROR (
"[pcl::PyramidFeatureHistogram::at] The given feature vector does not match the feature dimensions of the pyramid histogram: %u vs %u\n", feature.size (), nr_dimensions);
241 return hist_levels.front ().hist.front ();
243 if (level >= hist_levels.size ())
245 PCL_ERROR (
"[pcl::PyramidFeatureHistogram::at] Trying to access a too large level\n");
246 return hist_levels.front ().hist.front ();
249 std::vector<std::size_t> access;
250 for (std::size_t dim_i = 0; dim_i < nr_dimensions; ++dim_i)
251 access.push_back (static_cast<std::size_t> (std::floor ((feature[dim_i] - dimension_range_target_[dim_i].first) / hist_levels[level].bin_step[dim_i])));
253 return at (access, level);
257 template <
typename Po
intFeature>
void 258 PyramidFeatureHistogram<PointFeature>::convertFeatureToVector (
const PointFeature &feature,
259 std::vector<float> &feature_vector)
262 feature_vector.resize (feature_representation_->getNumberOfDimensions ());
263 feature_representation_->vectorize (feature, feature_vector);
266 for (std::size_t i = 0; i < feature_vector.size (); ++i)
267 feature_vector[i] = (feature_vector[i] - dimension_range_input_[i].first) / (dimension_range_input_[i].second - dimension_range_input_[i].first) *
268 (dimension_range_target_[i].second - dimension_range_target_[i].first) + dimension_range_target_[i].first;
272 template <
typename Po
intFeature>
void 275 if (!initializeHistogram ())
278 for (
const auto& point: *input_)
280 std::vector<float> feature_vector;
281 convertFeatureToVector (point, feature_vector);
282 addFeature (feature_vector);
289 template <
typename Po
intFeature>
void 292 for (std::size_t level_i = 0; level_i < nr_levels; ++level_i)
293 at (feature, level_i) ++;
298 #define PCL_INSTANTIATE_PyramidFeatureHistogram(PointFeature) template class PCL_EXPORTS pcl::PyramidFeatureHistogram<PointFeature>;
bool initCompute()
This method should get called before starting the actual computation.
static float comparePyramidFeatureHistograms(const PyramidFeatureHistogramPtr &pyramid_a, const PyramidFeatureHistogramPtr &pyramid_b)
Static method for comparing two pyramid histograms that returns a floating point value between 0 and ...
Class that compares two sets of features by using a multiscale representation of the features inside ...
PyramidFeatureHistogram()
Empty constructor that instantiates the feature representation variable.
void compute()
The central method for inserting the feature set inside the pyramid and obtaining the complete pyrami...
Ptr PyramidFeatureHistogramPtr
DefaultPointRepresentation extends PointRepresentation to define default behavior for common point ty...
Defines all the PCL and non-PCL macros used.