Point Cloud Library (PCL)  1.9.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/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 = boost::shared_ptr<FeatureHashMapType>;
82  using Ptr = boost::shared_ptr<PPFHashMapSearch>;
83  using ConstPtr = boost::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 boost::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 () { 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 () { 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 () { 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>
std::size_t operator()(const HashKeyStruct &s) const noexcept
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.
typename PointCloudTarget::Ptr PointCloudTargetPtr
boost::shared_ptr< PPFHashMapSearch > Ptr
This file defines compatibility wrappers for low level I/O functions.
Definition: convolution.h:45
float getModelDiameter()
Returns the maximum distance found between any feature pair in the given input feature cloud...
boost::shared_ptr< KdTreeFLANN< PointT, Dist > > Ptr
Definition: kdtree_flann.h:87
std::vector< PoseWithVotes, Eigen::aligned_allocator< PoseWithVotes > > PoseWithVotesList
PPFHashMapSearch::Ptr getSearchMethod()
Getter function for the search method of the class.
void setRotationClusteringThreshold(float clustering_rotation_diff_threshold)
Method for setting the rotation clustering parameter.
std::unordered_multimap< HashKeyStruct, std::pair< std::size_t, std::size_t >, HashKeyStruct > FeatureHashMapType
void setSceneReferencePointSamplingRate(unsigned int scene_reference_point_sampling_rate)
Method for setting the scene reference point sampling rate.
std::vector< std::vector< float > > alpha_m_
void setPositionClusteringThreshold(float clustering_position_diff_threshold)
Method for setting the position difference clustering parameter.
typename PointCloudSource::Ptr PointCloudSourcePtr
boost::shared_ptr< FeatureHashMapType > FeatureHashMapTypePtr
typename PointCloudSource::ConstPtr PointCloudSourceConstPtr
void setSearchMethod(PPFHashMapSearch::Ptr search_method)
Function that sets the search method for the algorithm.
float getAngleDiscretizationStep()
Returns the angle discretization step parameter (the step value between each bin of the hash map for ...
float getRotationClusteringThreshold()
Returns the parameter defining the rotation clustering threshold.
boost::shared_ptr< const PPFHashMapSearch > ConstPtr
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...
typename PointCloudTarget::ConstPtr PointCloudTargetConstPtr
boost::shared_ptr< PointCloud< PointSource > > Ptr
Definition: point_cloud.h:415
Registration represents the base registration class for general purpose, ICP-like methods...
Definition: registration.h:60
unsigned int getSceneReferencePointSamplingRate()
Returns the parameter for the scene reference point sampling rate of the algorithm.
float getPositionClusteringThreshold()
Returns the parameter defining the position difference clustering parameter - distance threshold belo...
Structure for storing a pose (represented as an Eigen::Affine3f) and an integer for counting votes...
boost::shared_ptr< const PointCloud< PointT > > ConstPtr
Definition: point_cloud.h:416
float getDistanceDiscretizationStep()
Returns the distance discretization step parameter (the step value between each bin of the hash map f...
Class that registers two point clouds based on their sets of PPFSignatures.
PPFRegistration()
Empty constructor that initializes all the parameters of the algorithm with default values...
#define PCL_EXPORTS
Definition: pcl_macros.h:241
HashKeyStruct(int a, int b, int c, int d)
Data structure to hold the information for the key in the feature hash map of the PPFHashMapSearch cl...