Point Cloud Library (PCL)  1.7.0
/tmp/buildd/pcl-1.7-1.7.0/registration/include/pcl/registration/ppf_registration.h
00001 /*
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2011, Alexandru-Eugen Ichim
00005  *                      Willow Garage, Inc
00006  *  Copyright (c) 2012-, Open Perception, Inc.
00007  *
00008  *  All rights reserved.
00009  *
00010  *  Redistribution and use in source and binary forms, with or without
00011  *  modification, are permitted provided that the following conditions
00012  *  are met:
00013  *
00014  *   * Redistributions of source code must retain the above copyright
00015  *     notice, this list of conditions and the following disclaimer.
00016  *   * Redistributions in binary form must reproduce the above
00017  *     copyright notice, this list of conditions and the following
00018  *     disclaimer in the documentation and/or other materials provided
00019  *     with the distribution.
00020  *   * Neither the name of the copyright holder(s) nor the names of its
00021  *     contributors may be used to endorse or promote products derived
00022  *     from this software without specific prior written permission.
00023  *
00024  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00025  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00026  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00027  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00028  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00029  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00030  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00031  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00032  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00033  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00034  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00035  *  POSSIBILITY OF SUCH DAMAGE.
00036  *
00037  * $Id$
00038  *
00039  */
00040 
00041 
00042 #ifndef PCL_PPF_REGISTRATION_H_
00043 #define PCL_PPF_REGISTRATION_H_
00044 
00045 #include <pcl/registration/boost.h>
00046 #include <pcl/registration/registration.h>
00047 #include <pcl/features/ppf.h>
00048 
00049 namespace pcl
00050 {
00051   class PCL_EXPORTS PPFHashMapSearch
00052   {
00053     public:
00054       /** \brief Data structure to hold the information for the key in the feature hash map of the
00055         * PPFHashMapSearch class
00056         * \note It uses multiple pair levels in order to enable the usage of the boost::hash function
00057         * which has the std::pair implementation (i.e., does not require a custom hash function)
00058         */
00059       struct HashKeyStruct : public std::pair <int, std::pair <int, std::pair <int, int> > >
00060       {
00061         HashKeyStruct(int a, int b, int c, int d)
00062         {
00063           this->first = a;
00064           this->second.first = b;
00065           this->second.second.first = c;
00066           this->second.second.second = d;
00067         }
00068       };
00069       typedef boost::unordered_multimap<HashKeyStruct, std::pair<size_t, size_t> > FeatureHashMapType;
00070       typedef boost::shared_ptr<FeatureHashMapType> FeatureHashMapTypePtr;
00071       typedef boost::shared_ptr<PPFHashMapSearch> Ptr;
00072 
00073 
00074       /** \brief Constructor for the PPFHashMapSearch class which sets the two step parameters for the enclosed data structure
00075        * \param angle_discretization_step the step value between each bin of the hash map for the angular values
00076        * \param distance_discretization_step the step value between each bin of the hash map for the distance values
00077        */
00078       PPFHashMapSearch (float angle_discretization_step = 12.0f / 180.0f * static_cast<float> (M_PI),
00079                         float distance_discretization_step = 0.01f)
00080         : alpha_m_ ()
00081         , feature_hash_map_ (new FeatureHashMapType)
00082         , internals_initialized_ (false)
00083         , angle_discretization_step_ (angle_discretization_step)
00084         , distance_discretization_step_ (distance_discretization_step)
00085         , max_dist_ (-1.0f)
00086       {
00087       }
00088 
00089       /** \brief Method that sets the feature cloud to be inserted in the hash map
00090        * \param feature_cloud a const smart pointer to the PPFSignature feature cloud
00091        */
00092       void
00093       setInputFeatureCloud (PointCloud<PPFSignature>::ConstPtr feature_cloud);
00094 
00095       /** \brief Function for finding the nearest neighbors for the given feature inside the discretized hash map
00096        * \param f1 The 1st value describing the query PPFSignature feature
00097        * \param f2 The 2nd value describing the query PPFSignature feature
00098        * \param f3 The 3rd value describing the query PPFSignature feature
00099        * \param f4 The 4th value describing the query PPFSignature feature
00100        * \param indices a vector of pair indices representing the feature pairs that have been found in the bin
00101        * corresponding to the query feature
00102        */
00103       void
00104       nearestNeighborSearch (float &f1, float &f2, float &f3, float &f4,
00105                              std::vector<std::pair<size_t, size_t> > &indices);
00106 
00107       /** \brief Convenience method for returning a copy of the class instance as a boost::shared_ptr */
00108       Ptr
00109       makeShared() { return Ptr (new PPFHashMapSearch (*this)); }
00110 
00111       /** \brief Returns the angle discretization step parameter (the step value between each bin of the hash map for the angular values) */
00112       inline float
00113       getAngleDiscretizationStep () { return angle_discretization_step_; }
00114 
00115       /** \brief Returns the distance discretization step parameter (the step value between each bin of the hash map for the distance values) */
00116       inline float
00117       getDistanceDiscretizationStep () { return distance_discretization_step_; }
00118 
00119       /** \brief Returns the maximum distance found between any feature pair in the given input feature cloud */
00120       inline float
00121       getModelDiameter () { return max_dist_; }
00122 
00123       std::vector <std::vector <float> > alpha_m_;
00124     private:
00125       FeatureHashMapTypePtr feature_hash_map_;
00126       bool internals_initialized_;
00127 
00128       float angle_discretization_step_, distance_discretization_step_;
00129       float max_dist_;
00130   };
00131 
00132   /** \brief Class that registers two point clouds based on their sets of PPFSignatures.
00133    * Please refer to the following publication for more details:
00134    *    B. Drost, M. Ulrich, N. Navab, S. Ilic
00135    *    Model Globally, Match Locally: Efficient and Robust 3D Object Recognition
00136    *    2010 IEEE Conference on Computer Vision and Pattern Recognition (CVPR)
00137    *    13-18 June 2010, San Francisco, CA
00138    *
00139    * \note This class works in tandem with the PPFEstimation class
00140    *
00141    * \author Alexandru-Eugen Ichim
00142    */
00143   template <typename PointSource, typename PointTarget>
00144   class PPFRegistration : public Registration<PointSource, PointTarget>
00145   {
00146     public:
00147       /** \brief Structure for storing a pose (represented as an Eigen::Affine3f) and an integer for counting votes
00148         * \note initially used std::pair<Eigen::Affine3f, unsigned int>, but it proved problematic
00149         * because of the Eigen structures alignment problems - std::pair does not have a custom allocator
00150         */
00151       struct PoseWithVotes
00152       {
00153         PoseWithVotes(Eigen::Affine3f &a_pose, unsigned int &a_votes)
00154         : pose (a_pose),
00155           votes (a_votes)
00156         {}
00157 
00158         Eigen::Affine3f pose;
00159         unsigned int votes;
00160       };
00161       typedef std::vector<PoseWithVotes, Eigen::aligned_allocator<PoseWithVotes> > PoseWithVotesList;
00162 
00163       /// input_ is the model cloud
00164       using Registration<PointSource, PointTarget>::input_;
00165       /// target_ is the scene cloud
00166       using Registration<PointSource, PointTarget>::target_;
00167       using Registration<PointSource, PointTarget>::converged_;
00168       using Registration<PointSource, PointTarget>::final_transformation_;
00169       using Registration<PointSource, PointTarget>::transformation_;
00170 
00171       typedef pcl::PointCloud<PointSource> PointCloudSource;
00172       typedef typename PointCloudSource::Ptr PointCloudSourcePtr;
00173       typedef typename PointCloudSource::ConstPtr PointCloudSourceConstPtr;
00174 
00175       typedef pcl::PointCloud<PointTarget> PointCloudTarget;
00176       typedef typename PointCloudTarget::Ptr PointCloudTargetPtr;
00177       typedef typename PointCloudTarget::ConstPtr PointCloudTargetConstPtr;
00178 
00179 
00180       /** \brief Empty constructor that initializes all the parameters of the algorithm with default values */
00181       PPFRegistration ()
00182       :  Registration<PointSource, PointTarget> (),
00183          search_method_ (),
00184          scene_reference_point_sampling_rate_ (5),
00185          clustering_position_diff_threshold_ (0.01f),
00186          clustering_rotation_diff_threshold_ (20.0f / 180.0f * static_cast<float> (M_PI))
00187       {}
00188 
00189       /** \brief Method for setting the position difference clustering parameter
00190        * \param clustering_position_diff_threshold distance threshold below which two poses are
00191        * considered close enough to be in the same cluster (for the clustering phase of the algorithm)
00192        */
00193       inline void
00194       setPositionClusteringThreshold (float clustering_position_diff_threshold) { clustering_position_diff_threshold_ = clustering_position_diff_threshold; }
00195 
00196       /** \brief Returns the parameter defining the position difference clustering parameter -
00197        * distance threshold below which two poses are considered close enough to be in the same cluster
00198        * (for the clustering phase of the algorithm)
00199        */
00200       inline float
00201       getPositionClusteringThreshold () { return clustering_position_diff_threshold_; }
00202 
00203       /** \brief Method for setting the rotation clustering parameter
00204        * \param clustering_rotation_diff_threshold rotation difference threshold below which two
00205        * poses are considered to be in the same cluster (for the clustering phase of the algorithm)
00206        */
00207       inline void
00208       setRotationClusteringThreshold (float clustering_rotation_diff_threshold) { clustering_rotation_diff_threshold_ = clustering_rotation_diff_threshold; }
00209 
00210       /** \brief Returns the parameter defining the rotation clustering threshold
00211        */
00212       inline float
00213       getRotationClusteringThreshold () { return clustering_rotation_diff_threshold_; }
00214 
00215       /** \brief Method for setting the scene reference point sampling rate
00216        * \param scene_reference_point_sampling_rate sampling rate for the scene reference point
00217        */
00218       inline void
00219       setSceneReferencePointSamplingRate (unsigned int scene_reference_point_sampling_rate) { scene_reference_point_sampling_rate_ = scene_reference_point_sampling_rate; }
00220 
00221       /** \brief Returns the parameter for the scene reference point sampling rate of the algorithm */
00222       inline unsigned int
00223       getSceneReferencePointSamplingRate () { return scene_reference_point_sampling_rate_; }
00224 
00225       /** \brief Function that sets the search method for the algorithm
00226        * \note Right now, the only available method is the one initially proposed by
00227        * the authors - by using a hash map with discretized feature vectors
00228        * \param search_method smart pointer to the search method to be set
00229        */
00230       inline void
00231       setSearchMethod (PPFHashMapSearch::Ptr search_method) { search_method_ = search_method; }
00232 
00233       /** \brief Getter function for the search method of the class */
00234       inline PPFHashMapSearch::Ptr
00235       getSearchMethod () { return search_method_; }
00236 
00237       /** \brief Provide a pointer to the input target (e.g., the point cloud that we want to align the input source to)
00238        * \param cloud the input point cloud target
00239        */
00240       void
00241       setInputTarget (const PointCloudTargetConstPtr &cloud);
00242 
00243 
00244     private:
00245       /** \brief Method that calculates the transformation between the input_ and target_ point clouds, based on the PPF features */
00246       void
00247       computeTransformation (PointCloudSource &output, const Eigen::Matrix4f& guess);
00248 
00249 
00250       /** \brief the search method that is going to be used to find matching feature pairs */
00251       PPFHashMapSearch::Ptr search_method_;
00252 
00253       /** \brief parameter for the sampling rate of the scene reference points */
00254       unsigned int scene_reference_point_sampling_rate_;
00255 
00256       /** \brief position and rotation difference thresholds below which two
00257         * poses are considered to be in the same cluster (for the clustering phase of the algorithm) */
00258       float clustering_position_diff_threshold_, clustering_rotation_diff_threshold_;
00259 
00260       /** \brief use a kd-tree with range searches of range max_dist to skip an O(N) pass through the point cloud */
00261       typename pcl::KdTreeFLANN<PointTarget>::Ptr scene_search_tree_;
00262 
00263       /** \brief static method used for the std::sort function to order two PoseWithVotes
00264        * instances by their number of votes*/
00265       static bool
00266       poseWithVotesCompareFunction (const PoseWithVotes &a,
00267                                     const PoseWithVotes &b);
00268 
00269       /** \brief static method used for the std::sort function to order two pairs <index, votes>
00270        * by the number of votes (unsigned integer value) */
00271       static bool
00272       clusterVotesCompareFunction (const std::pair<size_t, unsigned int> &a,
00273                                    const std::pair<size_t, unsigned int> &b);
00274 
00275       /** \brief Method that clusters a set of given poses by using the clustering thresholds
00276        * and their corresponding number of votes (see publication for more details) */
00277       void
00278       clusterPoses (PoseWithVotesList &poses,
00279                     PoseWithVotesList &result);
00280 
00281       /** \brief Method that checks whether two poses are close together - based on the clustering threshold parameters
00282        * of the class */
00283       bool
00284       posesWithinErrorBounds (Eigen::Affine3f &pose1,
00285                               Eigen::Affine3f &pose2);
00286   };
00287 }
00288 
00289 #include <pcl/registration/impl/ppf_registration.hpp>
00290 
00291 #endif // PCL_PPF_REGISTRATION_H_