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_
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
Definition: point_cloud.h:429
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...
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
Definition: point_cloud.h:410
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
Definition: kdtree_flann.h:86