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