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>
51template <
typename Po
intFeature>
59 PCL_ERROR(
"[pcl::PyramidFeatureMatching::comparePyramidFeatureHistograms] The two "
60 "given pyramids have different numbers of dimensions: %u vs %u\n",
66 PCL_ERROR(
"[pcl::PyramidFeatureMatching::comparePyramidFeatureHistograms] The two "
67 "given pyramids have different numbers of levels: %u vs %u\n",
75 PCL_ERROR(
"[pcl::PyramidFeatureMatching::comparePyramidFeatureHistograms] The two "
76 "given pyramids have different numbers of bins on level 0: %u vs %u\n",
94 "[pcl::PyramidFeatureMatching::comparePyramidFeatureHistograms] The two "
95 "given pyramids have different numbers of bins on level %u: %u vs %u\n",
123 PCL_DEBUG(
"[pcl::PyramidFeatureMatching::comparePyramidFeatureHistograms] Self "
124 "similarity measures: %f, %f\n",
132template <
typename Po
intFeature>
142template <
typename Po
intFeature>
145 PointFeature>::PyramidFeatureHistogramLevel::initializeHistogramLevel()
148 for (std::vector<std::size_t>::iterator
dim_it = bins_per_dimension.begin();
149 dim_it != bins_per_dimension.end();
156template <
typename Po
intFeature>
158PyramidFeatureHistogram<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();
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);
206 std::vector<std::size_t> bins_per_dimension(nr_dimensions);
207 std::vector<float> bin_step(nr_dimensions);
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) /
213 std::sqrt(
static_cast<float>(nr_dimensions)))));
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(),
224 PCL_DEBUG(
"%u ", bins_per_dimension[
dim_i]);
231template <
typename Po
intFeature>
233PyramidFeatureHistogram<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();
251 for (
int i =
static_cast<int>(
access.size()) - 1; i >= 0; --i) {
259template <
typename Po
intFeature>
261PyramidFeatureHistogram<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;
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])));
286template <
typename Po
intFeature>
288PyramidFeatureHistogram<PointFeature>::convertFeatureToVector(
289 const PointFeature& feature, std::vector<float>& feature_vector)
292 feature_vector.resize(feature_representation_->getNumberOfDimensions());
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;
304template <
typename Po
intFeature>
308 if (!initializeHistogram())
311 for (
const auto& point : *input_) {
323template <
typename Po
intFeature>
333#define PCL_INSTANTIATE_PyramidFeatureHistogram(PointFeature) \
334 template class PCL_EXPORTS pcl::PyramidFeatureHistogram<PointFeature>;
Iterator class for point clouds with or without given indices.
ConstCloudIterator(const PointCloud< PointT > &cloud)
std::size_t size() const
Size of the range the iterator is going through.
DefaultPointRepresentation extends PointRepresentation to define default behavior for common point ty...
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.