Point Cloud Library (PCL) 1.15.0
Loading...
Searching...
No Matches
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
48namespace pcl {
49class PCL_EXPORTS PPFHashMapSearch {
50public:
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 /// RS hash function https://www.partow.net/programming/hashfunctions/index.html
71 std::size_t b_ = 378551;
72 std::size_t a_ = 63689;
73 std::size_t hash = 0;
74 hash = hash * a_ + s.first;
75 a_ = a_ * b_;
76 hash = hash * a_ + s.second.first;
77 a_ = a_ * b_;
78 hash = hash * a_ + s.second.second.first;
79 a_ = a_ * b_;
80 hash = hash * a_ + s.second.second.second;
81 return hash;
82 }
83 };
85 std::unordered_multimap<HashKeyStruct,
86 std::pair<std::size_t, std::size_t>,
88 using FeatureHashMapTypePtr = shared_ptr<FeatureHashMapType>;
89 using Ptr = shared_ptr<PPFHashMapSearch>;
90 using ConstPtr = shared_ptr<const PPFHashMapSearch>;
91
92 /** \brief Constructor for the PPFHashMapSearch class which sets the two step
93 * parameters for the enclosed data structure \param angle_discretization_step the
94 * step value between each bin of the hash map for the angular values \param
95 * distance_discretization_step the step value between each bin of the hash map for
96 * the distance values
97 */
98 PPFHashMapSearch(float angle_discretization_step = 12.0f / 180.0f *
99 static_cast<float>(M_PI),
100 float distance_discretization_step = 0.01f)
101 : feature_hash_map_(new FeatureHashMapType)
102 , angle_discretization_step_(angle_discretization_step)
103 , distance_discretization_step_(distance_discretization_step)
104 {}
105
106 /** \brief Method that sets the feature cloud to be inserted in the hash map
107 * \param feature_cloud a const smart pointer to the PPFSignature feature cloud
108 */
109 void
111
112 /** \brief Function for finding the nearest neighbors for the given feature inside the
113 * discretized hash map \param f1 The 1st value describing the query PPFSignature
114 * feature \param f2 The 2nd value describing the query PPFSignature feature \param f3
115 * The 3rd value describing the query PPFSignature feature \param f4 The 4th value
116 * describing the query PPFSignature feature \param indices a vector of pair indices
117 * representing the feature pairs that have been found in the bin corresponding to the
118 * query feature
119 */
120 void
122 float& f2,
123 float& f3,
124 float& f4,
125 std::vector<std::pair<std::size_t, std::size_t>>& indices);
126
127 /** \brief Convenience method for returning a copy of the class instance as a
128 * shared_ptr */
129 Ptr
131 {
132 return Ptr(new PPFHashMapSearch(*this));
133 }
134
135 /** \brief Returns the angle discretization step parameter (the step value between
136 * each bin of the hash map for the angular values) */
137 inline float
139 {
140 return angle_discretization_step_;
141 }
142
143 /** \brief Returns the distance discretization step parameter (the step value between
144 * each bin of the hash map for the distance values) */
145 inline float
147 {
148 return distance_discretization_step_;
149 }
150
151 /** \brief Returns the maximum distance found between any feature pair in the given
152 * input feature cloud */
153 inline float
155 {
156 return max_dist_;
157 }
158
159 std::vector<std::vector<float>> alpha_m_;
160
161private:
162 FeatureHashMapTypePtr feature_hash_map_;
163 bool internals_initialized_{false};
164
165 float angle_discretization_step_, distance_discretization_step_;
166 float max_dist_{-1.0f};
167};
168
169/** \brief Class that registers two point clouds based on their sets of PPFSignatures.
170 * Please refer to the following publication for more details:
171 * B. Drost, M. Ulrich, N. Navab, S. Ilic
172 * Model Globally, Match Locally: Efficient and Robust 3D Object Recognition
173 * 2010 IEEE Conference on Computer Vision and Pattern Recognition (CVPR)
174 * 13-18 June 2010, San Francisco, CA
175 *
176 * \note This class works in tandem with the PPFEstimation class
177 * \ingroup registration
178 *
179 * \author Alexandru-Eugen Ichim
180 */
181template <typename PointSource, typename PointTarget>
182class PPFRegistration : public Registration<PointSource, PointTarget> {
183public:
184 /** \brief Structure for storing a pose (represented as an Eigen::Affine3f) and an
185 * integer for counting votes \note initially used std::pair<Eigen::Affine3f, unsigned
186 * int>, but it proved problematic because of the Eigen structures alignment problems
187 * - std::pair does not have a custom allocator
188 */
190 PoseWithVotes(const Eigen::Affine3f& a_pose, unsigned int& a_votes)
191 : pose(a_pose), votes(a_votes)
192 {}
193
194 Eigen::Affine3f pose;
195 unsigned int votes;
196 };
198 std::vector<PoseWithVotes, Eigen::aligned_allocator<PoseWithVotes>>;
199
200 /// input_ is the model cloud
201 using Registration<PointSource, PointTarget>::input_;
202 /// target_ is the scene cloud
203 using Registration<PointSource, PointTarget>::target_;
204 using Registration<PointSource, PointTarget>::converged_;
205 using Registration<PointSource, PointTarget>::final_transformation_;
206 using Registration<PointSource, PointTarget>::transformation_;
207
211
215
216 /** \brief Empty constructor that initializes all the parameters of the algorithm with
217 * default values */
219 : Registration<PointSource, PointTarget>()
220 , clustering_rotation_diff_threshold_(20.0f / 180.0f * static_cast<float>(M_PI))
221 {}
222
223 /** \brief Method for setting the position difference clustering parameter
224 * \param clustering_position_diff_threshold distance threshold below which two poses
225 * are considered close enough to be in the same cluster (for the clustering phase of
226 * the algorithm)
227 */
228 inline void
229 setPositionClusteringThreshold(float clustering_position_diff_threshold)
230 {
231 clustering_position_diff_threshold_ = clustering_position_diff_threshold;
232 }
233
234 /** \brief Returns the parameter defining the position difference clustering parameter
235 * - distance threshold below which two poses are considered close enough to be in the
236 * same cluster (for the clustering phase of the algorithm)
237 */
238 inline float
240 {
241 return clustering_position_diff_threshold_;
242 }
243
244 /** \brief Method for setting the rotation clustering parameter
245 * \param clustering_rotation_diff_threshold rotation difference threshold below which
246 * two poses are considered to be in the same cluster (for the clustering phase of the
247 * algorithm)
248 */
249 inline void
250 setRotationClusteringThreshold(float clustering_rotation_diff_threshold)
251 {
252 clustering_rotation_diff_threshold_ = clustering_rotation_diff_threshold;
253 }
254
255 /** \brief Returns the parameter defining the rotation clustering threshold
256 */
257 inline float
259 {
260 return clustering_rotation_diff_threshold_;
261 }
262
263 /** \brief Method for setting the scene reference point sampling rate
264 * \param scene_reference_point_sampling_rate sampling rate for the scene reference
265 * point
266 */
267 inline void
268 setSceneReferencePointSamplingRate(unsigned int scene_reference_point_sampling_rate)
269 {
270 scene_reference_point_sampling_rate_ = scene_reference_point_sampling_rate;
271 }
272
273 /** \brief Returns the parameter for the scene reference point sampling rate of the
274 * algorithm */
275 inline unsigned int
277 {
278 return scene_reference_point_sampling_rate_;
279 }
280
281 /** \brief Function that sets the search method for the algorithm
282 * \note Right now, the only available method is the one initially proposed by
283 * the authors - by using a hash map with discretized feature vectors
284 * \param search_method smart pointer to the search method to be set
285 */
286 inline void
288 {
289 search_method_ = search_method;
290 }
291
292 /** \brief Getter function for the search method of the class */
295 {
296 return search_method_;
297 }
298
299 /** \brief Provide a pointer to the input target (e.g., the point cloud that we want
300 * to align the input source to) \param cloud the input point cloud target
301 */
302 void
303 setInputTarget(const PointCloudTargetConstPtr& cloud) override;
304
305 /** \brief Returns the most promising pose candidates, after clustering. The pose with
306 * the most votes is the result of the registration. It may make sense to check the
307 * next best pose candidates if the registration did not give the right result, or if
308 * there are more than one correct results. You need to call the align function before
309 * this one.
310 */
311 inline PoseWithVotesList
313 {
314 return best_pose_candidates;
315 }
316
317private:
318 /** \brief Method that calculates the transformation between the input_ and target_
319 * point clouds, based on the PPF features */
320 void
321 computeTransformation(PointCloudSource& output,
322 const Eigen::Matrix4f& guess) override;
323
324 /** \brief the search method that is going to be used to find matching feature pairs
325 */
326 PPFHashMapSearch::Ptr search_method_;
327
328 /** \brief parameter for the sampling rate of the scene reference points */
329 uindex_t scene_reference_point_sampling_rate_{5};
330
331 /** \brief position and rotation difference thresholds below which two
332 * poses are considered to be in the same cluster (for the clustering phase of the
333 * algorithm) */
334 float clustering_position_diff_threshold_{0.01f}, clustering_rotation_diff_threshold_;
335
336 /** \brief use a kd-tree with range searches of range max_dist to skip an O(N) pass
337 * through the point cloud */
338 typename pcl::KdTreeFLANN<PointTarget>::Ptr scene_search_tree_;
339
340 /** \brief List with the most promising pose candidates, after clustering. The pose
341 * with the most votes is returned as the registration result. */
342 PoseWithVotesList best_pose_candidates;
343
344 /** \brief static method used for the std::sort function to order two PoseWithVotes
345 * instances by their number of votes*/
346 static bool
347 poseWithVotesCompareFunction(const PoseWithVotes& a, const PoseWithVotes& b);
348
349 /** \brief static method used for the std::sort function to order two pairs <index,
350 * votes> by the number of votes (unsigned integer value) */
351 static bool
352 clusterVotesCompareFunction(const std::pair<std::size_t, unsigned int>& a,
353 const std::pair<std::size_t, unsigned int>& b);
354
355 /** \brief Method that clusters a set of given poses by using the clustering
356 * thresholds and their corresponding number of votes (see publication for more
357 * details) */
358 void
359 clusterPoses(PoseWithVotesList& poses, PoseWithVotesList& result);
360
361 /** \brief Method that checks whether two poses are close together - based on the
362 * clustering threshold parameters of the class */
363 bool
364 posesWithinErrorBounds(Eigen::Affine3f& pose1,
365 Eigen::Affine3f& pose2,
366 float& position_diff,
367 float& rotation_diff_angle);
368};
369} // namespace pcl
370
371#include <pcl/registration/impl/ppf_registration.hpp>
shared_ptr< KdTreeFLANN< PointT, Dist > > Ptr
PointCloudConstPtr input_
The input point cloud dataset.
Definition pcl_base.h:147
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
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 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.
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
shared_ptr< const PointCloud< PointT > > ConstPtr
Registration represents the base registration class for general purpose, ICP-like methods.
Matrix4 final_transformation_
The final transformation matrix estimated by the registration method after N iterations.
Matrix4 transformation_
The transformation matrix estimated by the registration method.
bool converged_
Holds internal convergence state, given user parameters.
PointCloudTargetConstPtr target_
The input point cloud dataset target.
detail::int_type_t< detail::index_type_size, false > uindex_t
Type used for an unsigned index in PCL.
Definition types.h:120
#define M_PI
Definition pcl_macros.h:203
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)