Point Cloud Library (PCL)  1.7.1
ppf_registration.h
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2011, Alexandru-Eugen Ichim
5  * Willow Garage, Inc
6  * Copyright (c) 2012-, Open Perception, Inc.
7  *
8  * All rights reserved.
9  *
10  * Redistribution and use in source and binary forms, with or without
11  * modification, are permitted provided that the following conditions
12  * are met:
13  *
14  * * Redistributions of source code must retain the above copyright
15  * notice, this list of conditions and the following disclaimer.
16  * * Redistributions in binary form must reproduce the above
17  * copyright notice, this list of conditions and the following
18  * disclaimer in the documentation and/or other materials provided
19  * with the distribution.
20  * * Neither the name of the copyright holder(s) nor the names of its
21  * contributors may be used to endorse or promote products derived
22  * from this software without specific prior written permission.
23  *
24  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
25  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
26  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
27  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
28  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
29  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
30  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
31  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
32  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
33  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
34  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
35  * POSSIBILITY OF SUCH DAMAGE.
36  *
37  * $Id$
38  *
39  */
40 
41 
42 #ifndef PCL_PPF_REGISTRATION_H_
43 #define PCL_PPF_REGISTRATION_H_
44 
45 #include <pcl/registration/boost.h>
46 #include <pcl/registration/registration.h>
47 #include <pcl/features/ppf.h>
48 
49 namespace pcl
50 {
51  class PCL_EXPORTS PPFHashMapSearch
52  {
53  public:
54  /** \brief Data structure to hold the information for the key in the feature hash map of the
55  * PPFHashMapSearch class
56  * \note It uses multiple pair levels in order to enable the usage of the boost::hash function
57  * which has the std::pair implementation (i.e., does not require a custom hash function)
58  */
59  struct HashKeyStruct : public std::pair <int, std::pair <int, std::pair <int, int> > >
60  {
61  HashKeyStruct(int a, int b, int c, int d)
62  {
63  this->first = a;
64  this->second.first = b;
65  this->second.second.first = c;
66  this->second.second.second = d;
67  }
68  };
69  typedef boost::unordered_multimap<HashKeyStruct, std::pair<size_t, size_t> > FeatureHashMapType;
70  typedef boost::shared_ptr<FeatureHashMapType> FeatureHashMapTypePtr;
71  typedef boost::shared_ptr<PPFHashMapSearch> Ptr;
72 
73 
74  /** \brief Constructor for the PPFHashMapSearch class which sets the two step parameters for the enclosed data structure
75  * \param angle_discretization_step the step value between each bin of the hash map for the angular values
76  * \param distance_discretization_step the step value between each bin of the hash map for the distance values
77  */
78  PPFHashMapSearch (float angle_discretization_step = 12.0f / 180.0f * static_cast<float> (M_PI),
79  float distance_discretization_step = 0.01f)
80  : alpha_m_ ()
81  , feature_hash_map_ (new FeatureHashMapType)
82  , internals_initialized_ (false)
83  , angle_discretization_step_ (angle_discretization_step)
84  , distance_discretization_step_ (distance_discretization_step)
85  , max_dist_ (-1.0f)
86  {
87  }
88 
89  /** \brief Method that sets the feature cloud to be inserted in the hash map
90  * \param feature_cloud a const smart pointer to the PPFSignature feature cloud
91  */
92  void
93  setInputFeatureCloud (PointCloud<PPFSignature>::ConstPtr feature_cloud);
94 
95  /** \brief Function for finding the nearest neighbors for the given feature inside the discretized hash map
96  * \param f1 The 1st value describing the query PPFSignature feature
97  * \param f2 The 2nd value describing the query PPFSignature feature
98  * \param f3 The 3rd value describing the query PPFSignature feature
99  * \param f4 The 4th value describing the query PPFSignature feature
100  * \param indices a vector of pair indices representing the feature pairs that have been found in the bin
101  * corresponding to the query feature
102  */
103  void
104  nearestNeighborSearch (float &f1, float &f2, float &f3, float &f4,
105  std::vector<std::pair<size_t, size_t> > &indices);
106 
107  /** \brief Convenience method for returning a copy of the class instance as a boost::shared_ptr */
108  Ptr
109  makeShared() { return Ptr (new PPFHashMapSearch (*this)); }
110 
111  /** \brief Returns the angle discretization step parameter (the step value between each bin of the hash map for the angular values) */
112  inline float
113  getAngleDiscretizationStep () { return angle_discretization_step_; }
114 
115  /** \brief Returns the distance discretization step parameter (the step value between each bin of the hash map for the distance values) */
116  inline float
117  getDistanceDiscretizationStep () { return distance_discretization_step_; }
118 
119  /** \brief Returns the maximum distance found between any feature pair in the given input feature cloud */
120  inline float
121  getModelDiameter () { return max_dist_; }
122 
123  std::vector <std::vector <float> > alpha_m_;
124  private:
125  FeatureHashMapTypePtr feature_hash_map_;
126  bool internals_initialized_;
127 
128  float angle_discretization_step_, distance_discretization_step_;
129  float max_dist_;
130  };
131 
132  /** \brief Class that registers two point clouds based on their sets of PPFSignatures.
133  * Please refer to the following publication for more details:
134  * B. Drost, M. Ulrich, N. Navab, S. Ilic
135  * Model Globally, Match Locally: Efficient and Robust 3D Object Recognition
136  * 2010 IEEE Conference on Computer Vision and Pattern Recognition (CVPR)
137  * 13-18 June 2010, San Francisco, CA
138  *
139  * \note This class works in tandem with the PPFEstimation class
140  *
141  * \author Alexandru-Eugen Ichim
142  */
143  template <typename PointSource, typename PointTarget>
144  class PPFRegistration : public Registration<PointSource, PointTarget>
145  {
146  public:
147  /** \brief Structure for storing a pose (represented as an Eigen::Affine3f) and an integer for counting votes
148  * \note initially used std::pair<Eigen::Affine3f, unsigned int>, but it proved problematic
149  * because of the Eigen structures alignment problems - std::pair does not have a custom allocator
150  */
152  {
153  PoseWithVotes(Eigen::Affine3f &a_pose, unsigned int &a_votes)
154  : pose (a_pose),
155  votes (a_votes)
156  {}
157 
158  Eigen::Affine3f pose;
159  unsigned int votes;
160  };
161  typedef std::vector<PoseWithVotes, Eigen::aligned_allocator<PoseWithVotes> > PoseWithVotesList;
162 
163  /// input_ is the model cloud
165  /// target_ is the scene cloud
170 
174 
178 
179 
180  /** \brief Empty constructor that initializes all the parameters of the algorithm with default values */
182  : Registration<PointSource, PointTarget> (),
183  search_method_ (),
184  scene_reference_point_sampling_rate_ (5),
185  clustering_position_diff_threshold_ (0.01f),
186  clustering_rotation_diff_threshold_ (20.0f / 180.0f * static_cast<float> (M_PI))
187  {}
188 
189  /** \brief Method for setting the position difference clustering parameter
190  * \param clustering_position_diff_threshold distance threshold below which two poses are
191  * considered close enough to be in the same cluster (for the clustering phase of the algorithm)
192  */
193  inline void
194  setPositionClusteringThreshold (float clustering_position_diff_threshold) { clustering_position_diff_threshold_ = clustering_position_diff_threshold; }
195 
196  /** \brief Returns the parameter defining the position difference clustering parameter -
197  * distance threshold below which two poses are considered close enough to be in the same cluster
198  * (for the clustering phase of the algorithm)
199  */
200  inline float
201  getPositionClusteringThreshold () { return clustering_position_diff_threshold_; }
202 
203  /** \brief Method for setting the rotation clustering parameter
204  * \param clustering_rotation_diff_threshold rotation difference threshold below which two
205  * poses are considered to be in the same cluster (for the clustering phase of the algorithm)
206  */
207  inline void
208  setRotationClusteringThreshold (float clustering_rotation_diff_threshold) { clustering_rotation_diff_threshold_ = clustering_rotation_diff_threshold; }
209 
210  /** \brief Returns the parameter defining the rotation clustering threshold
211  */
212  inline float
213  getRotationClusteringThreshold () { return clustering_rotation_diff_threshold_; }
214 
215  /** \brief Method for setting the scene reference point sampling rate
216  * \param scene_reference_point_sampling_rate sampling rate for the scene reference point
217  */
218  inline void
219  setSceneReferencePointSamplingRate (unsigned int scene_reference_point_sampling_rate) { scene_reference_point_sampling_rate_ = scene_reference_point_sampling_rate; }
220 
221  /** \brief Returns the parameter for the scene reference point sampling rate of the algorithm */
222  inline unsigned int
223  getSceneReferencePointSamplingRate () { return scene_reference_point_sampling_rate_; }
224 
225  /** \brief Function that sets the search method for the algorithm
226  * \note Right now, the only available method is the one initially proposed by
227  * the authors - by using a hash map with discretized feature vectors
228  * \param search_method smart pointer to the search method to be set
229  */
230  inline void
231  setSearchMethod (PPFHashMapSearch::Ptr search_method) { search_method_ = search_method; }
232 
233  /** \brief Getter function for the search method of the class */
234  inline PPFHashMapSearch::Ptr
235  getSearchMethod () { return search_method_; }
236 
237  /** \brief Provide a pointer to the input target (e.g., the point cloud that we want to align the input source to)
238  * \param cloud the input point cloud target
239  */
240  void
242 
243 
244  private:
245  /** \brief Method that calculates the transformation between the input_ and target_ point clouds, based on the PPF features */
246  void
247  computeTransformation (PointCloudSource &output, const Eigen::Matrix4f& guess);
248 
249 
250  /** \brief the search method that is going to be used to find matching feature pairs */
251  PPFHashMapSearch::Ptr search_method_;
252 
253  /** \brief parameter for the sampling rate of the scene reference points */
254  unsigned int scene_reference_point_sampling_rate_;
255 
256  /** \brief position and rotation difference thresholds below which two
257  * poses are considered to be in the same cluster (for the clustering phase of the algorithm) */
258  float clustering_position_diff_threshold_, clustering_rotation_diff_threshold_;
259 
260  /** \brief use a kd-tree with range searches of range max_dist to skip an O(N) pass through the point cloud */
261  typename pcl::KdTreeFLANN<PointTarget>::Ptr scene_search_tree_;
262 
263  /** \brief static method used for the std::sort function to order two PoseWithVotes
264  * instances by their number of votes*/
265  static bool
266  poseWithVotesCompareFunction (const PoseWithVotes &a,
267  const PoseWithVotes &b);
268 
269  /** \brief static method used for the std::sort function to order two pairs <index, votes>
270  * by the number of votes (unsigned integer value) */
271  static bool
272  clusterVotesCompareFunction (const std::pair<size_t, unsigned int> &a,
273  const std::pair<size_t, unsigned int> &b);
274 
275  /** \brief Method that clusters a set of given poses by using the clustering thresholds
276  * and their corresponding number of votes (see publication for more details) */
277  void
278  clusterPoses (PoseWithVotesList &poses,
279  PoseWithVotesList &result);
280 
281  /** \brief Method that checks whether two poses are close together - based on the clustering threshold parameters
282  * of the class */
283  bool
284  posesWithinErrorBounds (Eigen::Affine3f &pose1,
285  Eigen::Affine3f &pose2);
286  };
287 }
288 
289 #include <pcl/registration/impl/ppf_registration.hpp>
290 
291 #endif // PCL_PPF_REGISTRATION_H_
PointCloudTarget::Ptr PointCloudTargetPtr
Registration represents the base registration class for general purpose, ICP-like methods...
Definition: registration.h:62
void setSearchMethod(PPFHashMapSearch::Ptr search_method)
Function that sets the search method for the algorithm.
PPFHashMapSearch::Ptr getSearchMethod()
Getter function for the search method of the class.
boost::shared_ptr< const PointCloud< PointT > > ConstPtr
Definition: point_cloud.h:429
float getAngleDiscretizationStep()
Returns the angle discretization step parameter (the step value between each bin of the hash map for ...
std::vector< std::vector< float > > alpha_m_
void setSceneReferencePointSamplingRate(unsigned int scene_reference_point_sampling_rate)
Method for setting the scene reference point sampling rate.
boost::shared_ptr< PointCloud< PointT > > Ptr
Definition: point_cloud.h:428
PPFHashMapSearch(float angle_discretization_step=12.0f/180.0f *static_cast< float >(M_PI), float distance_discretization_step=0.01f)
Constructor for the PPFHashMapSearch class which sets the two step parameters for the enclosed data s...
std::vector< PoseWithVotes, Eigen::aligned_allocator< PoseWithVotes > > PoseWithVotesList
boost::shared_ptr< KdTreeFLANN< PointT > > Ptr
Definition: kdtree_flann.h:87
float getDistanceDiscretizationStep()
Returns the distance discretization step parameter (the step value between each bin of the hash map f...
pcl::PointCloud< PointSource > PointCloudSource
float getModelDiameter()
Returns the maximum distance found between any feature pair in the given input feature cloud...
void setInputTarget(const PointCloudTargetConstPtr &cloud)
Provide a pointer to the input target (e.g., the point cloud that we want to align the input source t...
PPFRegistration()
Empty constructor that initializes all the parameters of the algorithm with default values...
PoseWithVotes(Eigen::Affine3f &a_pose, unsigned int &a_votes)
Ptr makeShared()
Convenience method for returning a copy of the class instance as a boost::shared_ptr.
float getRotationClusteringThreshold()
Returns the parameter defining the rotation clustering threshold.
unsigned int getSceneReferencePointSamplingRate()
Returns the parameter for the scene reference point sampling rate of the algorithm.
boost::unordered_multimap< HashKeyStruct, std::pair< size_t, size_t > > FeatureHashMapType
PointCloudSource::ConstPtr PointCloudSourceConstPtr
pcl::PointCloud< PointTarget > PointCloudTarget
PointCloudTarget::ConstPtr PointCloudTargetConstPtr
Structure for storing a pose (represented as an Eigen::Affine3f) and an integer for counting votes...
Class that registers two point clouds based on their sets of PPFSignatures.
float getPositionClusteringThreshold()
Returns the parameter defining the position difference clustering parameter - distance threshold belo...
boost::shared_ptr< PPFHashMapSearch > Ptr
Data structure to hold the information for the key in the feature hash map of the PPFHashMapSearch cl...
void setRotationClusteringThreshold(float clustering_rotation_diff_threshold)
Method for setting the rotation clustering parameter.
PointCloudSource::Ptr PointCloudSourcePtr
void setPositionClusteringThreshold(float clustering_position_diff_threshold)
Method for setting the position difference clustering parameter.
HashKeyStruct(int a, int b, int c, int d)
boost::shared_ptr< FeatureHashMapType > FeatureHashMapTypePtr