42 #ifndef PCL_REGISTRATION_IMPL_PPF_REGISTRATION_H_
43 #define PCL_REGISTRATION_IMPL_PPF_REGISTRATION_H_
45 #include <pcl/common/transforms.h>
46 #include <pcl/features/pfh.h>
47 #include <pcl/features/pfh_tools.h>
48 #include <pcl/features/ppf.h>
49 #include <pcl/registration/ppf_registration.h>
51 template <
typename Po
intSource,
typename Po
intTarget>
60 scene_search_tree_->setInputCloud(target_);
64 template <
typename Po
intSource,
typename Po
intTarget>
67 PointCloudSource& output,
const Eigen::Matrix4f& guess)
69 if (!search_method_) {
70 PCL_ERROR(
"[pcl::PPFRegistration::computeTransformation] Search method not set - "
71 "skipping computeTransformation!\n");
75 if (guess != Eigen::Matrix4f::Identity()) {
76 PCL_ERROR(
"[pcl::PPFRegistration::computeTransformation] setting initial transform "
77 "(guess) not implemented!\n");
80 const auto aux_size =
static_cast<std::size_t
>(
81 std::floor(2 *
M_PI / search_method_->getAngleDiscretizationStep()));
83 const std::vector<unsigned int> tmp_vec(aux_size, 0);
84 std::vector<std::vector<unsigned int>> accumulator_array(input_->size(), tmp_vec);
86 PCL_INFO(
"Accumulator array size: %u x %u.\n",
87 accumulator_array.size(),
88 accumulator_array.back().size());
90 PoseWithVotesList voted_poses;
94 for (
index_t scene_reference_index = 0;
95 scene_reference_index < static_cast<index_t>(target_->size());
96 scene_reference_index += scene_reference_point_sampling_rate_) {
97 Eigen::Vector3f scene_reference_point =
98 (*target_)[scene_reference_index].getVector3fMap(),
99 scene_reference_normal =
100 (*target_)[scene_reference_index].getNormalVector3fMap();
102 float rotation_angle_sg =
103 std::acos(scene_reference_normal.dot(Eigen::Vector3f::UnitX()));
104 bool parallel_to_x_sg =
105 (scene_reference_normal.y() == 0.0f && scene_reference_normal.z() == 0.0f);
106 Eigen::Vector3f rotation_axis_sg =
108 ? (Eigen::Vector3f::UnitY())
109 : (scene_reference_normal.cross(Eigen::Vector3f::UnitX()).
normalized());
110 Eigen::AngleAxisf rotation_sg(rotation_angle_sg, rotation_axis_sg);
111 Eigen::Affine3f transform_sg(
112 Eigen::Translation3f(rotation_sg * ((-1) * scene_reference_point)) *
117 std::vector<float> distances;
118 scene_search_tree_->radiusSearch((*target_)[scene_reference_index],
119 search_method_->getModelDiameter() / 2,
122 for (
const auto& scene_point_index : indices)
126 if (scene_reference_index != scene_point_index) {
128 (*target_)[scene_reference_index].getVector4fMap(),
129 (*target_)[scene_reference_index].getNormalVector4fMap(),
130 (*target_)[scene_point_index].getVector4fMap(),
131 (*target_)[scene_point_index].getNormalVector4fMap(),
136 std::vector<std::pair<std::size_t, std::size_t>> nearest_indices;
137 search_method_->nearestNeighborSearch(f1, f2, f3, f4, nearest_indices);
140 Eigen::Vector3f scene_point = (*target_)[scene_point_index].getVector3fMap();
142 Eigen::Vector3f scene_point_transformed = transform_sg * scene_point;
144 std::atan2(-scene_point_transformed(2), scene_point_transformed(1));
145 if (std::sin(alpha_s) * scene_point_transformed(2) < 0.0f)
150 for (
const auto& nearest_index : nearest_indices) {
151 std::size_t model_reference_index = nearest_index.first;
152 std::size_t model_point_index = nearest_index.second;
155 search_method_->alpha_m_[model_reference_index][model_point_index] -
157 unsigned int alpha_discretized =
static_cast<unsigned int>(
159 std::floor(
M_PI / search_method_->getAngleDiscretizationStep()));
160 accumulator_array[model_reference_index][alpha_discretized]++;
164 PCL_ERROR(
"[pcl::PPFRegistration::computeTransformation] Computing pair "
165 "feature vector between points %u and %u went wrong.\n",
166 scene_reference_index,
171 std::size_t max_votes_i = 0, max_votes_j = 0;
172 unsigned int max_votes = 0;
174 for (std::size_t i = 0; i < accumulator_array.size(); ++i)
175 for (std::size_t j = 0; j < accumulator_array.back().size(); ++j) {
176 if (accumulator_array[i][j] > max_votes) {
177 max_votes = accumulator_array[i][j];
183 accumulator_array[i][j] = 0;
186 Eigen::Vector3f model_reference_point = (*input_)[max_votes_i].getVector3fMap(),
187 model_reference_normal =
188 (*input_)[max_votes_i].getNormalVector3fMap();
189 float rotation_angle_mg =
190 std::acos(model_reference_normal.dot(Eigen::Vector3f::UnitX()));
191 bool parallel_to_x_mg =
192 (model_reference_normal.y() == 0.0f && model_reference_normal.z() == 0.0f);
193 Eigen::Vector3f rotation_axis_mg =
195 ? (Eigen::Vector3f::UnitY())
196 : (model_reference_normal.cross(Eigen::Vector3f::UnitX()).
normalized());
197 Eigen::AngleAxisf rotation_mg(rotation_angle_mg, rotation_axis_mg);
198 Eigen::Affine3f transform_mg(
199 Eigen::Translation3f(rotation_mg * ((-1) * model_reference_point)) *
201 Eigen::Affine3f max_transform =
202 transform_sg.inverse() *
203 Eigen::AngleAxisf((
static_cast<float>(max_votes_j) -
204 std::floor(
static_cast<float>(
M_PI) /
205 search_method_->getAngleDiscretizationStep())) *
206 search_method_->getAngleDiscretizationStep(),
207 Eigen::Vector3f::UnitX()) *
210 voted_poses.push_back(PoseWithVotes(max_transform, max_votes));
212 PCL_DEBUG(
"Done with the Hough Transform ...\n");
215 PoseWithVotesList results;
216 clusterPoses(voted_poses, results);
220 transformation_ = final_transformation_ = results.front().pose.matrix();
225 template <
typename Po
intSource,
typename Po
intTarget>
231 PCL_INFO(
"Clustering poses ...\n");
233 sort(poses.begin(), poses.end(), poseWithVotesCompareFunction);
235 std::vector<PoseWithVotesList> clusters;
236 std::vector<std::pair<std::size_t, unsigned int>> cluster_votes;
237 for (std::size_t poses_i = 0; poses_i < poses.size(); ++poses_i) {
238 bool found_cluster =
false;
239 for (std::size_t clusters_i = 0; clusters_i < clusters.size(); ++clusters_i) {
240 if (posesWithinErrorBounds(poses[poses_i].pose,
241 clusters[clusters_i].front().pose)) {
242 found_cluster =
true;
243 clusters[clusters_i].push_back(poses[poses_i]);
244 cluster_votes[clusters_i].second += poses[poses_i].votes;
249 if (!found_cluster) {
251 PoseWithVotesList new_cluster;
252 new_cluster.push_back(poses[poses_i]);
253 clusters.push_back(new_cluster);
254 cluster_votes.push_back(std::pair<std::size_t, unsigned int>(
255 clusters.size() - 1, poses[poses_i].votes));
260 std::sort(cluster_votes.begin(), cluster_votes.end(), clusterVotesCompareFunction);
265 std::size_t max_clusters = (clusters.size() < 3) ? clusters.size() : 3;
266 for (std::size_t cluster_i = 0; cluster_i < max_clusters; ++cluster_i) {
267 PCL_INFO(
"Winning cluster has #votes: %d and #poses voted: %d.\n",
268 cluster_votes[cluster_i].second,
269 clusters[cluster_votes[cluster_i].first].size());
270 Eigen::Vector3f translation_average(0.0, 0.0, 0.0);
271 Eigen::Vector4f rotation_average(0.0, 0.0, 0.0, 0.0);
272 for (
typename PoseWithVotesList::iterator v_it =
273 clusters[cluster_votes[cluster_i].first].begin();
274 v_it != clusters[cluster_votes[cluster_i].first].end();
276 translation_average += v_it->pose.translation();
279 rotation_average += Eigen::Quaternionf(v_it->pose.rotation()).coeffs();
282 translation_average /=
283 static_cast<float>(clusters[cluster_votes[cluster_i].first].size());
285 static_cast<float>(clusters[cluster_votes[cluster_i].first].size());
287 Eigen::Affine3f transform_average;
288 transform_average.translation().matrix() = translation_average;
289 transform_average.linear().matrix() =
290 Eigen::Quaternionf(rotation_average).normalized().toRotationMatrix();
292 result.push_back(PoseWithVotes(transform_average, cluster_votes[cluster_i].second));
297 template <
typename Po
intSource,
typename Po
intTarget>
300 Eigen::Affine3f& pose1, Eigen::Affine3f& pose2)
302 float position_diff = (pose1.translation() - pose2.translation()).
norm();
303 Eigen::AngleAxisf rotation_diff_mat(
304 (pose1.rotation().inverse().lazyProduct(pose2.rotation()).eval()));
306 float rotation_diff_angle = std::abs(rotation_diff_mat.angle());
308 return (position_diff < clustering_position_diff_threshold_ &&
309 rotation_diff_angle < clustering_rotation_diff_threshold_);
313 template <
typename Po
intSource,
typename Po
intTarget>
323 template <
typename Po
intSource,
typename Po
intTarget>
326 const std::pair<std::size_t, unsigned int>& a,
327 const std::pair<std::size_t, unsigned int>& b)
329 return (a.second > b.second);
shared_ptr< KdTreeFLANN< PointT, Dist > > Ptr
Class that registers two point clouds based on their sets of PPFSignatures.
std::vector< PoseWithVotes, Eigen::aligned_allocator< PoseWithVotes > > PoseWithVotesList
typename PointCloudTarget::ConstPtr PointCloudTargetConstPtr
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...
Registration represents the base registration class for general purpose, ICP-like methods.
void transformPointCloud(const pcl::PointCloud< PointT > &cloud_in, pcl::PointCloud< PointT > &cloud_out, const Eigen::Matrix< Scalar, 4, 4 > &transform, bool copy_all_fields)
Apply a rigid transform defined by a 4x4 matrix.
PCL_EXPORTS bool computePairFeatures(const Eigen::Vector4f &p1, const Eigen::Vector4f &n1, const Eigen::Vector4f &p2, const Eigen::Vector4f &n2, float &f1, float &f2, float &f3, float &f4)
Compute the 4-tuple representation containing the three angles and one distance between two points re...
__device__ __forceinline__ float3 normalized(const float3 &v)
__device__ __host__ __forceinline__ float norm(const float3 &v1, const float3 &v2)
detail::int_type_t< detail::index_type_size, detail::index_type_signed > index_t
Type used for an index in PCL.
IndicesAllocator<> Indices
Type used for indices in PCL.
Structure for storing a pose (represented as an Eigen::Affine3f) and an integer for counting votes.