ppf_registration.h

Go to the documentation of this file.

/*
 * 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:
      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;


      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)
      {
      }

      void
      setInputFeatureCloud (PointCloud<PPFSignature>::ConstPtr feature_cloud);

      void
      nearestNeighborSearch (float &f1, float &f2, float &f3, float &f4,
                             std::vector<std::pair<size_t, size_t> > &indices);

      Ptr
      makeShared() { return Ptr (new PPFHashMapSearch (*this)); }

      inline float
      getAngleDiscretizationStep () { return angle_discretization_step_; }

      inline float
      getDistanceDiscretizationStep () { return distance_discretization_step_; }

      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_;
  };

  template <typename PointSource, typename PointTarget>
  class PPFRegistration : public Registration<PointSource, PointTarget>
  {
    public:
      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;

      using Registration<PointSource, PointTarget>::input_;
      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;


      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))
      {}

      inline void
      setPositionClusteringThreshold (float clustering_position_diff_threshold) { clustering_position_diff_threshold_ = clustering_position_diff_threshold; }

      inline float
      getPositionClusteringThreshold () { return clustering_position_diff_threshold_; }

      inline void
      setRotationClusteringThreshold (float clustering_rotation_diff_threshold) { clustering_rotation_diff_threshold_ = clustering_rotation_diff_threshold; }

      inline float
      getRotationClusteringThreshold () { return clustering_rotation_diff_threshold_; }

      inline void
      setSceneReferencePointSamplingRate (unsigned int scene_reference_point_sampling_rate) { scene_reference_point_sampling_rate_ = scene_reference_point_sampling_rate; }

      inline unsigned int
      getSceneReferencePointSamplingRate () { return scene_reference_point_sampling_rate_; }

      inline void
      setSearchMethod (PPFHashMapSearch::Ptr search_method) { search_method_ = search_method; }

      inline PPFHashMapSearch::Ptr
      getSearchMethod () { return search_method_; }

      void
      setInputTarget (const PointCloudTargetConstPtr &cloud);


    private:
      void
      computeTransformation (PointCloudSource &output, const Eigen::Matrix4f& guess);


      PPFHashMapSearch::Ptr search_method_;

      unsigned int scene_reference_point_sampling_rate_;

      float clustering_position_diff_threshold_, clustering_rotation_diff_threshold_;

      typename pcl::KdTreeFLANN<PointTarget>::Ptr scene_search_tree_;

      static bool
      poseWithVotesCompareFunction (const PoseWithVotes &a,
                                    const PoseWithVotes &b);

      static bool
      clusterVotesCompareFunction (const std::pair<size_t, unsigned int> &a,
                                   const std::pair<size_t, unsigned int> &b);

      void
      clusterPoses (PoseWithVotesList &poses,
                    PoseWithVotesList &result);

      bool
      posesWithinErrorBounds (Eigen::Affine3f &pose1,
                              Eigen::Affine3f &pose2);
  };
}

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

#endif // PCL_PPF_REGISTRATION_H_