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