Point Cloud Library (PCL)  1.7.0
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/features/ppf.h>
47 #include <pcl/common/transforms.h>
48 
49 #include <pcl/features/pfh.h>
50 //////////////////////////////////////////////////////////////////////////////////////////////
51 void
53 {
54  // Discretize the feature cloud and insert it in the hash map
55  feature_hash_map_->clear ();
56  unsigned int n = static_cast<unsigned int> (sqrt (static_cast<float> (feature_cloud->points.size ())));
57  int d1, d2, d3, d4;
58  max_dist_ = -1.0;
59  alpha_m_.resize (n);
60  for (size_t i = 0; i < n; ++i)
61  {
62  std::vector <float> alpha_m_row (n);
63  for (size_t j = 0; j < n; ++j)
64  {
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;
71 
72  if (max_dist_ < feature_cloud->points[i*n + j].f4)
73  max_dist_ = feature_cloud->points[i*n + j].f4;
74  }
75  alpha_m_[i] = alpha_m_row;
76  }
77 
78  internals_initialized_ = true;
79 }
80 
81 
82 //////////////////////////////////////////////////////////////////////////////////////////////
83 void
84 pcl::PPFHashMapSearch::nearestNeighborSearch (float &f1, float &f2, float &f3, float &f4,
85  std::vector<std::pair<size_t, size_t> > &indices)
86 {
87  if (!internals_initialized_)
88  {
89  PCL_ERROR("[pcl::PPFRegistration::nearestNeighborSearch]: input feature cloud has not been set - skipping search!\n");
90  return;
91  }
92 
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_));
97 
98  indices.clear ();
99  HashKeyStruct key = HashKeyStruct (d1, d2, d3, d4);
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));
104 }
105 
106 
107 //////////////////////////////////////////////////////////////////////////////////////////////
108 template <typename PointSource, typename PointTarget> void
110 {
112 
113  scene_search_tree_ = typename pcl::KdTreeFLANN<PointTarget>::Ptr (new pcl::KdTreeFLANN<PointTarget>);
114  scene_search_tree_->setInputCloud (target_);
115 }
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  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);
155 
156  // For every other point in the scene => now have pair (s_r, s_i) fixed
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,
161  indices,
162  distances);
163  for(size_t i = 0; i < indices.size (); ++i)
164 // for(size_t i = 0; i < target_->points.size (); ++i)
165  {
166  //size_t scene_point_index = i;
167  size_t scene_point_index = indices[i];
168  if (scene_reference_index != scene_point_index)
169  {
170  if (/*pcl::computePPFPairFeature*/pcl::computePairFeatures (target_->points[scene_reference_index].getVector4fMap (),
171  target_->points[scene_reference_index].getNormalVector4fMap (),
172  target_->points[scene_point_index].getVector4fMap (),
173  target_->points[scene_point_index].getNormalVector4fMap (),
174  f1, f2, f3, f4))
175  {
176  std::vector<std::pair<size_t, size_t> > nearest_indices;
177  search_method_->nearestNeighborSearch (f1, f2, f3, f4, nearest_indices);
178 
179  // Compute alpha_s angle
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;
184 // float alpha_s = acos (Eigen::Vector3f::UnitY ().dot ((transform_sg * scene_point).normalized ()));
185 
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)
189  {
190  PCL_ERROR ("alpha_s is nan\n");
191  continue;
192  }
193  if (sin (alpha_s) * scene_point_transformed(2) < 0.0f)
194  alpha_s *= (-1);
195  alpha_s *= (-1);
196 
197  // Go through point pairs in the model with the same discretized feature
198  for (std::vector<std::pair<size_t, size_t> >::iterator v_it = nearest_indices.begin (); v_it != nearest_indices.end (); ++ v_it)
199  {
200  size_t model_reference_index = v_it->first,
201  model_point_index = v_it->second;
202  // Calculate angle alpha = alpha_m - alpha_s
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] ++;
206  }
207  }
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);
209  }
210  }
211 
212  size_t max_votes_i = 0, max_votes_j = 0;
213  unsigned int max_votes = 0;
214 
215  for (size_t i = 0; i < accumulator_array.size (); ++i)
216  for (size_t j = 0; j < accumulator_array.back ().size (); ++j)
217  {
218  if (accumulator_array[i][j] > max_votes)
219  {
220  max_votes = accumulator_array[i][j];
221  max_votes_i = i;
222  max_votes_j = j;
223  }
224  // Reset accumulator_array for the next set of iterations with a new scene reference point
225  accumulator_array[i][j] = 0;
226  }
227 
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 ()) *
235  transform_mg;
236 
237  voted_poses.push_back (PoseWithVotes (max_transform, max_votes));
238  }
239  PCL_DEBUG ("Done with the Hough Transform ...\n");
240 
241  // Cluster poses for filtering out outliers and obtaining more precise results
242  PoseWithVotesList results;
243  clusterPoses (voted_poses, results);
244 
245  pcl::transformPointCloud (*input_, output, results.front ().pose);
246 
247  transformation_ = final_transformation_ = results.front ().pose.matrix ();
248  converged_ = true;
249 }
250 
251 
252 //////////////////////////////////////////////////////////////////////////////////////////////
253 template <typename PointSource, typename PointTarget> void
256 {
257  PCL_INFO ("Clustering poses ...\n");
258  // Start off by sorting the poses by the number of votes
259  sort(poses.begin (), poses.end (), poseWithVotesCompareFunction);
260 
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)
264  {
265  bool found_cluster = false;
266  for (size_t clusters_i = 0; clusters_i < clusters.size(); ++ clusters_i)
267  {
268  if (posesWithinErrorBounds (poses[poses_i].pose, clusters[clusters_i].front ().pose))
269  {
270  found_cluster = true;
271  clusters[clusters_i].push_back (poses[poses_i]);
272  cluster_votes[clusters_i].second += poses[poses_i].votes;
273  break;
274  }
275  }
276 
277  if (found_cluster == false)
278  {
279  // Create a new cluster with the current pose
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));
284  }
285  }
286 
287  // Sort clusters by total number of votes
288  std::sort (cluster_votes.begin (), cluster_votes.end (), clusterVotesCompareFunction);
289  // Compute pose average and put them in result vector
290  /// @todo some kind of threshold for determining whether a cluster has enough votes or not...
291  /// now just taking the first three clusters
292  result.clear ();
293  size_t max_clusters = (clusters.size () < 3) ? clusters.size () : 3;
294  for (size_t cluster_i = 0; cluster_i < max_clusters; ++ cluster_i)
295  {
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)
300  {
301  translation_average += v_it->pose.translation ();
302  /// averaging rotations by just averaging the quaternions in 4D space - reference "On Averaging Rotations" by CLAUS GRAMKOW
303  rotation_average += Eigen::Quaternionf (v_it->pose.rotation ()).coeffs ();
304  }
305 
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 ());
308 
309  Eigen::Affine3f transform_average;
310  transform_average.translation ().matrix () = translation_average;
311  transform_average.linear ().matrix () = Eigen::Quaternionf (rotation_average).normalized().toRotationMatrix ();
312 
313  result.push_back (PoseWithVotes (transform_average, cluster_votes[cluster_i].second));
314  }
315 }
316 
317 
318 //////////////////////////////////////////////////////////////////////////////////////////////
319 template <typename PointSource, typename PointTarget> bool
321  Eigen::Affine3f &pose2)
322 {
323  float position_diff = (pose1.translation () - pose2.translation ()).norm ();
324  Eigen::AngleAxisf rotation_diff_mat (pose1.rotation ().inverse () * pose2.rotation ());
325 
326  float rotation_diff_angle = fabsf (rotation_diff_mat.angle ());
327 
328  if (position_diff < clustering_position_diff_threshold_ && rotation_diff_angle < clustering_rotation_diff_threshold_)
329  return true;
330  else return false;
331 }
332 
333 
334 //////////////////////////////////////////////////////////////////////////////////////////////
335 template <typename PointSource, typename PointTarget> bool
338 {
339  return (a.votes > b.votes);
340 }
341 
342 
343 //////////////////////////////////////////////////////////////////////////////////////////////
344 template <typename PointSource, typename PointTarget> bool
346  const std::pair<size_t, unsigned int> &b)
347 {
348  return (a.second > b.second);
349 }
350 
351 //#define PCL_INSTANTIATE_PPFRegistration(PointSource,PointTarget) template class PCL_EXPORTS pcl::PPFRegistration<PointSource, PointTarget>;
352 
353 #endif // PCL_REGISTRATION_IMPL_PPF_REGISTRATION_H_