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