pyramid_feature_matching.hpp

Go to the documentation of this file.

/*
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2011, Alexandru-Eugen Ichim
 *                      Willow Garage, Inc
 *  All rights reserved.
 *
 *  Redistribution and use in source and binary forms, with or without
 *  modification, are permitted provided that the following conditions
 *  are met:
 *
 *   * Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above
 *     copyright notice, this list of conditions and the following
 *     disclaimer in the documentation and/or other materials provided
 *     with the distribution.
 *   * Neither the name of Willow Garage, Inc. nor the names of its
 *     contributors may be used to endorse or promote products derived
 *     from this software without specific prior written permission.
 *
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *  POSSIBILITY OF SUCH DAMAGE.
 *
 * $Id: pyramid_feature_matching.hpp 5026 2012-03-12 02:51:44Z rusu $
 */

#ifndef PCL_REGISTRATION_IMPL_PYRAMID_FEATURE_MATCHING_H_
#define PCL_REGISTRATION_IMPL_PYRAMID_FEATURE_MATCHING_H_

#include <pcl/pcl_macros.h>

#include <pcl/registration/pyramid_feature_matching.h>

__inline double
Log2 (double n_arg)
{
  return log (n_arg) / M_LN2;
}


template <typename PointFeature> float
pcl::PyramidFeatureHistogram<PointFeature>::comparePyramidFeatureHistograms (const PyramidFeatureHistogramPtr &pyramid_a,
                                                                             const PyramidFeatureHistogramPtr &pyramid_b)
{
  // do a few consistency checks before and during the computation
  if (pyramid_a->nr_dimensions != pyramid_b->nr_dimensions)
  {
    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);
    return -1;
  }
  if (pyramid_a->nr_levels != pyramid_b->nr_levels)
  {
    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);
    return -1;
  }


  // calculate for level 0 first
  if (pyramid_a->hist_levels[0].hist.size () != pyramid_b->hist_levels[0].hist.size ())
  {
    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 ());
    return -1;
  }
  float match_count_level = 0.0f, match_count_prev_level = 0.0f;
  for (size_t bin_i = 0; bin_i < pyramid_a->hist_levels[0].hist.size (); ++bin_i)
  {
    if (pyramid_a->hist_levels[0].hist[bin_i] < pyramid_b->hist_levels[0].hist[bin_i])
      match_count_level += static_cast<float> (pyramid_a->hist_levels[0].hist[bin_i]);
    else
      match_count_level += static_cast<float> (pyramid_b->hist_levels[0].hist[bin_i]);
  }


  float match_count = match_count_level;
  for (size_t level_i = 1; level_i < pyramid_a->nr_levels; ++level_i)
  {
    if (pyramid_a->hist_levels[level_i].hist.size () != pyramid_b->hist_levels[level_i].hist.size ())
    {
      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 ());
      return -1;
    }

    match_count_prev_level = match_count_level;
    match_count_level = 0.0f;
    for (size_t bin_i = 0; bin_i < pyramid_a->hist_levels[level_i].hist.size (); ++bin_i)
    {
      if (pyramid_a->hist_levels[level_i].hist[bin_i] < pyramid_b->hist_levels[level_i].hist[bin_i])
        match_count_level += static_cast<float> (pyramid_a->hist_levels[level_i].hist[bin_i]);
      else
        match_count_level += static_cast<float> (pyramid_b->hist_levels[level_i].hist[bin_i]);
    }

    float level_normalization_factor = powf (2.0f, static_cast<float> (level_i));
    match_count += (match_count_level - match_count_prev_level) / level_normalization_factor;
  }


  // include self-similarity factors
  float self_similarity_a = static_cast<float> (pyramid_a->nr_features),
        self_similarity_b = static_cast<float> (pyramid_b->nr_features);
  PCL_DEBUG ("[pcl::PyramidFeatureMatching::comparePyramidFeatureHistograms] Self similarity measures: %f, %f\n", self_similarity_a, self_similarity_b);
  match_count /= sqrtf (self_similarity_a * self_similarity_b);

  return match_count;
}


template <typename PointFeature>
pcl::PyramidFeatureHistogram<PointFeature>::PyramidFeatureHistogram () :
  nr_dimensions (0), nr_levels (0), nr_features (0),
  dimension_range_input_ (), dimension_range_target_ (),
  feature_representation_ (new DefaultPointRepresentation<PointFeature>),
  is_computed_ (false),
  hist_levels ()
{
}

template <typename PointFeature> void
pcl::PyramidFeatureHistogram<PointFeature>::PyramidFeatureHistogramLevel::initializeHistogramLevel ()
{
  size_t total_vector_size = 1;
  for (std::vector<size_t>::iterator dim_it = bins_per_dimension.begin (); dim_it != bins_per_dimension.end (); ++dim_it)
    total_vector_size *= *dim_it;

  hist.resize (total_vector_size, 0);
}


template <typename PointFeature> bool
pcl::PyramidFeatureHistogram<PointFeature>::initializeHistogram ()
{
  // a few consistency checks before starting the computations
  if (!PCLBase<PointFeature>::initCompute ())
  {
    PCL_ERROR ("[pcl::PyramidFeatureHistogram::initializeHistogram] PCLBase initCompute failed\n");
    return false;
  }

  if (dimension_range_input_.size () == 0)
  {
    PCL_ERROR ("[pcl::PyramidFeatureHistogram::initializeHistogram] Input dimension range was not set\n");
    return false;
  }

  if (dimension_range_target_.size () == 0)
  {
    PCL_ERROR ("[pcl::PyramidFeatureHistogram::initializeHistogram] Target dimension range was not set\n");
    return false;
  }

  if (dimension_range_input_.size () != dimension_range_target_.size ())
  {
    PCL_ERROR ("[pcl::PyramidFeatureHistogram::initializeHistogram] Input and target dimension ranges do not agree in size: %u vs %u\n",
               dimension_range_input_.size (), dimension_range_target_.size ());
    return false;
  }


  nr_dimensions = dimension_range_target_.size ();
  nr_features = input_->points.size ();
  float D = 0.0f;
  for (std::vector<std::pair<float, float> >::iterator range_it = dimension_range_target_.begin (); range_it != dimension_range_target_.end (); ++range_it)
  {
    float aux = range_it->first - range_it->second;
    D += aux * aux;
  }
  D = sqrtf (D);
  nr_levels = static_cast<size_t> (ceilf (log2f (D)));
  PCL_DEBUG ("[pcl::PyramidFeatureHistogram::initializeHistogram] Pyramid will have %u levels with a hyper-parallelepiped diagonal size of %f\n", nr_levels, D);


  hist_levels.resize (nr_levels);
  for (size_t level_i = 0; level_i < nr_levels; ++level_i)
  {
    std::vector<size_t> bins_per_dimension (nr_dimensions);
    std::vector<float> bin_step (nr_dimensions);
    for (size_t dim_i = 0; dim_i < nr_dimensions; ++dim_i) 
    {
      bins_per_dimension[dim_i] = 
        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)))));
      bin_step[dim_i] = powf (2.0f, static_cast<float> (level_i)) * sqrtf (static_cast<float> (nr_dimensions));
    }
    hist_levels[level_i] = PyramidFeatureHistogramLevel (bins_per_dimension, bin_step);

    PCL_DEBUG ("[pcl::PyramidFeatureHistogram::initializeHistogram] Created vector of size %u at level %u\nwith #bins per dimension:", hist_levels.back ().hist.size (), level_i);
    for (size_t dim_i = 0; dim_i < nr_dimensions; ++dim_i)
      PCL_DEBUG ("%u ", bins_per_dimension[dim_i]);
    PCL_DEBUG ("\n");
  }

  return true;
}


template <typename PointFeature> unsigned int&
pcl::PyramidFeatureHistogram<PointFeature>::at (std::vector<size_t> &access,
                                                size_t &level)
{
  if (access.size () != nr_dimensions)
  {
    PCL_ERROR ("[pcl::PyramidFeatureHistogram::at] Cannot access histogram position because the access point does not have the right number of dimensions\n");
    return hist_levels.front ().hist.front ();
  }
  if (level >= hist_levels.size ())
  {
    PCL_ERROR ("[pcl::PyramidFeatureHistogram::at] Trying to access a too large level\n");
    return hist_levels.front ().hist.front ();
  }

  size_t vector_position = 0;
  size_t dim_accumulator = 1;

  for (int i = static_cast<int> (access.size ()) - 1; i >= 0; --i)
  {
    vector_position += access[i] * dim_accumulator;
    dim_accumulator *= hist_levels[level].bins_per_dimension[i];
  }

  return hist_levels[level].hist[vector_position];
}


template <typename PointFeature> unsigned int&
pcl::PyramidFeatureHistogram<PointFeature>::at (std::vector<float> &feature,
                                                size_t &level)
{
  if (feature.size () != nr_dimensions)
  {
    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);
    return hist_levels.front ().hist.front ();
  }
  if (level >= hist_levels.size ())
  {
    PCL_ERROR ("[pcl::PyramidFeatureHistogram::at] Trying to access a too large level\n");
    return hist_levels.front ().hist.front ();
  }

  std::vector<size_t> access;
  for (size_t dim_i = 0; dim_i < nr_dimensions; ++dim_i)
    access.push_back (static_cast<size_t> (floor ((feature[dim_i] - dimension_range_target_[dim_i].first) / hist_levels[level].bin_step[dim_i])));

  return at (access, level);
}


template <typename PointFeature> void
pcl::PyramidFeatureHistogram<PointFeature>::convertFeatureToVector (const PointFeature &feature,
                                                                    std::vector<float> &feature_vector)
{
  // convert feature to vector representation
  feature_vector.resize (feature_representation_->getNumberOfDimensions ());
  feature_representation_->vectorize (feature, feature_vector);

  // adapt the values from the input range to the target range
  for (size_t i = 0; i < feature_vector.size (); ++i)
    feature_vector[i] = (feature_vector[i] - dimension_range_input_[i].first) / (dimension_range_input_[i].second - dimension_range_input_[i].first) *
    (dimension_range_target_[i].second - dimension_range_target_[i].first) + dimension_range_target_[i].first;
}


template <typename PointFeature> void
pcl::PyramidFeatureHistogram<PointFeature>::compute ()
{
  if (!initializeHistogram ())
    return;

  for (size_t feature_i = 0; feature_i < input_->points.size (); ++feature_i)
  {
    std::vector<float> feature_vector;
    convertFeatureToVector (input_->points[feature_i], feature_vector);
    addFeature (feature_vector);
  }

  is_computed_ = true;
}


template <typename PointFeature> void
pcl::PyramidFeatureHistogram<PointFeature>::addFeature (std::vector<float> &feature)
{
  for (size_t level_i = 0; level_i < nr_levels; ++level_i)
    at (feature, level_i) ++;
}

#define PCL_INSTANTIATE_PyramidFeatureHistogram(PointFeature) template class PCL_EXPORTS pcl::PyramidFeatureHistogram<PointFeature>;

#endif /* PCL_REGISTRATION_IMPL_PYRAMID_FEATURE_MATCHING_H_ */