Point Cloud Library (PCL)  1.3.1
ppf_registration.h
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: ppf_registration.h 3042 2011-11-01 04:44:47Z svn $
00036  */
00037 
00038 
00039 #ifndef PCL_PPF_REGISTRATION_H_
00040 #define PCL_PPF_REGISTRATION_H_
00041 
00042 #include "pcl/registration/registration.h"
00043 #include <pcl/features/ppf.h>
00044 #include <boost/unordered_map.hpp>
00045 
00046 namespace pcl
00047 {
00048   class PCL_EXPORTS PPFHashMapSearch
00049   {
00050     public:
00056       struct HashKeyStruct : public std::pair <int, std::pair <int, std::pair <int, int> > >
00057       {
00058         HashKeyStruct(int a, int b, int c, int d)
00059         {
00060           this->first = a;
00061           this->second.first = b;
00062           this->second.second.first = c;
00063           this->second.second.second = d;
00064         }
00065       };
00066       typedef boost::unordered_multimap<HashKeyStruct, std::pair<size_t, size_t> > FeatureHashMapType;
00067       typedef boost::shared_ptr<FeatureHashMapType> FeatureHashMapTypePtr;
00068       typedef boost::shared_ptr<PPFHashMapSearch> Ptr;
00069 
00070 
00075       PPFHashMapSearch (float angle_discretization_step = 12.0 / 180 * M_PI,
00076                         float distance_discretization_step = 0.01)
00077       :  angle_discretization_step_ (angle_discretization_step),
00078          distance_discretization_step_ (distance_discretization_step)
00079       {
00080         feature_hash_map_ = FeatureHashMapTypePtr (new FeatureHashMapType);
00081         internals_initialized_ = false;
00082         max_dist_ = -1.0;
00083       }
00084 
00088       void
00089       setInputFeatureCloud (PointCloud<PPFSignature>::ConstPtr feature_cloud);
00090 
00099       void
00100       nearestNeighborSearch (float &f1, float &f2, float &f3, float &f4,
00101                              std::vector<std::pair<size_t, size_t> > &indices);
00102 
00104       Ptr
00105       makeShared() { return Ptr (new PPFHashMapSearch (*this)); }
00106 
00108       inline float
00109       getAngleDiscretizationStep () { return angle_discretization_step_; }
00110 
00112       inline float
00113       getDistanceDiscretizationStep () { return distance_discretization_step_; }
00114 
00116       inline float
00117       getModelDiameter () { return max_dist_; }
00118 
00119       std::vector <std::vector <float> > alpha_m_;
00120     private:
00121       FeatureHashMapTypePtr feature_hash_map_;
00122       bool internals_initialized_;
00123 
00124       float angle_discretization_step_, distance_discretization_step_;
00125       float max_dist_;
00126   };
00127 
00139   template <typename PointSource, typename PointTarget>
00140   class PPFRegistration : public Registration<PointSource, PointTarget>
00141   {
00142     public:
00147       struct PoseWithVotes
00148       {
00149         PoseWithVotes(Eigen::Affine3f &a_pose, unsigned int &a_votes)
00150         : pose (a_pose),
00151           votes (a_votes)
00152         {}
00153 
00154         Eigen::Affine3f pose;
00155         unsigned int votes;
00156       };
00157       typedef std::vector<PoseWithVotes, Eigen::aligned_allocator<PoseWithVotes> > PoseWithVotesList;
00158 
00160       using Registration<PointSource, PointTarget>::input_;
00162       using Registration<PointSource, PointTarget>::target_;
00163       using Registration<PointSource, PointTarget>::converged_;
00164       using Registration<PointSource, PointTarget>::final_transformation_;
00165       using Registration<PointSource, PointTarget>::transformation_;
00166 
00167       typedef pcl::PointCloud<PointSource> PointCloudSource;
00168       typedef typename PointCloudSource::Ptr PointCloudSourcePtr;
00169       typedef typename PointCloudSource::ConstPtr PointCloudSourceConstPtr;
00170 
00171       typedef pcl::PointCloud<PointTarget> PointCloudTarget;
00172       typedef typename PointCloudTarget::Ptr PointCloudTargetPtr;
00173       typedef typename PointCloudTarget::ConstPtr PointCloudTargetConstPtr;
00174 
00175 
00177       PPFRegistration ()
00178       :  Registration<PointSource, PointTarget> (),
00179          search_method_ (),
00180          scene_reference_point_sampling_rate_ (5),
00181          clustering_position_diff_threshold_ (0.01),
00182          clustering_rotation_diff_threshold_ (20.0 / 180 * M_PI)
00183       {}
00184 
00189       inline void
00190       setPositionClusteringThreshold (float clustering_position_diff_threshold) { clustering_position_diff_threshold_ = clustering_position_diff_threshold; }
00191 
00196       inline float
00197       getPositionClusteringThreshold () { return clustering_position_diff_threshold_; }
00198 
00203       inline void
00204       setRotationClusteringThreshold (float clustering_rotation_diff_threshold) { clustering_rotation_diff_threshold_ = clustering_rotation_diff_threshold; }
00205 
00208       inline float
00209       getRotationClusteringThreshold () { return clustering_rotation_diff_threshold_; }
00210 
00214       inline void
00215       setSceneReferencePointSamplingRate (unsigned int scene_reference_point_sampling_rate) { scene_reference_point_sampling_rate_ = scene_reference_point_sampling_rate; }
00216 
00218       inline unsigned int
00219       getSceneReferencePointSamplingRate () { return scene_reference_point_sampling_rate_; }
00220 
00226       inline void
00227       setSearchMethod (PPFHashMapSearch::Ptr search_method) { search_method_ = search_method; }
00228 
00230       inline PPFHashMapSearch::Ptr
00231       getSearchMethod () { return search_method_; }
00232 
00236       void
00237       setInputTarget (const PointCloudTargetConstPtr &cloud);
00238 
00239 
00240     private:
00242       void
00243       computeTransformation (PointCloudSource &output);
00244 
00245 
00247       PPFHashMapSearch::Ptr search_method_;
00248 
00250       unsigned int scene_reference_point_sampling_rate_;
00251 
00254       float clustering_position_diff_threshold_, clustering_rotation_diff_threshold_;
00255 
00257       typename pcl::KdTreeFLANN<PointTarget>::Ptr scene_search_tree_;
00258 
00261       static bool
00262       poseWithVotesCompareFunction (const PoseWithVotes &a,
00263                                     const PoseWithVotes &b);
00264 
00267       static bool
00268       clusterVotesCompareFunction (const std::pair<size_t, unsigned int> &a,
00269                                    const std::pair<size_t, unsigned int> &b);
00270 
00273       void
00274       clusterPoses (PoseWithVotesList &poses,
00275                     PoseWithVotesList &result);
00276 
00279       bool
00280       posesWithinErrorBounds (Eigen::Affine3f &pose1,
00281                               Eigen::Affine3f &pose2);
00282   };
00283 }
00284 
00285 #include "pcl/registration/impl/ppf_registration.hpp"
00286 
00287 #endif // PCL_PPF_REGISTRATION_H_
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Defines