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>
56 feature_hash_map_->clear ();
57 unsigned int n =
static_cast<unsigned int> (sqrt (static_cast<float> (feature_cloud->
points.size ())));
61 for (
size_t i = 0; i < n; ++i)
63 std::vector <float> alpha_m_row (n);
64 for (
size_t j = 0; j < n; ++j)
66 d1 =
static_cast<int> (floor (feature_cloud->
points[i*n+j].f1 / angle_discretization_step_));
67 d2 =
static_cast<int> (floor (feature_cloud->
points[i*n+j].f2 / angle_discretization_step_));
68 d3 =
static_cast<int> (floor (feature_cloud->
points[i*n+j].f3 / angle_discretization_step_));
69 d4 =
static_cast<int> (floor (feature_cloud->
points[i*n+j].f4 / distance_discretization_step_));
70 feature_hash_map_->insert (std::pair<
HashKeyStruct, std::pair<size_t, size_t> > (
HashKeyStruct (d1, d2, d3, d4), std::pair<size_t, size_t> (i, j)));
71 alpha_m_row [j] = feature_cloud->
points[i*n + j].alpha_m;
73 if (max_dist_ < feature_cloud->points[i*n + j].f4)
74 max_dist_ = feature_cloud->
points[i*n + j].f4;
79 internals_initialized_ =
true;
86 std::vector<std::pair<size_t, size_t> > &indices)
88 if (!internals_initialized_)
90 PCL_ERROR(
"[pcl::PPFRegistration::nearestNeighborSearch]: input feature cloud has not been set - skipping search!\n");
94 int d1 =
static_cast<int> (floor (f1 / angle_discretization_step_)),
95 d2 = static_cast<int> (floor (f2 / angle_discretization_step_)),
96 d3 = static_cast<int> (floor (f3 / angle_discretization_step_)),
97 d4 = static_cast<int> (floor (f4 / distance_discretization_step_));
101 std::pair <FeatureHashMapType::iterator, FeatureHashMapType::iterator> map_iterator_pair = feature_hash_map_->equal_range (key);
102 for (; map_iterator_pair.first != map_iterator_pair.second; ++ map_iterator_pair.first)
103 indices.push_back (std::pair<size_t, size_t> (map_iterator_pair.first->second.first,
104 map_iterator_pair.first->second.second));
109 template <
typename Po
intSource,
typename Po
intTarget>
void
115 scene_search_tree_->setInputCloud (target_);
119 template <
typename Po
intSource,
typename Po
intTarget>
void
124 PCL_ERROR(
"[pcl::PPFRegistration::computeTransformation] Search method not set - skipping computeTransformation!\n");
128 if (guess != Eigen::Matrix4f::Identity ())
130 PCL_ERROR(
"[pcl::PPFRegistration::computeTransformation] setting initial transform (guess) not implemented!\n");
133 PoseWithVotesList voted_poses;
134 std::vector <std::vector <unsigned int> > accumulator_array;
135 accumulator_array.resize (input_->points.size ());
137 size_t aux_size =
static_cast<size_t> (floor (2 * M_PI / search_method_->getAngleDiscretizationStep ()));
138 for (
size_t i = 0; i < input_->points.size (); ++i)
140 std::vector<unsigned int> aux (aux_size);
141 accumulator_array[i] = aux;
143 PCL_INFO (
"Accumulator array size: %u x %u.\n", accumulator_array.size (), accumulator_array.back ().size ());
146 float f1, f2, f3, f4;
147 for (
size_t scene_reference_index = 0; scene_reference_index < target_->points.size (); scene_reference_index += scene_reference_point_sampling_rate_)
149 Eigen::Vector3f scene_reference_point = target_->points[scene_reference_index].getVector3fMap (),
150 scene_reference_normal = target_->points[scene_reference_index].getNormalVector3fMap ();
152 float rotation_angle_sg = acosf (scene_reference_normal.dot (Eigen::Vector3f::UnitX ()));
153 bool parallel_to_x_sg = (scene_reference_normal.y() == 0.0f && scene_reference_normal.z() == 0.0f);
154 Eigen::Vector3f rotation_axis_sg = (parallel_to_x_sg)?(Eigen::Vector3f::UnitY ()):(scene_reference_normal.cross (Eigen::Vector3f::UnitX ()). normalized());
155 Eigen::AngleAxisf rotation_sg (rotation_angle_sg, rotation_axis_sg);
156 Eigen::Affine3f transform_sg (Eigen::Translation3f ( rotation_sg * ((-1) * scene_reference_point)) * rotation_sg);
159 std::vector<int> indices;
160 std::vector<float> distances;
161 scene_search_tree_->radiusSearch (target_->points[scene_reference_index],
162 search_method_->getModelDiameter () /2,
165 for(
size_t i = 0; i < indices.size (); ++i)
169 size_t scene_point_index = indices[i];
170 if (scene_reference_index != scene_point_index)
173 target_->points[scene_reference_index].getNormalVector4fMap (),
174 target_->points[scene_point_index].getVector4fMap (),
175 target_->points[scene_point_index].getNormalVector4fMap (),
178 std::vector<std::pair<size_t, size_t> > nearest_indices;
179 search_method_->nearestNeighborSearch (f1, f2, f3, f4, nearest_indices);
182 Eigen::Vector3f scene_point = target_->points[scene_point_index].getVector3fMap ();
184 Eigen::Vector3f scene_point_transformed = transform_sg * scene_point;
185 float alpha_s = atan2f ( -scene_point_transformed(2), scene_point_transformed(1));
186 if (sin (alpha_s) * scene_point_transformed(2) < 0.0f)
191 for (std::vector<std::pair<size_t, size_t> >::iterator v_it = nearest_indices.begin (); v_it != nearest_indices.end (); ++ v_it)
193 size_t model_reference_index = v_it->first,
194 model_point_index = v_it->second;
196 float alpha = search_method_->alpha_m_[model_reference_index][model_point_index] - alpha_s;
197 unsigned int alpha_discretized =
static_cast<unsigned int> (floor (alpha) + floor (M_PI / search_method_->getAngleDiscretizationStep ()));
198 accumulator_array[model_reference_index][alpha_discretized] ++;
201 else PCL_ERROR (
"[pcl::PPFRegistration::computeTransformation] Computing pair feature vector between points %u and %u went wrong.\n", scene_reference_index, scene_point_index);
205 size_t max_votes_i = 0, max_votes_j = 0;
206 unsigned int max_votes = 0;
208 for (
size_t i = 0; i < accumulator_array.size (); ++i)
209 for (
size_t j = 0; j < accumulator_array.back ().size (); ++j)
211 if (accumulator_array[i][j] > max_votes)
213 max_votes = accumulator_array[i][j];
218 accumulator_array[i][j] = 0;
221 Eigen::Vector3f model_reference_point = input_->points[max_votes_i].getVector3fMap (),
222 model_reference_normal = input_->points[max_votes_i].getNormalVector3fMap ();
223 float rotation_angle_mg = acosf (model_reference_normal.dot (Eigen::Vector3f::UnitX ()));
224 bool parallel_to_x_mg = (model_reference_normal.y() == 0.0f && model_reference_normal.z() == 0.0f);
225 Eigen::Vector3f rotation_axis_mg = (parallel_to_x_mg)?(Eigen::Vector3f::UnitY ()):(model_reference_normal.cross (Eigen::Vector3f::UnitX ()). normalized());
226 Eigen::AngleAxisf rotation_mg (rotation_angle_mg, rotation_axis_mg);
227 Eigen::Affine3f transform_mg (Eigen::Translation3f ( rotation_mg * ((-1) * model_reference_point)) * rotation_mg);
228 Eigen::Affine3f max_transform =
229 transform_sg.inverse () *
230 Eigen::AngleAxisf ((static_cast<float> (max_votes_j) - floorf (static_cast<float> (M_PI) / search_method_->getAngleDiscretizationStep ())) * search_method_->getAngleDiscretizationStep (), Eigen::Vector3f::UnitX ()) *
233 voted_poses.push_back (PoseWithVotes (max_transform, max_votes));
235 PCL_DEBUG (
"Done with the Hough Transform ...\n");
238 PoseWithVotesList results;
239 clusterPoses (voted_poses, results);
243 transformation_ = final_transformation_ = results.front ().pose.matrix ();
249 template <
typename Po
intSource,
typename Po
intTarget>
void
253 PCL_INFO (
"Clustering poses ...\n");
255 sort(poses.begin (), poses.end (), poseWithVotesCompareFunction);
257 std::vector<PoseWithVotesList> clusters;
258 std::vector<std::pair<size_t, unsigned int> > cluster_votes;
259 for (
size_t poses_i = 0; poses_i < poses.size(); ++ poses_i)
261 bool found_cluster =
false;
262 for (
size_t clusters_i = 0; clusters_i < clusters.size(); ++ clusters_i)
264 if (posesWithinErrorBounds (poses[poses_i].pose, clusters[clusters_i].front ().pose))
266 found_cluster =
true;
267 clusters[clusters_i].push_back (poses[poses_i]);
268 cluster_votes[clusters_i].second += poses[poses_i].votes;
273 if (found_cluster ==
false)
276 PoseWithVotesList new_cluster;
277 new_cluster.push_back (poses[poses_i]);
278 clusters.push_back (new_cluster);
279 cluster_votes.push_back (std::pair<size_t, unsigned int> (clusters.size () - 1, poses[poses_i].votes));
284 std::sort (cluster_votes.begin (), cluster_votes.end (), clusterVotesCompareFunction);
289 size_t max_clusters = (clusters.size () < 3) ? clusters.size () : 3;
290 for (
size_t cluster_i = 0; cluster_i < max_clusters; ++ cluster_i)
292 PCL_INFO (
"Winning cluster has #votes: %d and #poses voted: %d.\n", cluster_votes[cluster_i].second, clusters[cluster_votes[cluster_i].first].size ());
293 Eigen::Vector3f translation_average (0.0, 0.0, 0.0);
294 Eigen::Vector4f rotation_average (0.0, 0.0, 0.0, 0.0);
295 for (
typename PoseWithVotesList::iterator v_it = clusters[cluster_votes[cluster_i].first].begin (); v_it != clusters[cluster_votes[cluster_i].first].end (); ++ v_it)
297 translation_average += v_it->pose.translation ();
299 rotation_average += Eigen::Quaternionf (v_it->pose.rotation ()).coeffs ();
302 translation_average /=
static_cast<float> (clusters[cluster_votes[cluster_i].first].size ());
303 rotation_average /=
static_cast<float> (clusters[cluster_votes[cluster_i].first].size ());
305 Eigen::Affine3f transform_average;
306 transform_average.translation ().matrix () = translation_average;
307 transform_average.linear ().matrix () = Eigen::Quaternionf (rotation_average).normalized().toRotationMatrix ();
309 result.push_back (PoseWithVotes (transform_average, cluster_votes[cluster_i].second));
315 template <
typename Po
intSource,
typename Po
intTarget>
bool
317 Eigen::Affine3f &pose2)
319 float position_diff = (pose1.translation () - pose2.translation ()).norm ();
320 Eigen::AngleAxisf rotation_diff_mat (pose1.rotation ().inverse () * pose2.rotation ());
322 float rotation_diff_angle = fabsf (rotation_diff_mat.angle ());
324 if (position_diff < clustering_position_diff_threshold_ && rotation_diff_angle < clustering_rotation_diff_threshold_)
331 template <
typename Po
intSource,
typename Po
intTarget>
bool
340 template <
typename Po
intSource,
typename Po
intTarget>
bool
342 const std::pair<size_t, unsigned int> &b)
344 return (a.second > b.second);
349 #endif // PCL_REGISTRATION_IMPL_PPF_REGISTRATION_H_
void setInputFeatureCloud(PointCloud< PPFSignature >::ConstPtr feature_cloud)
Method that sets the feature cloud to be inserted in the hash map.
PointCloudTarget::ConstPtr PointCloudTargetConstPtr
boost::shared_ptr< const PointCloud< PointT > > ConstPtr
std::vector< std::vector< float > > alpha_m_
void transformPointCloud(const pcl::PointCloud< PointT > &cloud_in, pcl::PointCloud< PointT > &cloud_out, const Eigen::Transform< Scalar, 3, Eigen::Affine > &transform)
Apply an affine transform defined by an Eigen Transform.
virtual 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...
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
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...
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.
void nearestNeighborSearch(float &f1, float &f2, float &f3, float &f4, std::vector< std::pair< size_t, size_t > > &indices)
Function for finding the nearest neighbors for the given feature inside the discretized hash map...
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...
Data structure to hold the information for the key in the feature hash map of the PPFHashMapSearch cl...
boost::shared_ptr< KdTreeFLANN< PointT > > Ptr