Point Cloud Library (PCL)  1.10.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/pcl_macros.h>
46 #include <pcl/console/print.h>
47 
48 //////////////////////////////////////////////////////////////////////////////////////////////
49 template <typename PointFeature> float
51  const PyramidFeatureHistogramPtr &pyramid_b)
52 {
53  // do a few consistency checks before and during the computation
54  if (pyramid_a->nr_dimensions != pyramid_b->nr_dimensions)
55  {
56  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);
57  return -1;
58  }
59  if (pyramid_a->nr_levels != pyramid_b->nr_levels)
60  {
61  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);
62  return -1;
63  }
64 
65 
66  // calculate for level 0 first
67  if (pyramid_a->hist_levels[0].hist.size () != pyramid_b->hist_levels[0].hist.size ())
68  {
69  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 ());
70  return -1;
71  }
72  float match_count_level = 0.0f;
73  for (std::size_t bin_i = 0; bin_i < pyramid_a->hist_levels[0].hist.size (); ++bin_i)
74  {
75  if (pyramid_a->hist_levels[0].hist[bin_i] < pyramid_b->hist_levels[0].hist[bin_i])
76  match_count_level += static_cast<float> (pyramid_a->hist_levels[0].hist[bin_i]);
77  else
78  match_count_level += static_cast<float> (pyramid_b->hist_levels[0].hist[bin_i]);
79  }
80 
81 
82  float match_count = match_count_level;
83  for (std::size_t level_i = 1; level_i < pyramid_a->nr_levels; ++level_i)
84  {
85  if (pyramid_a->hist_levels[level_i].hist.size () != pyramid_b->hist_levels[level_i].hist.size ())
86  {
87  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 ());
88  return -1;
89  }
90 
91  float match_count_prev_level = match_count_level;
92  match_count_level = 0.0f;
93  for (std::size_t bin_i = 0; bin_i < pyramid_a->hist_levels[level_i].hist.size (); ++bin_i)
94  {
95  if (pyramid_a->hist_levels[level_i].hist[bin_i] < pyramid_b->hist_levels[level_i].hist[bin_i])
96  match_count_level += static_cast<float> (pyramid_a->hist_levels[level_i].hist[bin_i]);
97  else
98  match_count_level += static_cast<float> (pyramid_b->hist_levels[level_i].hist[bin_i]);
99  }
100 
101  float level_normalization_factor = powf (2.0f, static_cast<float> (level_i));
102  match_count += (match_count_level - match_count_prev_level) / level_normalization_factor;
103  }
104 
105 
106  // include self-similarity factors
107  float self_similarity_a = static_cast<float> (pyramid_a->nr_features),
108  self_similarity_b = static_cast<float> (pyramid_b->nr_features);
109  PCL_DEBUG ("[pcl::PyramidFeatureMatching::comparePyramidFeatureHistograms] Self similarity measures: %f, %f\n", self_similarity_a, self_similarity_b);
110  match_count /= std::sqrt (self_similarity_a * self_similarity_b);
111 
112  return match_count;
113 }
114 
115 
116 //////////////////////////////////////////////////////////////////////////////////////////////
117 template <typename PointFeature>
119  nr_dimensions (0), nr_levels (0), nr_features (0),
120  feature_representation_ (new DefaultPointRepresentation<PointFeature>),
121  is_computed_ (false),
122  hist_levels ()
123 {
124 }
125 
126 //////////////////////////////////////////////////////////////////////////////////////////////
127 template <typename PointFeature> void
129 {
130  std::size_t total_vector_size = 1;
131  for (std::vector<std::size_t>::iterator dim_it = bins_per_dimension.begin (); dim_it != bins_per_dimension.end (); ++dim_it)
132  total_vector_size *= *dim_it;
133 
134  hist.resize (total_vector_size, 0);
135 }
136 
137 
138 //////////////////////////////////////////////////////////////////////////////////////////////
139 template <typename PointFeature> bool
141 {
142  // a few consistency checks before starting the computations
144  {
145  PCL_ERROR ("[pcl::PyramidFeatureHistogram::initializeHistogram] PCLBase initCompute failed\n");
146  return false;
147  }
148 
149  if (dimension_range_input_.empty ())
150  {
151  PCL_ERROR ("[pcl::PyramidFeatureHistogram::initializeHistogram] Input dimension range was not set\n");
152  return false;
153  }
154 
155  if (dimension_range_target_.empty ())
156  {
157  PCL_ERROR ("[pcl::PyramidFeatureHistogram::initializeHistogram] Target dimension range was not set\n");
158  return false;
159  }
160 
161  if (dimension_range_input_.size () != dimension_range_target_.size ())
162  {
163  PCL_ERROR ("[pcl::PyramidFeatureHistogram::initializeHistogram] Input and target dimension ranges do not agree in size: %u vs %u\n",
164  dimension_range_input_.size (), dimension_range_target_.size ());
165  return false;
166  }
167 
168 
169  nr_dimensions = dimension_range_target_.size ();
170  nr_features = input_->points.size ();
171  float D = 0.0f;
172  for (std::vector<std::pair<float, float> >::iterator range_it = dimension_range_target_.begin (); range_it != dimension_range_target_.end (); ++range_it)
173  {
174  float aux = range_it->first - range_it->second;
175  D += aux * aux;
176  }
177  D = std::sqrt (D);
178  nr_levels = static_cast<std::size_t> (std::ceil (std::log2(D)));
179  PCL_DEBUG ("[pcl::PyramidFeatureHistogram::initializeHistogram] Pyramid will have %u levels with a hyper-parallelepiped diagonal size of %f\n", nr_levels, D);
180 
181 
182  hist_levels.resize (nr_levels);
183  for (std::size_t level_i = 0; level_i < nr_levels; ++level_i)
184  {
185  std::vector<std::size_t> bins_per_dimension (nr_dimensions);
186  std::vector<float> bin_step (nr_dimensions);
187  for (std::size_t dim_i = 0; dim_i < nr_dimensions; ++dim_i)
188  {
189  bins_per_dimension[dim_i] =
190  static_cast<std::size_t> (std::ceil ((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)))));
191  bin_step[dim_i] = powf (2.0f, static_cast<float> (level_i)) * std::sqrt (static_cast<float> (nr_dimensions));
192  }
193  hist_levels[level_i] = PyramidFeatureHistogramLevel (bins_per_dimension, bin_step);
194 
195  PCL_DEBUG ("[pcl::PyramidFeatureHistogram::initializeHistogram] Created vector of size %u at level %u\nwith #bins per dimension:", hist_levels.back ().hist.size (), level_i);
196  for (std::size_t dim_i = 0; dim_i < nr_dimensions; ++dim_i)
197  PCL_DEBUG ("%u ", bins_per_dimension[dim_i]);
198  PCL_DEBUG ("\n");
199  }
200 
201  return true;
202 }
203 
204 
205 //////////////////////////////////////////////////////////////////////////////////////////////
206 template <typename PointFeature> unsigned int&
207 pcl::PyramidFeatureHistogram<PointFeature>::at (std::vector<std::size_t> &access,
208  std::size_t &level)
209 {
210  if (access.size () != nr_dimensions)
211  {
212  PCL_ERROR ("[pcl::PyramidFeatureHistogram::at] Cannot access histogram position because the access point does not have the right number of dimensions\n");
213  return hist_levels.front ().hist.front ();
214  }
215  if (level >= hist_levels.size ())
216  {
217  PCL_ERROR ("[pcl::PyramidFeatureHistogram::at] Trying to access a too large level\n");
218  return hist_levels.front ().hist.front ();
219  }
220 
221  std::size_t vector_position = 0;
222  std::size_t dim_accumulator = 1;
223 
224  for (int i = static_cast<int> (access.size ()) - 1; i >= 0; --i)
225  {
226  vector_position += access[i] * dim_accumulator;
227  dim_accumulator *= hist_levels[level].bins_per_dimension[i];
228  }
229 
230  return hist_levels[level].hist[vector_position];
231 }
232 
233 
234 //////////////////////////////////////////////////////////////////////////////////////////////
235 template <typename PointFeature> unsigned int&
236 pcl::PyramidFeatureHistogram<PointFeature>::at (std::vector<float> &feature,
237  std::size_t &level)
238 {
239  if (feature.size () != nr_dimensions)
240  {
241  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);
242  return hist_levels.front ().hist.front ();
243  }
244  if (level >= hist_levels.size ())
245  {
246  PCL_ERROR ("[pcl::PyramidFeatureHistogram::at] Trying to access a too large level\n");
247  return hist_levels.front ().hist.front ();
248  }
249 
250  std::vector<std::size_t> access;
251  for (std::size_t dim_i = 0; dim_i < nr_dimensions; ++dim_i)
252  access.push_back (static_cast<std::size_t> (std::floor ((feature[dim_i] - dimension_range_target_[dim_i].first) / hist_levels[level].bin_step[dim_i])));
253 
254  return at (access, level);
255 }
256 
257 
258 //////////////////////////////////////////////////////////////////////////////////////////////
259 template <typename PointFeature> void
261  std::vector<float> &feature_vector)
262 {
263  // convert feature to vector representation
264  feature_vector.resize (feature_representation_->getNumberOfDimensions ());
265  feature_representation_->vectorize (feature, feature_vector);
266 
267  // adapt the values from the input range to the target range
268  for (std::size_t i = 0; i < feature_vector.size (); ++i)
269  feature_vector[i] = (feature_vector[i] - dimension_range_input_[i].first) / (dimension_range_input_[i].second - dimension_range_input_[i].first) *
270  (dimension_range_target_[i].second - dimension_range_target_[i].first) + dimension_range_target_[i].first;
271 }
272 
273 
274 //////////////////////////////////////////////////////////////////////////////////////////////
275 template <typename PointFeature> void
277 {
278  if (!initializeHistogram ())
279  return;
280 
281  for (std::size_t feature_i = 0; feature_i < input_->points.size (); ++feature_i)
282  {
283  std::vector<float> feature_vector;
284  convertFeatureToVector (input_->points[feature_i], feature_vector);
285  addFeature (feature_vector);
286  }
287 
288  is_computed_ = true;
289 }
290 
291 
292 //////////////////////////////////////////////////////////////////////////////////////////////
293 template <typename PointFeature> void
294 pcl::PyramidFeatureHistogram<PointFeature>::addFeature (std::vector<float> &feature)
295 {
296  for (std::size_t level_i = 0; level_i < nr_levels; ++level_i)
297  at (feature, level_i) ++;
298 }
299 
300 #define PCL_INSTANTIATE_PyramidFeatureHistogram(PointFeature) template class PCL_EXPORTS pcl::PyramidFeatureHistogram<PointFeature>;
301 
302 #endif /* PCL_REGISTRATION_IMPL_PYRAMID_FEATURE_MATCHING_H_ */
pcl_macros.h
Defines all the PCL and non-PCL macros used.
pcl::PyramidFeatureHistogram::PyramidFeatureHistogram
PyramidFeatureHistogram()
Empty constructor that instantiates the feature representation variable.
Definition: pyramid_feature_matching.hpp:118
pcl::PyramidFeatureHistogram::PyramidFeatureHistogramPtr
Ptr PyramidFeatureHistogramPtr
Definition: pyramid_feature_matching.h:74
pcl::DefaultPointRepresentation
DefaultPointRepresentation extends PointRepresentation to define default behavior for common point ty...
Definition: point_representation.h:178
pcl::PyramidFeatureHistogram
Class that compares two sets of features by using a multiscale representation of the features inside ...
Definition: pyramid_feature_matching.h:67
pcl::PyramidFeatureHistogram::comparePyramidFeatureHistograms
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 ...
Definition: pyramid_feature_matching.hpp:50
pcl::PyramidFeatureHistogram::compute
void compute()
The central method for inserting the feature set inside the pyramid and obtaining the complete pyrami...
Definition: pyramid_feature_matching.hpp:276
pcl::PCLBase< PointFeature >::initCompute
bool initCompute()
This method should get called before starting the actual computation.
Definition: pcl_base.hpp:138