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;
70 const std::size_t h1 = std::hash<int>{}(s.first);
71 const std::size_t h2 = std::hash<int>{}(s.second.first);
72 const std::size_t h3 = std::hash<int>{}(s.second.second.first);
73 const std::size_t h4 = std::hash<int>{}(s.second.second.second);
74 return h1 ^ (h2 << 1) ^ (h3 << 2) ^ (h4 << 3);
77 using FeatureHashMapType =
78 std::unordered_multimap<HashKeyStruct,
79 std::pair<std::size_t, std::size_t>,
82 using Ptr = shared_ptr<PPFHashMapSearch>;
83 using ConstPtr = shared_ptr<const PPFHashMapSearch>;
92 static_cast<float>(M_PI),
93 float distance_discretization_step = 0.01f)
95 , angle_discretization_step_(angle_discretization_step)
96 , distance_discretization_step_(distance_discretization_step)
114 nearestNeighborSearch(
float& f1,
118 std::vector<std::pair<std::size_t, std::size_t>>& indices);
133 return angle_discretization_step_;
141 return distance_discretization_step_;
156 bool internals_initialized_{
false};
158 float angle_discretization_step_, distance_discretization_step_;
159 float max_dist_{-1.0f};
174 template <
typename Po
intSource,
typename Po
intTarget>
191 std::vector<PoseWithVotes, Eigen::aligned_allocator<PoseWithVotes>>;
213 , clustering_rotation_diff_threshold_(20.0f / 180.0f * static_cast<float>(M_PI))
224 clustering_position_diff_threshold_ = clustering_position_diff_threshold;
234 return clustering_position_diff_threshold_;
245 clustering_rotation_diff_threshold_ = clustering_rotation_diff_threshold;
253 return clustering_rotation_diff_threshold_;
263 scene_reference_point_sampling_rate_ = scene_reference_point_sampling_rate;
271 return scene_reference_point_sampling_rate_;
282 search_method_ = search_method;
289 return search_method_;
307 return best_pose_candidates;
315 const Eigen::Matrix4f& guess)
override;
322 uindex_t scene_reference_point_sampling_rate_{5};
327 float clustering_position_diff_threshold_{0.01f}, clustering_rotation_diff_threshold_;
340 poseWithVotesCompareFunction(
const PoseWithVotes& a,
const PoseWithVotes& b);
345 clusterVotesCompareFunction(
const std::pair<std::size_t, unsigned int>& a,
346 const std::pair<std::size_t, unsigned int>& b);
357 posesWithinErrorBounds(Eigen::Affine3f& pose1,
358 Eigen::Affine3f& pose2,
359 float& position_diff,
360 float& rotation_diff_angle);
364 #include <pcl/registration/impl/ppf_registration.hpp>
std::vector< PoseWithVotes, Eigen::aligned_allocator< PoseWithVotes >> PoseWithVotesList
shared_ptr< PointCloud< PointSource > > Ptr
Ptr makeShared()
Convenience method for returning a copy of the class instance as a shared_ptr.
float getModelDiameter() const
Returns the maximum distance found between any feature pair in the given input feature cloud...
typename PointCloudTarget::Ptr PointCloudTargetPtr
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.
pcl::PointCloud< PointSource > PointCloudSource
void setPositionClusteringThreshold(float clustering_position_diff_threshold)
Method for setting the position difference clustering parameter.
shared_ptr< PPFHashMapSearch > Ptr
typename PointCloudSource::Ptr PointCloudSourcePtr
shared_ptr< FeatureHashMapType > FeatureHashMapTypePtr
PoseWithVotesList getBestPoseCandidates()
Returns the most promising pose candidates, after clustering.
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...
typename PointCloudSource::ConstPtr PointCloudSourceConstPtr
PoseWithVotes(const Eigen::Affine3f &a_pose, unsigned int &a_votes)
void setSearchMethod(PPFHashMapSearch::Ptr search_method)
Function that sets the search method for the algorithm.
float getRotationClusteringThreshold()
Returns the parameter defining the rotation clustering threshold.
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...
typename PointCloudTarget::ConstPtr PointCloudTargetConstPtr
shared_ptr< const PPFHashMapSearch > ConstPtr
float getAngleDiscretizationStep() const
Returns the angle discretization step parameter (the step value between each bin of the hash map for ...
Registration represents the base registration class for general purpose, ICP-like methods...
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.
Structure for storing a pose (represented as an Eigen::Affine3f) and an integer for counting votes...
std::size_t operator()(const HashKeyStruct &s) const noexcept
shared_ptr< const PointCloud< PointT > > ConstPtr
shared_ptr< KdTreeFLANN< PointT, Dist >> Ptr
Class that registers two point clouds based on their sets of PPFSignatures.
detail::int_type_t< detail::index_type_size, false > uindex_t
Type used for an unsigned index in PCL.
PPFRegistration()
Empty constructor that initializes all the parameters of the algorithm with default values...
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...
std::vector< std::vector< float > > alpha_m_