38 #ifndef PCL_REGISTRATION_IMPL_IA_FPCS_H_
39 #define PCL_REGISTRATION_IMPL_IA_FPCS_H_
41 #include <pcl/registration/ia_fpcs.h>
44 #include <pcl/common/utils.h>
45 #include <pcl/sample_consensus/sac_model_plane.h>
46 #include <pcl/registration/transformation_estimation_3point.h>
49 template <
typename Po
intT>
inline float
52 const float max_dist_sqr = max_dist * max_dist;
53 const std::size_t s = cloud.
size ();
58 float mean_dist = 0.f;
60 std::vector <int> ids (2);
61 std::vector <float> dists_sqr (2);
64 #pragma omp parallel for \
67 private(ids, dists_sqr) \
68 reduction(+:mean_dist, num) \
69 firstprivate(s, max_dist_sqr) \
70 num_threads(nr_threads)
71 for (
int i = 0; i < 1000; i++)
74 if (dists_sqr[1] < max_dist_sqr)
76 mean_dist += std::sqrt (dists_sqr[1]);
81 return (mean_dist / num);
86 template <
typename Po
intT>
inline float
88 float max_dist,
int nr_threads)
90 const float max_dist_sqr = max_dist * max_dist;
91 const std::size_t s = indices.size ();
96 float mean_dist = 0.f;
98 std::vector <int> ids (2);
99 std::vector <float> dists_sqr (2);
102 #pragma omp parallel for \
104 shared(tree, cloud, indices) \
105 private(ids, dists_sqr) \
106 reduction(+:mean_dist, num) \
107 firstprivate(s, max_dist_sqr) \
108 num_threads(nr_threads)
109 for (
int i = 0; i < 1000; i++)
112 if (dists_sqr[1] < max_dist_sqr)
114 mean_dist += std::sqrt (dists_sqr[1]);
119 return (mean_dist / num);
124 template <
typename Po
intSource,
typename Po
intTarget,
typename NormalT,
typename Scalar>
129 approx_overlap_ (0.5f),
131 score_threshold_ (FLT_MAX),
133 max_norm_diff_ (90.f),
135 fitness_score_ (FLT_MAX),
137 max_base_diameter_sqr_ (),
138 use_normals_ (
false),
139 normalize_delta_ (
true),
142 coincidation_limit_ (),
144 max_inlier_dist_sqr_ (),
145 small_error_ (0.00001f)
147 reg_name_ =
"pcl::registration::FPCSInitialAlignment";
149 ransac_iterations_ = 1000;
155 template <
typename Po
intSource,
typename Po
intTarget,
typename NormalT,
typename Scalar>
void
158 const Eigen::Matrix4f &guess)
163 final_transformation_ = guess;
165 std::vector <MatchingCandidates> all_candidates (max_iterations_);
168 #pragma omp parallel \
170 shared(abort, all_candidates, timer) \
171 num_threads(nr_threads_)
174 std::srand (
static_cast <unsigned int> (std::time (NULL)) ^ omp_get_thread_num ());
175 #pragma omp for schedule(dynamic)
177 for (
int i = 0; i < max_iterations_; i++)
179 #pragma omp flush (abort)
182 std::vector <int> base_indices (4);
183 all_candidates[i] = candidates;
189 if (selectBase (base_indices, ratio) == 0)
193 if (bruteForceCorrespondences (base_indices[0], base_indices[1], pairs_a) == 0 &&
194 bruteForceCorrespondences (base_indices[2], base_indices[3], pairs_b) == 0)
197 std::vector <std::vector <int> > matches;
198 if (determineBaseMatches (base_indices, matches, pairs_a, pairs_b, ratio) == 0)
201 handleMatches (base_indices, matches, candidates);
202 if (!candidates.empty ())
203 all_candidates[i] = candidates;
209 abort = (!candidates.empty () ? candidates[0].fitness_score < score_threshold_ : abort);
213 #pragma omp flush (abort)
220 finalCompute (all_candidates);
230 template <
typename Po
intSource,
typename Po
intTarget,
typename NormalT,
typename Scalar>
bool
233 std::srand (
static_cast <unsigned int> (std::time (
nullptr)));
240 if (!input_ || !target_)
242 PCL_ERROR (
"[%s::initCompute] Source or target dataset not given!\n", reg_name_.c_str ());
246 if (!target_indices_ || target_indices_->empty ())
248 target_indices_.reset (
new std::vector <int> (
static_cast <int> (target_->size ())));
250 for (
int &target_index : *target_indices_)
251 target_index = index++;
252 target_cloud_updated_ =
true;
256 if (nr_samples_ != 0)
258 const int ss =
static_cast <int> (indices_->size ());
259 const int sample_fraction_src = std::max (1,
static_cast <int> (ss / nr_samples_));
262 for (
int i = 0; i < ss; i++)
263 if (rand () % sample_fraction_src == 0)
264 source_indices_->push_back ((*indices_) [i]);
267 source_indices_ = indices_;
270 if (source_normals_ && target_normals_ && source_normals_->size () == input_->size () && target_normals_->size () == target_->size ())
274 if (target_cloud_updated_)
276 tree_->setInputCloud (target_, target_indices_);
277 target_cloud_updated_ =
false;
281 const int min_iterations = 4;
282 const float diameter_fraction = 0.3f;
285 Eigen::Vector4f pt_min, pt_max;
287 diameter_ = (pt_max - pt_min).norm ();
290 float max_base_diameter = diameter_* approx_overlap_ * 2.f;
291 max_base_diameter_sqr_ = max_base_diameter * max_base_diameter;
294 if (normalize_delta_)
296 float mean_dist = getMeanPointDensity <PointTarget> (target_, *target_indices_, 0.05f * diameter_, nr_threads_);
301 if (max_iterations_ == 0)
303 float first_est = std::log (small_error_) / std::log (1.0 - std::pow ((
double) approx_overlap_, (
double) min_iterations));
304 max_iterations_ =
static_cast <int> (first_est / (diameter_fraction * approx_overlap_ * 2.f));
308 if (score_threshold_ == FLT_MAX)
309 score_threshold_ = 1.f - approx_overlap_;
311 if (max_iterations_ < 4)
314 if (max_runtime_ < 1)
315 max_runtime_ = INT_MAX;
318 max_pair_diff_ = delta_ * 2.f;
319 max_edge_diff_ = delta_ * 4.f;
320 coincidation_limit_ = delta_ * 2.f;
321 max_mse_ = powf (delta_* 2.f, 2.f);
322 max_inlier_dist_sqr_ = powf (delta_ * 2.f, 2.f);
325 fitness_score_ = FLT_MAX;
332 template <
typename Po
intSource,
typename Po
intTarget,
typename NormalT,
typename Scalar>
int
334 std::vector <int> &base_indices,
337 const float too_close_sqr = max_base_diameter_sqr_*0.01;
339 Eigen::VectorXf coefficients (4);
342 Eigen::Vector4f centre_pt;
343 float nearest_to_plane = FLT_MAX;
346 for (
int i = 0; i < ransac_iterations_; i++)
349 if (selectBaseTriangle (base_indices) < 0)
352 std::vector <int> base_triple (base_indices.begin (), base_indices.end () - 1);
357 const PointTarget *pt1 = &(target_->points[base_indices[0]]);
358 const PointTarget *pt2 = &(target_->points[base_indices[1]]);
359 const PointTarget *pt3 = &(target_->points[base_indices[2]]);
361 for (
const int &target_index : *target_indices_)
363 const PointTarget *pt4 = &(target_->points[target_index]);
368 float d4 = (pt4->getVector3fMap () - centre_pt.head (3)).squaredNorm ();
371 if (d1 < too_close_sqr || d2 < too_close_sqr || d3 < too_close_sqr || d4 < too_close_sqr ||
372 d1 > max_base_diameter_sqr_ || d2 > max_base_diameter_sqr_ || d3 > max_base_diameter_sqr_)
377 if (dist_to_plane < nearest_to_plane)
379 base_indices[3] = target_index;
380 nearest_to_plane = dist_to_plane;
385 if (nearest_to_plane != FLT_MAX)
388 setupBase (base_indices, ratio);
399 template <
typename Po
intSource,
typename Po
intTarget,
typename NormalT,
typename Scalar>
int
402 int nr_points =
static_cast <int> (target_indices_->size ());
406 base_indices[0] = (*target_indices_)[rand () % nr_points];
407 int *index1 = &base_indices[0];
410 for (
int i = 0; i < ransac_iterations_; i++)
412 int *index2 = &(*target_indices_)[rand () % nr_points];
413 int *index3 = &(*target_indices_)[rand () % nr_points];
415 Eigen::Vector3f u = target_->points[*index2].getVector3fMap () - target_->points[*index1].getVector3fMap ();
416 Eigen::Vector3f v = target_->points[*index3].getVector3fMap () - target_->points[*index1].getVector3fMap ();
417 float t = u.cross (v).squaredNorm ();
420 if (t > best_t && u.squaredNorm () < max_base_diameter_sqr_ && v.squaredNorm () < max_base_diameter_sqr_)
423 base_indices[1] = *index2;
424 base_indices[2] = *index3;
429 return (best_t == 0.f ? -1 : 0);
434 template <
typename Po
intSource,
typename Po
intTarget,
typename NormalT,
typename Scalar>
void
436 std::vector <int> &base_indices,
439 float best_t = FLT_MAX;
440 const std::vector <int> copy (base_indices.begin (), base_indices.end ());
441 std::vector <int> temp (base_indices.begin (), base_indices.end ());
444 for (std::vector <int>::const_iterator i = copy.begin (), i_e = copy.end (); i != i_e; ++i)
445 for (std::vector <int>::const_iterator j = copy.begin (), j_e = copy.end (); j != j_e; ++j)
450 for (std::vector <int>::const_iterator k = copy.begin (), k_e = copy.end (); k != k_e; ++k)
452 if (k == j || k == i)
455 std::vector <int>::const_iterator l = copy.begin ();
456 while (l == i || l == j || l == k)
466 float t = segmentToSegmentDist (temp, ratio_temp);
470 ratio[0] = ratio_temp[0];
471 ratio[1] = ratio_temp[1];
480 template <
typename Po
intSource,
typename Po
intTarget,
typename NormalT,
typename Scalar>
float
482 const std::vector <int> &base_indices,
486 Eigen::Vector3f u = target_->points[base_indices[1]].getVector3fMap () - target_->points[base_indices[0]].getVector3fMap ();
487 Eigen::Vector3f v = target_->points[base_indices[3]].getVector3fMap () - target_->points[base_indices[2]].getVector3fMap ();
488 Eigen::Vector3f w = target_->points[base_indices[0]].getVector3fMap () - target_->points[base_indices[2]].getVector3fMap ();
496 float D = a * c - b * b;
497 float sN = 0.f, sD = D;
498 float tN = 0.f, tD = D;
501 if (D < small_error_)
510 sN = (b * e - c * d);
511 tN = (a * e - b * d);
551 else if ((-d + b) > a)
562 ratio[0] = (std::abs (sN) < small_error_) ? 0.f : sN / sD;
563 ratio[1] = (std::abs (tN) < small_error_) ? 0.f : tN / tD;
565 Eigen::Vector3f x = w + (ratio[0] * u) - (ratio[1] * v);
571 template <
typename Po
intSource,
typename Po
intTarget,
typename NormalT,
typename Scalar>
int
577 const float max_norm_diff = 0.5f * max_norm_diff_ *
M_PI / 180.f;
581 float ref_norm_angle = (use_normals_ ? (target_normals_->points[idx1].getNormalVector3fMap () -
582 target_normals_->points[idx2].getNormalVector3fMap ()).norm () : 0.f);
585 auto it_out = source_indices_->begin (), it_out_e = source_indices_->end () - 1;
586 auto it_in_e = source_indices_->end ();
587 for ( ; it_out != it_out_e; it_out++)
589 auto it_in = it_out + 1;
590 const PointSource *pt1 = &(*input_)[*it_out];
591 for ( ; it_in != it_in_e; it_in++)
593 const PointSource *pt2 = &(*input_)[*it_in];
597 if (std::abs(dist - ref_dist) < max_pair_diff_)
602 const NormalT *pt1_n = &(source_normals_->points[*it_out]);
603 const NormalT *pt2_n = &(source_normals_->points[*it_in]);
605 float norm_angle_1 = (pt1_n->getNormalVector3fMap () - pt2_n->getNormalVector3fMap ()).norm ();
606 float norm_angle_2 = (pt1_n->getNormalVector3fMap () + pt2_n->getNormalVector3fMap ()).norm ();
608 float norm_diff = std::min <float> (std::abs (norm_angle_1 - ref_norm_angle), std::abs (norm_angle_2 - ref_norm_angle));
609 if (norm_diff > max_norm_diff)
620 return (pairs.empty () ? -1 : 0);
625 template <
typename Po
intSource,
typename Po
intTarget,
typename NormalT,
typename Scalar>
int
627 const std::vector <int> &base_indices,
628 std::vector <std::vector <int> > &matches,
631 const float (&ratio)[2])
635 dist_base[0] =
pcl::euclideanDistance (target_->points[base_indices[0]], target_->points[base_indices[2]]);
636 dist_base[1] =
pcl::euclideanDistance (target_->points[base_indices[0]], target_->points[base_indices[3]]);
637 dist_base[2] =
pcl::euclideanDistance (target_->points[base_indices[1]], target_->points[base_indices[2]]);
638 dist_base[3] =
pcl::euclideanDistance (target_->points[base_indices[1]], target_->points[base_indices[3]]);
642 cloud_e->resize (pairs_a.size () * 2);
643 PointCloudSourceIterator it_pt = cloud_e->begin ();
644 for (
const auto &pair : pairs_a)
646 const PointSource *pt1 = &(input_->points[pair.index_match]);
647 const PointSource *pt2 = &(input_->points[pair.index_query]);
650 for (
int i = 0; i < 2; i++, it_pt++)
652 it_pt->x = pt1->x + ratio[i] * (pt2->x - pt1->x);
653 it_pt->y = pt1->y + ratio[i] * (pt2->y - pt1->y);
654 it_pt->z = pt1->z + ratio[i] * (pt2->z - pt1->z);
660 tree_e->setInputCloud (cloud_e);
662 std::vector <int> ids;
663 std::vector <float> dists_sqr;
666 for (
const auto &pair : pairs_b)
668 const PointTarget *pt1 = &(input_->points[pair.index_match]);
669 const PointTarget *pt2 = &(input_->points[pair.index_query]);
672 for (
const float &r : ratio)
675 pt_e.x = pt1->x + r * (pt2->x - pt1->x);
676 pt_e.y = pt1->y + r * (pt2->y - pt1->y);
677 pt_e.z = pt1->z + r * (pt2->z - pt1->z);
680 tree_e->radiusSearch (pt_e, coincidation_limit_, ids, dists_sqr);
681 for (
const int &
id : ids)
683 std::vector <int> match_indices (4);
685 match_indices[0] = pairs_a[
static_cast <int> (std::floor ((
float)(
id/2.f)))].index_match;
686 match_indices[1] = pairs_a[
static_cast <int> (std::floor ((
float)(
id/2.f)))].index_query;
687 match_indices[2] = pair.index_match;
688 match_indices[3] = pair.index_query;
691 if (checkBaseMatch (match_indices, dist_base) < 0)
694 matches.push_back (match_indices);
700 return (!matches.empty () ? 0 : -1);
705 template <
typename Po
intSource,
typename Po
intTarget,
typename NormalT,
typename Scalar>
int
707 const std::vector <int> &match_indices,
708 const float (&dist_ref)[4])
716 return (std::abs (d0 - dist_ref[0]) < max_edge_diff_ && std::abs (d1 - dist_ref[1]) < max_edge_diff_ &&
717 std::abs (d2 - dist_ref[2]) < max_edge_diff_ && std::abs (d3 - dist_ref[3]) < max_edge_diff_) ? 0 : -1;
722 template <
typename Po
intSource,
typename Po
intTarget,
typename NormalT,
typename Scalar>
void
724 const std::vector <int> &base_indices,
725 std::vector <std::vector <int> > &matches,
728 candidates.resize (1);
729 float fitness_score = FLT_MAX;
732 for (
auto &match : matches)
734 Eigen::Matrix4f transformation_temp;
738 linkMatchWithBase (base_indices, match, correspondences_temp);
741 if (validateMatch (base_indices, match, correspondences_temp, transformation_temp) < 0)
745 if (validateTransformation (transformation_temp, fitness_score) < 0)
749 candidates[0].fitness_score = fitness_score;
750 candidates [0].transformation = transformation_temp;
751 correspondences_temp.erase (correspondences_temp.end () - 1);
752 candidates[0].correspondences = correspondences_temp;
758 template <
typename Po
intSource,
typename Po
intTarget,
typename NormalT,
typename Scalar>
void
760 const std::vector <int> &base_indices,
761 std::vector <int> &match_indices,
765 Eigen::Vector4f centre_base {0, 0, 0, 0}, centre_match {0, 0, 0, 0};
769 PointTarget centre_pt_base;
770 centre_pt_base.x = centre_base[0];
771 centre_pt_base.y = centre_base[1];
772 centre_pt_base.z = centre_base[2];
774 PointSource centre_pt_match;
775 centre_pt_match.x = centre_match[0];
776 centre_pt_match.y = centre_match[1];
777 centre_pt_match.z = centre_match[2];
780 std::vector <int> copy = match_indices;
782 auto it_match_orig = match_indices.begin ();
783 for (
auto it_base = base_indices.cbegin (), it_base_e = base_indices.cend (); it_base != it_base_e; it_base++, it_match_orig++)
786 float best_diff_sqr = FLT_MAX;
789 for (
const int &match_index : copy)
793 float diff_sqr = std::abs(dist_sqr_1 - dist_sqr_2);
795 if (diff_sqr < best_diff_sqr)
797 best_diff_sqr = diff_sqr;
798 best_index = match_index;
804 *it_match_orig = best_index;
810 template <
typename Po
intSource,
typename Po
intTarget,
typename NormalT,
typename Scalar>
int
812 const std::vector <int> &base_indices,
813 const std::vector <int> &match_indices,
815 Eigen::Matrix4f &transformation)
819 correspondences_temp.erase (correspondences_temp.end () - 1);
822 transformation_estimation_->estimateRigidTransformation (*input_, *target_, correspondences_temp, transformation);
829 std::size_t nr_points = correspondences_temp.size ();
831 for (std::size_t i = 0; i < nr_points; i++)
835 return (mse < max_mse_ ? 0 : -1);
840 template <
typename Po
intSource,
typename Po
intTarget,
typename NormalT,
typename Scalar>
int
842 Eigen::Matrix4f &transformation,
843 float &fitness_score)
849 std::size_t nr_points = source_transformed.
size ();
850 std::size_t terminate_value = fitness_score > 1 ? 0 :
static_cast <std::size_t
> ((1.f - fitness_score) * nr_points);
852 float inlier_score_temp = 0;
853 std::vector <int> ids;
854 std::vector <float> dists_sqr;
855 PointCloudSourceIterator it = source_transformed.
begin ();
857 for (std::size_t i = 0; i < nr_points; it++, i++)
860 tree_->nearestKSearch (*it, 1, ids, dists_sqr);
861 inlier_score_temp += (dists_sqr[0] < max_inlier_dist_sqr_ ? 1 : 0);
864 if (nr_points - i + inlier_score_temp < terminate_value)
869 inlier_score_temp /=
static_cast <float> (nr_points);
870 float fitness_score_temp = 1.f - inlier_score_temp;
872 if (fitness_score_temp > fitness_score)
875 fitness_score = fitness_score_temp;
881 template <
typename Po
intSource,
typename Po
intTarget,
typename NormalT,
typename Scalar>
void
883 const std::vector <MatchingCandidates > &candidates)
886 int nr_candidates =
static_cast <int> (candidates.size ());
888 float best_score = FLT_MAX;
889 for (
int i = 0; i < nr_candidates; i++)
891 const float &fitness_score = candidates [i][0].fitness_score;
892 if (fitness_score < best_score)
894 best_score = fitness_score;
900 if (!(best_index < 0))
902 fitness_score_ = candidates [best_index][0].fitness_score;
903 final_transformation_ = candidates [best_index][0].transformation;
904 *correspondences_ = candidates [best_index][0].correspondences;
907 converged_ = fitness_score_ < score_threshold_;
913 #endif // PCL_REGISTRATION_IMPL_IA_4PCS_H_