Point Cloud Library (PCL)  1.8.0
ia_fpcs.hpp
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Point Cloud Library (PCL) - www.pointclouds.org
5  * Copyright (c) 2014-, Open Perception, Inc.
6  * Copyright (C) 2008 Ben Gurion University of the Negev, Beer Sheva, Israel.
7  *
8  * All rights reserved
9  *
10  * Redistribution and use in source and binary forms, with or without
11  * modification, are permitted provided that the following conditions are met
12  *
13  * * The use for research only (no for any commercial application).
14  * * Redistributions of source code must retain the above copyright
15  * notice, this list of conditions and the following disclaimer.
16  * * Redistributions in binary form must reproduce the above
17  * copyright notice, this list of conditions and the following
18  * disclaimer in the documentation and/or other materials provided
19  * with the distribution.
20  * * Neither the name of the copyright holder(s) nor the names of its
21  * contributors may be used to endorse or promote products derived
22  * from this software without specific prior written permission.
23  *
24  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
25  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
26  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
27  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
28  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
29  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
30  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
31  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
32  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
33  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
34  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
35  * POSSIBILITY OF SUCH DAMAGE.
36  *
37  */
38 
39 #ifndef PCL_REGISTRATION_IMPL_IA_FPCS_H_
40 #define PCL_REGISTRATION_IMPL_IA_FPCS_H_
41 
42 #include <pcl/registration/ia_fpcs.h>
43 #include <pcl/common/time.h>
44 #include <pcl/common/distances.h>
45 #include <pcl/sample_consensus/sac_model_plane.h>
46 #include <pcl/registration/transformation_estimation_3point.h>
47 
48 ///////////////////////////////////////////////////////////////////////////////////////////
49 template <typename PointT> inline float
50 pcl::getMeanPointDensity (const typename pcl::PointCloud<PointT>::ConstPtr &cloud, float max_dist, int nr_threads)
51 {
52  const float max_dist_sqr = max_dist * max_dist;
53  const std::size_t s = cloud.size ();
54 
56  tree.setInputCloud (cloud);
57 
58  float mean_dist = 0.f;
59  int num = 0;
60  std::vector <int> ids (2);
61  std::vector <float> dists_sqr (2);
62 
63 #ifdef _OPENMP
64 #pragma omp parallel for \
65  reduction (+:mean_dist, num) \
66  private (ids, dists_sqr) shared (tree, cloud) \
67  default (none)num_threads (nr_threads)
68 #endif
69 
70  for (int i = 0; i < 1000; i++)
71  {
72  tree.nearestKSearch (cloud->points[rand () % s], 2, ids, dists_sqr);
73  if (dists_sqr[1] < max_dist_sqr)
74  {
75  mean_dist += std::sqrt (dists_sqr[1]);
76  num++;
77  }
78  }
79 
80  return (mean_dist / num);
81 };
82 
83 
84 ///////////////////////////////////////////////////////////////////////////////////////////
85 template <typename PointT> inline float
86 pcl::getMeanPointDensity (const typename pcl::PointCloud<PointT>::ConstPtr &cloud, const std::vector <int> &indices,
87  float max_dist, int nr_threads)
88 {
89  const float max_dist_sqr = max_dist * max_dist;
90  const std::size_t s = indices.size ();
91 
93  tree.setInputCloud (cloud);
94 
95  float mean_dist = 0.f;
96  int num = 0;
97  std::vector <int> ids (2);
98  std::vector <float> dists_sqr (2);
99 
100 #ifdef _OPENMP
101 #pragma omp parallel for \
102  reduction (+:mean_dist, num) \
103  private (ids, dists_sqr) shared (tree, cloud, indices) \
104  default (none)num_threads (nr_threads)
105 #endif
106 
107  for (int i = 0; i < 1000; i++)
108  {
109  tree.nearestKSearch (cloud->points[indices[rand () % s]], 2, ids, dists_sqr);
110  if (dists_sqr[1] < max_dist_sqr)
111  {
112  mean_dist += std::sqrt (dists_sqr[1]);
113  num++;
114  }
115  }
116 
117  return (mean_dist / num);
118 };
119 
120 
121 ///////////////////////////////////////////////////////////////////////////////////////////
122 template <typename PointSource, typename PointTarget, typename NormalT, typename Scalar>
124  source_normals_ (),
125  target_normals_ (),
126  nr_threads_ (1),
127  approx_overlap_ (0.5f),
128  delta_ (1.f),
129  score_threshold_ (FLT_MAX),
130  nr_samples_ (0),
131  max_norm_diff_ (90.f),
132  max_runtime_ (0),
133  fitness_score_ (FLT_MAX),
134  diameter_ (),
135  max_base_diameter_sqr_ (),
136  use_normals_ (false),
137  normalize_delta_ (true),
138  max_pair_diff_ (),
139  max_edge_diff_ (),
140  coincidation_limit_ (),
141  max_mse_ (),
142  max_inlier_dist_sqr_ (),
143  small_error_ (0.00001f)
144 {
145  reg_name_ = "pcl::registration::FPCSInitialAlignment";
146  max_iterations_ = 0;
147  ransac_iterations_ = 1000;
149 }
150 
151 
152 ///////////////////////////////////////////////////////////////////////////////////////////
153 template <typename PointSource, typename PointTarget, typename NormalT, typename Scalar> void
155  PointCloudSource &output,
156  const Eigen::Matrix4f &guess)
157 {
158  if (!initCompute ())
159  return;
160 
161  final_transformation_ = guess;
162  bool abort = false;
163  std::vector <MatchingCandidates> all_candidates (max_iterations_);
164  pcl::StopWatch timer;
165 
166  #ifdef _OPENMP
167  #pragma omp parallel num_threads (nr_threads_)
168  #endif
169  {
170  #ifdef _OPENMP
171  std::srand (static_cast <unsigned int> (std::time (NULL)) ^ omp_get_thread_num ());
172  #pragma omp for schedule (dynamic)
173  #endif
174  for (int i = 0; i < max_iterations_; i++)
175  {
176 
177  #ifdef _OPENMP
178  #pragma omp flush (abort)
179  #endif
180 
181  MatchingCandidates candidates (1);
182  std::vector <int> base_indices (4);
183  float ratio[2];
184  all_candidates[i] = candidates;
185 
186  if (!abort)
187  {
188  // select four coplanar point base
189  if (selectBase (base_indices, ratio) == 0)
190  {
191  // calculate candidate pair correspondences using diagonal lenghts of base
192  pcl::Correspondences pairs_a, pairs_b;
193  if (bruteForceCorrespondences (base_indices[0], base_indices[1], pairs_a) == 0 &&
194  bruteForceCorrespondences (base_indices[2], base_indices[3], pairs_b) == 0)
195  {
196  // determine candidate matches by combining pair correspondences based on segment distances
197  std::vector <std::vector <int> > matches;
198  if (determineBaseMatches (base_indices, matches, pairs_a, pairs_b, ratio) == 0)
199  {
200  // check and evaluate candidate matches and store them
201  handleMatches (base_indices, matches, candidates);
202  if (candidates.size () != 0)
203  all_candidates[i] = candidates;
204  }
205  }
206  }
207 
208  // check terminate early (time or fitness_score threshold reached)
209  abort = (candidates.size () > 0 ? candidates[0].fitness_score < score_threshold_ : abort);
210  abort = (abort ? abort : timer.getTimeSeconds () > max_runtime_);
211 
212 
213  #ifdef _OPENMP
214  #pragma omp flush (abort)
215  #endif
216  }
217  }
218  }
219 
220 
221  // determine best match over all trys
222  finalCompute (all_candidates);
223 
224  // apply the final transformation
225  pcl::transformPointCloud (*input_, output, final_transformation_);
226 
227  deinitCompute ();
228 }
229 
230 
231 ///////////////////////////////////////////////////////////////////////////////////////////
232 template <typename PointSource, typename PointTarget, typename NormalT, typename Scalar> bool
234 {
235  std::srand (static_cast <unsigned int> (std::time (NULL)));
236 
237  // basic pcl initialization
239  return (false);
240 
241  // check if source and target are given
242  if (!input_ || !target_)
243  {
244  PCL_ERROR ("[%s::initCompute] Source or target dataset not given!\n", reg_name_.c_str ());
245  return (false);
246  }
247 
248  if (!target_indices_ || target_indices_->size () == 0)
249  {
250  target_indices_.reset (new std::vector <int> (static_cast <int> (target_->size ())));
251  int index = 0;
252  for (std::vector <int>::iterator it = target_indices_->begin (), it_e = target_indices_->end (); it != it_e; it++)
253  *it = index++;
254  target_cloud_updated_ = true;
255  }
256 
257  // if a sample size for the point clouds is given; prefarably no sampling of target cloud
258  if (nr_samples_ != 0)
259  {
260  const int ss = static_cast <int> (indices_->size ());
261  const int sample_fraction_src = std::max (1, static_cast <int> (ss / nr_samples_));
262 
263  source_indices_ = pcl::IndicesPtr (new std::vector <int>);
264  for (int i = 0; i < ss; i++)
265  if (rand () % sample_fraction_src == 0)
266  source_indices_->push_back ((*indices_) [i]);
267  }
268  else
269  source_indices_ = indices_;
270 
271  // check usage of normals
272  if (source_normals_ && target_normals_ && source_normals_->size () == input_->size () && target_normals_->size () == target_->size ())
273  use_normals_ = true;
274 
275  // set up tree structures
276  if (target_cloud_updated_)
277  {
278  tree_->setInputCloud (target_, target_indices_);
279  target_cloud_updated_ = false;
280  }
281 
282  // set predefined variables
283  const int min_iterations = 4;
284  const float diameter_fraction = 0.3f;
285 
286  // get diameter of input cloud (distance between farthest points)
287  Eigen::Vector4f pt_min, pt_max;
288  pcl::getMinMax3D (*target_, *target_indices_, pt_min, pt_max);
289  diameter_ = (pt_max - pt_min).norm ();
290 
291  // derive the limits for the random base selection
292  float max_base_diameter = diameter_* approx_overlap_ * 2.f;
293  max_base_diameter_sqr_ = max_base_diameter * max_base_diameter;
294 
295  // normalize the delta
296  if (normalize_delta_)
297  {
298  float mean_dist = getMeanPointDensity <PointTarget> (target_, *target_indices_, 0.05f * diameter_, nr_threads_);
299  delta_ *= mean_dist;
300  }
301 
302  // heuristic determination of number of trials to have high probabilty of finding a good solution
303  if (max_iterations_ == 0)
304  {
305  float first_est = std::log (small_error_) / std::log (1.0 - std::pow ((double) approx_overlap_, (double) min_iterations));
306  max_iterations_ = static_cast <int> (first_est / (diameter_fraction * approx_overlap_ * 2.f));
307  }
308 
309  // set further parameter
310  if (score_threshold_ == FLT_MAX)
311  score_threshold_ = 1.f - approx_overlap_;
312 
313  if (max_iterations_ < 4)
314  max_iterations_ = 4;
315 
316  if (max_runtime_ < 1)
317  max_runtime_ = INT_MAX;
318 
319  // calculate internal parameters based on the the estimated point density
320  max_pair_diff_ = delta_ * 2.f;
321  max_edge_diff_ = delta_ * 4.f;
322  coincidation_limit_ = delta_ * 2.f; // EDITED: originally std::sqrt (delta_ * 2.f)
323  max_mse_ = powf (delta_* 2.f, 2.f);
324  max_inlier_dist_sqr_ = powf (delta_ * 2.f, 2.f);
325 
326  // reset fitness_score
327  fitness_score_ = FLT_MAX;
328 
329  return (true);
330 }
331 
332 
333 ///////////////////////////////////////////////////////////////////////////////////////////
334 template <typename PointSource, typename PointTarget, typename NormalT, typename Scalar> int
336  std::vector <int> &base_indices,
337  float (&ratio)[2])
338 {
339  const float too_close_sqr = max_base_diameter_sqr_*0.01;
340 
341  Eigen::VectorXf coefficients (4);
343  plane.setIndices (target_indices_);
344  Eigen::Vector4f centre_pt;
345  float nearest_to_plane = FLT_MAX;
346 
347  // repeat base search until valid quadruple was found or ransac_iterations_ number of trys were unsuccessfull
348  for (int i = 0; i < ransac_iterations_; i++)
349  {
350  // random select an appropriate point triple
351  if (selectBaseTriangle (base_indices) < 0)
352  continue;
353 
354  std::vector <int> base_triple (base_indices.begin (), base_indices.end () - 1);
355  plane.computeModelCoefficients (base_triple, coefficients);
356  pcl::compute3DCentroid (*target_, base_triple, centre_pt);
357 
358  // loop over all points in source cloud to find most suitable fourth point
359  const PointTarget *pt1 = &(target_->points[base_indices[0]]);
360  const PointTarget *pt2 = &(target_->points[base_indices[1]]);
361  const PointTarget *pt3 = &(target_->points[base_indices[2]]);
362 
363  for (std::vector <int>::iterator it = target_indices_->begin (), it_e = target_indices_->end (); it != it_e; it++)
364  {
365  const PointTarget *pt4 = &(target_->points[*it]);
366 
367  float d1 = pcl::squaredEuclideanDistance (*pt4, *pt1);
368  float d2 = pcl::squaredEuclideanDistance (*pt4, *pt2);
369  float d3 = pcl::squaredEuclideanDistance (*pt4, *pt3);
370  float d4 = (pt4->getVector3fMap () - centre_pt.head (3)).squaredNorm ();
371 
372  // check distance between points w.r.t minimum sampling distance; EDITED -> 4th point now also limited by max base line
373  if (d1 < too_close_sqr || d2 < too_close_sqr || d3 < too_close_sqr || d4 < too_close_sqr ||
374  d1 > max_base_diameter_sqr_ || d2 > max_base_diameter_sqr_ || d3 > max_base_diameter_sqr_)
375  continue;
376 
377  // check distance to plane to get point closest to plane
378  float dist_to_plane = pcl::pointToPlaneDistance (*pt4, coefficients);
379  if (dist_to_plane < nearest_to_plane)
380  {
381  base_indices[3] = *it;
382  nearest_to_plane = dist_to_plane;
383  }
384  }
385 
386  // check if at least one point fullfilled the conditions
387  if (nearest_to_plane != FLT_MAX)
388  {
389  // order points to build largest quadrangle and calcuate intersection ratios of diagonals
390  setupBase (base_indices, ratio);
391  return (0);
392  }
393  }
394 
395  // return unsuccessfull if no quadruple was selected
396  return (-1);
397 }
398 
399 
400 ///////////////////////////////////////////////////////////////////////////////////////////
401 template <typename PointSource, typename PointTarget, typename NormalT, typename Scalar> int
403 {
404  int nr_points = static_cast <int> (target_indices_->size ());
405  float best_t = 0.f;
406 
407  // choose random first point
408  base_indices[0] = (*target_indices_)[rand () % nr_points];
409  int *index1 = &base_indices[0];
410 
411  // random search for 2 other points (as far away as overlap allows)
412  for (int i = 0; i < ransac_iterations_; i++)
413  {
414  int *index2 = &(*target_indices_)[rand () % nr_points];
415  int *index3 = &(*target_indices_)[rand () % nr_points];
416 
417  Eigen::Vector3f u = target_->points[*index2].getVector3fMap () - target_->points[*index1].getVector3fMap ();
418  Eigen::Vector3f v = target_->points[*index3].getVector3fMap () - target_->points[*index1].getVector3fMap ();
419  float t = u.cross (v).squaredNorm (); // triangle area (0.5 * sqrt(t)) should be maximal
420 
421  // check for most suitable point triple
422  if (t > best_t && u.squaredNorm () < max_base_diameter_sqr_ && v.squaredNorm () < max_base_diameter_sqr_)
423  {
424  best_t = t;
425  base_indices[1] = *index2;
426  base_indices[2] = *index3;
427  }
428  }
429 
430  // return if a triplet could be selected
431  return (best_t == 0.f ? -1 : 0);
432 }
433 
434 
435 ///////////////////////////////////////////////////////////////////////////////////////////
436 template <typename PointSource, typename PointTarget, typename NormalT, typename Scalar> void
438  std::vector <int> &base_indices,
439  float (&ratio)[2])
440 {
441  float best_t = FLT_MAX;
442  const std::vector <int> copy (base_indices.begin (), base_indices.end ());
443  std::vector <int> temp (base_indices.begin (), base_indices.end ());
444 
445  // loop over all combinations of base points
446  for (std::vector <int>::const_iterator i = copy.begin (), i_e = copy.end (); i != i_e; i++)
447  for (std::vector <int>::const_iterator j = copy.begin (), j_e = copy.end (); j != j_e; j++)
448  {
449  if (i == j)
450  continue;
451 
452  for (std::vector <int>::const_iterator k = copy.begin (), k_e = copy.end (); k != k_e; k++)
453  {
454  if (k == j || k == i)
455  continue;
456 
457  std::vector <int>::const_iterator l = copy.begin ();
458  while (l == i || l == j || l == k)
459  l++;
460 
461  temp[0] = *i;
462  temp[1] = *j;
463  temp[2] = *k;
464  temp[3] = *l;
465 
466  // calculate diagonal intersection ratios and check for suitable segment to segment distances
467  float ratio_temp[2];
468  float t = segmentToSegmentDist (temp, ratio_temp);
469  if (t < best_t)
470  {
471  best_t = t;
472  ratio[0] = ratio_temp[0];
473  ratio[1] = ratio_temp[1];
474  base_indices = temp;
475  }
476  }
477  }
478 }
479 
480 
481 ///////////////////////////////////////////////////////////////////////////////////////////
482 template <typename PointSource, typename PointTarget, typename NormalT, typename Scalar> float
484  const std::vector <int> &base_indices,
485  float (&ratio)[2])
486 {
487  // get point vectors
488  Eigen::Vector3f u = target_->points[base_indices[1]].getVector3fMap () - target_->points[base_indices[0]].getVector3fMap ();
489  Eigen::Vector3f v = target_->points[base_indices[3]].getVector3fMap () - target_->points[base_indices[2]].getVector3fMap ();
490  Eigen::Vector3f w = target_->points[base_indices[0]].getVector3fMap () - target_->points[base_indices[2]].getVector3fMap ();
491 
492  // calculate segment distances
493  float a = u.dot (u);
494  float b = u.dot (v);
495  float c = v.dot (v);
496  float d = u.dot (w);
497  float e = v.dot (w);
498  float D = a * c - b * b;
499  float sN = 0.f, sD = D;
500  float tN = 0.f, tD = D;
501 
502  // check segments
503  if (D < small_error_)
504  {
505  sN = 0.f;
506  sD = 1.f;
507  tN = e;
508  tD = c;
509  }
510  else
511  {
512  sN = (b * e - c * d);
513  tN = (a * e - b * d);
514 
515  if (sN < 0.f)
516  {
517  sN = 0.f;
518  tN = e;
519  tD = c;
520  }
521  else if (sN > sD)
522  {
523  sN = sD;
524  tN = e + b;
525  tD = c;
526  }
527  }
528 
529  if (tN < 0.f)
530  {
531  tN = 0.f;
532 
533  if (-d < 0.f)
534  sN = 0.f;
535 
536  else if (-d > a)
537  sN = sD;
538 
539  else
540  {
541  sN = -d;
542  sD = a;
543  }
544  }
545 
546  else if (tN > tD)
547  {
548  tN = tD;
549 
550  if ((-d + b) < 0.f)
551  sN = 0.f;
552 
553  else if ((-d + b) > a)
554  sN = sD;
555 
556  else
557  {
558  sN = (-d + b);
559  sD = a;
560  }
561  }
562 
563  // set intersection ratios
564  ratio[0] = (std::abs (sN) < small_error_) ? 0.f : sN / sD;
565  ratio[1] = (std::abs (tN) < small_error_) ? 0.f : tN / tD;
566 
567  Eigen::Vector3f x = w + (ratio[0] * u) - (ratio[1] * v);
568  return (x.norm ());
569 }
570 
571 
572 ///////////////////////////////////////////////////////////////////////////////////////////
573 template <typename PointSource, typename PointTarget, typename NormalT, typename Scalar> int
575  int idx1,
576  int idx2,
577  pcl::Correspondences &pairs)
578 {
579  const float max_norm_diff = 0.5f * max_norm_diff_ * M_PI / 180.f;
580 
581  // calculate reference segment distance and normal angle
582  float ref_dist = pcl::euclideanDistance (target_->points[idx1], target_->points[idx2]);
583  float ref_norm_angle = (use_normals_ ? (target_normals_->points[idx1].getNormalVector3fMap () -
584  target_normals_->points[idx2].getNormalVector3fMap ()).norm () : 0.f);
585 
586  // loop over all pairs of points in source point cloud
587  std::vector <int>::iterator it_out = source_indices_->begin (), it_out_e = source_indices_->end () - 1;
588  std::vector <int>::iterator it_in, it_in_e = source_indices_->end ();
589  for ( ; it_out != it_out_e; it_out++)
590  {
591  it_in = it_out + 1;
592  const PointSource *pt1 = &(*input_)[*it_out];
593  for ( ; it_in != it_in_e; it_in++)
594  {
595  const PointSource *pt2 = &(*input_)[*it_in];
596 
597  // check point distance compared to reference dist (from base)
598  float dist = pcl::euclideanDistance (*pt1, *pt2);
599  if (std::abs(dist - ref_dist) < max_pair_diff_)
600  {
601  // add here normal evaluation if normals are given
602  if (use_normals_)
603  {
604  const NormalT *pt1_n = &(source_normals_->points[*it_out]);
605  const NormalT *pt2_n = &(source_normals_->points[*it_in]);
606 
607  float norm_angle_1 = (pt1_n->getNormalVector3fMap () - pt2_n->getNormalVector3fMap ()).norm ();
608  float norm_angle_2 = (pt1_n->getNormalVector3fMap () + pt2_n->getNormalVector3fMap ()).norm ();
609 
610  float norm_diff = std::min <float> (std::abs (norm_angle_1 - ref_norm_angle), std::abs (norm_angle_2 - ref_norm_angle));
611  if (norm_diff > max_norm_diff)
612  continue;
613  }
614 
615  pairs.push_back (pcl::Correspondence (*it_in, *it_out, dist));
616  pairs.push_back (pcl::Correspondence (*it_out, *it_in, dist));
617  }
618  }
619  }
620 
621  // return success if at least one correspondence was found
622  return (pairs.size () == 0 ? -1 : 0);
623 }
624 
625 
626 ///////////////////////////////////////////////////////////////////////////////////////////
627 template <typename PointSource, typename PointTarget, typename NormalT, typename Scalar> int
629  const std::vector <int> &base_indices,
630  std::vector <std::vector <int> > &matches,
631  const pcl::Correspondences &pairs_a,
632  const pcl::Correspondences &pairs_b,
633  const float (&ratio)[2])
634 {
635  // calculate edge lengths of base
636  float dist_base[4];
637  dist_base[0] = pcl::euclideanDistance (target_->points[base_indices[0]], target_->points[base_indices[2]]);
638  dist_base[1] = pcl::euclideanDistance (target_->points[base_indices[0]], target_->points[base_indices[3]]);
639  dist_base[2] = pcl::euclideanDistance (target_->points[base_indices[1]], target_->points[base_indices[2]]);
640  dist_base[3] = pcl::euclideanDistance (target_->points[base_indices[1]], target_->points[base_indices[3]]);
641 
642  // loop over first point pair correspondences and store intermediate points 'e' in new point cloud
644  cloud_e->resize (pairs_a.size () * 2);
645  PointCloudSourceIterator it_pt = cloud_e->begin ();
646  for (pcl::Correspondences::const_iterator it_pair = pairs_a.begin (), it_pair_e = pairs_a.end () ; it_pair != it_pair_e; it_pair++)
647  {
648  const PointSource *pt1 = &(input_->points[it_pair->index_match]);
649  const PointSource *pt2 = &(input_->points[it_pair->index_query]);
650 
651  // calculate intermediate points using both ratios from base (r1,r2)
652  for (int i = 0; i < 2; i++, it_pt++)
653  {
654  it_pt->x = pt1->x + ratio[i] * (pt2->x - pt1->x);
655  it_pt->y = pt1->y + ratio[i] * (pt2->y - pt1->y);
656  it_pt->z = pt1->z + ratio[i] * (pt2->z - pt1->z);
657  }
658  }
659 
660  // initialize new kd tree of intermediate points from first point pair correspondences
662  tree_e->setInputCloud (cloud_e);
663 
664  std::vector <int> ids;
665  std::vector <float> dists_sqr;
666 
667  // loop over second point pair correspondences
668  for (pcl::Correspondences::const_iterator it_pair = pairs_b.begin (), it_pair_e = pairs_b.end () ; it_pair != it_pair_e; it_pair++)
669  {
670  const PointTarget *pt1 = &(input_->points[it_pair->index_match]);
671  const PointTarget *pt2 = &(input_->points[it_pair->index_query]);
672 
673  // calculate intermediate points using both ratios from base (r1,r2)
674  for (int i = 0; i < 2; i++)
675  {
676  PointTarget pt_e;
677  pt_e.x = pt1->x + ratio[i] * (pt2->x - pt1->x);
678  pt_e.y = pt1->y + ratio[i] * (pt2->y - pt1->y);
679  pt_e.z = pt1->z + ratio[i] * (pt2->z - pt1->z);
680 
681  // search for corresponding intermediate points
682  tree_e->radiusSearch (pt_e, coincidation_limit_, ids, dists_sqr);
683  for (std::vector <int>::iterator it = ids.begin (), it_e = ids.end (); it != it_e; it++)
684  {
685  std::vector <int> match_indices (4);
686 
687  match_indices[0] = pairs_a[static_cast <int> (std::floor ((float)(*it/2.f)))].index_match;
688  match_indices[1] = pairs_a[static_cast <int> (std::floor ((float)(*it/2.f)))].index_query;
689  match_indices[2] = it_pair->index_match;
690  match_indices[3] = it_pair->index_query;
691 
692  // EDITED: added coarse check of match based on edge length (due to rigid-body )
693  if (checkBaseMatch (match_indices, dist_base) < 0)
694  continue;
695 
696  matches.push_back (match_indices);
697  }
698  }
699  }
700 
701  // return unsuccessfull if no match was found
702  return (matches.size () > 0 ? 0 : -1);
703 }
704 
705 
706 ///////////////////////////////////////////////////////////////////////////////////////////
707 template <typename PointSource, typename PointTarget, typename NormalT, typename Scalar> int
709  const std::vector <int> &match_indices,
710  const float (&dist_ref)[4])
711 {
712  float d0 = pcl::euclideanDistance (input_->points[match_indices[0]], input_->points[match_indices[2]]);
713  float d1 = pcl::euclideanDistance (input_->points[match_indices[0]], input_->points[match_indices[3]]);
714  float d2 = pcl::euclideanDistance (input_->points[match_indices[1]], input_->points[match_indices[2]]);
715  float d3 = pcl::euclideanDistance (input_->points[match_indices[1]], input_->points[match_indices[3]]);
716 
717  // check edge distances of match w.r.t the base
718  return (std::abs (d0 - dist_ref[0]) < max_edge_diff_ && std::abs (d1 - dist_ref[1]) < max_edge_diff_ &&
719  std::abs (d2 - dist_ref[2]) < max_edge_diff_ && std::abs (d3 - dist_ref[3]) < max_edge_diff_) ? 0 : -1;
720 }
721 
722 
723 ///////////////////////////////////////////////////////////////////////////////////////////
724 template <typename PointSource, typename PointTarget, typename NormalT, typename Scalar> void
726  const std::vector <int> &base_indices,
727  std::vector <std::vector <int> > &matches,
728  MatchingCandidates &candidates)
729 {
730  candidates.resize (1);
731  float fitness_score = FLT_MAX;
732 
733  // loop over all Candidate matches
734  for (std::vector <std::vector <int> >::iterator match_indices = matches.begin (), it_e = matches.end (); match_indices != it_e; match_indices++)
735  {
736  Eigen::Matrix4f transformation_temp;
737  pcl::Correspondences correspondences_temp;
738 
739  // determine corresondences between base and match according to their distance to centroid
740  linkMatchWithBase (base_indices, *match_indices, correspondences_temp);
741 
742  // check match based on residuals of the corresponding points after
743  if (validateMatch (base_indices, *match_indices, correspondences_temp, transformation_temp) < 0)
744  continue;
745 
746  // check resulting using a sub sample of the source point cloud and compare to previous matches
747  if (validateTransformation (transformation_temp, fitness_score) < 0)
748  continue;
749 
750  // store best match as well as associated fitness_score and transformation
751  candidates[0].fitness_score = fitness_score;
752  candidates [0].transformation = transformation_temp;
753  correspondences_temp.erase (correspondences_temp.end () - 1);
754  candidates[0].correspondences = correspondences_temp;
755  }
756 }
757 
758 
759 ///////////////////////////////////////////////////////////////////////////////////////////
760 template <typename PointSource, typename PointTarget, typename NormalT, typename Scalar> void
762  const std::vector <int> &base_indices,
763  std::vector <int> &match_indices,
764  pcl::Correspondences &correspondences)
765 {
766  // calculate centroid of base and target
767  Eigen::Vector4f centre_base, centre_match;
768  pcl::compute3DCentroid (*target_, base_indices, centre_base);
769  pcl::compute3DCentroid (*input_, match_indices, centre_match);
770 
771  PointTarget centre_pt_base;
772  centre_pt_base.x = centre_base[0];
773  centre_pt_base.y = centre_base[1];
774  centre_pt_base.z = centre_base[2];
775 
776  PointSource centre_pt_match;
777  centre_pt_match.x = centre_match[0];
778  centre_pt_match.y = centre_match[1];
779  centre_pt_match.z = centre_match[2];
780 
781  // find corresponding points according to their distance to the centroid
782  std::vector <int> copy = match_indices;
783 
784  std::vector <int>::const_iterator it_base = base_indices.begin (), it_base_e = base_indices.end ();
785  std::vector <int>::iterator it_match, it_match_e = copy.end ();
786  std::vector <int>::iterator it_match_orig = match_indices.begin ();
787  for (; it_base != it_base_e; it_base++, it_match_orig++)
788  {
789  float dist_sqr_1 = pcl::squaredEuclideanDistance (target_->points[*it_base], centre_pt_base);
790  float best_diff_sqr = FLT_MAX;
791  int best_index;
792 
793  for (it_match = copy.begin (); it_match != it_match_e; it_match++)
794  {
795  // calculate difference of distances to centre point
796  float dist_sqr_2 = pcl::squaredEuclideanDistance (input_->points[*it_match], centre_pt_match);
797  float diff_sqr = std::abs(dist_sqr_1 - dist_sqr_2);
798 
799  if (diff_sqr < best_diff_sqr)
800  {
801  best_diff_sqr = diff_sqr;
802  best_index = *it_match;
803  }
804  }
805 
806  // assign new correspondence and update indices of matched targets
807  correspondences.push_back (pcl::Correspondence (best_index, *it_base, best_diff_sqr));
808  *it_match_orig = best_index;
809  }
810 }
811 
812 
813 ///////////////////////////////////////////////////////////////////////////////////////////
814 template <typename PointSource, typename PointTarget, typename NormalT, typename Scalar> int
816  const std::vector <int> &base_indices,
817  const std::vector <int> &match_indices,
818  const pcl::Correspondences &correspondences,
819  Eigen::Matrix4f &transformation)
820 {
821  // only use triplet of points to simlify process (possible due to planar case)
822  pcl::Correspondences correspondences_temp = correspondences;
823  correspondences_temp.erase (correspondences_temp.end () - 1);
824 
825  // estimate transformation between correspondence set
826  transformation_estimation_->estimateRigidTransformation (*input_, *target_, correspondences_temp, transformation);
827 
828  // transform base points
829  PointCloudSource match_transformed;
830  pcl::transformPointCloud (*input_, match_indices, match_transformed, transformation);
831 
832  // calculate residuals of transformation and check against maximum threshold
833  std::size_t nr_points = correspondences_temp.size ();
834  float mse = 0.f;
835  for (std::size_t i = 0; i < nr_points; i++)
836  mse += pcl::squaredEuclideanDistance (match_transformed.points [i], target_->points [base_indices[i]]);
837 
838  mse /= nr_points;
839  return (mse < max_mse_ ? 0 : -1);
840 }
841 
842 
843 ///////////////////////////////////////////////////////////////////////////////////////////
844 template <typename PointSource, typename PointTarget, typename NormalT, typename Scalar> int
846  Eigen::Matrix4f &transformation,
847  float &fitness_score)
848 {
849  // transform source point cloud
850  PointCloudSource source_transformed;
851  pcl::transformPointCloud (*input_, *source_indices_, source_transformed, transformation);
852 
853  std::size_t nr_points = source_transformed.size ();
854  std::size_t terminate_value = fitness_score > 1 ? 0 : static_cast <std::size_t> ((1.f - fitness_score) * nr_points);
855 
856  float inlier_score_temp = 0;
857  std::vector <int> ids;
858  std::vector <float> dists_sqr;
859  PointCloudSourceIterator it = source_transformed.begin ();
860 
861  for (std::size_t i = 0; i < nr_points; it++, i++)
862  {
863  // search for nearest point using kd tree search
864  tree_->nearestKSearch (*it, 1, ids, dists_sqr);
865  inlier_score_temp += (dists_sqr[0] < max_inlier_dist_sqr_ ? 1 : 0);
866 
867  // early terminating
868  if (nr_points - i + inlier_score_temp < terminate_value)
869  break;
870  }
871 
872  // check current costs and return unsuccessfull if larger than previous ones
873  inlier_score_temp /= static_cast <float> (nr_points);
874  float fitness_score_temp = 1.f - inlier_score_temp;
875 
876  if (fitness_score_temp > fitness_score)
877  return (-1);
878 
879  fitness_score = fitness_score_temp;
880  return (0);
881 }
882 
883 
884 ///////////////////////////////////////////////////////////////////////////////////////////
885 template <typename PointSource, typename PointTarget, typename NormalT, typename Scalar> void
887  const std::vector <MatchingCandidates > &candidates)
888 {
889  // get best fitness_score over all trys
890  int nr_candidates = static_cast <int> (candidates.size ());
891  int best_index = -1;
892  float best_score = FLT_MAX;
893  for (int i = 0; i < nr_candidates; i++)
894  {
895  const float &fitness_score = candidates [i][0].fitness_score;
896  if (fitness_score < best_score)
897  {
898  best_score = fitness_score;
899  best_index = i;
900  }
901  }
902 
903  // check if a valid candidate was available
904  if (!(best_index < 0))
905  {
906  fitness_score_ = candidates [best_index][0].fitness_score;
907  final_transformation_ = candidates [best_index][0].transformation;
908  *correspondences_ = candidates [best_index][0].correspondences;
909 
910  // here we define convergence if resulting fitness_score is below 1-threshold
911  converged_ = fitness_score_ < score_threshold_;
912  }
913 }
914 
915 ///////////////////////////////////////////////////////////////////////////////////////////
916 
917 #endif // PCL_REGISTRATION_IMPL_IA_4PCS_H_
A point structure representing normal coordinates and the surface curvature estimate.
size_t size() const
Definition: point_cloud.h:440
std::vector< MatchingCandidate, Eigen::aligned_allocator< MatchingCandidate > > MatchingCandidates
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
Definition: point_cloud.h:410
FPCSInitialAlignment computes corresponding four point congruent sets as described in: "4-points cong...
Definition: ia_fpcs.h:78
boost::shared_ptr< std::vector< int > > IndicesPtr
Definition: pcl_base.h:60
void setIndices(const boost::shared_ptr< std::vector< int > > &indices)
Provide a pointer to the vector of indices that represents the input data.
Definition: sac_model.h:327
float squaredEuclideanDistance(const PointType1 &p1, const PointType2 &p2)
Calculate the squared euclidean distance between the two given points.
Definition: distances.h:174
Correspondence represents a match between two entities (e.g., points, descriptors, etc).
float euclideanDistance(const PointType1 &p1, const PointType2 &p2)
Calculate the euclidean distance between the two given points.
Definition: distances.h:196
PointCloudSource::Ptr PointCloudSourcePtr
Definition: registration.h:83
double pointToPlaneDistance(const Point &p, double a, double b, double c, double d)
Get the distance from a point to a plane (unsigned) defined by ax+by+cz+d=0.
TransformationEstimation3Points represents the class for transformation estimation based on: ...
bool computeModelCoefficients(const std::vector< int > &samples, Eigen::VectorXf &model_coefficients)
Check whether the given index samples can form a valid plane model, compute the model coefficients fr...
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=true)
Apply an affine transform defined by an Eigen Transform.
Definition: transforms.hpp:42
PCL base class.
Definition: pcl_base.h:68
std::vector< pcl::Correspondence, Eigen::aligned_allocator< pcl::Correspondence > > Correspondences
void getMinMax3D(const pcl::PointCloud< PointT > &cloud, PointT &min_pt, PointT &max_pt)
Get the minimum and maximum values on each of the 3 (x-y-z) dimensions in a given pointcloud...
Definition: common.hpp:228
float getMeanPointDensity(const typename pcl::PointCloud< PointT >::ConstPtr &cloud, float max_dist, int nr_threads=1)
Compute the mean point density of a given point cloud.
Definition: ia_fpcs.hpp:50
boost::shared_ptr< const PointCloud< PointT > > ConstPtr
Definition: point_cloud.h:429
double getTimeSeconds()
Retrieve the time in seconds spent since the last call to reset().
Definition: time.h:86
void setInputCloud(const PointCloudConstPtr &cloud, const IndicesConstPtr &indices=IndicesConstPtr())
Provide a pointer to the input dataset.
Definition: kdtree.hpp:77
Simple stopwatch.
Definition: time.h:65
KdTreeReciprocal::Ptr KdTreeReciprocalPtr
Definition: registration.h:80
iterator begin()
Definition: point_cloud.h:434
unsigned int compute3DCentroid(ConstCloudIterator< PointT > &cloud_iterator, Eigen::Matrix< Scalar, 4, 1 > &centroid)
Compute the 3D (X-Y-Z) centroid of a set of points and return it as a 3D vector.
Definition: centroid.hpp:50
int nearestKSearch(const PointT &point, int k, std::vector< int > &k_indices, std::vector< float > &k_sqr_distances) const
Search for the k-nearest neighbors for the given query point.
Definition: kdtree.hpp:88
SampleConsensusModelPlane defines a model for 3D plane segmentation.