Point Cloud Library (PCL)  1.13.0-dev
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 : nr_dimensions(0)
135 , nr_levels(0)
136 , nr_features(0)
137 , feature_representation_(new DefaultPointRepresentation<PointFeature>)
138 , is_computed_(false)
139 , hist_levels()
140 {}
141 
142 template <typename PointFeature>
143 void
145  PointFeature>::PyramidFeatureHistogramLevel::initializeHistogramLevel()
146 {
147  std::size_t total_vector_size = 1;
148  for (const auto& bin : bins_per_dimension) {
149  total_vector_size *= bin;
150  }
151 
152  hist.resize(total_vector_size, 0);
153 }
154 
155 template <typename PointFeature>
156 bool
157 PyramidFeatureHistogram<PointFeature>::initializeHistogram()
158 {
159  // a few consistency checks before starting the computations
161  PCL_ERROR("[pcl::PyramidFeatureHistogram::initializeHistogram] PCLBase initCompute "
162  "failed\n");
163  return false;
164  }
165 
166  if (dimension_range_input_.empty()) {
167  PCL_ERROR("[pcl::PyramidFeatureHistogram::initializeHistogram] Input dimension "
168  "range was not set\n");
169  return false;
170  }
171 
172  if (dimension_range_target_.empty()) {
173  PCL_ERROR("[pcl::PyramidFeatureHistogram::initializeHistogram] Target dimension "
174  "range was not set\n");
175  return false;
176  }
177 
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());
183  return false;
184  }
185 
186  nr_dimensions = dimension_range_target_.size();
187  nr_features = input_->size();
188  float D = 0.0f;
189  for (const auto& dim : dimension_range_target_) {
190  float aux = dim.first - dim.second;
191  D += aux * aux;
192  }
193  D = std::sqrt(D);
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",
197  nr_levels,
198  D);
199 
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));
212  }
213  hist_levels[level_i] = PyramidFeatureHistogramLevel(bins_per_dimension, bin_step);
214 
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(),
218  level_i);
219  for (std::size_t dim_i = 0; dim_i < nr_dimensions; ++dim_i)
220  PCL_DEBUG("%u ", bins_per_dimension[dim_i]);
221  PCL_DEBUG("\n");
222  }
223 
224  return true;
225 }
226 
227 template <typename PointFeature>
228 unsigned int&
229 PyramidFeatureHistogram<PointFeature>::at(std::vector<std::size_t>& access,
230  std::size_t& level)
231 {
232  if (access.size() != nr_dimensions) {
233  PCL_ERROR(
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();
237  }
238  if (level >= hist_levels.size()) {
239  PCL_ERROR(
240  "[pcl::PyramidFeatureHistogram::at] Trying to access a too large level\n");
241  return hist_levels.front().hist.front();
242  }
243 
244  std::size_t vector_position = 0;
245  std::size_t dim_accumulator = 1;
246 
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];
250  }
251 
252  return hist_levels[level].hist[vector_position];
253 }
254 
255 template <typename PointFeature>
256 unsigned int&
257 PyramidFeatureHistogram<PointFeature>::at(std::vector<float>& feature,
258  std::size_t& level)
259 {
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",
263  feature.size(),
264  nr_dimensions);
265  return hist_levels.front().hist.front();
266  }
267  if (level >= hist_levels.size()) {
268  PCL_ERROR(
269  "[pcl::PyramidFeatureHistogram::at] Trying to access a too large level\n");
270  return hist_levels.front().hist.front();
271  }
272 
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])));
278 
279  return at(access, level);
280 }
281 
282 template <typename PointFeature>
283 void
284 PyramidFeatureHistogram<PointFeature>::convertFeatureToVector(
285  const PointFeature& feature, std::vector<float>& feature_vector)
286 {
287  // convert feature to vector representation
288  feature_vector.resize(feature_representation_->getNumberOfDimensions());
289  feature_representation_->vectorize(feature, feature_vector);
290 
291  // adapt the values from the input range to the target range
292  for (std::size_t i = 0; i < feature_vector.size(); ++i)
293  feature_vector[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;
298 }
299 
300 template <typename PointFeature>
301 void
303 {
304  if (!initializeHistogram())
305  return;
306 
307  for (const auto& point : *input_) {
308  std::vector<float> feature_vector;
309  // NaN is converted to very high number that gives out of bound exception.
310  if (!pcl::isFinite(point))
311  continue;
312  convertFeatureToVector(point, feature_vector);
313  addFeature(feature_vector);
314  }
315 
316  is_computed_ = true;
317 }
318 
319 template <typename PointFeature>
320 void
321 PyramidFeatureHistogram<PointFeature>::addFeature(std::vector<float>& feature)
322 {
323  for (std::size_t level_i = 0; level_i < nr_levels; ++level_i)
324  at(feature, level_i)++;
325 }
326 
327 } // namespace pcl
328 
329 #define PCL_INSTANTIATE_PyramidFeatureHistogram(PointFeature) \
330  template class PCL_EXPORTS pcl::PyramidFeatureHistogram<PointFeature>;
331 
332 #endif /* PCL_REGISTRATION_IMPL_PYRAMID_FEATURE_MATCHING_H_ */
DefaultPointRepresentation extends PointRepresentation to define default behavior for common point ty...
bool initCompute()
This method should get called before starting the actual computation.
Definition: pcl_base.hpp:138
Class that compares two sets of features by using a multiscale representation of the features inside ...
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...
Definition: point_tests.h:55
Defines all the PCL and non-PCL macros used.