Point Cloud Library (PCL)  1.9.1
 All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends Modules Pages
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 /** \brief Helper function to calculate the binary logarithm
50  * \param n_arg: some value
51  * \return binary logarithm (log2) of argument n_arg
52  */
53 __inline float
54 Log2 (float n_arg)
55 {
56  return std::log (n_arg) / float (M_LN2);
57 }
58 
59 
60 //////////////////////////////////////////////////////////////////////////////////////////////
61 template <typename PointFeature> float
63  const PyramidFeatureHistogramPtr &pyramid_b)
64 {
65  // do a few consistency checks before and during the computation
66  if (pyramid_a->nr_dimensions != pyramid_b->nr_dimensions)
67  {
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);
69  return -1;
70  }
71  if (pyramid_a->nr_levels != pyramid_b->nr_levels)
72  {
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);
74  return -1;
75  }
76 
77 
78  // calculate for level 0 first
79  if (pyramid_a->hist_levels[0].hist.size () != pyramid_b->hist_levels[0].hist.size ())
80  {
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 ());
82  return -1;
83  }
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)
86  {
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]);
89  else
90  match_count_level += static_cast<float> (pyramid_b->hist_levels[0].hist[bin_i]);
91  }
92 
93 
94  float match_count = match_count_level;
95  for (size_t level_i = 1; level_i < pyramid_a->nr_levels; ++level_i)
96  {
97  if (pyramid_a->hist_levels[level_i].hist.size () != pyramid_b->hist_levels[level_i].hist.size ())
98  {
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 ());
100  return -1;
101  }
102 
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)
106  {
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]);
109  else
110  match_count_level += static_cast<float> (pyramid_b->hist_levels[level_i].hist[bin_i]);
111  }
112 
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;
115  }
116 
117 
118  // include self-similarity factors
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);
123 
124  return match_count;
125 }
126 
127 
128 //////////////////////////////////////////////////////////////////////////////////////////////
129 template <typename PointFeature>
131  nr_dimensions (0), nr_levels (0), nr_features (0),
132  dimension_range_input_ (), dimension_range_target_ (),
133  feature_representation_ (new DefaultPointRepresentation<PointFeature>),
134  is_computed_ (false),
135  hist_levels ()
136 {
137 }
138 
139 //////////////////////////////////////////////////////////////////////////////////////////////
140 template <typename PointFeature> void
142 {
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;
146 
147  hist.resize (total_vector_size, 0);
148 }
149 
150 
151 //////////////////////////////////////////////////////////////////////////////////////////////
152 template <typename PointFeature> bool
154 {
155  // a few consistency checks before starting the computations
157  {
158  PCL_ERROR ("[pcl::PyramidFeatureHistogram::initializeHistogram] PCLBase initCompute failed\n");
159  return false;
160  }
161 
162  if (dimension_range_input_.size () == 0)
163  {
164  PCL_ERROR ("[pcl::PyramidFeatureHistogram::initializeHistogram] Input dimension range was not set\n");
165  return false;
166  }
167 
168  if (dimension_range_target_.size () == 0)
169  {
170  PCL_ERROR ("[pcl::PyramidFeatureHistogram::initializeHistogram] Target dimension range was not set\n");
171  return false;
172  }
173 
174  if (dimension_range_input_.size () != dimension_range_target_.size ())
175  {
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 ());
178  return false;
179  }
180 
181 
182  nr_dimensions = dimension_range_target_.size ();
183  nr_features = input_->points.size ();
184  float D = 0.0f;
185  for (std::vector<std::pair<float, float> >::iterator range_it = dimension_range_target_.begin (); range_it != dimension_range_target_.end (); ++range_it)
186  {
187  float aux = range_it->first - range_it->second;
188  D += aux * aux;
189  }
190  D = std::sqrt (D);
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);
193 
194 
195  hist_levels.resize (nr_levels);
196  for (size_t level_i = 0; level_i < nr_levels; ++level_i)
197  {
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)
201  {
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));
205  }
206  hist_levels[level_i] = PyramidFeatureHistogramLevel (bins_per_dimension, bin_step);
207 
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]);
211  PCL_DEBUG ("\n");
212  }
213 
214  return true;
215 }
216 
217 
218 //////////////////////////////////////////////////////////////////////////////////////////////
219 template <typename PointFeature> unsigned int&
220 pcl::PyramidFeatureHistogram<PointFeature>::at (std::vector<size_t> &access,
221  size_t &level)
222 {
223  if (access.size () != nr_dimensions)
224  {
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 ();
227  }
228  if (level >= hist_levels.size ())
229  {
230  PCL_ERROR ("[pcl::PyramidFeatureHistogram::at] Trying to access a too large level\n");
231  return hist_levels.front ().hist.front ();
232  }
233 
234  size_t vector_position = 0;
235  size_t dim_accumulator = 1;
236 
237  for (int i = static_cast<int> (access.size ()) - 1; i >= 0; --i)
238  {
239  vector_position += access[i] * dim_accumulator;
240  dim_accumulator *= hist_levels[level].bins_per_dimension[i];
241  }
242 
243  return hist_levels[level].hist[vector_position];
244 }
245 
246 
247 //////////////////////////////////////////////////////////////////////////////////////////////
248 template <typename PointFeature> unsigned int&
249 pcl::PyramidFeatureHistogram<PointFeature>::at (std::vector<float> &feature,
250  size_t &level)
251 {
252  if (feature.size () != nr_dimensions)
253  {
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 ();
256  }
257  if (level >= hist_levels.size ())
258  {
259  PCL_ERROR ("[pcl::PyramidFeatureHistogram::at] Trying to access a too large level\n");
260  return hist_levels.front ().hist.front ();
261  }
262 
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])));
266 
267  return at (access, level);
268 }
269 
270 
271 //////////////////////////////////////////////////////////////////////////////////////////////
272 template <typename PointFeature> void
274  std::vector<float> &feature_vector)
275 {
276  // convert feature to vector representation
277  feature_vector.resize (feature_representation_->getNumberOfDimensions ());
278  feature_representation_->vectorize (feature, feature_vector);
279 
280  // adapt the values from the input range to the target range
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;
284 }
285 
286 
287 //////////////////////////////////////////////////////////////////////////////////////////////
288 template <typename PointFeature> void
290 {
291  if (!initializeHistogram ())
292  return;
293 
294  for (size_t feature_i = 0; feature_i < input_->points.size (); ++feature_i)
295  {
296  std::vector<float> feature_vector;
297  convertFeatureToVector (input_->points[feature_i], feature_vector);
298  addFeature (feature_vector);
299  }
300 
301  is_computed_ = true;
302 }
303 
304 
305 //////////////////////////////////////////////////////////////////////////////////////////////
306 template <typename PointFeature> void
307 pcl::PyramidFeatureHistogram<PointFeature>::addFeature (std::vector<float> &feature)
308 {
309  for (size_t level_i = 0; level_i < nr_levels; ++level_i)
310  at (feature, level_i) ++;
311 }
312 
313 #define PCL_INSTANTIATE_PyramidFeatureHistogram(PointFeature) template class PCL_EXPORTS pcl::PyramidFeatureHistogram<PointFeature>;
314 
315 #endif /* PCL_REGISTRATION_IMPL_PYRAMID_FEATURE_MATCHING_H_ */
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...