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 PoseWithVotesList voted_poses;
77 std::vector <std::vector <unsigned int> > accumulator_array;
78 accumulator_array.resize (input_->points.size ());
80 std::size_t aux_size =
static_cast<std::size_t
> (std::floor (2 * M_PI / search_method_->getAngleDiscretizationStep ()));
81 for (std::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 (std::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 = std::acos (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(
const std::size_t &scene_point_index : indices)
112 if (scene_reference_index != scene_point_index)
115 target_->points[scene_reference_index].getNormalVector4fMap (),
116 target_->points[scene_point_index].getVector4fMap (),
117 target_->points[scene_point_index].getNormalVector4fMap (),
120 std::vector<std::pair<std::size_t, std::size_t> > nearest_indices;
121 search_method_->nearestNeighborSearch (f1, f2, f3, f4, nearest_indices);
124 Eigen::Vector3f scene_point = target_->points[scene_point_index].getVector3fMap ();
126 Eigen::Vector3f scene_point_transformed = transform_sg * scene_point;
127 float alpha_s = std::atan2 ( -scene_point_transformed(2), scene_point_transformed(1));
128 if (std::sin (alpha_s) * scene_point_transformed(2) < 0.0f)
133 for (
const auto &nearest_index : nearest_indices)
135 std::size_t model_reference_index = nearest_index.first;
136 std::size_t model_point_index = nearest_index.second;
138 float alpha = search_method_->alpha_m_[model_reference_index][model_point_index] - alpha_s;
139 unsigned int alpha_discretized =
static_cast<unsigned int> (std::floor (alpha) + std::floor (M_PI / search_method_->getAngleDiscretizationStep ()));
140 accumulator_array[model_reference_index][alpha_discretized] ++;
143 else PCL_ERROR (
"[pcl::PPFRegistration::computeTransformation] Computing pair feature vector between points %u and %u went wrong.\n", scene_reference_index, scene_point_index);
147 std::size_t max_votes_i = 0, max_votes_j = 0;
148 unsigned int max_votes = 0;
150 for (std::size_t i = 0; i < accumulator_array.size (); ++i)
151 for (std::size_t j = 0; j < accumulator_array.back ().size (); ++j)
153 if (accumulator_array[i][j] > max_votes)
155 max_votes = accumulator_array[i][j];
160 accumulator_array[i][j] = 0;
163 Eigen::Vector3f model_reference_point = input_->points[max_votes_i].getVector3fMap (),
164 model_reference_normal = input_->points[max_votes_i].getNormalVector3fMap ();
165 float rotation_angle_mg = std::acos (model_reference_normal.dot (Eigen::Vector3f::UnitX ()));
166 bool parallel_to_x_mg = (model_reference_normal.y() == 0.0f && model_reference_normal.z() == 0.0f);
167 Eigen::Vector3f rotation_axis_mg = (parallel_to_x_mg)?(Eigen::Vector3f::UnitY ()):(model_reference_normal.cross (Eigen::Vector3f::UnitX ()).
normalized());
168 Eigen::AngleAxisf rotation_mg (rotation_angle_mg, rotation_axis_mg);
169 Eigen::Affine3f transform_mg (Eigen::Translation3f ( rotation_mg * ((-1) * model_reference_point)) * rotation_mg);
170 Eigen::Affine3f max_transform =
171 transform_sg.inverse () *
172 Eigen::AngleAxisf ((static_cast<float> (max_votes_j) - std::floor (static_cast<float> (M_PI) / search_method_->getAngleDiscretizationStep ())) * search_method_->getAngleDiscretizationStep (), Eigen::Vector3f::UnitX ()) *
175 voted_poses.push_back (PoseWithVotes (max_transform, max_votes));
177 PCL_DEBUG (
"Done with the Hough Transform ...\n");
180 PoseWithVotesList results;
181 clusterPoses (voted_poses, results);
185 transformation_ = final_transformation_ = results.front ().pose.matrix ();
191 template <
typename Po
intSource,
typename Po
intTarget>
void
195 PCL_INFO (
"Clustering poses ...\n");
197 sort(poses.begin (), poses.end (), poseWithVotesCompareFunction);
199 std::vector<PoseWithVotesList> clusters;
200 std::vector<std::pair<std::size_t, unsigned int> > cluster_votes;
201 for (std::size_t poses_i = 0; poses_i < poses.size(); ++ poses_i)
203 bool found_cluster =
false;
204 for (std::size_t clusters_i = 0; clusters_i < clusters.size(); ++ clusters_i)
206 if (posesWithinErrorBounds (poses[poses_i].pose, clusters[clusters_i].front ().pose))
208 found_cluster =
true;
209 clusters[clusters_i].push_back (poses[poses_i]);
210 cluster_votes[clusters_i].second += poses[poses_i].votes;
218 PoseWithVotesList new_cluster;
219 new_cluster.push_back (poses[poses_i]);
220 clusters.push_back (new_cluster);
221 cluster_votes.push_back (std::pair<std::size_t, unsigned int> (clusters.size () - 1, poses[poses_i].votes));
226 std::sort (cluster_votes.begin (), cluster_votes.end (), clusterVotesCompareFunction);
231 std::size_t max_clusters = (clusters.size () < 3) ? clusters.size () : 3;
232 for (std::size_t cluster_i = 0; cluster_i < max_clusters; ++ cluster_i)
234 PCL_INFO (
"Winning cluster has #votes: %d and #poses voted: %d.\n", cluster_votes[cluster_i].second, clusters[cluster_votes[cluster_i].first].size ());
235 Eigen::Vector3f translation_average (0.0, 0.0, 0.0);
236 Eigen::Vector4f rotation_average (0.0, 0.0, 0.0, 0.0);
237 for (
typename PoseWithVotesList::iterator v_it = clusters[cluster_votes[cluster_i].first].begin (); v_it != clusters[cluster_votes[cluster_i].first].end (); ++ v_it)
239 translation_average += v_it->pose.translation ();
241 rotation_average += Eigen::Quaternionf (v_it->pose.rotation ()).coeffs ();
244 translation_average /=
static_cast<float> (clusters[cluster_votes[cluster_i].first].size ());
245 rotation_average /=
static_cast<float> (clusters[cluster_votes[cluster_i].first].size ());
247 Eigen::Affine3f transform_average;
248 transform_average.translation ().matrix () = translation_average;
249 transform_average.linear ().matrix () = Eigen::Quaternionf (rotation_average).normalized().toRotationMatrix ();
251 result.push_back (PoseWithVotes (transform_average, cluster_votes[cluster_i].second));
257 template <
typename Po
intSource,
typename Po
intTarget>
bool
259 Eigen::Affine3f &pose2)
261 float position_diff = (pose1.translation () - pose2.translation ()).norm ();
262 Eigen::AngleAxisf rotation_diff_mat ((pose1.rotation ().inverse ().lazyProduct (pose2.rotation ()).eval()));
264 float rotation_diff_angle = std::abs (rotation_diff_mat.angle ());
266 return (position_diff < clustering_position_diff_threshold_ && rotation_diff_angle < clustering_rotation_diff_threshold_);
271 template <
typename Po
intSource,
typename Po
intTarget>
bool
280 template <
typename Po
intSource,
typename Po
intTarget>
bool
282 const std::pair<std::size_t, unsigned int> &b)
284 return (a.second > b.second);
289 #endif // PCL_REGISTRATION_IMPL_PPF_REGISTRATION_H_
std::vector< PoseWithVotes, Eigen::aligned_allocator< PoseWithVotes > > PoseWithVotesList
__device__ __forceinline__ float3 normalized(const float3 &v)
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...
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)
Apply an affine transform defined by an Eigen Transform.
typename PointCloudTarget::ConstPtr PointCloudTargetConstPtr
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...
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...
shared_ptr< KdTreeFLANN< PointT, Dist > > Ptr