Point Cloud Library (PCL)  1.11.1
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 template <typename PointSource, typename PointTarget> void
54 {
56 
57  scene_search_tree_ = typename pcl::KdTreeFLANN<PointTarget>::Ptr (new pcl::KdTreeFLANN<PointTarget>);
58  scene_search_tree_->setInputCloud (target_);
59 }
60 
61 //////////////////////////////////////////////////////////////////////////////////////////////
62 template <typename PointSource, typename PointTarget> void
63 pcl::PPFRegistration<PointSource, PointTarget>::computeTransformation (PointCloudSource &output, const Eigen::Matrix4f& guess)
64 {
65  if (!search_method_)
66  {
67  PCL_ERROR("[pcl::PPFRegistration::computeTransformation] Search method not set - skipping computeTransformation!\n");
68  return;
69  }
70 
71  if (guess != Eigen::Matrix4f::Identity ())
72  {
73  PCL_ERROR("[pcl::PPFRegistration::computeTransformation] setting initial transform (guess) not implemented!\n");
74  }
75 
76  const auto aux_size = static_cast<std::size_t>(
77  std::floor(2 * M_PI / search_method_->getAngleDiscretizationStep()));
78 
79  const std::vector<unsigned int> tmp_vec(aux_size, 0);
80  std::vector<std::vector<unsigned int>> accumulator_array(input_->size(), tmp_vec);
81 
82  PCL_INFO ("Accumulator array size: %u x %u.\n", accumulator_array.size (), accumulator_array.back ().size ());
83 
84  PoseWithVotesList voted_poses;
85  // Consider every <scene_reference_point_sampling_rate>-th point as the reference point => fix s_r
86  float f1, f2, f3, f4;
87  for (index_t scene_reference_index = 0;
88  scene_reference_index < static_cast<index_t>(target_->size()); scene_reference_index += scene_reference_point_sampling_rate_) {
89  Eigen::Vector3f scene_reference_point = (*target_)[scene_reference_index].getVector3fMap (),
90  scene_reference_normal = (*target_)[scene_reference_index].getNormalVector3fMap ();
91 
92  float rotation_angle_sg = std::acos (scene_reference_normal.dot (Eigen::Vector3f::UnitX ()));
93  bool parallel_to_x_sg = (scene_reference_normal.y() == 0.0f && scene_reference_normal.z() == 0.0f);
94  Eigen::Vector3f rotation_axis_sg = (parallel_to_x_sg)?(Eigen::Vector3f::UnitY ()):(scene_reference_normal.cross (Eigen::Vector3f::UnitX ()). normalized());
95  Eigen::AngleAxisf rotation_sg (rotation_angle_sg, rotation_axis_sg);
96  Eigen::Affine3f transform_sg (Eigen::Translation3f ( rotation_sg * ((-1) * scene_reference_point)) * rotation_sg);
97 
98  // For every other point in the scene => now have pair (s_r, s_i) fixed
99  std::vector<int> indices;
100  std::vector<float> distances;
101  scene_search_tree_->radiusSearch ((*target_)[scene_reference_index],
102  search_method_->getModelDiameter () /2,
103  indices,
104  distances);
105  for(const std::size_t &scene_point_index : indices)
106 // for(std::size_t i = 0; i < target_->size (); ++i)
107  {
108  //size_t scene_point_index = i;
109  if (scene_reference_index != scene_point_index)
110  {
111  if (/*pcl::computePPFPairFeature*/pcl::computePairFeatures ((*target_)[scene_reference_index].getVector4fMap (),
112  (*target_)[scene_reference_index].getNormalVector4fMap (),
113  (*target_)[scene_point_index].getVector4fMap (),
114  (*target_)[scene_point_index].getNormalVector4fMap (),
115  f1, f2, f3, f4))
116  {
117  std::vector<std::pair<std::size_t, std::size_t> > nearest_indices;
118  search_method_->nearestNeighborSearch (f1, f2, f3, f4, nearest_indices);
119 
120  // Compute alpha_s angle
121  Eigen::Vector3f scene_point = (*target_)[scene_point_index].getVector3fMap ();
122 
123  Eigen::Vector3f scene_point_transformed = transform_sg * scene_point;
124  float alpha_s = std::atan2 ( -scene_point_transformed(2), scene_point_transformed(1));
125  if (std::sin (alpha_s) * scene_point_transformed(2) < 0.0f)
126  alpha_s *= (-1);
127  alpha_s *= (-1);
128 
129  // Go through point pairs in the model with the same discretized feature
130  for (const auto &nearest_index : nearest_indices)
131  {
132  std::size_t model_reference_index = nearest_index.first;
133  std::size_t model_point_index = nearest_index.second;
134  // Calculate angle alpha = alpha_m - alpha_s
135  float alpha = search_method_->alpha_m_[model_reference_index][model_point_index] - alpha_s;
136  unsigned int alpha_discretized = static_cast<unsigned int> (std::floor (alpha) + std::floor (M_PI / search_method_->getAngleDiscretizationStep ()));
137  accumulator_array[model_reference_index][alpha_discretized] ++;
138  }
139  }
140  else PCL_ERROR ("[pcl::PPFRegistration::computeTransformation] Computing pair feature vector between points %u and %u went wrong.\n", scene_reference_index, scene_point_index);
141  }
142  }
143 
144  std::size_t max_votes_i = 0, max_votes_j = 0;
145  unsigned int max_votes = 0;
146 
147  for (std::size_t i = 0; i < accumulator_array.size (); ++i)
148  for (std::size_t j = 0; j < accumulator_array.back ().size (); ++j)
149  {
150  if (accumulator_array[i][j] > max_votes)
151  {
152  max_votes = accumulator_array[i][j];
153  max_votes_i = i;
154  max_votes_j = j;
155  }
156  // Reset accumulator_array for the next set of iterations with a new scene reference point
157  accumulator_array[i][j] = 0;
158  }
159 
160  Eigen::Vector3f model_reference_point = (*input_)[max_votes_i].getVector3fMap (),
161  model_reference_normal = (*input_)[max_votes_i].getNormalVector3fMap ();
162  float rotation_angle_mg = std::acos (model_reference_normal.dot (Eigen::Vector3f::UnitX ()));
163  bool parallel_to_x_mg = (model_reference_normal.y() == 0.0f && model_reference_normal.z() == 0.0f);
164  Eigen::Vector3f rotation_axis_mg = (parallel_to_x_mg)?(Eigen::Vector3f::UnitY ()):(model_reference_normal.cross (Eigen::Vector3f::UnitX ()). normalized());
165  Eigen::AngleAxisf rotation_mg (rotation_angle_mg, rotation_axis_mg);
166  Eigen::Affine3f transform_mg (Eigen::Translation3f ( rotation_mg * ((-1) * model_reference_point)) * rotation_mg);
167  Eigen::Affine3f max_transform =
168  transform_sg.inverse () *
169  Eigen::AngleAxisf ((static_cast<float> (max_votes_j) - std::floor (static_cast<float> (M_PI) / search_method_->getAngleDiscretizationStep ())) * search_method_->getAngleDiscretizationStep (), Eigen::Vector3f::UnitX ()) *
170  transform_mg;
171 
172  voted_poses.push_back (PoseWithVotes (max_transform, max_votes));
173  }
174  PCL_DEBUG ("Done with the Hough Transform ...\n");
175 
176  // Cluster poses for filtering out outliers and obtaining more precise results
177  PoseWithVotesList results;
178  clusterPoses (voted_poses, results);
179 
180  pcl::transformPointCloud (*input_, output, results.front ().pose);
181 
182  transformation_ = final_transformation_ = results.front ().pose.matrix ();
183  converged_ = true;
184 }
185 
186 
187 //////////////////////////////////////////////////////////////////////////////////////////////
188 template <typename PointSource, typename PointTarget> void
191 {
192  PCL_INFO ("Clustering poses ...\n");
193  // Start off by sorting the poses by the number of votes
194  sort(poses.begin (), poses.end (), poseWithVotesCompareFunction);
195 
196  std::vector<PoseWithVotesList> clusters;
197  std::vector<std::pair<std::size_t, unsigned int> > cluster_votes;
198  for (std::size_t poses_i = 0; poses_i < poses.size(); ++ poses_i)
199  {
200  bool found_cluster = false;
201  for (std::size_t clusters_i = 0; clusters_i < clusters.size(); ++ clusters_i)
202  {
203  if (posesWithinErrorBounds (poses[poses_i].pose, clusters[clusters_i].front ().pose))
204  {
205  found_cluster = true;
206  clusters[clusters_i].push_back (poses[poses_i]);
207  cluster_votes[clusters_i].second += poses[poses_i].votes;
208  break;
209  }
210  }
211 
212  if (!found_cluster)
213  {
214  // Create a new cluster with the current pose
215  PoseWithVotesList new_cluster;
216  new_cluster.push_back (poses[poses_i]);
217  clusters.push_back (new_cluster);
218  cluster_votes.push_back (std::pair<std::size_t, unsigned int> (clusters.size () - 1, poses[poses_i].votes));
219  }
220  }
221 
222  // Sort clusters by total number of votes
223  std::sort (cluster_votes.begin (), cluster_votes.end (), clusterVotesCompareFunction);
224  // Compute pose average and put them in result vector
225  /// @todo some kind of threshold for determining whether a cluster has enough votes or not...
226  /// now just taking the first three clusters
227  result.clear ();
228  std::size_t max_clusters = (clusters.size () < 3) ? clusters.size () : 3;
229  for (std::size_t cluster_i = 0; cluster_i < max_clusters; ++ cluster_i)
230  {
231  PCL_INFO ("Winning cluster has #votes: %d and #poses voted: %d.\n", cluster_votes[cluster_i].second, clusters[cluster_votes[cluster_i].first].size ());
232  Eigen::Vector3f translation_average (0.0, 0.0, 0.0);
233  Eigen::Vector4f rotation_average (0.0, 0.0, 0.0, 0.0);
234  for (typename PoseWithVotesList::iterator v_it = clusters[cluster_votes[cluster_i].first].begin (); v_it != clusters[cluster_votes[cluster_i].first].end (); ++ v_it)
235  {
236  translation_average += v_it->pose.translation ();
237  /// averaging rotations by just averaging the quaternions in 4D space - reference "On Averaging Rotations" by CLAUS GRAMKOW
238  rotation_average += Eigen::Quaternionf (v_it->pose.rotation ()).coeffs ();
239  }
240 
241  translation_average /= static_cast<float> (clusters[cluster_votes[cluster_i].first].size ());
242  rotation_average /= static_cast<float> (clusters[cluster_votes[cluster_i].first].size ());
243 
244  Eigen::Affine3f transform_average;
245  transform_average.translation ().matrix () = translation_average;
246  transform_average.linear ().matrix () = Eigen::Quaternionf (rotation_average).normalized().toRotationMatrix ();
247 
248  result.push_back (PoseWithVotes (transform_average, cluster_votes[cluster_i].second));
249  }
250 }
251 
252 
253 //////////////////////////////////////////////////////////////////////////////////////////////
254 template <typename PointSource, typename PointTarget> bool
256  Eigen::Affine3f &pose2)
257 {
258  float position_diff = (pose1.translation () - pose2.translation ()).norm ();
259  Eigen::AngleAxisf rotation_diff_mat ((pose1.rotation ().inverse ().lazyProduct (pose2.rotation ()).eval()));
260 
261  float rotation_diff_angle = std::abs (rotation_diff_mat.angle ());
262 
263  return (position_diff < clustering_position_diff_threshold_ && rotation_diff_angle < clustering_rotation_diff_threshold_);
264 }
265 
266 
267 //////////////////////////////////////////////////////////////////////////////////////////////
268 template <typename PointSource, typename PointTarget> bool
271 {
272  return (a.votes > b.votes);
273 }
274 
275 
276 //////////////////////////////////////////////////////////////////////////////////////////////
277 template <typename PointSource, typename PointTarget> bool
278 pcl::PPFRegistration<PointSource, PointTarget>::clusterVotesCompareFunction (const std::pair<std::size_t, unsigned int> &a,
279  const std::pair<std::size_t, unsigned int> &b)
280 {
281  return (a.second > b.second);
282 }
283 
284 //#define PCL_INSTANTIATE_PPFRegistration(PointSource,PointTarget) template class PCL_EXPORTS pcl::PPFRegistration<PointSource, PointTarget>;
285 
286 #endif // PCL_REGISTRATION_IMPL_PPF_REGISTRATION_H_
std::vector< PoseWithVotes, Eigen::aligned_allocator< PoseWithVotes > > PoseWithVotesList
__device__ __forceinline__ float3 normalized(const float3 &v)
Definition: utils.hpp:101
detail::int_type_t< detail::index_type_size, detail::index_type_signed > index_t
Type used for an index in PCL.
Definition: types.h:120
#define M_PI
Definition: pcl_macros.h:192
void setInputTarget(const PointCloudTargetConstPtr &cloud) override
Provide a pointer to the input target (e.g., the point cloud that we want to align the input source t...
void transformPointCloud(const pcl::PointCloud< PointT > &cloud_in, pcl::PointCloud< PointT > &cloud_out, const Eigen::Transform< Scalar, 3, Eigen::Affine > &transform, bool copy_all_fields)
Apply an affine transform defined by an Eigen Transform.
Definition: transforms.hpp:221
typename PointCloudTarget::ConstPtr PointCloudTargetConstPtr
Registration represents the base registration class for general purpose, ICP-like methods...
Definition: registration.h:61
Structure for storing a pose (represented as an Eigen::Affine3f) and an integer for counting votes...
Class that registers two point clouds based on their sets of PPFSignatures.
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...
shared_ptr< KdTreeFLANN< PointT, Dist > > Ptr
Definition: kdtree_flann.h:87