Point Cloud Library (PCL)  1.7.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 /** \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 /= sqrtf (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 = sqrtf (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)) * sqrtf (static_cast<float> (nr_dimensions)))));
204  bin_step[dim_i] = powf (2.0f, static_cast<float> (level_i)) * sqrtf (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_ */