43 #ifndef PCL_REGISTRATION_IMPL_PPF_REGISTRATION_H_
44 #define PCL_REGISTRATION_IMPL_PPF_REGISTRATION_H_
46 #include <pcl/registration/ppf_registration.h>
47 #include <pcl/features/ppf.h>
48 #include <pcl/common/transforms.h>
50 #include <pcl/features/pfh.h>
52 template <
typename Po
intSource,
typename Po
intTarget>
void
58 scene_search_tree_->setInputCloud (target_);
62 template <
typename Po
intSource,
typename Po
intTarget>
void
67 PCL_ERROR(
"[pcl::PPFRegistration::computeTransformation] Search method not set - skipping computeTransformation!\n");
71 if (guess != Eigen::Matrix4f::Identity ())
73 PCL_ERROR(
"[pcl::PPFRegistration::computeTransformation] setting initial transform (guess) not implemented!\n");
76 const auto aux_size =
static_cast<std::size_t
>(
77 std::floor(2 *
M_PI / search_method_->getAngleDiscretizationStep()));
79 const std::vector<unsigned int> tmp_vec(aux_size, 0);
80 std::vector<std::vector<unsigned int>> accumulator_array(input_->size(), tmp_vec);
82 PCL_INFO (
"Accumulator array size: %u x %u.\n", accumulator_array.size (), accumulator_array.back ().size ());
84 PoseWithVotesList voted_poses;
87 for (std::size_t scene_reference_index = 0; scene_reference_index < target_->size (); scene_reference_index += scene_reference_point_sampling_rate_)
89 Eigen::Vector3f scene_reference_point = (*target_)[scene_reference_index].getVector3fMap (),
90 scene_reference_normal = (*target_)[scene_reference_index].getNormalVector3fMap ();
92 float rotation_angle_sg = std::acos (scene_reference_normal.dot (Eigen::Vector3f::UnitX ()));
93 bool parallel_to_x_sg = (scene_reference_normal.y() == 0.0f && scene_reference_normal.z() == 0.0f);
94 Eigen::Vector3f rotation_axis_sg = (parallel_to_x_sg)?(Eigen::Vector3f::UnitY ()):(scene_reference_normal.cross (Eigen::Vector3f::UnitX ()).
normalized());
95 Eigen::AngleAxisf rotation_sg (rotation_angle_sg, rotation_axis_sg);
96 Eigen::Affine3f transform_sg (Eigen::Translation3f ( rotation_sg * ((-1) * scene_reference_point)) * rotation_sg);
99 std::vector<int> indices;
100 std::vector<float> distances;
101 scene_search_tree_->radiusSearch ((*target_)[scene_reference_index],
102 search_method_->getModelDiameter () /2,
105 for(
const std::size_t &scene_point_index : indices)
109 if (scene_reference_index != scene_point_index)
112 (*target_)[scene_reference_index].getNormalVector4fMap (),
113 (*target_)[scene_point_index].getVector4fMap (),
114 (*target_)[scene_point_index].getNormalVector4fMap (),
117 std::vector<std::pair<std::size_t, std::size_t> > nearest_indices;
118 search_method_->nearestNeighborSearch (f1, f2, f3, f4, nearest_indices);
121 Eigen::Vector3f scene_point = (*target_)[scene_point_index].getVector3fMap ();
123 Eigen::Vector3f scene_point_transformed = transform_sg * scene_point;
124 float alpha_s = std::atan2 ( -scene_point_transformed(2), scene_point_transformed(1));
125 if (std::sin (alpha_s) * scene_point_transformed(2) < 0.0f)
130 for (
const auto &nearest_index : nearest_indices)
132 std::size_t model_reference_index = nearest_index.first;
133 std::size_t model_point_index = nearest_index.second;
135 float alpha = search_method_->alpha_m_[model_reference_index][model_point_index] - alpha_s;
136 unsigned int alpha_discretized =
static_cast<unsigned int> (std::floor (alpha) + std::floor (
M_PI / search_method_->getAngleDiscretizationStep ()));
137 accumulator_array[model_reference_index][alpha_discretized] ++;
140 else PCL_ERROR (
"[pcl::PPFRegistration::computeTransformation] Computing pair feature vector between points %u and %u went wrong.\n", scene_reference_index, scene_point_index);
144 std::size_t max_votes_i = 0, max_votes_j = 0;
145 unsigned int max_votes = 0;
147 for (std::size_t i = 0; i < accumulator_array.size (); ++i)
148 for (std::size_t j = 0; j < accumulator_array.back ().size (); ++j)
150 if (accumulator_array[i][j] > max_votes)
152 max_votes = accumulator_array[i][j];
157 accumulator_array[i][j] = 0;
160 Eigen::Vector3f model_reference_point = (*input_)[max_votes_i].getVector3fMap (),
161 model_reference_normal = (*input_)[max_votes_i].getNormalVector3fMap ();
162 float rotation_angle_mg = std::acos (model_reference_normal.dot (Eigen::Vector3f::UnitX ()));
163 bool parallel_to_x_mg = (model_reference_normal.y() == 0.0f && model_reference_normal.z() == 0.0f);
164 Eigen::Vector3f rotation_axis_mg = (parallel_to_x_mg)?(Eigen::Vector3f::UnitY ()):(model_reference_normal.cross (Eigen::Vector3f::UnitX ()).
normalized());
165 Eigen::AngleAxisf rotation_mg (rotation_angle_mg, rotation_axis_mg);
166 Eigen::Affine3f transform_mg (Eigen::Translation3f ( rotation_mg * ((-1) * model_reference_point)) * rotation_mg);
167 Eigen::Affine3f max_transform =
168 transform_sg.inverse () *
169 Eigen::AngleAxisf ((
static_cast<float> (max_votes_j) - std::floor (
static_cast<float> (
M_PI) / search_method_->getAngleDiscretizationStep ())) * search_method_->getAngleDiscretizationStep (), Eigen::Vector3f::UnitX ()) *
172 voted_poses.push_back (PoseWithVotes (max_transform, max_votes));
174 PCL_DEBUG (
"Done with the Hough Transform ...\n");
177 PoseWithVotesList results;
178 clusterPoses (voted_poses, results);
182 transformation_ = final_transformation_ = results.front ().pose.matrix ();
188 template <
typename Po
intSource,
typename Po
intTarget>
void
192 PCL_INFO (
"Clustering poses ...\n");
194 sort(poses.begin (), poses.end (), poseWithVotesCompareFunction);
196 std::vector<PoseWithVotesList> clusters;
197 std::vector<std::pair<std::size_t, unsigned int> > cluster_votes;
198 for (std::size_t poses_i = 0; poses_i < poses.size(); ++ poses_i)
200 bool found_cluster =
false;
201 for (std::size_t clusters_i = 0; clusters_i < clusters.size(); ++ clusters_i)
203 if (posesWithinErrorBounds (poses[poses_i].pose, clusters[clusters_i].front ().pose))
205 found_cluster =
true;
206 clusters[clusters_i].push_back (poses[poses_i]);
207 cluster_votes[clusters_i].second += poses[poses_i].votes;
215 PoseWithVotesList new_cluster;
216 new_cluster.push_back (poses[poses_i]);
217 clusters.push_back (new_cluster);
218 cluster_votes.push_back (std::pair<std::size_t, unsigned int> (clusters.size () - 1, poses[poses_i].votes));
223 std::sort (cluster_votes.begin (), cluster_votes.end (), clusterVotesCompareFunction);
228 std::size_t max_clusters = (clusters.size () < 3) ? clusters.size () : 3;
229 for (std::size_t cluster_i = 0; cluster_i < max_clusters; ++ cluster_i)
231 PCL_INFO (
"Winning cluster has #votes: %d and #poses voted: %d.\n", cluster_votes[cluster_i].second, clusters[cluster_votes[cluster_i].first].size ());
232 Eigen::Vector3f translation_average (0.0, 0.0, 0.0);
233 Eigen::Vector4f rotation_average (0.0, 0.0, 0.0, 0.0);
234 for (
typename PoseWithVotesList::iterator v_it = clusters[cluster_votes[cluster_i].first].begin (); v_it != clusters[cluster_votes[cluster_i].first].end (); ++ v_it)
236 translation_average += v_it->pose.translation ();
238 rotation_average += Eigen::Quaternionf (v_it->pose.rotation ()).coeffs ();
241 translation_average /=
static_cast<float> (clusters[cluster_votes[cluster_i].first].size ());
242 rotation_average /=
static_cast<float> (clusters[cluster_votes[cluster_i].first].size ());
244 Eigen::Affine3f transform_average;
245 transform_average.translation ().matrix () = translation_average;
246 transform_average.linear ().matrix () = Eigen::Quaternionf (rotation_average).normalized().toRotationMatrix ();
248 result.push_back (PoseWithVotes (transform_average, cluster_votes[cluster_i].second));
254 template <
typename Po
intSource,
typename Po
intTarget>
bool
256 Eigen::Affine3f &pose2)
258 float position_diff = (pose1.translation () - pose2.translation ()).norm ();
259 Eigen::AngleAxisf rotation_diff_mat ((pose1.rotation ().inverse ().lazyProduct (pose2.rotation ()).eval()));
261 float rotation_diff_angle = std::abs (rotation_diff_mat.angle ());
263 return (position_diff < clustering_position_diff_threshold_ && rotation_diff_angle < clustering_rotation_diff_threshold_);
268 template <
typename Po
intSource,
typename Po
intTarget>
bool
277 template <
typename Po
intSource,
typename Po
intTarget>
bool
279 const std::pair<std::size_t, unsigned int> &b)
281 return (a.second > b.second);
286 #endif // PCL_REGISTRATION_IMPL_PPF_REGISTRATION_H_