42 #ifndef PCL_REGISTRATION_IMPL_PYRAMID_FEATURE_MATCHING_H_
43 #define PCL_REGISTRATION_IMPL_PYRAMID_FEATURE_MATCHING_H_
45 #include <pcl/common/point_tests.h>
46 #include <pcl/console/print.h>
51 template <
typename Po
intFeature>
58 if (pyramid_a->nr_dimensions != pyramid_b->nr_dimensions) {
59 PCL_ERROR(
"[pcl::PyramidFeatureMatching::comparePyramidFeatureHistograms] The two "
60 "given pyramids have different numbers of dimensions: %u vs %u\n",
61 pyramid_a->nr_dimensions,
62 pyramid_b->nr_dimensions);
65 if (pyramid_a->nr_levels != pyramid_b->nr_levels) {
66 PCL_ERROR(
"[pcl::PyramidFeatureMatching::comparePyramidFeatureHistograms] The two "
67 "given pyramids have different numbers of levels: %u vs %u\n",
69 pyramid_b->nr_levels);
74 if (pyramid_a->hist_levels[0].hist.size() != pyramid_b->hist_levels[0].hist.size()) {
75 PCL_ERROR(
"[pcl::PyramidFeatureMatching::comparePyramidFeatureHistograms] The two "
76 "given pyramids have different numbers of bins on level 0: %u vs %u\n",
77 pyramid_a->hist_levels[0].hist.size(),
78 pyramid_b->hist_levels[0].hist.size());
81 float match_count_level = 0.0f;
82 for (std::size_t bin_i = 0; bin_i < pyramid_a->hist_levels[0].hist.size(); ++bin_i) {
83 if (pyramid_a->hist_levels[0].hist[bin_i] < pyramid_b->hist_levels[0].hist[bin_i])
84 match_count_level +=
static_cast<float>(pyramid_a->hist_levels[0].hist[bin_i]);
86 match_count_level +=
static_cast<float>(pyramid_b->hist_levels[0].hist[bin_i]);
89 float match_count = match_count_level;
90 for (std::size_t level_i = 1; level_i < pyramid_a->nr_levels; ++level_i) {
91 if (pyramid_a->hist_levels[level_i].hist.size() !=
92 pyramid_b->hist_levels[level_i].hist.size()) {
94 "[pcl::PyramidFeatureMatching::comparePyramidFeatureHistograms] The two "
95 "given pyramids have different numbers of bins on level %u: %u vs %u\n",
97 pyramid_a->hist_levels[level_i].hist.size(),
98 pyramid_b->hist_levels[level_i].hist.size());
102 float match_count_prev_level = match_count_level;
103 match_count_level = 0.0f;
104 for (std::size_t bin_i = 0; bin_i < pyramid_a->hist_levels[level_i].hist.size();
106 if (pyramid_a->hist_levels[level_i].hist[bin_i] <
107 pyramid_b->hist_levels[level_i].hist[bin_i])
109 static_cast<float>(pyramid_a->hist_levels[level_i].hist[bin_i]);
112 static_cast<float>(pyramid_b->hist_levels[level_i].hist[bin_i]);
115 float level_normalization_factor = powf(2.0f,
static_cast<float>(level_i));
117 (match_count_level - match_count_prev_level) / level_normalization_factor;
121 float self_similarity_a =
static_cast<float>(pyramid_a->nr_features),
122 self_similarity_b =
static_cast<float>(pyramid_b->nr_features);
123 PCL_DEBUG(
"[pcl::PyramidFeatureMatching::comparePyramidFeatureHistograms] Self "
124 "similarity measures: %f, %f\n",
127 match_count /= std::sqrt(self_similarity_a * self_similarity_b);
132 template <
typename Po
intFeature>
138 , is_computed_(false)
142 template <
typename Po
intFeature>
145 PointFeature>::PyramidFeatureHistogramLevel::initializeHistogramLevel()
147 std::size_t total_vector_size = 1;
148 for (
const auto& bin : bins_per_dimension) {
149 total_vector_size *= bin;
152 hist.resize(total_vector_size, 0);
155 template <
typename Po
intFeature>
157 PyramidFeatureHistogram<PointFeature>::initializeHistogram()
161 PCL_ERROR(
"[pcl::PyramidFeatureHistogram::initializeHistogram] PCLBase initCompute "
166 if (dimension_range_input_.empty()) {
167 PCL_ERROR(
"[pcl::PyramidFeatureHistogram::initializeHistogram] Input dimension "
168 "range was not set\n");
172 if (dimension_range_target_.empty()) {
173 PCL_ERROR(
"[pcl::PyramidFeatureHistogram::initializeHistogram] Target dimension "
174 "range was not set\n");
178 if (dimension_range_input_.size() != dimension_range_target_.size()) {
179 PCL_ERROR(
"[pcl::PyramidFeatureHistogram::initializeHistogram] Input and target "
180 "dimension ranges do not agree in size: %u vs %u\n",
181 dimension_range_input_.size(),
182 dimension_range_target_.size());
186 nr_dimensions = dimension_range_target_.size();
187 nr_features = input_->size();
189 for (
const auto& dim : dimension_range_target_) {
190 float aux = dim.first - dim.second;
194 nr_levels =
static_cast<std::size_t
>(std::ceil(std::log2(D)));
195 PCL_DEBUG(
"[pcl::PyramidFeatureHistogram::initializeHistogram] Pyramid will have %u "
196 "levels with a hyper-parallelepiped diagonal size of %f\n",
200 hist_levels.resize(nr_levels);
201 for (std::size_t level_i = 0; level_i < nr_levels; ++level_i) {
202 std::vector<std::size_t> bins_per_dimension(nr_dimensions);
203 std::vector<float> bin_step(nr_dimensions);
204 for (std::size_t dim_i = 0; dim_i < nr_dimensions; ++dim_i) {
205 bins_per_dimension[dim_i] =
static_cast<std::size_t
>(
206 std::ceil((dimension_range_target_[dim_i].second -
207 dimension_range_target_[dim_i].first) /
208 (powf(2.0f,
static_cast<float>(level_i)) *
209 std::sqrt(
static_cast<float>(nr_dimensions)))));
210 bin_step[dim_i] = powf(2.0f,
static_cast<float>(level_i)) *
211 std::sqrt(
static_cast<float>(nr_dimensions));
213 hist_levels[level_i] = PyramidFeatureHistogramLevel(bins_per_dimension, bin_step);
215 PCL_DEBUG(
"[pcl::PyramidFeatureHistogram::initializeHistogram] Created vector of "
216 "size %u at level %u\nwith #bins per dimension:",
217 hist_levels.back().hist.size(),
219 for (std::size_t dim_i = 0; dim_i < nr_dimensions; ++dim_i)
220 PCL_DEBUG(
"%u ", bins_per_dimension[dim_i]);
227 template <
typename Po
intFeature>
229 PyramidFeatureHistogram<PointFeature>::at(std::vector<std::size_t>& access,
232 if (access.size() != nr_dimensions) {
234 "[pcl::PyramidFeatureHistogram::at] Cannot access histogram position because "
235 "the access point does not have the right number of dimensions\n");
236 return hist_levels.front().hist.front();
238 if (level >= hist_levels.size()) {
240 "[pcl::PyramidFeatureHistogram::at] Trying to access a too large level\n");
241 return hist_levels.front().hist.front();
244 std::size_t vector_position = 0;
245 std::size_t dim_accumulator = 1;
247 for (
int i =
static_cast<int>(access.size()) - 1; i >= 0; --i) {
248 vector_position += access[i] * dim_accumulator;
249 dim_accumulator *= hist_levels[level].bins_per_dimension[i];
252 return hist_levels[level].hist[vector_position];
255 template <
typename Po
intFeature>
257 PyramidFeatureHistogram<PointFeature>::at(std::vector<float>& feature,
260 if (feature.size() != nr_dimensions) {
261 PCL_ERROR(
"[pcl::PyramidFeatureHistogram::at] The given feature vector does not "
262 "match the feature dimensions of the pyramid histogram: %u vs %u\n",
265 return hist_levels.front().hist.front();
267 if (level >= hist_levels.size()) {
269 "[pcl::PyramidFeatureHistogram::at] Trying to access a too large level\n");
270 return hist_levels.front().hist.front();
273 std::vector<std::size_t> access;
274 for (std::size_t dim_i = 0; dim_i < nr_dimensions; ++dim_i)
275 access.push_back(
static_cast<std::size_t
>(
276 std::floor((feature[dim_i] - dimension_range_target_[dim_i].first) /
277 hist_levels[level].bin_step[dim_i])));
279 return at(access, level);
282 template <
typename Po
intFeature>
284 PyramidFeatureHistogram<PointFeature>::convertFeatureToVector(
285 const PointFeature& feature, std::vector<float>& feature_vector)
288 feature_vector.resize(feature_representation_->getNumberOfDimensions());
289 feature_representation_->vectorize(feature, feature_vector);
292 for (std::size_t i = 0; i < feature_vector.size(); ++i)
294 (feature_vector[i] - dimension_range_input_[i].first) /
295 (dimension_range_input_[i].second - dimension_range_input_[i].first) *
296 (dimension_range_target_[i].second - dimension_range_target_[i].first) +
297 dimension_range_target_[i].first;
300 template <
typename Po
intFeature>
304 if (!initializeHistogram())
307 for (
const auto& point : *input_) {
308 std::vector<float> feature_vector;
312 convertFeatureToVector(point, feature_vector);
313 addFeature(feature_vector);
319 template <
typename Po
intFeature>
323 for (std::size_t level_i = 0; level_i < nr_levels; ++level_i)
324 at(feature, level_i)++;
329 #define PCL_INSTANTIATE_PyramidFeatureHistogram(PointFeature) \
330 template class PCL_EXPORTS pcl::PyramidFeatureHistogram<PointFeature>;
DefaultPointRepresentation extends PointRepresentation to define default behavior for common point ty...
bool initCompute()
This method should get called before starting the actual computation.
Class that compares two sets of features by using a multiscale representation of the features inside ...
Ptr PyramidFeatureHistogramPtr
void compute()
The central method for inserting the feature set inside the pyramid and obtaining the complete pyrami...
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 ...
PyramidFeatureHistogram()
Empty constructor that instantiates the feature representation variable.
bool isFinite(const PointT &pt)
Tests if the 3D components of a point are all finite param[in] pt point to be tested return true if f...
Defines all the PCL and non-PCL macros used.