42 #ifndef PCL_REGISTRATION_IMPL_PYRAMID_FEATURE_MATCHING_H_ 43 #define PCL_REGISTRATION_IMPL_PYRAMID_FEATURE_MATCHING_H_ 45 #include <pcl/pcl_macros.h> 46 #include <pcl/console/print.h> 56 return std::log (n_arg) / float (M_LN2);
61 template <
typename Po
intFeature>
float 66 if (pyramid_a->nr_dimensions != pyramid_b->nr_dimensions)
68 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);
71 if (pyramid_a->nr_levels != pyramid_b->nr_levels)
73 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);
79 if (pyramid_a->hist_levels[0].hist.size () != pyramid_b->hist_levels[0].hist.size ())
81 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 ());
84 float match_count_level = 0.0f, match_count_prev_level = 0.0f;
85 for (
size_t bin_i = 0; bin_i < pyramid_a->hist_levels[0].hist.size (); ++bin_i)
87 if (pyramid_a->hist_levels[0].hist[bin_i] < pyramid_b->hist_levels[0].hist[bin_i])
88 match_count_level +=
static_cast<float> (pyramid_a->hist_levels[0].hist[bin_i]);
90 match_count_level +=
static_cast<float> (pyramid_b->hist_levels[0].hist[bin_i]);
94 float match_count = match_count_level;
95 for (
size_t level_i = 1; level_i < pyramid_a->nr_levels; ++level_i)
97 if (pyramid_a->hist_levels[level_i].hist.size () != pyramid_b->hist_levels[level_i].hist.size ())
99 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 ());
103 match_count_prev_level = match_count_level;
104 match_count_level = 0.0f;
105 for (
size_t bin_i = 0; bin_i < pyramid_a->hist_levels[level_i].hist.size (); ++bin_i)
107 if (pyramid_a->hist_levels[level_i].hist[bin_i] < pyramid_b->hist_levels[level_i].hist[bin_i])
108 match_count_level +=
static_cast<float> (pyramid_a->hist_levels[level_i].hist[bin_i]);
110 match_count_level +=
static_cast<float> (pyramid_b->hist_levels[level_i].hist[bin_i]);
113 float level_normalization_factor = powf (2.0f, static_cast<float> (level_i));
114 match_count += (match_count_level - match_count_prev_level) / level_normalization_factor;
119 float self_similarity_a =
static_cast<float> (pyramid_a->nr_features),
120 self_similarity_b = static_cast<float> (pyramid_b->nr_features);
121 PCL_DEBUG (
"[pcl::PyramidFeatureMatching::comparePyramidFeatureHistograms] Self similarity measures: %f, %f\n", self_similarity_a, self_similarity_b);
122 match_count /= std::sqrt (self_similarity_a * self_similarity_b);
129 template <
typename Po
intFeature>
131 nr_dimensions (0), nr_levels (0), nr_features (0),
132 dimension_range_input_ (), dimension_range_target_ (),
134 is_computed_ (false),
140 template <
typename Po
intFeature>
void 143 size_t total_vector_size = 1;
144 for (std::vector<size_t>::iterator dim_it = bins_per_dimension.begin (); dim_it != bins_per_dimension.end (); ++dim_it)
145 total_vector_size *= *dim_it;
147 hist.resize (total_vector_size, 0);
152 template <
typename Po
intFeature>
bool 158 PCL_ERROR (
"[pcl::PyramidFeatureHistogram::initializeHistogram] PCLBase initCompute failed\n");
162 if (dimension_range_input_.size () == 0)
164 PCL_ERROR (
"[pcl::PyramidFeatureHistogram::initializeHistogram] Input dimension range was not set\n");
168 if (dimension_range_target_.size () == 0)
170 PCL_ERROR (
"[pcl::PyramidFeatureHistogram::initializeHistogram] Target dimension range was not set\n");
174 if (dimension_range_input_.size () != dimension_range_target_.size ())
176 PCL_ERROR (
"[pcl::PyramidFeatureHistogram::initializeHistogram] Input and target dimension ranges do not agree in size: %u vs %u\n",
177 dimension_range_input_.size (), dimension_range_target_.size ());
182 nr_dimensions = dimension_range_target_.size ();
183 nr_features = input_->points.size ();
185 for (std::vector<std::pair<float, float> >::iterator range_it = dimension_range_target_.begin (); range_it != dimension_range_target_.end (); ++range_it)
187 float aux = range_it->first - range_it->second;
191 nr_levels =
static_cast<size_t> (ceilf (Log2 (D)));
192 PCL_DEBUG (
"[pcl::PyramidFeatureHistogram::initializeHistogram] Pyramid will have %u levels with a hyper-parallelepiped diagonal size of %f\n", nr_levels, D);
195 hist_levels.resize (nr_levels);
196 for (
size_t level_i = 0; level_i < nr_levels; ++level_i)
198 std::vector<size_t> bins_per_dimension (nr_dimensions);
199 std::vector<float> bin_step (nr_dimensions);
200 for (
size_t dim_i = 0; dim_i < nr_dimensions; ++dim_i)
202 bins_per_dimension[dim_i] =
203 static_cast<size_t> (ceilf ((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)))));
204 bin_step[dim_i] = powf (2.0f, static_cast<float> (level_i)) * std::sqrt (static_cast<float> (nr_dimensions));
206 hist_levels[level_i] = PyramidFeatureHistogramLevel (bins_per_dimension, bin_step);
208 PCL_DEBUG (
"[pcl::PyramidFeatureHistogram::initializeHistogram] Created vector of size %u at level %u\nwith #bins per dimension:", hist_levels.back ().hist.size (), level_i);
209 for (
size_t dim_i = 0; dim_i < nr_dimensions; ++dim_i)
210 PCL_DEBUG (
"%u ", bins_per_dimension[dim_i]);
219 template <
typename Po
intFeature>
unsigned int&
223 if (access.size () != nr_dimensions)
225 PCL_ERROR (
"[pcl::PyramidFeatureHistogram::at] Cannot access histogram position because the access point does not have the right number of dimensions\n");
226 return hist_levels.front ().hist.front ();
228 if (level >= hist_levels.size ())
230 PCL_ERROR (
"[pcl::PyramidFeatureHistogram::at] Trying to access a too large level\n");
231 return hist_levels.front ().hist.front ();
234 size_t vector_position = 0;
235 size_t dim_accumulator = 1;
237 for (
int i = static_cast<int> (access.size ()) - 1; i >= 0; --i)
239 vector_position += access[i] * dim_accumulator;
240 dim_accumulator *= hist_levels[level].bins_per_dimension[i];
243 return hist_levels[level].hist[vector_position];
248 template <
typename Po
intFeature>
unsigned int&
252 if (feature.size () != nr_dimensions)
254 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);
255 return hist_levels.front ().hist.front ();
257 if (level >= hist_levels.size ())
259 PCL_ERROR (
"[pcl::PyramidFeatureHistogram::at] Trying to access a too large level\n");
260 return hist_levels.front ().hist.front ();
263 std::vector<size_t> access;
264 for (
size_t dim_i = 0; dim_i < nr_dimensions; ++dim_i)
265 access.push_back (static_cast<size_t> (floor ((feature[dim_i] - dimension_range_target_[dim_i].first) / hist_levels[level].bin_step[dim_i])));
267 return at (access, level);
272 template <
typename Po
intFeature>
void 274 std::vector<float> &feature_vector)
277 feature_vector.resize (feature_representation_->getNumberOfDimensions ());
278 feature_representation_->vectorize (feature, feature_vector);
281 for (
size_t i = 0; i < feature_vector.size (); ++i)
282 feature_vector[i] = (feature_vector[i] - dimension_range_input_[i].first) / (dimension_range_input_[i].second - dimension_range_input_[i].first) *
283 (dimension_range_target_[i].second - dimension_range_target_[i].first) + dimension_range_target_[i].first;
288 template <
typename Po
intFeature>
void 291 if (!initializeHistogram ())
294 for (
size_t feature_i = 0; feature_i < input_->points.size (); ++feature_i)
296 std::vector<float> feature_vector;
297 convertFeatureToVector (input_->points[feature_i], feature_vector);
298 addFeature (feature_vector);
306 template <
typename Po
intFeature>
void 309 for (
size_t level_i = 0; level_i < nr_levels; ++level_i)
310 at (feature, level_i) ++;
313 #define PCL_INSTANTIATE_PyramidFeatureHistogram(PointFeature) template class PCL_EXPORTS pcl::PyramidFeatureHistogram<PointFeature>; Ptr PyramidFeatureHistogramPtr
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...
DefaultPointRepresentation extends PointRepresentation to define default behavior for common point ty...