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