Point Cloud Library (PCL)  1.11.0
 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 namespace pcl
50 {
51 
52 template <typename PointFeature> float
54  const PyramidFeatureHistogramPtr &pyramid_b)
55 {
56  // do a few consistency checks before and during the computation
57  if (pyramid_a->nr_dimensions != pyramid_b->nr_dimensions)
58  {
59  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);
60  return -1;
61  }
62  if (pyramid_a->nr_levels != pyramid_b->nr_levels)
63  {
64  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);
65  return -1;
66  }
67 
68 
69  // calculate for level 0 first
70  if (pyramid_a->hist_levels[0].hist.size () != pyramid_b->hist_levels[0].hist.size ())
71  {
72  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 ());
73  return -1;
74  }
75  float match_count_level = 0.0f;
76  for (std::size_t bin_i = 0; bin_i < pyramid_a->hist_levels[0].hist.size (); ++bin_i)
77  {
78  if (pyramid_a->hist_levels[0].hist[bin_i] < pyramid_b->hist_levels[0].hist[bin_i])
79  match_count_level += static_cast<float> (pyramid_a->hist_levels[0].hist[bin_i]);
80  else
81  match_count_level += static_cast<float> (pyramid_b->hist_levels[0].hist[bin_i]);
82  }
83 
84 
85  float match_count = match_count_level;
86  for (std::size_t level_i = 1; level_i < pyramid_a->nr_levels; ++level_i)
87  {
88  if (pyramid_a->hist_levels[level_i].hist.size () != pyramid_b->hist_levels[level_i].hist.size ())
89  {
90  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 ());
91  return -1;
92  }
93 
94  float match_count_prev_level = match_count_level;
95  match_count_level = 0.0f;
96  for (std::size_t bin_i = 0; bin_i < pyramid_a->hist_levels[level_i].hist.size (); ++bin_i)
97  {
98  if (pyramid_a->hist_levels[level_i].hist[bin_i] < pyramid_b->hist_levels[level_i].hist[bin_i])
99  match_count_level += static_cast<float> (pyramid_a->hist_levels[level_i].hist[bin_i]);
100  else
101  match_count_level += static_cast<float> (pyramid_b->hist_levels[level_i].hist[bin_i]);
102  }
103 
104  float level_normalization_factor = powf (2.0f, static_cast<float> (level_i));
105  match_count += (match_count_level - match_count_prev_level) / level_normalization_factor;
106  }
107 
108 
109  // include self-similarity factors
110  float self_similarity_a = static_cast<float> (pyramid_a->nr_features),
111  self_similarity_b = static_cast<float> (pyramid_b->nr_features);
112  PCL_DEBUG ("[pcl::PyramidFeatureMatching::comparePyramidFeatureHistograms] Self similarity measures: %f, %f\n", self_similarity_a, self_similarity_b);
113  match_count /= std::sqrt (self_similarity_a * self_similarity_b);
114 
115  return match_count;
116 }
117 
118 
119 template <typename PointFeature>
121  nr_dimensions (0), nr_levels (0), nr_features (0),
122  feature_representation_ (new DefaultPointRepresentation<PointFeature>),
123  is_computed_ (false),
124  hist_levels ()
125 {
126 }
127 
128 
129 template <typename PointFeature> void
131 {
132  std::size_t total_vector_size = 1;
133  for (std::vector<std::size_t>::iterator dim_it = bins_per_dimension.begin (); dim_it != bins_per_dimension.end (); ++dim_it)
134  total_vector_size *= *dim_it;
135 
136  hist.resize (total_vector_size, 0);
137 }
138 
139 
140 template <typename PointFeature> bool
141 PyramidFeatureHistogram<PointFeature>::initializeHistogram ()
142 {
143  // a few consistency checks before starting the computations
145  {
146  PCL_ERROR ("[pcl::PyramidFeatureHistogram::initializeHistogram] PCLBase initCompute failed\n");
147  return false;
148  }
149 
150  if (dimension_range_input_.empty ())
151  {
152  PCL_ERROR ("[pcl::PyramidFeatureHistogram::initializeHistogram] Input dimension range was not set\n");
153  return false;
154  }
155 
156  if (dimension_range_target_.empty ())
157  {
158  PCL_ERROR ("[pcl::PyramidFeatureHistogram::initializeHistogram] Target dimension range was not set\n");
159  return false;
160  }
161 
162  if (dimension_range_input_.size () != dimension_range_target_.size ())
163  {
164  PCL_ERROR ("[pcl::PyramidFeatureHistogram::initializeHistogram] Input and target dimension ranges do not agree in size: %u vs %u\n",
165  dimension_range_input_.size (), dimension_range_target_.size ());
166  return false;
167  }
168 
169 
170  nr_dimensions = dimension_range_target_.size ();
171  nr_features = input_->points.size ();
172  float D = 0.0f;
173  for (std::vector<std::pair<float, float> >::iterator range_it = dimension_range_target_.begin (); range_it != dimension_range_target_.end (); ++range_it)
174  {
175  float aux = range_it->first - range_it->second;
176  D += aux * aux;
177  }
178  D = std::sqrt (D);
179  nr_levels = static_cast<std::size_t> (std::ceil (std::log2(D)));
180  PCL_DEBUG ("[pcl::PyramidFeatureHistogram::initializeHistogram] Pyramid will have %u levels with a hyper-parallelepiped diagonal size of %f\n", nr_levels, D);
181 
182 
183  hist_levels.resize (nr_levels);
184  for (std::size_t level_i = 0; level_i < nr_levels; ++level_i)
185  {
186  std::vector<std::size_t> bins_per_dimension (nr_dimensions);
187  std::vector<float> bin_step (nr_dimensions);
188  for (std::size_t dim_i = 0; dim_i < nr_dimensions; ++dim_i)
189  {
190  bins_per_dimension[dim_i] =
191  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)))));
192  bin_step[dim_i] = powf (2.0f, static_cast<float> (level_i)) * std::sqrt (static_cast<float> (nr_dimensions));
193  }
194  hist_levels[level_i] = PyramidFeatureHistogramLevel (bins_per_dimension, bin_step);
195 
196  PCL_DEBUG ("[pcl::PyramidFeatureHistogram::initializeHistogram] Created vector of size %u at level %u\nwith #bins per dimension:", hist_levels.back ().hist.size (), level_i);
197  for (std::size_t dim_i = 0; dim_i < nr_dimensions; ++dim_i)
198  PCL_DEBUG ("%u ", bins_per_dimension[dim_i]);
199  PCL_DEBUG ("\n");
200  }
201 
202  return true;
203 }
204 
205 
206 template <typename PointFeature> unsigned int&
207 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 template <typename PointFeature> unsigned int&
235 PyramidFeatureHistogram<PointFeature>::at (std::vector<float> &feature,
236  std::size_t &level)
237 {
238  if (feature.size () != nr_dimensions)
239  {
240  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);
241  return hist_levels.front ().hist.front ();
242  }
243  if (level >= hist_levels.size ())
244  {
245  PCL_ERROR ("[pcl::PyramidFeatureHistogram::at] Trying to access a too large level\n");
246  return hist_levels.front ().hist.front ();
247  }
248 
249  std::vector<std::size_t> access;
250  for (std::size_t dim_i = 0; dim_i < nr_dimensions; ++dim_i)
251  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])));
252 
253  return at (access, level);
254 }
255 
256 
257 template <typename PointFeature> void
258 PyramidFeatureHistogram<PointFeature>::convertFeatureToVector (const PointFeature &feature,
259  std::vector<float> &feature_vector)
260 {
261  // convert feature to vector representation
262  feature_vector.resize (feature_representation_->getNumberOfDimensions ());
263  feature_representation_->vectorize (feature, feature_vector);
264 
265  // adapt the values from the input range to the target range
266  for (std::size_t i = 0; i < feature_vector.size (); ++i)
267  feature_vector[i] = (feature_vector[i] - dimension_range_input_[i].first) / (dimension_range_input_[i].second - dimension_range_input_[i].first) *
268  (dimension_range_target_[i].second - dimension_range_target_[i].first) + dimension_range_target_[i].first;
269 }
270 
271 
272 template <typename PointFeature> void
274 {
275  if (!initializeHistogram ())
276  return;
277 
278  for (std::size_t feature_i = 0; feature_i < input_->points.size (); ++feature_i)
279  {
280  std::vector<float> feature_vector;
281  convertFeatureToVector (input_->points[feature_i], feature_vector);
282  addFeature (feature_vector);
283  }
284 
285  is_computed_ = true;
286 }
287 
288 
289 template <typename PointFeature> void
290 PyramidFeatureHistogram<PointFeature>::addFeature (std::vector<float> &feature)
291 {
292  for (std::size_t level_i = 0; level_i < nr_levels; ++level_i)
293  at (feature, level_i) ++;
294 }
295 
296 } // namespace pcl
297 
298 #define PCL_INSTANTIATE_PyramidFeatureHistogram(PointFeature) template class PCL_EXPORTS pcl::PyramidFeatureHistogram<PointFeature>;
299 
300 #endif /* PCL_REGISTRATION_IMPL_PYRAMID_FEATURE_MATCHING_H_ */
301 
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...