43 #ifndef PCL_REGISTRATION_IMPL_PPF_REGISTRATION_H_
44 #define PCL_REGISTRATION_IMPL_PPF_REGISTRATION_H_
46 #include <pcl/features/ppf.h>
47 #include <pcl/common/transforms.h>
49 #include <pcl/features/pfh.h>
55 feature_hash_map_->clear ();
56 unsigned int n =
static_cast<unsigned int> (sqrt (static_cast<float> (feature_cloud->
points.size ())));
60 for (
size_t i = 0; i < n; ++i)
62 std::vector <float> alpha_m_row (n);
63 for (
size_t j = 0; j < n; ++j)
65 d1 =
static_cast<int> (floor (feature_cloud->
points[i*n+j].f1 / angle_discretization_step_));
66 d2 =
static_cast<int> (floor (feature_cloud->
points[i*n+j].f2 / angle_discretization_step_));
67 d3 =
static_cast<int> (floor (feature_cloud->
points[i*n+j].f3 / angle_discretization_step_));
68 d4 =
static_cast<int> (floor (feature_cloud->
points[i*n+j].f4 / distance_discretization_step_));
69 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)));
70 alpha_m_row [j] = feature_cloud->
points[i*n + j].alpha_m;
72 if (max_dist_ < feature_cloud->points[i*n + j].f4)
73 max_dist_ = feature_cloud->
points[i*n + j].f4;
78 internals_initialized_ =
true;
85 std::vector<std::pair<size_t, size_t> > &indices)
87 if (!internals_initialized_)
89 PCL_ERROR(
"[pcl::PPFRegistration::nearestNeighborSearch]: input feature cloud has not been set - skipping search!\n");
93 int d1 =
static_cast<int> (floor (f1 / angle_discretization_step_)),
94 d2 = static_cast<int> (floor (f2 / angle_discretization_step_)),
95 d3 = static_cast<int> (floor (f3 / angle_discretization_step_)),
96 d4 = static_cast<int> (floor (f4 / distance_discretization_step_));
100 std::pair <FeatureHashMapType::iterator, FeatureHashMapType::iterator> map_iterator_pair = feature_hash_map_->equal_range (key);
101 for (; map_iterator_pair.first != map_iterator_pair.second; ++ map_iterator_pair.first)
102 indices.push_back (std::pair<size_t, size_t> (map_iterator_pair.first->second.first,
103 map_iterator_pair.first->second.second));
108 template <
typename Po
intSource,
typename Po
intTarget>
void
114 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 Eigen::AngleAxisf rotation_sg (acosf (scene_reference_normal.dot (Eigen::Vector3f::UnitX ())),
153 scene_reference_normal.cross (Eigen::Vector3f::UnitX ()). normalized());
154 Eigen::Affine3f transform_sg (Eigen::Translation3f (rotation_sg * ((-1) * scene_reference_point)) * rotation_sg);
157 std::vector<int> indices;
158 std::vector<float> distances;
159 scene_search_tree_->radiusSearch (target_->points[scene_reference_index],
160 search_method_->getModelDiameter () /2,
163 for(
size_t i = 0; i < indices.size (); ++i)
167 size_t scene_point_index = indices[i];
168 if (scene_reference_index != scene_point_index)
171 target_->points[scene_reference_index].getNormalVector4fMap (),
172 target_->points[scene_point_index].getVector4fMap (),
173 target_->points[scene_point_index].getNormalVector4fMap (),
176 std::vector<std::pair<size_t, size_t> > nearest_indices;
177 search_method_->nearestNeighborSearch (f1, f2, f3, f4, nearest_indices);
180 Eigen::Vector3f scene_point = target_->points[scene_point_index].getVector3fMap ();
181 Eigen::AngleAxisf rotation_sg (acosf (scene_reference_normal.dot (Eigen::Vector3f::UnitX ())),
182 scene_reference_normal.cross (Eigen::Vector3f::UnitX ()).normalized ());
183 Eigen::Affine3f transform_sg = Eigen::Translation3f ( rotation_sg * ((-1) * scene_reference_point)) * rotation_sg;
186 Eigen::Vector3f scene_point_transformed = transform_sg * scene_point;
187 float alpha_s = atan2f ( -scene_point_transformed(2), scene_point_transformed(1));
188 if ( alpha_s != alpha_s)
190 PCL_ERROR (
"alpha_s is nan\n");
193 if (sin (alpha_s) * scene_point_transformed(2) < 0.0f)
198 for (std::vector<std::pair<size_t, size_t> >::iterator v_it = nearest_indices.begin (); v_it != nearest_indices.end (); ++ v_it)
200 size_t model_reference_index = v_it->first,
201 model_point_index = v_it->second;
203 float alpha = search_method_->alpha_m_[model_reference_index][model_point_index] - alpha_s;
204 unsigned int alpha_discretized =
static_cast<unsigned int> (floor (alpha) + floor (M_PI / search_method_->getAngleDiscretizationStep ()));
205 accumulator_array[model_reference_index][alpha_discretized] ++;
208 else PCL_ERROR (
"[pcl::PPFRegistration::computeTransformation] Computing pair feature vector between points %zu and %zu went wrong.\n", scene_reference_index, scene_point_index);
212 size_t max_votes_i = 0, max_votes_j = 0;
213 unsigned int max_votes = 0;
215 for (
size_t i = 0; i < accumulator_array.size (); ++i)
216 for (
size_t j = 0; j < accumulator_array.back ().size (); ++j)
218 if (accumulator_array[i][j] > max_votes)
220 max_votes = accumulator_array[i][j];
225 accumulator_array[i][j] = 0;
228 Eigen::Vector3f model_reference_point = input_->points[max_votes_i].getVector3fMap (),
229 model_reference_normal = input_->points[max_votes_i].getNormalVector3fMap ();
230 Eigen::AngleAxisf rotation_mg (acosf (model_reference_normal.dot (Eigen::Vector3f::UnitX ())), model_reference_normal.cross (Eigen::Vector3f::UnitX ()).normalized ());
231 Eigen::Affine3f transform_mg = Eigen::Translation3f ( rotation_mg * ((-1) * model_reference_point)) * rotation_mg;
232 Eigen::Affine3f max_transform =
233 transform_sg.inverse () *
234 Eigen::AngleAxisf ((static_cast<float> (max_votes_j) - floorf (static_cast<float> (M_PI) / search_method_->getAngleDiscretizationStep ())) * search_method_->getAngleDiscretizationStep (), Eigen::Vector3f::UnitX ()) *
237 voted_poses.push_back (PoseWithVotes (max_transform, max_votes));
239 PCL_DEBUG (
"Done with the Hough Transform ...\n");
242 PoseWithVotesList results;
243 clusterPoses (voted_poses, results);
247 transformation_ = final_transformation_ = results.front ().pose.matrix ();
253 template <
typename Po
intSource,
typename Po
intTarget>
void
257 PCL_INFO (
"Clustering poses ...\n");
259 sort(poses.begin (), poses.end (), poseWithVotesCompareFunction);
261 std::vector<PoseWithVotesList> clusters;
262 std::vector<std::pair<size_t, unsigned int> > cluster_votes;
263 for (
size_t poses_i = 0; poses_i < poses.size(); ++ poses_i)
265 bool found_cluster =
false;
266 for (
size_t clusters_i = 0; clusters_i < clusters.size(); ++ clusters_i)
268 if (posesWithinErrorBounds (poses[poses_i].pose, clusters[clusters_i].front ().pose))
270 found_cluster =
true;
271 clusters[clusters_i].push_back (poses[poses_i]);
272 cluster_votes[clusters_i].second += poses[poses_i].votes;
277 if (found_cluster ==
false)
280 PoseWithVotesList new_cluster;
281 new_cluster.push_back (poses[poses_i]);
282 clusters.push_back (new_cluster);
283 cluster_votes.push_back (std::pair<size_t, unsigned int> (clusters.size () - 1, poses[poses_i].votes));
288 std::sort (cluster_votes.begin (), cluster_votes.end (), clusterVotesCompareFunction);
293 size_t max_clusters = (clusters.size () < 3) ? clusters.size () : 3;
294 for (
size_t cluster_i = 0; cluster_i < max_clusters; ++ cluster_i)
296 PCL_INFO (
"Winning cluster has #votes: %d and #poses voted: %d.\n", cluster_votes[cluster_i].second, clusters[cluster_votes[cluster_i].first].size ());
297 Eigen::Vector3f translation_average (0.0, 0.0, 0.0);
298 Eigen::Vector4f rotation_average (0.0, 0.0, 0.0, 0.0);
299 for (
typename PoseWithVotesList::iterator v_it = clusters[cluster_votes[cluster_i].first].begin (); v_it != clusters[cluster_votes[cluster_i].first].end (); ++ v_it)
301 translation_average += v_it->pose.translation ();
303 rotation_average += Eigen::Quaternionf (v_it->pose.rotation ()).coeffs ();
306 translation_average /=
static_cast<float> (clusters[cluster_votes[cluster_i].first].size ());
307 rotation_average /=
static_cast<float> (clusters[cluster_votes[cluster_i].first].size ());
309 Eigen::Affine3f transform_average;
310 transform_average.translation ().matrix () = translation_average;
311 transform_average.linear ().matrix () = Eigen::Quaternionf (rotation_average).normalized().toRotationMatrix ();
313 result.push_back (PoseWithVotes (transform_average, cluster_votes[cluster_i].second));
319 template <
typename Po
intSource,
typename Po
intTarget>
bool
321 Eigen::Affine3f &pose2)
323 float position_diff = (pose1.translation () - pose2.translation ()).norm ();
324 Eigen::AngleAxisf rotation_diff_mat (pose1.rotation ().inverse () * pose2.rotation ());
326 float rotation_diff_angle = fabsf (rotation_diff_mat.angle ());
328 if (position_diff < clustering_position_diff_threshold_ && rotation_diff_angle < clustering_rotation_diff_threshold_)
335 template <
typename Po
intSource,
typename Po
intTarget>
bool
344 template <
typename Po
intSource,
typename Po
intTarget>
bool
346 const std::pair<size_t, unsigned int> &b)
348 return (a.second > b.second);
353 #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