Point Cloud Library (PCL)  1.7.0
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_