Point Cloud Library (PCL)  1.14.1
pyramid_feature_matching.hpp
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Point Cloud Library (PCL) - www.pointclouds.org
5  * Copyright (c) 2011, Alexandru-Eugen Ichim
6  * Willow Garage, Inc
7  * Copyright (c) 2012-, Open Perception, Inc.
8  *
9  * All rights reserved.
10  *
11  * Redistribution and use in source and binary forms, with or without
12  * modification, are permitted provided that the following conditions
13  * are met:
14  *
15  * * Redistributions of source code must retain the above copyright
16  * notice, this list of conditions and the following disclaimer.
17  * * Redistributions in binary form must reproduce the above
18  * copyright notice, this list of conditions and the following
19  * disclaimer in the documentation and/or other materials provided
20  * with the distribution.
21  * * Neither the name of the copyright holder(s) nor the names of its
22  * contributors may be used to endorse or promote products derived
23  * from this software without specific prior written permission.
24  *
25  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
26  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
27  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
28  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
29  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
30  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
31  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
32  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
33  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
34  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
35  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
36  * POSSIBILITY OF SUCH DAMAGE.
37  *
38  * $Id$
39  *
40  */
41 
42 #ifndef PCL_REGISTRATION_IMPL_PYRAMID_FEATURE_MATCHING_H_
43 #define PCL_REGISTRATION_IMPL_PYRAMID_FEATURE_MATCHING_H_
44 
45 #include <pcl/common/point_tests.h> // for pcl::isFinite
46 #include <pcl/console/print.h>
47 #include <pcl/pcl_macros.h>
48 
49 namespace pcl {
50 
51 template <typename PointFeature>
52 float
54  const PyramidFeatureHistogramPtr& pyramid_a,
55  const PyramidFeatureHistogramPtr& pyramid_b)
56 {
57  // do a few consistency checks before and during the computation
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);
63  return -1;
64  }
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",
68  pyramid_a->nr_levels,
69  pyramid_b->nr_levels);
70  return -1;
71  }
72 
73  // calculate for level 0 first
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());
79  return -1;
80  }
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]);
85  else
86  match_count_level += static_cast<float>(pyramid_b->hist_levels[0].hist[bin_i]);
87  }
88 
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()) {
93  PCL_ERROR(
94  "[pcl::PyramidFeatureMatching::comparePyramidFeatureHistograms] The two "
95  "given pyramids have different numbers of bins on level %u: %u vs %u\n",
96  level_i,
97  pyramid_a->hist_levels[level_i].hist.size(),
98  pyramid_b->hist_levels[level_i].hist.size());
99  return -1;
100  }
101 
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();
105  ++bin_i) {
106  if (pyramid_a->hist_levels[level_i].hist[bin_i] <
107  pyramid_b->hist_levels[level_i].hist[bin_i])
108  match_count_level +=
109  static_cast<float>(pyramid_a->hist_levels[level_i].hist[bin_i]);
110  else
111  match_count_level +=
112  static_cast<float>(pyramid_b->hist_levels[level_i].hist[bin_i]);
113  }
114 
115  float level_normalization_factor = powf(2.0f, static_cast<float>(level_i));
116  match_count +=
117  (match_count_level - match_count_prev_level) / level_normalization_factor;
118  }
119 
120  // include self-similarity factors
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",
125  self_similarity_a,
126  self_similarity_b);
127  match_count /= std::sqrt(self_similarity_a * self_similarity_b);
128 
129  return match_count;
130 }
131 
132 template <typename PointFeature>
134 : feature_representation_(new DefaultPointRepresentation<PointFeature>), hist_levels()
135 {}
136 
137 template <typename PointFeature>
138 void
140  PointFeature>::PyramidFeatureHistogramLevel::initializeHistogramLevel()
141 {
142  std::size_t total_vector_size = 1;
143  for (const auto& bin : bins_per_dimension) {
144  total_vector_size *= bin;
145  }
146 
147  hist.resize(total_vector_size, 0);
148 }
149 
150 template <typename PointFeature>
151 bool
152 PyramidFeatureHistogram<PointFeature>::initializeHistogram()
153 {
154  // a few consistency checks before starting the computations
156  PCL_ERROR("[pcl::PyramidFeatureHistogram::initializeHistogram] PCLBase initCompute "
157  "failed\n");
158  return false;
159  }
160 
161  if (dimension_range_input_.empty()) {
162  PCL_ERROR("[pcl::PyramidFeatureHistogram::initializeHistogram] Input dimension "
163  "range was not set\n");
164  return false;
165  }
166 
167  if (dimension_range_target_.empty()) {
168  PCL_ERROR("[pcl::PyramidFeatureHistogram::initializeHistogram] Target dimension "
169  "range was not set\n");
170  return false;
171  }
172 
173  if (dimension_range_input_.size() != dimension_range_target_.size()) {
174  PCL_ERROR("[pcl::PyramidFeatureHistogram::initializeHistogram] Input and target "
175  "dimension ranges do not agree in size: %u vs %u\n",
176  dimension_range_input_.size(),
177  dimension_range_target_.size());
178  return false;
179  }
180 
181  nr_dimensions = dimension_range_target_.size();
182  nr_features = input_->size();
183  float D = 0.0f;
184  for (const auto& dim : dimension_range_target_) {
185  float aux = dim.first - dim.second;
186  D += aux * aux;
187  }
188  D = std::sqrt(D);
189  nr_levels = static_cast<std::size_t>(std::ceil(std::log2(D)));
190  PCL_DEBUG("[pcl::PyramidFeatureHistogram::initializeHistogram] Pyramid will have %u "
191  "levels with a hyper-parallelepiped diagonal size of %f\n",
192  nr_levels,
193  D);
194 
195  hist_levels.resize(nr_levels);
196  for (std::size_t level_i = 0; level_i < nr_levels; ++level_i) {
197  std::vector<std::size_t> bins_per_dimension(nr_dimensions);
198  std::vector<float> bin_step(nr_dimensions);
199  for (std::size_t dim_i = 0; dim_i < nr_dimensions; ++dim_i) {
200  bins_per_dimension[dim_i] = static_cast<std::size_t>(
201  std::ceil((dimension_range_target_[dim_i].second -
202  dimension_range_target_[dim_i].first) /
203  (powf(2.0f, static_cast<float>(level_i)) *
204  std::sqrt(static_cast<float>(nr_dimensions)))));
205  bin_step[dim_i] = powf(2.0f, static_cast<float>(level_i)) *
206  std::sqrt(static_cast<float>(nr_dimensions));
207  }
208  hist_levels[level_i] = PyramidFeatureHistogramLevel(bins_per_dimension, bin_step);
209 
210  PCL_DEBUG("[pcl::PyramidFeatureHistogram::initializeHistogram] Created vector of "
211  "size %u at level %u\nwith #bins per dimension:",
212  hist_levels.back().hist.size(),
213  level_i);
214  for (std::size_t dim_i = 0; dim_i < nr_dimensions; ++dim_i)
215  PCL_DEBUG("%u ", bins_per_dimension[dim_i]);
216  PCL_DEBUG("\n");
217  }
218 
219  return true;
220 }
221 
222 template <typename PointFeature>
223 unsigned int&
224 PyramidFeatureHistogram<PointFeature>::at(std::vector<std::size_t>& access,
225  std::size_t& level)
226 {
227  if (access.size() != nr_dimensions) {
228  PCL_ERROR(
229  "[pcl::PyramidFeatureHistogram::at] Cannot access histogram position because "
230  "the access point does not have the right number of dimensions\n");
231  return hist_levels.front().hist.front();
232  }
233  if (level >= hist_levels.size()) {
234  PCL_ERROR(
235  "[pcl::PyramidFeatureHistogram::at] Trying to access a too large level\n");
236  return hist_levels.front().hist.front();
237  }
238 
239  std::size_t vector_position = 0;
240  std::size_t dim_accumulator = 1;
241 
242  for (int i = static_cast<int>(access.size()) - 1; i >= 0; --i) {
243  vector_position += access[i] * dim_accumulator;
244  dim_accumulator *= hist_levels[level].bins_per_dimension[i];
245  }
246 
247  return hist_levels[level].hist[vector_position];
248 }
249 
250 template <typename PointFeature>
251 unsigned int&
252 PyramidFeatureHistogram<PointFeature>::at(std::vector<float>& feature,
253  std::size_t& level)
254 {
255  if (feature.size() != nr_dimensions) {
256  PCL_ERROR("[pcl::PyramidFeatureHistogram::at] The given feature vector does not "
257  "match the feature dimensions of the pyramid histogram: %u vs %u\n",
258  feature.size(),
259  nr_dimensions);
260  return hist_levels.front().hist.front();
261  }
262  if (level >= hist_levels.size()) {
263  PCL_ERROR(
264  "[pcl::PyramidFeatureHistogram::at] Trying to access a too large level\n");
265  return hist_levels.front().hist.front();
266  }
267 
268  std::vector<std::size_t> access;
269  for (std::size_t dim_i = 0; dim_i < nr_dimensions; ++dim_i)
270  access.push_back(static_cast<std::size_t>(
271  std::floor((feature[dim_i] - dimension_range_target_[dim_i].first) /
272  hist_levels[level].bin_step[dim_i])));
273 
274  return at(access, level);
275 }
276 
277 template <typename PointFeature>
278 void
279 PyramidFeatureHistogram<PointFeature>::convertFeatureToVector(
280  const PointFeature& feature, std::vector<float>& feature_vector)
281 {
282  // convert feature to vector representation
283  feature_vector.resize(feature_representation_->getNumberOfDimensions());
284  feature_representation_->vectorize(feature, feature_vector);
285 
286  // adapt the values from the input range to the target range
287  for (std::size_t i = 0; i < feature_vector.size(); ++i)
288  feature_vector[i] =
289  (feature_vector[i] - dimension_range_input_[i].first) /
290  (dimension_range_input_[i].second - dimension_range_input_[i].first) *
291  (dimension_range_target_[i].second - dimension_range_target_[i].first) +
292  dimension_range_target_[i].first;
293 }
294 
295 template <typename PointFeature>
296 void
298 {
299  if (!initializeHistogram())
300  return;
301 
302  for (const auto& point : *input_) {
303  std::vector<float> feature_vector;
304  // NaN is converted to very high number that gives out of bound exception.
305  if (!pcl::isFinite(point))
306  continue;
307  convertFeatureToVector(point, feature_vector);
308  addFeature(feature_vector);
309  }
310 
311  is_computed_ = true;
312 }
313 
314 template <typename PointFeature>
315 void
316 PyramidFeatureHistogram<PointFeature>::addFeature(std::vector<float>& feature)
317 {
318  for (std::size_t level_i = 0; level_i < nr_levels; ++level_i)
319  at(feature, level_i)++;
320 }
321 
322 } // namespace pcl
323 
324 #define PCL_INSTANTIATE_PyramidFeatureHistogram(PointFeature) \
325  template class PCL_EXPORTS pcl::PyramidFeatureHistogram<PointFeature>;
326 
327 #endif /* PCL_REGISTRATION_IMPL_PYRAMID_FEATURE_MATCHING_H_ */
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...
Definition: point_tests.h:55
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...