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 5026 2012-03-12 02:51:44Z 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/pcl_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 += static_cast<float> (pyramid_a->hist_levels[0].hist[bin_i]);
00085     else
00086       match_count_level += static_cast<float> (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 += static_cast<float> (pyramid_a->hist_levels[level_i].hist[bin_i]);
00105       else
00106         match_count_level += static_cast<float> (pyramid_b->hist_levels[level_i].hist[bin_i]);
00107     }
00108 
00109     float level_normalization_factor = powf (2.0f, static_cast<float> (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 = static_cast<float> (pyramid_a->nr_features),
00116         self_similarity_b = static_cast<float> (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 /= sqrtf (self_similarity_a * self_similarity_b);
00119 
00120   return match_count;
00121 }
00122 
00123 
00125 template <typename PointFeature>
00126 pcl::PyramidFeatureHistogram<PointFeature>::PyramidFeatureHistogram () :
00127   nr_dimensions (0), nr_levels (0), nr_features (0),
00128   dimension_range_input_ (), dimension_range_target_ (),
00129   feature_representation_ (new DefaultPointRepresentation<PointFeature>),
00130   is_computed_ (false),
00131   hist_levels ()
00132 {
00133 }
00134 
00136 template <typename PointFeature> void
00137 pcl::PyramidFeatureHistogram<PointFeature>::PyramidFeatureHistogramLevel::initializeHistogramLevel ()
00138 {
00139   size_t total_vector_size = 1;
00140   for (std::vector<size_t>::iterator dim_it = bins_per_dimension.begin (); dim_it != bins_per_dimension.end (); ++dim_it)
00141     total_vector_size *= *dim_it;
00142 
00143   hist.resize (total_vector_size, 0);
00144 }
00145 
00146 
00148 template <typename PointFeature> bool
00149 pcl::PyramidFeatureHistogram<PointFeature>::initializeHistogram ()
00150 {
00151   // a few consistency checks before starting the computations
00152   if (!PCLBase<PointFeature>::initCompute ())
00153   {
00154     PCL_ERROR ("[pcl::PyramidFeatureHistogram::initializeHistogram] PCLBase initCompute failed\n");
00155     return false;
00156   }
00157 
00158   if (dimension_range_input_.size () == 0)
00159   {
00160     PCL_ERROR ("[pcl::PyramidFeatureHistogram::initializeHistogram] Input dimension range was not set\n");
00161     return false;
00162   }
00163 
00164   if (dimension_range_target_.size () == 0)
00165   {
00166     PCL_ERROR ("[pcl::PyramidFeatureHistogram::initializeHistogram] Target dimension range was not set\n");
00167     return false;
00168   }
00169 
00170   if (dimension_range_input_.size () != dimension_range_target_.size ())
00171   {
00172     PCL_ERROR ("[pcl::PyramidFeatureHistogram::initializeHistogram] Input and target dimension ranges do not agree in size: %u vs %u\n",
00173                dimension_range_input_.size (), dimension_range_target_.size ());
00174     return false;
00175   }
00176 
00177 
00178   nr_dimensions = dimension_range_target_.size ();
00179   nr_features = input_->points.size ();
00180   float D = 0.0f;
00181   for (std::vector<std::pair<float, float> >::iterator range_it = dimension_range_target_.begin (); range_it != dimension_range_target_.end (); ++range_it)
00182   {
00183     float aux = range_it->first - range_it->second;
00184     D += aux * aux;
00185   }
00186   D = sqrtf (D);
00187   nr_levels = static_cast<size_t> (ceilf (log2f (D)));
00188   PCL_DEBUG ("[pcl::PyramidFeatureHistogram::initializeHistogram] Pyramid will have %u levels with a hyper-parallelepiped diagonal size of %f\n", nr_levels, D);
00189 
00190 
00191   hist_levels.resize (nr_levels);
00192   for (size_t level_i = 0; level_i < nr_levels; ++level_i)
00193   {
00194     std::vector<size_t> bins_per_dimension (nr_dimensions);
00195     std::vector<float> bin_step (nr_dimensions);
00196     for (size_t dim_i = 0; dim_i < nr_dimensions; ++dim_i) 
00197     {
00198       bins_per_dimension[dim_i] = 
00199         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)))));
00200       bin_step[dim_i] = powf (2.0f, static_cast<float> (level_i)) * sqrtf (static_cast<float> (nr_dimensions));
00201     }
00202     hist_levels[level_i] = PyramidFeatureHistogramLevel (bins_per_dimension, bin_step);
00203 
00204     PCL_DEBUG ("[pcl::PyramidFeatureHistogram::initializeHistogram] Created vector of size %u at level %u\nwith #bins per dimension:", hist_levels.back ().hist.size (), level_i);
00205     for (size_t dim_i = 0; dim_i < nr_dimensions; ++dim_i)
00206       PCL_DEBUG ("%u ", bins_per_dimension[dim_i]);
00207     PCL_DEBUG ("\n");
00208   }
00209 
00210   return true;
00211 }
00212 
00213 
00215 template <typename PointFeature> unsigned int&
00216 pcl::PyramidFeatureHistogram<PointFeature>::at (std::vector<size_t> &access,
00217                                                 size_t &level)
00218 {
00219   if (access.size () != nr_dimensions)
00220   {
00221     PCL_ERROR ("[pcl::PyramidFeatureHistogram::at] Cannot access histogram position because the access point does not have the right number of dimensions\n");
00222     return hist_levels.front ().hist.front ();
00223   }
00224   if (level >= hist_levels.size ())
00225   {
00226     PCL_ERROR ("[pcl::PyramidFeatureHistogram::at] Trying to access a too large level\n");
00227     return hist_levels.front ().hist.front ();
00228   }
00229 
00230   size_t vector_position = 0;
00231   size_t dim_accumulator = 1;
00232 
00233   for (int i = static_cast<int> (access.size ()) - 1; i >= 0; --i)
00234   {
00235     vector_position += access[i] * dim_accumulator;
00236     dim_accumulator *= hist_levels[level].bins_per_dimension[i];
00237   }
00238 
00239   return hist_levels[level].hist[vector_position];
00240 }
00241 
00242 
00244 template <typename PointFeature> unsigned int&
00245 pcl::PyramidFeatureHistogram<PointFeature>::at (std::vector<float> &feature,
00246                                                 size_t &level)
00247 {
00248   if (feature.size () != nr_dimensions)
00249   {
00250     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);
00251     return hist_levels.front ().hist.front ();
00252   }
00253   if (level >= hist_levels.size ())
00254   {
00255     PCL_ERROR ("[pcl::PyramidFeatureHistogram::at] Trying to access a too large level\n");
00256     return hist_levels.front ().hist.front ();
00257   }
00258 
00259   std::vector<size_t> access;
00260   for (size_t dim_i = 0; dim_i < nr_dimensions; ++dim_i)
00261     access.push_back (static_cast<size_t> (floor ((feature[dim_i] - dimension_range_target_[dim_i].first) / hist_levels[level].bin_step[dim_i])));
00262 
00263   return at (access, level);
00264 }
00265 
00266 
00268 template <typename PointFeature> void
00269 pcl::PyramidFeatureHistogram<PointFeature>::convertFeatureToVector (const PointFeature &feature,
00270                                                                     std::vector<float> &feature_vector)
00271 {
00272   // convert feature to vector representation
00273   feature_vector.resize (feature_representation_->getNumberOfDimensions ());
00274   feature_representation_->vectorize (feature, feature_vector);
00275 
00276   // adapt the values from the input range to the target range
00277   for (size_t i = 0; i < feature_vector.size (); ++i)
00278     feature_vector[i] = (feature_vector[i] - dimension_range_input_[i].first) / (dimension_range_input_[i].second - dimension_range_input_[i].first) *
00279     (dimension_range_target_[i].second - dimension_range_target_[i].first) + dimension_range_target_[i].first;
00280 }
00281 
00282 
00284 template <typename PointFeature> void
00285 pcl::PyramidFeatureHistogram<PointFeature>::compute ()
00286 {
00287   if (!initializeHistogram ())
00288     return;
00289 
00290   for (size_t feature_i = 0; feature_i < input_->points.size (); ++feature_i)
00291   {
00292     std::vector<float> feature_vector;
00293     convertFeatureToVector (input_->points[feature_i], feature_vector);
00294     addFeature (feature_vector);
00295   }
00296 
00297   is_computed_ = true;
00298 }
00299 
00300 
00302 template <typename PointFeature> void
00303 pcl::PyramidFeatureHistogram<PointFeature>::addFeature (std::vector<float> &feature)
00304 {
00305   for (size_t level_i = 0; level_i < nr_levels; ++level_i)
00306     at (feature, level_i) ++;
00307 }
00308 
00309 #define PCL_INSTANTIATE_PyramidFeatureHistogram(PointFeature) template class PCL_EXPORTS pcl::PyramidFeatureHistogram<PointFeature>;
00310 
00311 #endif /* PCL_REGISTRATION_IMPL_PYRAMID_FEATURE_MATCHING_H_ */


pcl
Author(s): Open Perception
autogenerated on Mon Oct 6 2014 03:17:28