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");
77 std::vector <std::vector <unsigned int> > accumulator_array;
78 accumulator_array.resize (input_->points.size ());
80 size_t aux_size =
static_cast<size_t> (floor (2 * M_PI / search_method_->getAngleDiscretizationStep ()));
81 for (
size_t i = 0; i < input_->points.size (); ++i)
83 std::vector<unsigned int> aux (aux_size);
84 accumulator_array[i] = aux;
86 PCL_INFO (
"Accumulator array size: %u x %u.\n", accumulator_array.size (), accumulator_array.back ().size ());
90 for (
size_t scene_reference_index = 0; scene_reference_index < target_->points.size (); scene_reference_index += scene_reference_point_sampling_rate_)
92 Eigen::Vector3f scene_reference_point = target_->points[scene_reference_index].getVector3fMap (),
93 scene_reference_normal = target_->points[scene_reference_index].getNormalVector3fMap ();
95 float rotation_angle_sg = acosf (scene_reference_normal.dot (Eigen::Vector3f::UnitX ()));
96 bool parallel_to_x_sg = (scene_reference_normal.y() == 0.0f && scene_reference_normal.z() == 0.0f);
97 Eigen::Vector3f rotation_axis_sg = (parallel_to_x_sg)?(Eigen::Vector3f::UnitY ()):(scene_reference_normal.cross (Eigen::Vector3f::UnitX ()). normalized());
98 Eigen::AngleAxisf rotation_sg (rotation_angle_sg, rotation_axis_sg);
99 Eigen::Affine3f transform_sg (Eigen::Translation3f ( rotation_sg * ((-1) * scene_reference_point)) * rotation_sg);
102 std::vector<int> indices;
103 std::vector<float> distances;
104 scene_search_tree_->radiusSearch (target_->points[scene_reference_index],
105 search_method_->getModelDiameter () /2,
108 for(
size_t i = 0; i < indices.size (); ++i)
112 size_t scene_point_index = indices[i];
113 if (scene_reference_index != scene_point_index)
116 target_->points[scene_reference_index].getNormalVector4fMap (),
117 target_->points[scene_point_index].getVector4fMap (),
118 target_->points[scene_point_index].getNormalVector4fMap (),
121 std::vector<std::pair<size_t, size_t> > nearest_indices;
122 search_method_->nearestNeighborSearch (f1, f2, f3, f4, nearest_indices);
125 Eigen::Vector3f scene_point = target_->points[scene_point_index].getVector3fMap ();
127 Eigen::Vector3f scene_point_transformed = transform_sg * scene_point;
128 float alpha_s = atan2f ( -scene_point_transformed(2), scene_point_transformed(1));
129 if (sin (alpha_s) * scene_point_transformed(2) < 0.0f)
134 for (std::vector<std::pair<size_t, size_t> >::iterator v_it = nearest_indices.begin (); v_it != nearest_indices.end (); ++ v_it)
136 size_t model_reference_index = v_it->first,
137 model_point_index = v_it->second;
139 float alpha = search_method_->alpha_m_[model_reference_index][model_point_index] - alpha_s;
140 unsigned int alpha_discretized =
static_cast<unsigned int> (floor (alpha) + floor (M_PI / search_method_->getAngleDiscretizationStep ()));
141 accumulator_array[model_reference_index][alpha_discretized] ++;
144 else PCL_ERROR (
"[pcl::PPFRegistration::computeTransformation] Computing pair feature vector between points %u and %u went wrong.\n", scene_reference_index, scene_point_index);
148 size_t max_votes_i = 0, max_votes_j = 0;
149 unsigned int max_votes = 0;
151 for (
size_t i = 0; i < accumulator_array.size (); ++i)
152 for (
size_t j = 0; j < accumulator_array.back ().size (); ++j)
154 if (accumulator_array[i][j] > max_votes)
156 max_votes = accumulator_array[i][j];
161 accumulator_array[i][j] = 0;
164 Eigen::Vector3f model_reference_point = input_->points[max_votes_i].getVector3fMap (),
165 model_reference_normal = input_->points[max_votes_i].getNormalVector3fMap ();
166 float rotation_angle_mg = acosf (model_reference_normal.dot (Eigen::Vector3f::UnitX ()));
167 bool parallel_to_x_mg = (model_reference_normal.y() == 0.0f && model_reference_normal.z() == 0.0f);
168 Eigen::Vector3f rotation_axis_mg = (parallel_to_x_mg)?(Eigen::Vector3f::UnitY ()):(model_reference_normal.cross (Eigen::Vector3f::UnitX ()). normalized());
169 Eigen::AngleAxisf rotation_mg (rotation_angle_mg, rotation_axis_mg);
170 Eigen::Affine3f transform_mg (Eigen::Translation3f ( rotation_mg * ((-1) * model_reference_point)) * rotation_mg);
171 Eigen::Affine3f max_transform =
172 transform_sg.inverse () *
173 Eigen::AngleAxisf ((static_cast<float> (max_votes_j) - floorf (static_cast<float> (M_PI) / search_method_->getAngleDiscretizationStep ())) * search_method_->getAngleDiscretizationStep (), Eigen::Vector3f::UnitX ()) *
176 voted_poses.push_back (
PoseWithVotes (max_transform, max_votes));
178 PCL_DEBUG (
"Done with the Hough Transform ...\n");
182 clusterPoses (voted_poses, results);
186 transformation_ = final_transformation_ = results.front ().pose.matrix ();
192 template <
typename Po
intSource,
typename Po
intTarget>
void 196 PCL_INFO (
"Clustering poses ...\n");
198 sort(poses.begin (), poses.end (), poseWithVotesCompareFunction);
200 std::vector<PoseWithVotesList> clusters;
201 std::vector<std::pair<size_t, unsigned int> > cluster_votes;
202 for (
size_t poses_i = 0; poses_i < poses.size(); ++ poses_i)
204 bool found_cluster =
false;
205 for (
size_t clusters_i = 0; clusters_i < clusters.size(); ++ clusters_i)
207 if (posesWithinErrorBounds (poses[poses_i].pose, clusters[clusters_i].front ().pose))
209 found_cluster =
true;
210 clusters[clusters_i].push_back (poses[poses_i]);
211 cluster_votes[clusters_i].second += poses[poses_i].votes;
216 if (found_cluster ==
false)
220 new_cluster.push_back (poses[poses_i]);
221 clusters.push_back (new_cluster);
222 cluster_votes.push_back (std::pair<size_t, unsigned int> (clusters.size () - 1, poses[poses_i].votes));
227 std::sort (cluster_votes.begin (), cluster_votes.end (), clusterVotesCompareFunction);
232 size_t max_clusters = (clusters.size () < 3) ? clusters.size () : 3;
233 for (
size_t cluster_i = 0; cluster_i < max_clusters; ++ cluster_i)
235 PCL_INFO (
"Winning cluster has #votes: %d and #poses voted: %d.\n", cluster_votes[cluster_i].second, clusters[cluster_votes[cluster_i].first].size ());
236 Eigen::Vector3f translation_average (0.0, 0.0, 0.0);
237 Eigen::Vector4f rotation_average (0.0, 0.0, 0.0, 0.0);
238 for (
typename PoseWithVotesList::iterator v_it = clusters[cluster_votes[cluster_i].first].begin (); v_it != clusters[cluster_votes[cluster_i].first].end (); ++ v_it)
240 translation_average += v_it->pose.translation ();
242 rotation_average += Eigen::Quaternionf (v_it->pose.rotation ()).coeffs ();
245 translation_average /=
static_cast<float> (clusters[cluster_votes[cluster_i].first].size ());
246 rotation_average /=
static_cast<float> (clusters[cluster_votes[cluster_i].first].size ());
248 Eigen::Affine3f transform_average;
249 transform_average.translation ().matrix () = translation_average;
250 transform_average.linear ().matrix () = Eigen::Quaternionf (rotation_average).normalized().toRotationMatrix ();
252 result.push_back (
PoseWithVotes (transform_average, cluster_votes[cluster_i].second));
258 template <
typename Po
intSource,
typename Po
intTarget>
bool 260 Eigen::Affine3f &pose2)
262 float position_diff = (pose1.translation () - pose2.translation ()).norm ();
263 Eigen::AngleAxisf rotation_diff_mat ((pose1.rotation ().inverse ().lazyProduct (pose2.rotation ()).eval()));
265 float rotation_diff_angle = fabsf (rotation_diff_mat.angle ());
267 if (position_diff < clustering_position_diff_threshold_ && rotation_diff_angle < clustering_rotation_diff_threshold_)
274 template <
typename Po
intSource,
typename Po
intTarget>
bool 283 template <
typename Po
intSource,
typename Po
intTarget>
bool 285 const std::pair<size_t, unsigned int> &b)
287 return (a.second > b.second);
292 #endif // PCL_REGISTRATION_IMPL_PPF_REGISTRATION_H_
PointCloudTarget::ConstPtr PointCloudTargetConstPtr
void transformPointCloud(const pcl::PointCloud< PointT > &cloud_in, pcl::PointCloud< PointT > &cloud_out, const Eigen::Transform< Scalar, 3, Eigen::Affine > &transform, bool copy_all_fields=true)
Apply an affine transform defined by an Eigen Transform.
const PointT & front() const
void setInputTarget(const PointCloudTargetConstPtr &cloud)
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...
Structure for storing a pose (represented as an Eigen::Affine3f) and an integer for counting votes...
std::vector< PoseWithVotes, Eigen::aligned_allocator< PoseWithVotes > > PoseWithVotesList
Class that registers two point clouds based on their sets of PPFSignatures.
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...
boost::shared_ptr< KdTreeFLANN< PointT > > Ptr