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 firstprivate(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 #if OPENMP_LEGACY_CONST_DATA_SHARING_RULE
103 #pragma omp parallel for \
105 shared(tree, cloud, indices) \
106 firstprivate(ids, dists_sqr) \
107 reduction(+:mean_dist, num) \
108 num_threads(nr_threads)
110 #pragma omp parallel for \
112 shared(tree, cloud, indices, s, max_dist_sqr) \
113 firstprivate(ids, dists_sqr) \
114 reduction(+:mean_dist, num) \
115 num_threads(nr_threads)
117 for (
int i = 0; i < 1000; i++)
119 tree.
nearestKSearch ((*cloud)[indices[rand () % s]], 2, ids, dists_sqr);
120 if (dists_sqr[1] < max_dist_sqr)
122 mean_dist += std::sqrt (dists_sqr[1]);
127 return (mean_dist / num);
132 template <
typename Po
intSource,
typename Po
intTarget,
typename NormalT,
typename Scalar>
137 approx_overlap_ (0.5f),
139 score_threshold_ (FLT_MAX),
141 max_norm_diff_ (90.f),
143 fitness_score_ (FLT_MAX),
145 max_base_diameter_sqr_ (),
146 use_normals_ (
false),
147 normalize_delta_ (
true),
150 coincidation_limit_ (),
152 max_inlier_dist_sqr_ (),
153 small_error_ (0.00001f)
155 reg_name_ =
"pcl::registration::FPCSInitialAlignment";
157 ransac_iterations_ = 1000;
163 template <
typename Po
intSource,
typename Po
intTarget,
typename NormalT,
typename Scalar>
void
165 PointCloudSource &output,
166 const Eigen::Matrix4f &guess)
171 final_transformation_ = guess;
173 std::vector <MatchingCandidates> all_candidates (max_iterations_);
176 #pragma omp parallel \
178 shared(abort, all_candidates, timer) \
179 num_threads(nr_threads_)
182 std::srand (
static_cast <unsigned int> (std::time (NULL)) ^ omp_get_thread_num ());
183 #pragma omp for schedule(dynamic)
185 for (
int i = 0; i < max_iterations_; i++)
187 #pragma omp flush (abort)
190 std::vector <int> base_indices (4);
191 all_candidates[i] = candidates;
197 if (selectBase (base_indices, ratio) == 0)
201 if (bruteForceCorrespondences (base_indices[0], base_indices[1], pairs_a) == 0 &&
202 bruteForceCorrespondences (base_indices[2], base_indices[3], pairs_b) == 0)
205 std::vector <std::vector <int> > matches;
206 if (determineBaseMatches (base_indices, matches, pairs_a, pairs_b, ratio) == 0)
209 handleMatches (base_indices, matches, candidates);
210 if (!candidates.empty ())
211 all_candidates[i] = candidates;
217 abort = (!candidates.empty () ? candidates[0].fitness_score < score_threshold_ : abort);
221 #pragma omp flush (abort)
228 finalCompute (all_candidates);
238 template <
typename Po
intSource,
typename Po
intTarget,
typename NormalT,
typename Scalar>
bool
241 std::srand (
static_cast <unsigned int> (std::time (
nullptr)));
248 if (!input_ || !target_)
250 PCL_ERROR (
"[%s::initCompute] Source or target dataset not given!\n", reg_name_.c_str ());
254 if (!target_indices_ || target_indices_->empty ())
256 target_indices_.reset (
new std::vector <int> (
static_cast <int> (target_->size ())));
258 for (
int &target_index : *target_indices_)
259 target_index = index++;
260 target_cloud_updated_ =
true;
264 if (nr_samples_ != 0)
266 const int ss =
static_cast <int> (indices_->size ());
267 const int sample_fraction_src = std::max (1,
static_cast <int> (ss / nr_samples_));
270 for (
int i = 0; i < ss; i++)
271 if (rand () % sample_fraction_src == 0)
272 source_indices_->push_back ((*indices_) [i]);
275 source_indices_ = indices_;
278 if (source_normals_ && target_normals_ && source_normals_->size () == input_->size () && target_normals_->size () == target_->size ())
282 if (target_cloud_updated_)
284 tree_->setInputCloud (target_, target_indices_);
285 target_cloud_updated_ =
false;
289 const int min_iterations = 4;
290 const float diameter_fraction = 0.3f;
293 Eigen::Vector4f pt_min, pt_max;
295 diameter_ = (pt_max - pt_min).norm ();
298 float max_base_diameter = diameter_* approx_overlap_ * 2.f;
299 max_base_diameter_sqr_ = max_base_diameter * max_base_diameter;
302 if (normalize_delta_)
304 float mean_dist = getMeanPointDensity <PointTarget> (target_, *target_indices_, 0.05f * diameter_, nr_threads_);
309 if (max_iterations_ == 0)
311 float first_est = std::log (small_error_) / std::log (1.0 - std::pow ((
double) approx_overlap_, (
double) min_iterations));
312 max_iterations_ =
static_cast <int> (first_est / (diameter_fraction * approx_overlap_ * 2.f));
316 if (score_threshold_ == FLT_MAX)
317 score_threshold_ = 1.f - approx_overlap_;
319 if (max_iterations_ < 4)
322 if (max_runtime_ < 1)
323 max_runtime_ = INT_MAX;
326 max_pair_diff_ = delta_ * 2.f;
327 max_edge_diff_ = delta_ * 4.f;
328 coincidation_limit_ = delta_ * 2.f;
329 max_mse_ = powf (delta_* 2.f, 2.f);
330 max_inlier_dist_sqr_ = powf (delta_ * 2.f, 2.f);
333 fitness_score_ = FLT_MAX;
340 template <
typename Po
intSource,
typename Po
intTarget,
typename NormalT,
typename Scalar>
int
342 std::vector <int> &base_indices,
345 const float too_close_sqr = max_base_diameter_sqr_*0.01;
347 Eigen::VectorXf coefficients (4);
349 plane.setIndices (target_indices_);
350 Eigen::Vector4f centre_pt;
351 float nearest_to_plane = FLT_MAX;
354 for (
int i = 0; i < ransac_iterations_; i++)
357 if (selectBaseTriangle (base_indices) < 0)
360 std::vector <int> base_triple (base_indices.begin (), base_indices.end () - 1);
361 plane.computeModelCoefficients (base_triple, coefficients);
365 const PointTarget *pt1 = &((*target_)[base_indices[0]]);
366 const PointTarget *pt2 = &((*target_)[base_indices[1]]);
367 const PointTarget *pt3 = &((*target_)[base_indices[2]]);
369 for (
const int &target_index : *target_indices_)
371 const PointTarget *pt4 = &((*target_)[target_index]);
376 float d4 = (pt4->getVector3fMap () - centre_pt.head (3)).squaredNorm ();
379 if (d1 < too_close_sqr || d2 < too_close_sqr || d3 < too_close_sqr || d4 < too_close_sqr ||
380 d1 > max_base_diameter_sqr_ || d2 > max_base_diameter_sqr_ || d3 > max_base_diameter_sqr_)
385 if (dist_to_plane < nearest_to_plane)
387 base_indices[3] = target_index;
388 nearest_to_plane = dist_to_plane;
393 if (nearest_to_plane != FLT_MAX)
396 setupBase (base_indices, ratio);
407 template <
typename Po
intSource,
typename Po
intTarget,
typename NormalT,
typename Scalar>
int
410 int nr_points =
static_cast <int> (target_indices_->size ());
414 base_indices[0] = (*target_indices_)[rand () % nr_points];
415 int *index1 = &base_indices[0];
418 for (
int i = 0; i < ransac_iterations_; i++)
420 int *index2 = &(*target_indices_)[rand () % nr_points];
421 int *index3 = &(*target_indices_)[rand () % nr_points];
423 Eigen::Vector3f u = (*target_)[*index2].getVector3fMap () - (*target_)[*index1].getVector3fMap ();
424 Eigen::Vector3f v = (*target_)[*index3].getVector3fMap () - (*target_)[*index1].getVector3fMap ();
425 float t = u.cross (v).squaredNorm ();
428 if (t > best_t && u.squaredNorm () < max_base_diameter_sqr_ && v.squaredNorm () < max_base_diameter_sqr_)
431 base_indices[1] = *index2;
432 base_indices[2] = *index3;
437 return (best_t == 0.f ? -1 : 0);
442 template <
typename Po
intSource,
typename Po
intTarget,
typename NormalT,
typename Scalar>
void
444 std::vector <int> &base_indices,
447 float best_t = FLT_MAX;
448 const std::vector <int> copy (base_indices.begin (), base_indices.end ());
449 std::vector <int> temp (base_indices.begin (), base_indices.end ());
452 for (std::vector <int>::const_iterator i = copy.begin (), i_e = copy.end (); i != i_e; ++i)
453 for (std::vector <int>::const_iterator j = copy.begin (), j_e = copy.end (); j != j_e; ++j)
458 for (std::vector <int>::const_iterator k = copy.begin (), k_e = copy.end (); k != k_e; ++k)
460 if (k == j || k == i)
463 std::vector <int>::const_iterator l = copy.begin ();
464 while (l == i || l == j || l == k)
474 float t = segmentToSegmentDist (temp, ratio_temp);
478 ratio[0] = ratio_temp[0];
479 ratio[1] = ratio_temp[1];
488 template <
typename Po
intSource,
typename Po
intTarget,
typename NormalT,
typename Scalar>
float
490 const std::vector <int> &base_indices,
494 Eigen::Vector3f u = (*target_)[base_indices[1]].getVector3fMap () - (*target_)[base_indices[0]].getVector3fMap ();
495 Eigen::Vector3f v = (*target_)[base_indices[3]].getVector3fMap () - (*target_)[base_indices[2]].getVector3fMap ();
496 Eigen::Vector3f w = (*target_)[base_indices[0]].getVector3fMap () - (*target_)[base_indices[2]].getVector3fMap ();
504 float D = a * c - b * b;
505 float sN = 0.f, sD = D;
506 float tN = 0.f, tD = D;
509 if (D < small_error_)
518 sN = (b * e - c * d);
519 tN = (a * e - b * d);
559 else if ((-d + b) > a)
570 ratio[0] = (std::abs (sN) < small_error_) ? 0.f : sN / sD;
571 ratio[1] = (std::abs (tN) < small_error_) ? 0.f : tN / tD;
573 Eigen::Vector3f x = w + (ratio[0] * u) - (ratio[1] * v);
579 template <
typename Po
intSource,
typename Po
intTarget,
typename NormalT,
typename Scalar>
int
585 const float max_norm_diff = 0.5f * max_norm_diff_ *
M_PI / 180.f;
589 float ref_norm_angle = (use_normals_ ? ((*target_normals_)[idx1].getNormalVector3fMap () -
590 (*target_normals_)[idx2].getNormalVector3fMap ()).norm () : 0.f);
593 auto it_out = source_indices_->begin (), it_out_e = source_indices_->end () - 1;
594 auto it_in_e = source_indices_->end ();
595 for ( ; it_out != it_out_e; it_out++)
597 auto it_in = it_out + 1;
598 const PointSource *pt1 = &(*input_)[*it_out];
599 for ( ; it_in != it_in_e; it_in++)
601 const PointSource *pt2 = &(*input_)[*it_in];
605 if (std::abs(dist - ref_dist) < max_pair_diff_)
610 const NormalT *pt1_n = &((*source_normals_)[*it_out]);
611 const NormalT *pt2_n = &((*source_normals_)[*it_in]);
613 float norm_angle_1 = (pt1_n->getNormalVector3fMap () - pt2_n->getNormalVector3fMap ()).norm ();
614 float norm_angle_2 = (pt1_n->getNormalVector3fMap () + pt2_n->getNormalVector3fMap ()).norm ();
616 float norm_diff = std::min <float> (std::abs (norm_angle_1 - ref_norm_angle), std::abs (norm_angle_2 - ref_norm_angle));
617 if (norm_diff > max_norm_diff)
628 return (pairs.empty () ? -1 : 0);
633 template <
typename Po
intSource,
typename Po
intTarget,
typename NormalT,
typename Scalar>
int
635 const std::vector <int> &base_indices,
636 std::vector <std::vector <int> > &matches,
639 const float (&ratio)[2])
650 cloud_e->resize (pairs_a.size () * 2);
651 PointCloudSourceIterator it_pt = cloud_e->begin ();
652 for (
const auto &pair : pairs_a)
654 const PointSource *pt1 = &((*input_)[pair.index_match]);
655 const PointSource *pt2 = &((*input_)[pair.index_query]);
658 for (
int i = 0; i < 2; i++, it_pt++)
660 it_pt->x = pt1->x + ratio[i] * (pt2->x - pt1->x);
661 it_pt->y = pt1->y + ratio[i] * (pt2->y - pt1->y);
662 it_pt->z = pt1->z + ratio[i] * (pt2->z - pt1->z);
668 tree_e->setInputCloud (cloud_e);
670 std::vector <int> ids;
671 std::vector <float> dists_sqr;
674 for (
const auto &pair : pairs_b)
676 const PointTarget *pt1 = &((*input_)[pair.index_match]);
677 const PointTarget *pt2 = &((*input_)[pair.index_query]);
680 for (
const float &r : ratio)
683 pt_e.x = pt1->x + r * (pt2->x - pt1->x);
684 pt_e.y = pt1->y + r * (pt2->y - pt1->y);
685 pt_e.z = pt1->z + r * (pt2->z - pt1->z);
688 tree_e->radiusSearch (pt_e, coincidation_limit_, ids, dists_sqr);
689 for (
const int &
id : ids)
691 std::vector <int> match_indices (4);
693 match_indices[0] = pairs_a[
static_cast <int> (std::floor ((
float)(
id/2.f)))].index_match;
694 match_indices[1] = 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;
699 if (checkBaseMatch (match_indices, dist_base) < 0)
702 matches.push_back (match_indices);
708 return (!matches.empty () ? 0 : -1);
713 template <
typename Po
intSource,
typename Po
intTarget,
typename NormalT,
typename Scalar>
int
715 const std::vector <int> &match_indices,
716 const float (&dist_ref)[4])
724 return (std::abs (d0 - dist_ref[0]) < max_edge_diff_ && std::abs (d1 - dist_ref[1]) < max_edge_diff_ &&
725 std::abs (d2 - dist_ref[2]) < max_edge_diff_ && std::abs (d3 - dist_ref[3]) < max_edge_diff_) ? 0 : -1;
730 template <
typename Po
intSource,
typename Po
intTarget,
typename NormalT,
typename Scalar>
void
732 const std::vector <int> &base_indices,
733 std::vector <std::vector <int> > &matches,
736 candidates.resize (1);
737 float fitness_score = FLT_MAX;
740 for (
auto &match : matches)
742 Eigen::Matrix4f transformation_temp;
746 linkMatchWithBase (base_indices, match, correspondences_temp);
749 if (validateMatch (base_indices, match, correspondences_temp, transformation_temp) < 0)
753 if (validateTransformation (transformation_temp, fitness_score) < 0)
757 candidates[0].fitness_score = fitness_score;
758 candidates [0].transformation = transformation_temp;
759 correspondences_temp.erase (correspondences_temp.end () - 1);
760 candidates[0].correspondences = correspondences_temp;
766 template <
typename Po
intSource,
typename Po
intTarget,
typename NormalT,
typename Scalar>
void
768 const std::vector <int> &base_indices,
769 std::vector <int> &match_indices,
773 Eigen::Vector4f centre_base {0, 0, 0, 0}, centre_match {0, 0, 0, 0};
777 PointTarget centre_pt_base;
778 centre_pt_base.x = centre_base[0];
779 centre_pt_base.y = centre_base[1];
780 centre_pt_base.z = centre_base[2];
782 PointSource centre_pt_match;
783 centre_pt_match.x = centre_match[0];
784 centre_pt_match.y = centre_match[1];
785 centre_pt_match.z = centre_match[2];
788 std::vector <int> copy = match_indices;
790 auto it_match_orig = match_indices.begin ();
791 for (
auto it_base = base_indices.cbegin (), it_base_e = base_indices.cend (); it_base != it_base_e; it_base++, it_match_orig++)
794 float best_diff_sqr = FLT_MAX;
797 for (
const int &match_index : copy)
801 float diff_sqr = std::abs(dist_sqr_1 - dist_sqr_2);
803 if (diff_sqr < best_diff_sqr)
805 best_diff_sqr = diff_sqr;
806 best_index = match_index;
812 *it_match_orig = best_index;
818 template <
typename Po
intSource,
typename Po
intTarget,
typename NormalT,
typename Scalar>
int
820 const std::vector <int> &base_indices,
821 const std::vector <int> &match_indices,
823 Eigen::Matrix4f &transformation)
827 correspondences_temp.erase (correspondences_temp.end () - 1);
830 transformation_estimation_->estimateRigidTransformation (*input_, *target_, correspondences_temp, transformation);
833 PointCloudSource match_transformed;
837 std::size_t nr_points = correspondences_temp.size ();
839 for (std::size_t i = 0; i < nr_points; i++)
843 return (mse < max_mse_ ? 0 : -1);
848 template <
typename Po
intSource,
typename Po
intTarget,
typename NormalT,
typename Scalar>
int
850 Eigen::Matrix4f &transformation,
851 float &fitness_score)
854 PointCloudSource source_transformed;
857 std::size_t nr_points = source_transformed.size ();
858 std::size_t terminate_value = fitness_score > 1 ? 0 :
static_cast <std::size_t
> ((1.f - fitness_score) * nr_points);
860 float inlier_score_temp = 0;
861 std::vector <int> ids;
862 std::vector <float> dists_sqr;
863 PointCloudSourceIterator it = source_transformed.begin ();
865 for (std::size_t i = 0; i < nr_points; it++, i++)
868 tree_->nearestKSearch (*it, 1, ids, dists_sqr);
869 inlier_score_temp += (dists_sqr[0] < max_inlier_dist_sqr_ ? 1 : 0);
872 if (nr_points - i + inlier_score_temp < terminate_value)
877 inlier_score_temp /=
static_cast <float> (nr_points);
878 float fitness_score_temp = 1.f - inlier_score_temp;
880 if (fitness_score_temp > fitness_score)
883 fitness_score = fitness_score_temp;
889 template <
typename Po
intSource,
typename Po
intTarget,
typename NormalT,
typename Scalar>
void
891 const std::vector <MatchingCandidates > &candidates)
894 int nr_candidates =
static_cast <int> (candidates.size ());
896 float best_score = FLT_MAX;
897 for (
int i = 0; i < nr_candidates; i++)
899 const float &fitness_score = candidates [i][0].fitness_score;
900 if (fitness_score < best_score)
902 best_score = fitness_score;
908 if (!(best_index < 0))
910 fitness_score_ = candidates [best_index][0].fitness_score;
911 final_transformation_ = candidates [best_index][0].transformation;
912 *correspondences_ = candidates [best_index][0].correspondences;
915 converged_ = fitness_score_ < score_threshold_;
921 #endif // PCL_REGISTRATION_IMPL_IA_4PCS_H_