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