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 (std::vector<std::size_t>::iterator dim_it = bins_per_dimension.begin();
149 dim_it != bins_per_dimension.end();
151 total_vector_size *= *dim_it;
153 hist.resize(total_vector_size, 0);
156 template <
typename Po
intFeature>
158 PyramidFeatureHistogram<PointFeature>::initializeHistogram()
162 PCL_ERROR(
"[pcl::PyramidFeatureHistogram::initializeHistogram] PCLBase initCompute "
167 if (dimension_range_input_.empty()) {
168 PCL_ERROR(
"[pcl::PyramidFeatureHistogram::initializeHistogram] Input dimension "
169 "range was not set\n");
173 if (dimension_range_target_.empty()) {
174 PCL_ERROR(
"[pcl::PyramidFeatureHistogram::initializeHistogram] Target dimension "
175 "range was not set\n");
179 if (dimension_range_input_.size() != dimension_range_target_.size()) {
180 PCL_ERROR(
"[pcl::PyramidFeatureHistogram::initializeHistogram] Input and target "
181 "dimension ranges do not agree in size: %u vs %u\n",
182 dimension_range_input_.size(),
183 dimension_range_target_.size());
187 nr_dimensions = dimension_range_target_.size();
188 nr_features = input_->size();
190 for (std::vector<std::pair<float, float>>::iterator range_it =
191 dimension_range_target_.begin();
192 range_it != dimension_range_target_.end();
194 float aux = range_it->first - range_it->second;
198 nr_levels =
static_cast<std::size_t
>(std::ceil(std::log2(D)));
199 PCL_DEBUG(
"[pcl::PyramidFeatureHistogram::initializeHistogram] Pyramid will have %u "
200 "levels with a hyper-parallelepiped diagonal size of %f\n",
204 hist_levels.resize(nr_levels);
205 for (std::size_t level_i = 0; level_i < nr_levels; ++level_i) {
206 std::vector<std::size_t> bins_per_dimension(nr_dimensions);
207 std::vector<float> bin_step(nr_dimensions);
208 for (std::size_t dim_i = 0; dim_i < nr_dimensions; ++dim_i) {
209 bins_per_dimension[dim_i] =
static_cast<std::size_t
>(
210 std::ceil((dimension_range_target_[dim_i].second -
211 dimension_range_target_[dim_i].first) /
212 (powf(2.0f,
static_cast<float>(level_i)) *
213 std::sqrt(
static_cast<float>(nr_dimensions)))));
214 bin_step[dim_i] = powf(2.0f,
static_cast<float>(level_i)) *
215 std::sqrt(
static_cast<float>(nr_dimensions));
217 hist_levels[level_i] = PyramidFeatureHistogramLevel(bins_per_dimension, bin_step);
219 PCL_DEBUG(
"[pcl::PyramidFeatureHistogram::initializeHistogram] Created vector of "
220 "size %u at level %u\nwith #bins per dimension:",
221 hist_levels.back().hist.size(),
223 for (std::size_t dim_i = 0; dim_i < nr_dimensions; ++dim_i)
224 PCL_DEBUG(
"%u ", bins_per_dimension[dim_i]);
231 template <
typename Po
intFeature>
233 PyramidFeatureHistogram<PointFeature>::at(std::vector<std::size_t>& access,
236 if (access.size() != nr_dimensions) {
238 "[pcl::PyramidFeatureHistogram::at] Cannot access histogram position because "
239 "the access point does not have the right number of dimensions\n");
240 return hist_levels.front().hist.front();
242 if (level >= hist_levels.size()) {
244 "[pcl::PyramidFeatureHistogram::at] Trying to access a too large level\n");
245 return hist_levels.front().hist.front();
248 std::size_t vector_position = 0;
249 std::size_t dim_accumulator = 1;
251 for (
int i =
static_cast<int>(access.size()) - 1; i >= 0; --i) {
252 vector_position += access[i] * dim_accumulator;
253 dim_accumulator *= hist_levels[level].bins_per_dimension[i];
256 return hist_levels[level].hist[vector_position];
259 template <
typename Po
intFeature>
261 PyramidFeatureHistogram<PointFeature>::at(std::vector<float>& feature,
264 if (feature.size() != nr_dimensions) {
265 PCL_ERROR(
"[pcl::PyramidFeatureHistogram::at] The given feature vector does not "
266 "match the feature dimensions of the pyramid histogram: %u vs %u\n",
269 return hist_levels.front().hist.front();
271 if (level >= hist_levels.size()) {
273 "[pcl::PyramidFeatureHistogram::at] Trying to access a too large level\n");
274 return hist_levels.front().hist.front();
277 std::vector<std::size_t> access;
278 for (std::size_t dim_i = 0; dim_i < nr_dimensions; ++dim_i)
279 access.push_back(
static_cast<std::size_t
>(
280 std::floor((feature[dim_i] - dimension_range_target_[dim_i].first) /
281 hist_levels[level].bin_step[dim_i])));
283 return at(access, level);
286 template <
typename Po
intFeature>
288 PyramidFeatureHistogram<PointFeature>::convertFeatureToVector(
289 const PointFeature& feature, std::vector<float>& feature_vector)
292 feature_vector.resize(feature_representation_->getNumberOfDimensions());
293 feature_representation_->vectorize(feature, feature_vector);
296 for (std::size_t i = 0; i < feature_vector.size(); ++i)
298 (feature_vector[i] - dimension_range_input_[i].first) /
299 (dimension_range_input_[i].second - dimension_range_input_[i].first) *
300 (dimension_range_target_[i].second - dimension_range_target_[i].first) +
301 dimension_range_target_[i].first;
304 template <
typename Po
intFeature>
308 if (!initializeHistogram())
311 for (
const auto& point : *input_) {
312 std::vector<float> feature_vector;
316 convertFeatureToVector(point, feature_vector);
317 addFeature(feature_vector);
323 template <
typename Po
intFeature>
327 for (std::size_t level_i = 0; level_i < nr_levels; ++level_i)
328 at(feature, level_i)++;
333 #define PCL_INSTANTIATE_PyramidFeatureHistogram(PointFeature) \
334 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.