Point Cloud Library (PCL)  1.14.1-dev
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 #pragma once
42 
43 #include <pcl/features/ppf.h>
44 #include <pcl/registration/registration.h>
45 
46 #include <unordered_map>
47 
48 namespace pcl {
50 public:
51  /** \brief Data structure to hold the information for the key in the feature hash map
52  * of the PPFHashMapSearch class \note It uses multiple pair levels in order to enable
53  * the usage of the boost::hash function which has the std::pair implementation (i.e.,
54  * does not require a custom hash function)
55  */
56  struct HashKeyStruct : public std::pair<int, std::pair<int, std::pair<int, int>>> {
57  HashKeyStruct() = default;
58 
59  HashKeyStruct(int a, int b, int c, int d)
60  {
61  this->first = a;
62  this->second.first = b;
63  this->second.second.first = c;
64  this->second.second.second = d;
65  }
66 
67  std::size_t
68  operator()(const HashKeyStruct& s) const noexcept
69  {
70  const std::size_t h1 = std::hash<int>{}(s.first);
71  const std::size_t h2 = std::hash<int>{}(s.second.first);
72  const std::size_t h3 = std::hash<int>{}(s.second.second.first);
73  const std::size_t h4 = std::hash<int>{}(s.second.second.second);
74  return h1 ^ (h2 << 1) ^ (h3 << 2) ^ (h4 << 3);
75  }
76  };
78  std::unordered_multimap<HashKeyStruct,
79  std::pair<std::size_t, std::size_t>,
81  using FeatureHashMapTypePtr = shared_ptr<FeatureHashMapType>;
82  using Ptr = shared_ptr<PPFHashMapSearch>;
83  using ConstPtr = shared_ptr<const PPFHashMapSearch>;
84 
85  /** \brief Constructor for the PPFHashMapSearch class which sets the two step
86  * parameters for the enclosed data structure \param angle_discretization_step the
87  * step value between each bin of the hash map for the angular values \param
88  * distance_discretization_step the step value between each bin of the hash map for
89  * the distance values
90  */
91  PPFHashMapSearch(float angle_discretization_step = 12.0f / 180.0f *
92  static_cast<float>(M_PI),
93  float distance_discretization_step = 0.01f)
94  : feature_hash_map_(new FeatureHashMapType)
95  , angle_discretization_step_(angle_discretization_step)
96  , distance_discretization_step_(distance_discretization_step)
97  {}
98 
99  /** \brief Method that sets the feature cloud to be inserted in the hash map
100  * \param feature_cloud a const smart pointer to the PPFSignature feature cloud
101  */
102  void
104 
105  /** \brief Function for finding the nearest neighbors for the given feature inside the
106  * discretized hash map \param f1 The 1st value describing the query PPFSignature
107  * feature \param f2 The 2nd value describing the query PPFSignature feature \param f3
108  * The 3rd value describing the query PPFSignature feature \param f4 The 4th value
109  * describing the query PPFSignature feature \param indices a vector of pair indices
110  * representing the feature pairs that have been found in the bin corresponding to the
111  * query feature
112  */
113  void
115  float& f2,
116  float& f3,
117  float& f4,
118  std::vector<std::pair<std::size_t, std::size_t>>& indices);
119 
120  /** \brief Convenience method for returning a copy of the class instance as a
121  * shared_ptr */
122  Ptr
124  {
125  return Ptr(new PPFHashMapSearch(*this));
126  }
127 
128  /** \brief Returns the angle discretization step parameter (the step value between
129  * each bin of the hash map for the angular values) */
130  inline float
132  {
133  return angle_discretization_step_;
134  }
135 
136  /** \brief Returns the distance discretization step parameter (the step value between
137  * each bin of the hash map for the distance values) */
138  inline float
140  {
141  return distance_discretization_step_;
142  }
143 
144  /** \brief Returns the maximum distance found between any feature pair in the given
145  * input feature cloud */
146  inline float
148  {
149  return max_dist_;
150  }
151 
152  std::vector<std::vector<float>> alpha_m_;
153 
154 private:
155  FeatureHashMapTypePtr feature_hash_map_;
156  bool internals_initialized_{false};
157 
158  float angle_discretization_step_, distance_discretization_step_;
159  float max_dist_{-1.0f};
160 };
161 
162 /** \brief Class that registers two point clouds based on their sets of PPFSignatures.
163  * Please refer to the following publication for more details:
164  * B. Drost, M. Ulrich, N. Navab, S. Ilic
165  * Model Globally, Match Locally: Efficient and Robust 3D Object Recognition
166  * 2010 IEEE Conference on Computer Vision and Pattern Recognition (CVPR)
167  * 13-18 June 2010, San Francisco, CA
168  *
169  * \note This class works in tandem with the PPFEstimation class
170  * \ingroup registration
171  *
172  * \author Alexandru-Eugen Ichim
173  */
174 template <typename PointSource, typename PointTarget>
175 class PPFRegistration : public Registration<PointSource, PointTarget> {
176 public:
177  /** \brief Structure for storing a pose (represented as an Eigen::Affine3f) and an
178  * integer for counting votes \note initially used std::pair<Eigen::Affine3f, unsigned
179  * int>, but it proved problematic because of the Eigen structures alignment problems
180  * - std::pair does not have a custom allocator
181  */
182  struct PoseWithVotes {
183  PoseWithVotes(const Eigen::Affine3f& a_pose, unsigned int& a_votes)
184  : pose(a_pose), votes(a_votes)
185  {}
186 
187  Eigen::Affine3f pose;
188  unsigned int votes;
189  };
191  std::vector<PoseWithVotes, Eigen::aligned_allocator<PoseWithVotes>>;
192 
193  /// input_ is the model cloud
195  /// target_ is the scene cloud
200 
204 
208 
209  /** \brief Empty constructor that initializes all the parameters of the algorithm with
210  * default values */
212  : Registration<PointSource, PointTarget>()
213  , clustering_rotation_diff_threshold_(20.0f / 180.0f * static_cast<float>(M_PI))
214  {}
215 
216  /** \brief Method for setting the position difference clustering parameter
217  * \param clustering_position_diff_threshold distance threshold below which two poses
218  * are considered close enough to be in the same cluster (for the clustering phase of
219  * the algorithm)
220  */
221  inline void
222  setPositionClusteringThreshold(float clustering_position_diff_threshold)
223  {
224  clustering_position_diff_threshold_ = clustering_position_diff_threshold;
225  }
226 
227  /** \brief Returns the parameter defining the position difference clustering parameter
228  * - distance threshold below which two poses are considered close enough to be in the
229  * same cluster (for the clustering phase of the algorithm)
230  */
231  inline float
233  {
234  return clustering_position_diff_threshold_;
235  }
236 
237  /** \brief Method for setting the rotation clustering parameter
238  * \param clustering_rotation_diff_threshold rotation difference threshold below which
239  * two poses are considered to be in the same cluster (for the clustering phase of the
240  * algorithm)
241  */
242  inline void
243  setRotationClusteringThreshold(float clustering_rotation_diff_threshold)
244  {
245  clustering_rotation_diff_threshold_ = clustering_rotation_diff_threshold;
246  }
247 
248  /** \brief Returns the parameter defining the rotation clustering threshold
249  */
250  inline float
252  {
253  return clustering_rotation_diff_threshold_;
254  }
255 
256  /** \brief Method for setting the scene reference point sampling rate
257  * \param scene_reference_point_sampling_rate sampling rate for the scene reference
258  * point
259  */
260  inline void
261  setSceneReferencePointSamplingRate(unsigned int scene_reference_point_sampling_rate)
262  {
263  scene_reference_point_sampling_rate_ = scene_reference_point_sampling_rate;
264  }
265 
266  /** \brief Returns the parameter for the scene reference point sampling rate of the
267  * algorithm */
268  inline unsigned int
270  {
271  return scene_reference_point_sampling_rate_;
272  }
273 
274  /** \brief Function that sets the search method for the algorithm
275  * \note Right now, the only available method is the one initially proposed by
276  * the authors - by using a hash map with discretized feature vectors
277  * \param search_method smart pointer to the search method to be set
278  */
279  inline void
281  {
282  search_method_ = search_method;
283  }
284 
285  /** \brief Getter function for the search method of the class */
286  inline PPFHashMapSearch::Ptr
288  {
289  return search_method_;
290  }
291 
292  /** \brief Provide a pointer to the input target (e.g., the point cloud that we want
293  * to align the input source to) \param cloud the input point cloud target
294  */
295  void
296  setInputTarget(const PointCloudTargetConstPtr& cloud) override;
297 
298  /** \brief Returns the most promising pose candidates, after clustering. The pose with
299  * the most votes is the result of the registration. It may make sense to check the
300  * next best pose candidates if the registration did not give the right result, or if
301  * there are more than one correct results. You need to call the align function before
302  * this one.
303  */
304  inline PoseWithVotesList
306  {
307  return best_pose_candidates;
308  }
309 
310 private:
311  /** \brief Method that calculates the transformation between the input_ and target_
312  * point clouds, based on the PPF features */
313  void
314  computeTransformation(PointCloudSource& output,
315  const Eigen::Matrix4f& guess) override;
316 
317  /** \brief the search method that is going to be used to find matching feature pairs
318  */
319  PPFHashMapSearch::Ptr search_method_;
320 
321  /** \brief parameter for the sampling rate of the scene reference points */
322  uindex_t scene_reference_point_sampling_rate_{5};
323 
324  /** \brief position and rotation difference thresholds below which two
325  * poses are considered to be in the same cluster (for the clustering phase of the
326  * algorithm) */
327  float clustering_position_diff_threshold_{0.01f}, clustering_rotation_diff_threshold_;
328 
329  /** \brief use a kd-tree with range searches of range max_dist to skip an O(N) pass
330  * through the point cloud */
331  typename pcl::KdTreeFLANN<PointTarget>::Ptr scene_search_tree_;
332 
333  /** \brief List with the most promising pose candidates, after clustering. The pose
334  * with the most votes is returned as the registration result. */
335  PoseWithVotesList best_pose_candidates;
336 
337  /** \brief static method used for the std::sort function to order two PoseWithVotes
338  * instances by their number of votes*/
339  static bool
340  poseWithVotesCompareFunction(const PoseWithVotes& a, const PoseWithVotes& b);
341 
342  /** \brief static method used for the std::sort function to order two pairs <index,
343  * votes> by the number of votes (unsigned integer value) */
344  static bool
345  clusterVotesCompareFunction(const std::pair<std::size_t, unsigned int>& a,
346  const std::pair<std::size_t, unsigned int>& b);
347 
348  /** \brief Method that clusters a set of given poses by using the clustering
349  * thresholds and their corresponding number of votes (see publication for more
350  * details) */
351  void
352  clusterPoses(PoseWithVotesList& poses, PoseWithVotesList& result);
353 
354  /** \brief Method that checks whether two poses are close together - based on the
355  * clustering threshold parameters of the class */
356  bool
357  posesWithinErrorBounds(Eigen::Affine3f& pose1,
358  Eigen::Affine3f& pose2,
359  float& position_diff,
360  float& rotation_diff_angle);
361 };
362 } // namespace pcl
363 
364 #include <pcl/registration/impl/ppf_registration.hpp>
shared_ptr< KdTreeFLANN< PointT, Dist > > Ptr
Definition: kdtree_flann.h:151
float getAngleDiscretizationStep() const
Returns the angle discretization step parameter (the step value between each bin of the hash map for ...
std::vector< std::vector< float > > alpha_m_
shared_ptr< FeatureHashMapType > FeatureHashMapTypePtr
std::unordered_multimap< HashKeyStruct, std::pair< std::size_t, std::size_t >, HashKeyStruct > FeatureHashMapType
shared_ptr< PPFHashMapSearch > Ptr
shared_ptr< const PPFHashMapSearch > ConstPtr
void nearestNeighborSearch(float &f1, float &f2, float &f3, float &f4, std::vector< std::pair< std::size_t, std::size_t >> &indices)
Function for finding the nearest neighbors for the given feature inside the discretized hash map.
Ptr makeShared()
Convenience method for returning a copy of the class instance as a shared_ptr.
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...
float getDistanceDiscretizationStep() const
Returns the distance discretization step parameter (the step value between each bin of the hash map f...
void setInputFeatureCloud(PointCloud< PPFSignature >::ConstPtr feature_cloud)
Method that sets the feature cloud to be inserted in the hash map.
float getModelDiameter() const
Returns the maximum distance found between any feature pair in the given input feature cloud.
Class that registers two point clouds based on their sets of PPFSignatures.
typename PointCloudSource::Ptr PointCloudSourcePtr
unsigned int getSceneReferencePointSamplingRate()
Returns the parameter for the scene reference point sampling rate of the algorithm.
float getRotationClusteringThreshold()
Returns the parameter defining the rotation clustering threshold.
typename PointCloudTarget::Ptr PointCloudTargetPtr
void setRotationClusteringThreshold(float clustering_rotation_diff_threshold)
Method for setting the rotation clustering parameter.
PPFHashMapSearch::Ptr getSearchMethod()
Getter function for the search method of the class.
typename PointCloudSource::ConstPtr PointCloudSourceConstPtr
PPFRegistration()
Empty constructor that initializes all the parameters of the algorithm with default values.
pcl::PointCloud< PointSource > PointCloudSource
void setSceneReferencePointSamplingRate(unsigned int scene_reference_point_sampling_rate)
Method for setting the scene reference point sampling rate.
PoseWithVotesList getBestPoseCandidates()
Returns the most promising pose candidates, after clustering.
std::vector< PoseWithVotes, Eigen::aligned_allocator< PoseWithVotes > > PoseWithVotesList
float getPositionClusteringThreshold()
Returns the parameter defining the position difference clustering parameter.
typename PointCloudTarget::ConstPtr PointCloudTargetConstPtr
void setPositionClusteringThreshold(float clustering_position_diff_threshold)
Method for setting the position difference clustering parameter.
void setInputTarget(const PointCloudTargetConstPtr &cloud) override
Provide a pointer to the input target (e.g., the point cloud that we want to align the input source t...
void setSearchMethod(PPFHashMapSearch::Ptr search_method)
Function that sets the search method for the algorithm.
shared_ptr< PointCloud< PointSource > > Ptr
Definition: point_cloud.h:413
shared_ptr< const PointCloud< PointT > > ConstPtr
Definition: point_cloud.h:414
Registration represents the base registration class for general purpose, ICP-like methods.
Definition: registration.h:57
detail::int_type_t< detail::index_type_size, false > uindex_t
Type used for an unsigned index in PCL.
Definition: types.h:120
#define PCL_EXPORTS
Definition: pcl_macros.h:323
#define M_PI
Definition: pcl_macros.h:201
Data structure to hold the information for the key in the feature hash map of the PPFHashMapSearch cl...
HashKeyStruct(int a, int b, int c, int d)
std::size_t operator()(const HashKeyStruct &s) const noexcept
Structure for storing a pose (represented as an Eigen::Affine3f) and an integer for counting votes.
PoseWithVotes(const Eigen::Affine3f &a_pose, unsigned int &a_votes)