ppf_registration.h

/*
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2011, Alexandru-Eugen Ichim
 *                      Willow Garage, Inc
 *  Copyright (c) 2012-, Open Perception, 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 the copyright holder(s) 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$
 *
 */


#ifndef PCL_PPF_REGISTRATION_H_
#define PCL_PPF_REGISTRATION_H_

#include <pcl/registration/boost.h>
#include <pcl/registration/registration.h>
#include <pcl/features/ppf.h>

namespace pcl
{
  class PCL_EXPORTS PPFHashMapSearch
  {
    public:
      /** \brief Data structure to hold the information for the key in the feature hash map of the
        * PPFHashMapSearch class
        * \note It uses multiple pair levels in order to enable the usage of the boost::hash function
        * which has the std::pair implementation (i.e., does not require a custom hash function)
        */
      struct HashKeyStruct : public std::pair <int, std::pair <int, std::pair <int, int> > >
      {
        HashKeyStruct(int a, int b, int c, int d)
        {
          this->first = a;
          this->second.first = b;
          this->second.second.first = c;
          this->second.second.second = d;
        }
      };
      typedef boost::unordered_multimap<HashKeyStruct, std::pair<size_t, size_t> > FeatureHashMapType;
      typedef boost::shared_ptr<FeatureHashMapType> FeatureHashMapTypePtr;
      typedef boost::shared_ptr<PPFHashMapSearch> Ptr;


      /** \brief Constructor for the PPFHashMapSearch class which sets the two step parameters for the enclosed data structure
       * \param angle_discretization_step the step value between each bin of the hash map for the angular values
       * \param distance_discretization_step the step value between each bin of the hash map for the distance values
       */
      PPFHashMapSearch (float angle_discretization_step = 12.0f / 180.0f * static_cast<float> (M_PI),
                        float distance_discretization_step = 0.01f)
        : alpha_m_ ()
        , feature_hash_map_ (new FeatureHashMapType)
        , internals_initialized_ (false)
        , angle_discretization_step_ (angle_discretization_step)
        , distance_discretization_step_ (distance_discretization_step)
        , max_dist_ (-1.0f)
      {
      }

      /** \brief Method that sets the feature cloud to be inserted in the hash map
       * \param feature_cloud a const smart pointer to the PPFSignature feature cloud
       */
      void
      setInputFeatureCloud (PointCloud<PPFSignature>::ConstPtr feature_cloud);

      /** \brief Function for finding the nearest neighbors for the given feature inside the discretized hash map
       * \param f1 The 1st value describing the query PPFSignature feature
       * \param f2 The 2nd value describing the query PPFSignature feature
       * \param f3 The 3rd value describing the query PPFSignature feature
       * \param f4 The 4th value describing the query PPFSignature feature
       * \param indices a vector of pair indices representing the feature pairs that have been found in the bin
       * corresponding to the query feature
       */
      void
      nearestNeighborSearch (float &f1, float &f2, float &f3, float &f4,
                             std::vector<std::pair<size_t, size_t> > &indices);

      /** \brief Convenience method for returning a copy of the class instance as a boost::shared_ptr */
      Ptr
      makeShared() { return Ptr (new PPFHashMapSearch (*this)); }

      /** \brief Returns the angle discretization step parameter (the step value between each bin of the hash map for the angular values) */
      inline float
      getAngleDiscretizationStep () { return angle_discretization_step_; }

      /** \brief Returns the distance discretization step parameter (the step value between each bin of the hash map for the distance values) */
      inline float
      getDistanceDiscretizationStep () { return distance_discretization_step_; }

      /** \brief Returns the maximum distance found between any feature pair in the given input feature cloud */
      inline float
      getModelDiameter () { return max_dist_; }

      std::vector <std::vector <float> > alpha_m_;
    private:
      FeatureHashMapTypePtr feature_hash_map_;
      bool internals_initialized_;

      float angle_discretization_step_, distance_discretization_step_;
      float max_dist_;
  };

  /** \brief Class that registers two point clouds based on their sets of PPFSignatures.
   * Please refer to the following publication for more details:
   *    B. Drost, M. Ulrich, N. Navab, S. Ilic
   *    Model Globally, Match Locally: Efficient and Robust 3D Object Recognition
   *    2010 IEEE Conference on Computer Vision and Pattern Recognition (CVPR)
   *    13-18 June 2010, San Francisco, CA
   *
   * \note This class works in tandem with the PPFEstimation class
   *
   * \author Alexandru-Eugen Ichim
   */
  template <typename PointSource, typename PointTarget>
  class PPFRegistration : public Registration<PointSource, PointTarget>
  {
    public:
      /** \brief Structure for storing a pose (represented as an Eigen::Affine3f) and an integer for counting votes
        * \note initially used std::pair<Eigen::Affine3f, unsigned int>, but it proved problematic
        * because of the Eigen structures alignment problems - std::pair does not have a custom allocator
        */
      struct PoseWithVotes
      {
        PoseWithVotes(Eigen::Affine3f &a_pose, unsigned int &a_votes)
        : pose (a_pose),
          votes (a_votes)
        {}

        Eigen::Affine3f pose;
        unsigned int votes;
      };
      typedef std::vector<PoseWithVotes, Eigen::aligned_allocator<PoseWithVotes> > PoseWithVotesList;

      /// input_ is the model cloud
      using Registration<PointSource, PointTarget>::input_;
      /// target_ is the scene cloud
      using Registration<PointSource, PointTarget>::target_;
      using Registration<PointSource, PointTarget>::converged_;
      using Registration<PointSource, PointTarget>::final_transformation_;
      using Registration<PointSource, PointTarget>::transformation_;

      typedef pcl::PointCloud<PointSource> PointCloudSource;
      typedef typename PointCloudSource::Ptr PointCloudSourcePtr;
      typedef typename PointCloudSource::ConstPtr PointCloudSourceConstPtr;

      typedef pcl::PointCloud<PointTarget> PointCloudTarget;
      typedef typename PointCloudTarget::Ptr PointCloudTargetPtr;
      typedef typename PointCloudTarget::ConstPtr PointCloudTargetConstPtr;


      /** \brief Empty constructor that initializes all the parameters of the algorithm with default values */
      PPFRegistration ()
      :  Registration<PointSource, PointTarget> (),
         search_method_ (),
         scene_reference_point_sampling_rate_ (5),
         clustering_position_diff_threshold_ (0.01f),
         clustering_rotation_diff_threshold_ (20.0f / 180.0f * static_cast<float> (M_PI))
      {}

      /** \brief Method for setting the position difference clustering parameter
       * \param clustering_position_diff_threshold distance threshold below which two poses are
       * considered close enough to be in the same cluster (for the clustering phase of the algorithm)
       */
      inline void
      setPositionClusteringThreshold (float clustering_position_diff_threshold) { clustering_position_diff_threshold_ = clustering_position_diff_threshold; }

      /** \brief Returns the parameter defining the position difference clustering parameter -
       * distance threshold below which two poses are considered close enough to be in the same cluster
       * (for the clustering phase of the algorithm)
       */
      inline float
      getPositionClusteringThreshold () { return clustering_position_diff_threshold_; }

      /** \brief Method for setting the rotation clustering parameter
       * \param clustering_rotation_diff_threshold rotation difference threshold below which two
       * poses are considered to be in the same cluster (for the clustering phase of the algorithm)
       */
      inline void
      setRotationClusteringThreshold (float clustering_rotation_diff_threshold) { clustering_rotation_diff_threshold_ = clustering_rotation_diff_threshold; }

      /** \brief Returns the parameter defining the rotation clustering threshold
       */
      inline float
      getRotationClusteringThreshold () { return clustering_rotation_diff_threshold_; }

      /** \brief Method for setting the scene reference point sampling rate
       * \param scene_reference_point_sampling_rate sampling rate for the scene reference point
       */
      inline void
      setSceneReferencePointSamplingRate (unsigned int scene_reference_point_sampling_rate) { scene_reference_point_sampling_rate_ = scene_reference_point_sampling_rate; }

      /** \brief Returns the parameter for the scene reference point sampling rate of the algorithm */
      inline unsigned int
      getSceneReferencePointSamplingRate () { return scene_reference_point_sampling_rate_; }

      /** \brief Function that sets the search method for the algorithm
       * \note Right now, the only available method is the one initially proposed by
       * the authors - by using a hash map with discretized feature vectors
       * \param search_method smart pointer to the search method to be set
       */
      inline void
      setSearchMethod (PPFHashMapSearch::Ptr search_method) { search_method_ = search_method; }

      /** \brief Getter function for the search method of the class */
      inline PPFHashMapSearch::Ptr
      getSearchMethod () { return search_method_; }

      /** \brief Provide a pointer to the input target (e.g., the point cloud that we want to align the input source to)
       * \param cloud the input point cloud target
       */
      void
      setInputTarget (const PointCloudTargetConstPtr &cloud);


    private:
      /** \brief Method that calculates the transformation between the input_ and target_ point clouds, based on the PPF features */
      void
      computeTransformation (PointCloudSource &output, const Eigen::Matrix4f& guess);


      /** \brief the search method that is going to be used to find matching feature pairs */
      PPFHashMapSearch::Ptr search_method_;

      /** \brief parameter for the sampling rate of the scene reference points */
      unsigned int scene_reference_point_sampling_rate_;

      /** \brief position and rotation difference thresholds below which two
        * poses are considered to be in the same cluster (for the clustering phase of the algorithm) */
      float clustering_position_diff_threshold_, clustering_rotation_diff_threshold_;

      /** \brief use a kd-tree with range searches of range max_dist to skip an O(N) pass through the point cloud */
      typename pcl::KdTreeFLANN<PointTarget>::Ptr scene_search_tree_;

      /** \brief static method used for the std::sort function to order two PoseWithVotes
       * instances by their number of votes*/
      static bool
      poseWithVotesCompareFunction (const PoseWithVotes &a,
                                    const PoseWithVotes &b);

      /** \brief static method used for the std::sort function to order two pairs <index, votes>
       * by the number of votes (unsigned integer value) */
      static bool
      clusterVotesCompareFunction (const std::pair<size_t, unsigned int> &a,
                                   const std::pair<size_t, unsigned int> &b);

      /** \brief Method that clusters a set of given poses by using the clustering thresholds
       * and their corresponding number of votes (see publication for more details) */
      void
      clusterPoses (PoseWithVotesList &poses,
                    PoseWithVotesList &result);

      /** \brief Method that checks whether two poses are close together - based on the clustering threshold parameters
       * of the class */
      bool
      posesWithinErrorBounds (Eigen::Affine3f &pose1,
                              Eigen::Affine3f &pose2);
  };
}

#include <pcl/registration/impl/ppf_registration.hpp>

#endif // PCL_PPF_REGISTRATION_H_