Point Cloud Library (PCL)  1.14.1-dev
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 #ifndef PCL_REGISTRATION_IMPL_PPF_REGISTRATION_H_
43 #define PCL_REGISTRATION_IMPL_PPF_REGISTRATION_H_
44 
45 #include <pcl/common/transforms.h>
46 #include <pcl/features/pfh.h>
47 #include <pcl/features/pfh_tools.h> // for computePairFeatures
48 #include <pcl/features/ppf.h>
49 #include <pcl/registration/ppf_registration.h>
50 //////////////////////////////////////////////////////////////////////////////////////////////
51 template <typename PointSource, typename PointTarget>
52 void
54  const PointCloudTargetConstPtr& cloud)
55 {
57 
58  scene_search_tree_ =
60  scene_search_tree_->setInputCloud(target_);
61 }
62 
63 //////////////////////////////////////////////////////////////////////////////////////////////
64 template <typename PointSource, typename PointTarget>
65 void
67  PointCloudSource& output, const Eigen::Matrix4f& guess)
68 {
69  if (!search_method_) {
70  PCL_ERROR("[pcl::PPFRegistration::computeTransformation] Search method not set - "
71  "skipping computeTransformation!\n");
72  return;
73  }
74 
75  if (guess != Eigen::Matrix4f::Identity()) {
76  PCL_ERROR("[pcl::PPFRegistration::computeTransformation] setting initial transform "
77  "(guess) not implemented!\n");
78  }
79 
80  const auto aux_size = static_cast<std::size_t>(
81  std::ceil(2 * M_PI / search_method_->getAngleDiscretizationStep()));
82  if (std::abs(std::round(2 * M_PI / search_method_->getAngleDiscretizationStep()) -
83  2 * M_PI / search_method_->getAngleDiscretizationStep()) > 0.1) {
84  PCL_WARN("[pcl::PPFRegistration::computeTransformation] The chosen angle "
85  "discretization step (%g) does not result in a uniform discretization. "
86  "Consider using e.g. 2pi/%zu or 2pi/%zu\n",
87  search_method_->getAngleDiscretizationStep(),
88  aux_size - 1,
89  aux_size);
90  }
91 
92  const std::vector<unsigned int> tmp_vec(aux_size, 0);
93  std::vector<std::vector<unsigned int>> accumulator_array(input_->size(), tmp_vec);
94 
95  PCL_DEBUG("[PPFRegistration] Accumulator array size: %u x %u.\n",
96  accumulator_array.size(),
97  accumulator_array.back().size());
98 
99  PoseWithVotesList voted_poses;
100  // Consider every <scene_reference_point_sampling_rate>-th point as the reference
101  // point => fix s_r
102  float f1, f2, f3, f4;
103  for (index_t scene_reference_index = 0;
104  scene_reference_index < static_cast<index_t>(target_->size());
105  scene_reference_index += scene_reference_point_sampling_rate_) {
106  Eigen::Vector3f scene_reference_point =
107  (*target_)[scene_reference_index].getVector3fMap(),
108  scene_reference_normal =
109  (*target_)[scene_reference_index].getNormalVector3fMap();
110 
111  float rotation_angle_sg =
112  std::acos(scene_reference_normal.dot(Eigen::Vector3f::UnitX()));
113  bool parallel_to_x_sg =
114  (scene_reference_normal.y() == 0.0f && scene_reference_normal.z() == 0.0f);
115  Eigen::Vector3f rotation_axis_sg =
116  (parallel_to_x_sg)
117  ? (Eigen::Vector3f::UnitY())
118  : (scene_reference_normal.cross(Eigen::Vector3f::UnitX()).normalized());
119  Eigen::AngleAxisf rotation_sg(rotation_angle_sg, rotation_axis_sg);
120  Eigen::Affine3f transform_sg(
121  Eigen::Translation3f(rotation_sg * ((-1) * scene_reference_point)) *
122  rotation_sg);
123 
124  // For every other point in the scene => now have pair (s_r, s_i) fixed
125  pcl::Indices indices;
126  std::vector<float> distances;
127  scene_search_tree_->radiusSearch((*target_)[scene_reference_index],
128  search_method_->getModelDiameter() / 2,
129  indices,
130  distances);
131  for (const auto& scene_point_index : indices)
132  // for(std::size_t i = 0; i < target_->size (); ++i)
133  {
134  // size_t scene_point_index = i;
135  if (scene_reference_index != scene_point_index) {
136  if (/*pcl::computePPFPairFeature*/ pcl::computePairFeatures(
137  (*target_)[scene_reference_index].getVector4fMap(),
138  (*target_)[scene_reference_index].getNormalVector4fMap(),
139  (*target_)[scene_point_index].getVector4fMap(),
140  (*target_)[scene_point_index].getNormalVector4fMap(),
141  f1,
142  f2,
143  f3,
144  f4)) {
145  std::vector<std::pair<std::size_t, std::size_t>> nearest_indices;
146  search_method_->nearestNeighborSearch(f1, f2, f3, f4, nearest_indices);
147 
148  // Compute alpha_s angle
149  const Eigen::Vector3f scene_point =
150  (*target_)[scene_point_index].getVector3fMap();
151 
152  const Eigen::Vector3f scene_point_transformed = transform_sg * scene_point;
153  float alpha_s =
154  std::atan2(-scene_point_transformed(2), scene_point_transformed(1));
155  if (std::sin(alpha_s) * scene_point_transformed(2) < 0.0f)
156  alpha_s *= (-1);
157  alpha_s *= (-1);
158 
159  // Go through point pairs in the model with the same discretized feature
160  for (const auto& nearest_index : nearest_indices) {
161  std::size_t model_reference_index = nearest_index.first;
162  std::size_t model_point_index = nearest_index.second;
163  // Calculate angle alpha = alpha_m - alpha_s
164  float alpha =
165  search_method_->alpha_m_[model_reference_index][model_point_index] -
166  alpha_s;
167  if (alpha < -M_PI) {
168  alpha += (2 * M_PI);
169  }
170  else if (alpha > M_PI) {
171  alpha -= (2 * M_PI);
172  }
173  auto alpha_discretized = static_cast<unsigned int>(std::floor(
174  (alpha + M_PI) / search_method_->getAngleDiscretizationStep()));
175  accumulator_array[model_reference_index][alpha_discretized]++;
176  }
177  }
178  else
179  PCL_ERROR("[pcl::PPFRegistration::computeTransformation] Computing pair "
180  "feature vector between points %u and %u went wrong.\n",
181  scene_reference_index,
182  scene_point_index);
183  }
184  }
185 
186  // the paper says: "For stability reasons, all peaks that received a certain amount
187  // of votes relative to the maximum peak are used." No specific value is mentioned,
188  // but 90% seems good
189  unsigned int max_votes = 0;
190  const std::size_t size_i = accumulator_array.size(),
191  size_j = accumulator_array.back().size();
192  for (std::size_t i = 0; i < size_i; ++i)
193  for (std::size_t j = 0; j < size_j; ++j) {
194  if (accumulator_array[i][j] > max_votes)
195  max_votes = accumulator_array[i][j];
196  }
197  max_votes *= 0.9;
198  for (std::size_t i = 0; i < size_i; ++i)
199  for (std::size_t j = 0; j < size_j; ++j) {
200  if (accumulator_array[i][j] >= max_votes) {
201  const Eigen::Vector3f model_reference_point = (*input_)[i].getVector3fMap(),
202  model_reference_normal =
203  (*input_)[i].getNormalVector3fMap();
204  const float rotation_angle_mg =
205  std::acos(model_reference_normal.dot(Eigen::Vector3f::UnitX()));
206  const bool parallel_to_x_mg = (model_reference_normal.y() == 0.0f &&
207  model_reference_normal.z() == 0.0f);
208  const Eigen::Vector3f rotation_axis_mg =
209  (parallel_to_x_mg)
210  ? (Eigen::Vector3f::UnitY())
211  : (model_reference_normal.cross(Eigen::Vector3f::UnitX())
212  .normalized());
213  const Eigen::AngleAxisf rotation_mg(rotation_angle_mg, rotation_axis_mg);
214  const Eigen::Affine3f transform_mg(
215  Eigen::Translation3f(rotation_mg * ((-1) * model_reference_point)) *
216  rotation_mg);
217  const Eigen::Affine3f max_transform =
218  transform_sg.inverse() *
219  Eigen::AngleAxisf((static_cast<float>(j + 0.5) *
220  search_method_->getAngleDiscretizationStep() -
221  M_PI),
222  Eigen::Vector3f::UnitX()) *
223  transform_mg;
224 
225  voted_poses.push_back(PoseWithVotes(max_transform, accumulator_array[i][j]));
226  }
227  // Reset accumulator_array for the next set of iterations with a new scene
228  // reference point
229  accumulator_array[i][j] = 0;
230  }
231  }
232  PCL_DEBUG("[PPFRegistration] Done with the Hough Transform ...\n");
233 
234  // Cluster poses for filtering out outliers and obtaining more precise results
235  clusterPoses(voted_poses, best_pose_candidates);
236 
237  pcl::transformPointCloud(*input_, output, best_pose_candidates.front().pose);
238 
239  transformation_ = final_transformation_ = best_pose_candidates.front().pose.matrix();
240  converged_ = true;
241 }
242 
243 //////////////////////////////////////////////////////////////////////////////////////////////
244 template <typename PointSource, typename PointTarget>
245 void
249 {
250  PCL_DEBUG("[PPFRegistration] Clustering poses (initially got %zu poses)\n",
251  poses.size());
252  // Start off by sorting the poses by the number of votes
253  sort(poses.begin(), poses.end(), poseWithVotesCompareFunction);
254 
255  std::vector<PoseWithVotesList> clusters;
256  std::vector<std::pair<std::size_t, unsigned int>> cluster_votes;
257  for (std::size_t poses_i = 0; poses_i < poses.size(); ++poses_i) {
258  bool found_cluster = false;
259  float lowest_position_diff = std::numeric_limits<float>::max(),
260  lowest_rotation_diff_angle = std::numeric_limits<float>::max();
261  std::size_t best_cluster = 0;
262  for (std::size_t clusters_i = 0; clusters_i < clusters.size(); ++clusters_i) {
263  // if a pose can be added to more than one cluster (posesWithinErrorBounds returns
264  // true), then add it to the one where position and rotation difference are
265  // smallest
266  float position_diff, rotation_diff_angle;
267  if (posesWithinErrorBounds(poses[poses_i].pose,
268  clusters[clusters_i].front().pose,
269  position_diff,
270  rotation_diff_angle)) {
271  if (!found_cluster) {
272  found_cluster = true;
273  best_cluster = clusters_i;
274  lowest_position_diff = position_diff;
275  lowest_rotation_diff_angle = rotation_diff_angle;
276  }
277  else if (position_diff < lowest_position_diff &&
278  rotation_diff_angle < lowest_rotation_diff_angle) {
279  best_cluster = clusters_i;
280  lowest_position_diff = position_diff;
281  lowest_rotation_diff_angle = rotation_diff_angle;
282  }
283  }
284  }
285  if (found_cluster) {
286  clusters[best_cluster].push_back(poses[poses_i]);
287  cluster_votes[best_cluster].second += poses[poses_i].votes;
288  }
289  else {
290  // Create a new cluster with the current pose
291  PoseWithVotesList new_cluster;
292  new_cluster.push_back(poses[poses_i]);
293  clusters.push_back(new_cluster);
294  cluster_votes.push_back(std::pair<std::size_t, unsigned int>(
295  clusters.size() - 1, poses[poses_i].votes));
296  }
297  }
298  PCL_DEBUG("[PPFRegistration] %zu poses remaining after clustering. Now averaging "
299  "each cluster and removing clusters with too few votes.\n",
300  clusters.size());
301 
302  // Sort clusters by total number of votes
303  std::sort(cluster_votes.begin(), cluster_votes.end(), clusterVotesCompareFunction);
304  // Compute pose average and put them in result vector
305  result.clear();
306  for (std::size_t cluster_i = 0; cluster_i < clusters.size(); ++cluster_i) {
307  // Remove all clusters that have less than 10% of the votes of the peak cluster.
308  // This way, if there is e.g. one cluster with far more votes than all other
309  // clusters, only that one is kept.
310  if (cluster_votes[cluster_i].second < 0.1 * cluster_votes[0].second)
311  continue;
312  PCL_DEBUG("Winning cluster has #votes: %d and #poses voted: %d.\n",
313  cluster_votes[cluster_i].second,
314  clusters[cluster_votes[cluster_i].first].size());
315  Eigen::Vector3f translation_average(0.0, 0.0, 0.0);
316  Eigen::Vector4f rotation_average(0.0, 0.0, 0.0, 0.0);
317  for (const auto& vote : clusters[cluster_votes[cluster_i].first]) {
318  translation_average += vote.pose.translation();
319  /// averaging rotations by just averaging the quaternions in 4D space - reference
320  /// "On Averaging Rotations" by CLAUS GRAMKOW
321  rotation_average += Eigen::Quaternionf(vote.pose.rotation()).coeffs();
322  }
323 
324  translation_average /=
325  static_cast<float>(clusters[cluster_votes[cluster_i].first].size());
326  rotation_average /=
327  static_cast<float>(clusters[cluster_votes[cluster_i].first].size());
328 
329  Eigen::Affine3f transform_average;
330  transform_average.translation().matrix() = translation_average;
331  transform_average.linear().matrix() =
332  Eigen::Quaternionf(rotation_average).normalized().toRotationMatrix();
333 
334  result.push_back(PoseWithVotes(transform_average, cluster_votes[cluster_i].second));
335  }
336 }
337 
338 //////////////////////////////////////////////////////////////////////////////////////////////
339 template <typename PointSource, typename PointTarget>
340 bool
342  Eigen::Affine3f& pose1,
343  Eigen::Affine3f& pose2,
344  float& position_diff,
345  float& rotation_diff_angle)
346 {
347  position_diff = (pose1.translation() - pose2.translation()).norm();
348  Eigen::AngleAxisf rotation_diff_mat(
349  (pose1.rotation().inverse().lazyProduct(pose2.rotation()).eval()));
350 
351  rotation_diff_angle = std::abs(rotation_diff_mat.angle());
352 
353  return (position_diff < clustering_position_diff_threshold_ &&
354  rotation_diff_angle < clustering_rotation_diff_threshold_);
355 }
356 
357 //////////////////////////////////////////////////////////////////////////////////////////////
358 template <typename PointSource, typename PointTarget>
359 bool
363 {
364  return (a.votes > b.votes);
365 }
366 
367 //////////////////////////////////////////////////////////////////////////////////////////////
368 template <typename PointSource, typename PointTarget>
369 bool
371  const std::pair<std::size_t, unsigned int>& a,
372  const std::pair<std::size_t, unsigned int>& b)
373 {
374  return (a.second > b.second);
375 }
376 
377 //#define PCL_INSTANTIATE_PPFRegistration(PointSource,PointTarget) template class
378 // PCL_EXPORTS pcl::PPFRegistration<PointSource, PointTarget>;
379 
380 #endif // PCL_REGISTRATION_IMPL_PPF_REGISTRATION_H_
shared_ptr< KdTreeFLANN< PointT, Dist > > Ptr
Definition: kdtree_flann.h:151
Class that registers two point clouds based on their sets of PPFSignatures.
std::vector< PoseWithVotes, Eigen::aligned_allocator< PoseWithVotes > > PoseWithVotesList
typename PointCloudTarget::ConstPtr PointCloudTargetConstPtr
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...
Registration represents the base registration class for general purpose, ICP-like methods.
Definition: registration.h:57
void transformPointCloud(const pcl::PointCloud< PointT > &cloud_in, pcl::PointCloud< PointT > &cloud_out, const Eigen::Matrix< Scalar, 4, 4 > &transform, bool copy_all_fields)
Apply a rigid transform defined by a 4x4 matrix.
Definition: transforms.hpp:221
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...
__device__ __forceinline__ float3 normalized(const float3 &v)
Definition: utils.hpp:101
__device__ __host__ __forceinline__ float norm(const float3 &v1, const float3 &v2)
detail::int_type_t< detail::index_type_size, detail::index_type_signed > index_t
Type used for an index in PCL.
Definition: types.h:112
IndicesAllocator<> Indices
Type used for indices in PCL.
Definition: types.h:133
#define M_PI
Definition: pcl_macros.h:201
Structure for storing a pose (represented as an Eigen::Affine3f) and an integer for counting votes.