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