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