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 , internals_initialized_(false)
96 , angle_discretization_step_(angle_discretization_step)
97 , distance_discretization_step_(distance_discretization_step)
116 nearestNeighborSearch(
float& f1,
120 std::vector<std::pair<std::size_t, std::size_t>>& indices);
135 return angle_discretization_step_;
143 return distance_discretization_step_;
158 bool internals_initialized_;
160 float angle_discretization_step_, distance_discretization_step_;
175 template <
typename Po
intSource,
typename Po
intTarget>
192 std::vector<PoseWithVotes, Eigen::aligned_allocator<PoseWithVotes>>;
214 , scene_reference_point_sampling_rate_(5)
215 , clustering_position_diff_threshold_(0.01f)
216 , clustering_rotation_diff_threshold_(20.0f / 180.0f * static_cast<float>(
M_PI))
227 clustering_position_diff_threshold_ = clustering_position_diff_threshold;
237 return clustering_position_diff_threshold_;
248 clustering_rotation_diff_threshold_ = clustering_rotation_diff_threshold;
256 return clustering_rotation_diff_threshold_;
266 scene_reference_point_sampling_rate_ = scene_reference_point_sampling_rate;
274 return scene_reference_point_sampling_rate_;
285 search_method_ = search_method;
292 return search_method_;
306 const Eigen::Matrix4f& guess)
override;
313 unsigned int scene_reference_point_sampling_rate_;
318 float clustering_position_diff_threshold_, clustering_rotation_diff_threshold_;
327 poseWithVotesCompareFunction(
const PoseWithVotes& a,
const PoseWithVotes& b);
332 clusterVotesCompareFunction(
const std::pair<std::size_t, unsigned int>& a,
333 const std::pair<std::size_t, unsigned int>& b);
344 posesWithinErrorBounds(Eigen::Affine3f& pose1, Eigen::Affine3f& pose2);
348 #include <pcl/registration/impl/ppf_registration.hpp>