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_->points.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 (std::size_t feature_i = 0; feature_i < input_->points.size (); ++feature_i)
280 std::vector<float> feature_vector;
281 convertFeatureToVector (input_->points[feature_i], 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>;