Point Cloud Library (PCL)  1.3.1
pyramid_feature_matching.hpp
Go to the documentation of this file.
00001 /*
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2011, Alexandru-Eugen Ichim
00005  *                      Willow Garage, Inc
00006  *  All rights reserved.
00007  *
00008  *  Redistribution and use in source and binary forms, with or without
00009  *  modification, are permitted provided that the following conditions
00010  *  are met:
00011  *
00012  *   * Redistributions of source code must retain the above copyright
00013  *     notice, this list of conditions and the following disclaimer.
00014  *   * Redistributions in binary form must reproduce the above
00015  *     copyright notice, this list of conditions and the following
00016  *     disclaimer in the documentation and/or other materials provided
00017  *     with the distribution.
00018  *   * Neither the name of Willow Garage, Inc. nor the names of its
00019  *     contributors may be used to endorse or promote products derived
00020  *     from this software without specific prior written permission.
00021  *
00022  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00023  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00024  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00025  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00026  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00027  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00028  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00029  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00030  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00031  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00032  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00033  *  POSSIBILITY OF SUCH DAMAGE.
00034  *
00035  * $Id: pyramid_feature_matching.hpp 2503 2011-09-19 03:17:36Z rusu $
00036  */
00037 
00038 #ifndef PCL_REGISTRATION_IMPL_PYRAMID_FEATURE_MATCHING_H_
00039 #define PCL_REGISTRATION_IMPL_PYRAMID_FEATURE_MATCHING_H_
00040 
00041 #include <pcl/win32_macros.h>
00042 
00043 #include "pcl/registration/pyramid_feature_matching.h"
00044 
00049 __inline double
00050 Log2 (double n_arg)
00051 {
00052   return log (n_arg) / M_LN2;
00053 }
00054 
00055 
00057 template <typename PointFeature> float
00058 pcl::PyramidFeatureHistogram<PointFeature>::comparePyramidFeatureHistograms (const PyramidFeatureHistogramPtr &pyramid_a,
00059                                                                              const PyramidFeatureHistogramPtr &pyramid_b)
00060 {
00061   // do a few consistency checks before and during the computation
00062   if (pyramid_a->nr_dimensions != pyramid_b->nr_dimensions)
00063   {
00064     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);
00065     return -1;
00066   }
00067   if (pyramid_a->nr_levels != pyramid_b->nr_levels)
00068   {
00069     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);
00070     return -1;
00071   }
00072 
00073 
00074   // calculate for level 0 first
00075   if (pyramid_a->hist_levels[0].hist.size () != pyramid_b->hist_levels[0].hist.size ())
00076   {
00077     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 ());
00078     return -1;
00079   }
00080   float match_count_level = 0.0f, match_count_prev_level = 0.0f;
00081   for (size_t bin_i = 0; bin_i < pyramid_a->hist_levels[0].hist.size (); ++bin_i)
00082   {
00083     if (pyramid_a->hist_levels[0].hist[bin_i] < pyramid_b->hist_levels[0].hist[bin_i])
00084       match_count_level += pyramid_a->hist_levels[0].hist[bin_i];
00085     else
00086       match_count_level += pyramid_b->hist_levels[0].hist[bin_i];
00087   }
00088 
00089 
00090   float match_count = match_count_level;
00091   for (size_t level_i = 1; level_i < pyramid_a->nr_levels; ++level_i)
00092   {
00093     if (pyramid_a->hist_levels[level_i].hist.size () != pyramid_b->hist_levels[level_i].hist.size ())
00094     {
00095       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 ());
00096       return -1;
00097     }
00098 
00099     match_count_prev_level = match_count_level;
00100     match_count_level = 0.0f;
00101     for (size_t bin_i = 0; bin_i < pyramid_a->hist_levels[level_i].hist.size (); ++bin_i)
00102     {
00103       if (pyramid_a->hist_levels[level_i].hist[bin_i] < pyramid_b->hist_levels[level_i].hist[bin_i])
00104         match_count_level += pyramid_a->hist_levels[level_i].hist[bin_i];
00105       else
00106         match_count_level += pyramid_b->hist_levels[level_i].hist[bin_i];
00107     }
00108 
00109     float level_normalization_factor = pow(2.0f, (int) level_i);
00110     match_count += (match_count_level - match_count_prev_level) / level_normalization_factor;
00111   }
00112 
00113 
00114   // include self-similarity factors
00115   float self_similarity_a = pyramid_a->nr_features,
00116       self_similarity_b = pyramid_b->nr_features;
00117   PCL_DEBUG ("[pcl::PyramidFeatureMatching::comparePyramidFeatureHistograms] Self similarity measures: %f, %f\n", self_similarity_a, self_similarity_b);
00118   match_count /= sqrt (self_similarity_a * self_similarity_b);
00119 
00120   return match_count;
00121 }
00122 
00123 
00125 template <typename PointFeature>
00126 pcl::PyramidFeatureHistogram<PointFeature>::PyramidFeatureHistogram ()
00127 {
00128   feature_representation_.reset (new DefaultPointRepresentation<PointFeature>);
00129   is_computed_ = false;
00130 }
00131 
00132 template <typename PointFeature> void
00133 pcl::PyramidFeatureHistogram<PointFeature>::PyramidFeatureHistogramLevel::initializeHistogramLevel ()
00134 {
00135   size_t total_vector_size = 1;
00136   for (std::vector<size_t>::iterator dim_it = bins_per_dimension.begin (); dim_it != bins_per_dimension.end (); ++dim_it)
00137     total_vector_size *= *dim_it;
00138 
00139   hist.resize (total_vector_size, 0);
00140 }
00141 
00142 
00144 template <typename PointFeature> bool
00145 pcl::PyramidFeatureHistogram<PointFeature>::initializeHistogram ()
00146 {
00147   // a few consistency checks before starting the computations
00148   if (!PCLBase<PointFeature>::initCompute ())
00149   {
00150     PCL_ERROR ("[pcl::PyramidFeatureHistogram::initializeHistogram] PCLBase initCompute failed\n");
00151     return false;
00152   }
00153 
00154   if (dimension_range_input_.size () == 0)
00155   {
00156     PCL_ERROR ("[pcl::PyramidFeatureHistogram::initializeHistogram] Input dimension range was not set\n");
00157     return false;
00158   }
00159 
00160   if (dimension_range_target_.size () == 0)
00161   {
00162     PCL_ERROR ("[pcl::PyramidFeatureHistogram::initializeHistogram] Target dimension range was not set\n");
00163     return false;
00164   }
00165 
00166   if (dimension_range_input_.size () != dimension_range_target_.size ())
00167   {
00168     PCL_ERROR ("[pcl::PyramidFeatureHistogram::initializeHistogram] Input and target dimension ranges do not agree in size: %u vs %u\n",
00169                dimension_range_input_.size (), dimension_range_target_.size ());
00170     return false;
00171   }
00172 
00173 
00174   nr_dimensions = dimension_range_target_.size ();
00175   nr_features = input_->points.size ();
00176   float D = 0.0f;
00177   for (std::vector<std::pair<float, float> >::iterator range_it = dimension_range_target_.begin (); range_it != dimension_range_target_.end (); ++range_it)
00178   {
00179     float aux = range_it->first - range_it->second;
00180     D += aux * aux;
00181   }
00182   D = sqrt (D);
00183   nr_levels = ceil (Log2 (D));
00184   PCL_DEBUG ("[pcl::PyramidFeatureHistogram::initializeHistogram] Pyramid will have %u levels with a hyper-parallelepiped diagonal size of %f\n", nr_levels, D);
00185 
00186 
00187   hist_levels.resize (nr_levels);
00188   for (size_t level_i = 0; level_i < nr_levels; ++level_i)
00189   {
00190     std::vector<size_t> bins_per_dimension (nr_dimensions);
00191     std::vector<float> bin_step (nr_dimensions);
00192     for (size_t dim_i = 0; dim_i < nr_dimensions; ++dim_i) {
00193       bins_per_dimension[dim_i] = ceil ( (dimension_range_target_[dim_i].second - dimension_range_target_[dim_i].first) / (pow (2.0f, (int) level_i) * sqrt ((float) nr_dimensions)));
00194       bin_step[dim_i] = pow (2.0f, (int) level_i) * sqrt ((float) nr_dimensions);
00195     }
00196     hist_levels[level_i] = PyramidFeatureHistogramLevel (bins_per_dimension, bin_step);
00197 
00198     PCL_DEBUG ("[pcl::PyramidFeatureHistogram::initializeHistogram] Created vector of size %u at level %u\nwith #bins per dimension:", hist_levels.back ().hist.size (), level_i);
00199     for (size_t dim_i = 0; dim_i < nr_dimensions; ++dim_i)
00200       PCL_DEBUG ("%u ", bins_per_dimension[dim_i]);
00201     PCL_DEBUG ("\n");
00202   }
00203 
00204   return true;
00205 }
00206 
00207 
00209 template <typename PointFeature> unsigned int&
00210 pcl::PyramidFeatureHistogram<PointFeature>::at (std::vector<size_t> &access,
00211                                                 size_t &level)
00212 {
00213   if (access.size () != nr_dimensions)
00214   {
00215     PCL_ERROR ("[pcl::PyramidFeatureHistogram::at] Cannot access histogram position because the access point does not have the right number of dimensions\n");
00216     return hist_levels.front ().hist.front ();
00217   }
00218   if (level >= hist_levels.size ())
00219   {
00220     PCL_ERROR ("[pcl::PyramidFeatureHistogram::at] Trying to access a too large level\n");
00221     return hist_levels.front ().hist.front ();
00222   }
00223 
00224   size_t vector_position = 0;
00225   size_t dim_accumulator = 1;
00226 
00227   for (int i = access.size ()-1; i >= 0; --i)
00228   {
00229     vector_position += access[i] * dim_accumulator;
00230     dim_accumulator *= hist_levels[level].bins_per_dimension[i];
00231   }
00232 
00233   return hist_levels[level].hist[vector_position];
00234 }
00235 
00236 
00238 template <typename PointFeature> unsigned int&
00239 pcl::PyramidFeatureHistogram<PointFeature>::at (std::vector<float> &feature,
00240                                                 size_t &level)
00241 {
00242   if (feature.size () != nr_dimensions)
00243   {
00244     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);
00245     return hist_levels.front ().hist.front ();
00246   }
00247   if (level >= hist_levels.size ())
00248   {
00249     PCL_ERROR ("[pcl::PyramidFeatureHistogram::at] Trying to access a too large level\n");
00250     return hist_levels.front ().hist.front ();
00251   }
00252 
00253   std::vector<size_t> access;
00254   for (size_t dim_i = 0; dim_i < nr_dimensions; ++dim_i)
00255     access.push_back ( floor ((feature[dim_i] - dimension_range_target_[dim_i].first) / hist_levels[level].bin_step[dim_i]));
00256 
00257   return at (access, level);
00258 }
00259 
00260 
00262 template <typename PointFeature> void
00263 pcl::PyramidFeatureHistogram<PointFeature>::convertFeatureToVector (const PointFeature &feature,
00264                                                                     std::vector<float> &feature_vector)
00265 {
00266   // convert feature to vector representation
00267   feature_vector.resize (feature_representation_->getNumberOfDimensions ());
00268   feature_representation_->vectorize (feature, feature_vector);
00269 
00270   // adapt the values from the input range to the target range
00271   for (size_t i = 0; i < feature_vector.size (); ++i)
00272     feature_vector[i] = (feature_vector[i] - dimension_range_input_[i].first) / (dimension_range_input_[i].second - dimension_range_input_[i].first) *
00273     (dimension_range_target_[i].second - dimension_range_target_[i].first) + dimension_range_target_[i].first;
00274 }
00275 
00276 
00278 template <typename PointFeature> void
00279 pcl::PyramidFeatureHistogram<PointFeature>::compute ()
00280 {
00281   if (!initializeHistogram ())
00282     return;
00283 
00284   for (size_t feature_i = 0; feature_i < input_->points.size (); ++feature_i)
00285   {
00286     std::vector<float> feature_vector;
00287     convertFeatureToVector (input_->points[feature_i], feature_vector);
00288     addFeature (feature_vector);
00289   }
00290 
00291   is_computed_ = true;
00292 }
00293 
00294 
00296 template <typename PointFeature> void
00297 pcl::PyramidFeatureHistogram<PointFeature>::addFeature (std::vector<float> &feature)
00298 {
00299   for (size_t level_i = 0; level_i < nr_levels; ++level_i)
00300     at (feature, level_i) ++;
00301 }
00302 
00303 #define PCL_INSTANTIATE_PyramidFeatureHistogram(PointFeature) template class PCL_EXPORTS pcl::PyramidFeatureHistogram<PointFeature>;
00304 
00305 #endif /* PCL_REGISTRATION_IMPL_PYRAMID_FEATURE_MATCHING_H_ */
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Defines