Point Cloud Library (PCL)  1.7.2
ppf_registration.hpp
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Point Cloud Library (PCL) - www.pointclouds.org
5  * Copyright (c) 2011, Alexandru-Eugen Ichim
6  * Willow Garage, Inc
7  * Copyright (c) 2012-, Open Perception, Inc.
8  *
9  * All rights reserved.
10  *
11  * Redistribution and use in source and binary forms, with or without
12  * modification, are permitted provided that the following conditions
13  * are met:
14  *
15  * * Redistributions of source code must retain the above copyright
16  * notice, this list of conditions and the following disclaimer.
17  * * Redistributions in binary form must reproduce the above
18  * copyright notice, this list of conditions and the following
19  * disclaimer in the documentation and/or other materials provided
20  * with the distribution.
21  * * Neither the name of the copyright holder(s) nor the names of its
22  * contributors may be used to endorse or promote products derived
23  * from this software without specific prior written permission.
24  *
25  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
26  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
27  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
28  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
29  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
30  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
31  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
32  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
33  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
34  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
35  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
36  * POSSIBILITY OF SUCH DAMAGE.
37  *
38  * $Id$
39  *
40  */
41 
42 
43 #ifndef PCL_REGISTRATION_IMPL_PPF_REGISTRATION_H_
44 #define PCL_REGISTRATION_IMPL_PPF_REGISTRATION_H_
45 
46 #include <pcl/registration/ppf_registration.h>
47 #include <pcl/features/ppf.h>
48 #include <pcl/common/transforms.h>
49 
50 #include <pcl/features/pfh.h>
51 //////////////////////////////////////////////////////////////////////////////////////////////
52 void
54 {
55  // Discretize the feature cloud and insert it in the hash map
56  feature_hash_map_->clear ();
57  unsigned int n = static_cast<unsigned int> (sqrt (static_cast<float> (feature_cloud->points.size ())));
58  int d1, d2, d3, d4;
59  max_dist_ = -1.0;
60  alpha_m_.resize (n);
61  for (size_t i = 0; i < n; ++i)
62  {
63  std::vector <float> alpha_m_row (n);
64  for (size_t j = 0; j < n; ++j)
65  {
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;
72 
73  if (max_dist_ < feature_cloud->points[i*n + j].f4)
74  max_dist_ = feature_cloud->points[i*n + j].f4;
75  }
76  alpha_m_[i] = alpha_m_row;
77  }
78 
79  internals_initialized_ = true;
80 }
81 
82 
83 //////////////////////////////////////////////////////////////////////////////////////////////
84 void
85 pcl::PPFHashMapSearch::nearestNeighborSearch (float &f1, float &f2, float &f3, float &f4,
86  std::vector<std::pair<size_t, size_t> > &indices)
87 {
88  if (!internals_initialized_)
89  {
90  PCL_ERROR("[pcl::PPFRegistration::nearestNeighborSearch]: input feature cloud has not been set - skipping search!\n");
91  return;
92  }
93 
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_));
98 
99  indices.clear ();
100  HashKeyStruct key = HashKeyStruct (d1, d2, d3, d4);
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));
105 }
106 
107 
108 //////////////////////////////////////////////////////////////////////////////////////////////
109 template <typename PointSource, typename PointTarget> void
111 {
113 
114  scene_search_tree_ = typename pcl::KdTreeFLANN<PointTarget>::Ptr (new pcl::KdTreeFLANN<PointTarget>);
115  scene_search_tree_->setInputCloud (target_);
116 }
117 
118 //////////////////////////////////////////////////////////////////////////////////////////////
119 template <typename PointSource, typename PointTarget> void
120 pcl::PPFRegistration<PointSource, PointTarget>::computeTransformation (PointCloudSource &output, const Eigen::Matrix4f& guess)
121 {
122  if (!search_method_)
123  {
124  PCL_ERROR("[pcl::PPFRegistration::computeTransformation] Search method not set - skipping computeTransformation!\n");
125  return;
126  }
127 
128  if (guess != Eigen::Matrix4f::Identity ())
129  {
130  PCL_ERROR("[pcl::PPFRegistration::computeTransformation] setting initial transform (guess) not implemented!\n");
131  }
132 
133  PoseWithVotesList voted_poses;
134  std::vector <std::vector <unsigned int> > accumulator_array;
135  accumulator_array.resize (input_->points.size ());
136 
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)
139  {
140  std::vector<unsigned int> aux (aux_size);
141  accumulator_array[i] = aux;
142  }
143  PCL_INFO ("Accumulator array size: %u x %u.\n", accumulator_array.size (), accumulator_array.back ().size ());
144 
145  // Consider every <scene_reference_point_sampling_rate>-th point as the reference point => fix s_r
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_)
148  {
149  Eigen::Vector3f scene_reference_point = target_->points[scene_reference_index].getVector3fMap (),
150  scene_reference_normal = target_->points[scene_reference_index].getNormalVector3fMap ();
151 
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);
157 
158  // For every other point in the scene => now have pair (s_r, s_i) fixed
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,
163  indices,
164  distances);
165  for(size_t i = 0; i < indices.size (); ++i)
166 // for(size_t i = 0; i < target_->points.size (); ++i)
167  {
168  //size_t scene_point_index = i;
169  size_t scene_point_index = indices[i];
170  if (scene_reference_index != scene_point_index)
171  {
172  if (/*pcl::computePPFPairFeature*/pcl::computePairFeatures (target_->points[scene_reference_index].getVector4fMap (),
173  target_->points[scene_reference_index].getNormalVector4fMap (),
174  target_->points[scene_point_index].getVector4fMap (),
175  target_->points[scene_point_index].getNormalVector4fMap (),
176  f1, f2, f3, f4))
177  {
178  std::vector<std::pair<size_t, size_t> > nearest_indices;
179  search_method_->nearestNeighborSearch (f1, f2, f3, f4, nearest_indices);
180 
181  // Compute alpha_s angle
182  Eigen::Vector3f scene_point = target_->points[scene_point_index].getVector3fMap ();
183 
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)
187  alpha_s *= (-1);
188  alpha_s *= (-1);
189 
190  // Go through point pairs in the model with the same discretized feature
191  for (std::vector<std::pair<size_t, size_t> >::iterator v_it = nearest_indices.begin (); v_it != nearest_indices.end (); ++ v_it)
192  {
193  size_t model_reference_index = v_it->first,
194  model_point_index = v_it->second;
195  // Calculate angle alpha = alpha_m - alpha_s
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] ++;
199  }
200  }
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);
202  }
203  }
204 
205  size_t max_votes_i = 0, max_votes_j = 0;
206  unsigned int max_votes = 0;
207 
208  for (size_t i = 0; i < accumulator_array.size (); ++i)
209  for (size_t j = 0; j < accumulator_array.back ().size (); ++j)
210  {
211  if (accumulator_array[i][j] > max_votes)
212  {
213  max_votes = accumulator_array[i][j];
214  max_votes_i = i;
215  max_votes_j = j;
216  }
217  // Reset accumulator_array for the next set of iterations with a new scene reference point
218  accumulator_array[i][j] = 0;
219  }
220 
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 ()) *
231  transform_mg;
232 
233  voted_poses.push_back (PoseWithVotes (max_transform, max_votes));
234  }
235  PCL_DEBUG ("Done with the Hough Transform ...\n");
236 
237  // Cluster poses for filtering out outliers and obtaining more precise results
238  PoseWithVotesList results;
239  clusterPoses (voted_poses, results);
240 
241  pcl::transformPointCloud (*input_, output, results.front ().pose);
242 
243  transformation_ = final_transformation_ = results.front ().pose.matrix ();
244  converged_ = true;
245 }
246 
247 
248 //////////////////////////////////////////////////////////////////////////////////////////////
249 template <typename PointSource, typename PointTarget> void
252 {
253  PCL_INFO ("Clustering poses ...\n");
254  // Start off by sorting the poses by the number of votes
255  sort(poses.begin (), poses.end (), poseWithVotesCompareFunction);
256 
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)
260  {
261  bool found_cluster = false;
262  for (size_t clusters_i = 0; clusters_i < clusters.size(); ++ clusters_i)
263  {
264  if (posesWithinErrorBounds (poses[poses_i].pose, clusters[clusters_i].front ().pose))
265  {
266  found_cluster = true;
267  clusters[clusters_i].push_back (poses[poses_i]);
268  cluster_votes[clusters_i].second += poses[poses_i].votes;
269  break;
270  }
271  }
272 
273  if (found_cluster == false)
274  {
275  // Create a new cluster with the current pose
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));
280  }
281  }
282 
283  // Sort clusters by total number of votes
284  std::sort (cluster_votes.begin (), cluster_votes.end (), clusterVotesCompareFunction);
285  // Compute pose average and put them in result vector
286  /// @todo some kind of threshold for determining whether a cluster has enough votes or not...
287  /// now just taking the first three clusters
288  result.clear ();
289  size_t max_clusters = (clusters.size () < 3) ? clusters.size () : 3;
290  for (size_t cluster_i = 0; cluster_i < max_clusters; ++ cluster_i)
291  {
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)
296  {
297  translation_average += v_it->pose.translation ();
298  /// averaging rotations by just averaging the quaternions in 4D space - reference "On Averaging Rotations" by CLAUS GRAMKOW
299  rotation_average += Eigen::Quaternionf (v_it->pose.rotation ()).coeffs ();
300  }
301 
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 ());
304 
305  Eigen::Affine3f transform_average;
306  transform_average.translation ().matrix () = translation_average;
307  transform_average.linear ().matrix () = Eigen::Quaternionf (rotation_average).normalized().toRotationMatrix ();
308 
309  result.push_back (PoseWithVotes (transform_average, cluster_votes[cluster_i].second));
310  }
311 }
312 
313 
314 //////////////////////////////////////////////////////////////////////////////////////////////
315 template <typename PointSource, typename PointTarget> bool
317  Eigen::Affine3f &pose2)
318 {
319  float position_diff = (pose1.translation () - pose2.translation ()).norm ();
320  Eigen::AngleAxisf rotation_diff_mat (pose1.rotation ().inverse () * pose2.rotation ());
321 
322  float rotation_diff_angle = fabsf (rotation_diff_mat.angle ());
323 
324  if (position_diff < clustering_position_diff_threshold_ && rotation_diff_angle < clustering_rotation_diff_threshold_)
325  return true;
326  else return false;
327 }
328 
329 
330 //////////////////////////////////////////////////////////////////////////////////////////////
331 template <typename PointSource, typename PointTarget> bool
334 {
335  return (a.votes > b.votes);
336 }
337 
338 
339 //////////////////////////////////////////////////////////////////////////////////////////////
340 template <typename PointSource, typename PointTarget> bool
342  const std::pair<size_t, unsigned int> &b)
343 {
344  return (a.second > b.second);
345 }
346 
347 //#define PCL_INSTANTIATE_PPFRegistration(PointSource,PointTarget) template class PCL_EXPORTS pcl::PPFRegistration<PointSource, PointTarget>;
348 
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
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
Definition: point_cloud.h:410
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.
Definition: transforms.hpp:42
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...
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...
boost::shared_ptr< const PointCloud< PointT > > ConstPtr
Definition: point_cloud.h:429
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
Definition: kdtree_flann.h:89