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