43 #include <pcl/features/ppf.h>
44 #include <pcl/registration/registration.h>
46 #include <unordered_map>
56 struct HashKeyStruct :
public std::pair<int, std::pair<int, std::pair<int, int>>> {
62 this->second.first = b;
63 this->second.second.first = c;
64 this->second.second.second = d;
71 std::size_t b_ = 378551;
72 std::size_t a_ = 63689;
74 hash = hash * a_ + s.first;
76 hash = hash * a_ + s.second.first;
78 hash = hash * a_ + s.second.second.first;
80 hash = hash * a_ + s.second.second.second;
86 std::pair<std::size_t, std::size_t>,
89 using Ptr = shared_ptr<PPFHashMapSearch>;
90 using ConstPtr = shared_ptr<const PPFHashMapSearch>;
99 static_cast<float>(
M_PI),
100 float distance_discretization_step = 0.01f)
102 , angle_discretization_step_(angle_discretization_step)
103 , distance_discretization_step_(distance_discretization_step)
125 std::vector<std::pair<std::size_t, std::size_t>>& indices);
140 return angle_discretization_step_;
148 return distance_discretization_step_;
163 bool internals_initialized_{
false};
165 float angle_discretization_step_, distance_discretization_step_;
166 float max_dist_{-1.0f};
181 template <
typename Po
intSource,
typename Po
intTarget>
198 std::vector<PoseWithVotes, Eigen::aligned_allocator<PoseWithVotes>>;
220 , clustering_rotation_diff_threshold_(20.0f / 180.0f * static_cast<float>(
M_PI))
231 clustering_position_diff_threshold_ = clustering_position_diff_threshold;
241 return clustering_position_diff_threshold_;
252 clustering_rotation_diff_threshold_ = clustering_rotation_diff_threshold;
260 return clustering_rotation_diff_threshold_;
270 scene_reference_point_sampling_rate_ = scene_reference_point_sampling_rate;
278 return scene_reference_point_sampling_rate_;
289 search_method_ = search_method;
296 return search_method_;
314 return best_pose_candidates;
322 const Eigen::Matrix4f& guess)
override;
329 uindex_t scene_reference_point_sampling_rate_{5};
334 float clustering_position_diff_threshold_{0.01f}, clustering_rotation_diff_threshold_;
347 poseWithVotesCompareFunction(
const PoseWithVotes& a,
const PoseWithVotes& b);
352 clusterVotesCompareFunction(
const std::pair<std::size_t, unsigned int>& a,
353 const std::pair<std::size_t, unsigned int>& b);
364 posesWithinErrorBounds(Eigen::Affine3f& pose1,
365 Eigen::Affine3f& pose2,
366 float& position_diff,
367 float& rotation_diff_angle);
371 #include <pcl/registration/impl/ppf_registration.hpp>
shared_ptr< KdTreeFLANN< PointT, Dist > > Ptr
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
shared_ptr< const PointCloud< PointT > > ConstPtr
Registration represents the base registration class for general purpose, ICP-like methods.
detail::int_type_t< detail::index_type_size, false > uindex_t
Type used for an unsigned index in PCL.
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)