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_