38 #ifndef PCL_REGISTRATION_IMPL_IA_FPCS_H_
39 #define PCL_REGISTRATION_IMPL_IA_FPCS_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>
49 template <
typename Po
intT>
55 const float max_dist_sqr = max_dist * max_dist;
56 const std::size_t s = cloud.
size();
61 float mean_dist = 0.f;
63 std::vector<int> ids(2);
64 std::vector<float> dists_sqr(2);
67 #pragma omp parallel for \
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++) {
76 if (dists_sqr[1] < max_dist_sqr) {
77 mean_dist += std::sqrt(dists_sqr[1]);
82 return (mean_dist / num);
86 template <
typename Po
intT>
89 const std::vector<int>& indices,
93 const float max_dist_sqr = max_dist * max_dist;
94 const std::size_t s = indices.size();
99 float mean_dist = 0.f;
101 std::vector<int> ids(2);
102 std::vector<float> dists_sqr(2);
105 #if OPENMP_LEGACY_CONST_DATA_SHARING_RULE
106 #pragma omp parallel for \
108 shared(tree, cloud, indices) \
109 firstprivate(ids, dists_sqr) \
110 reduction(+:mean_dist, num) \
111 num_threads(nr_threads)
113 #pragma omp parallel for \
115 shared(tree, cloud, indices, s, max_dist_sqr) \
116 firstprivate(ids, dists_sqr) \
117 reduction(+:mean_dist, num) \
118 num_threads(nr_threads)
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]);
128 return (mean_dist / num);
132 template <
typename Po
intSource,
typename Po
intTarget,
typename NormalT,
typename Scalar>
138 , approx_overlap_(0.5f)
140 , score_threshold_(FLT_MAX)
142 , max_norm_diff_(90.f)
144 , fitness_score_(FLT_MAX)
146 , max_base_diameter_sqr_()
147 , use_normals_(false)
148 , normalize_delta_(true)
151 , coincidation_limit_()
153 , max_inlier_dist_sqr_()
154 , small_error_(0.00001f)
156 reg_name_ =
"pcl::registration::FPCSInitialAlignment";
158 ransac_iterations_ = 1000;
159 transformation_estimation_.reset(
164 template <
typename Po
intSource,
typename Po
intTarget,
typename NormalT,
typename Scalar>
172 final_transformation_ = guess;
174 std::vector<MatchingCandidates> all_candidates(max_iterations_);
177 #pragma omp parallel default(none) shared(abort, all_candidates, timer) \
178 num_threads(nr_threads_)
181 std::srand(
static_cast<unsigned int>(std::time(NULL)) ^ omp_get_thread_num());
182 #pragma omp for schedule(dynamic)
184 for (
int i = 0; i < max_iterations_; i++) {
185 #pragma omp flush(abort)
188 std::vector<int> base_indices(4);
189 all_candidates[i] = candidates;
194 if (selectBase(base_indices, ratio) == 0) {
197 if (bruteForceCorrespondences(base_indices[0], base_indices[1], pairs_a) ==
199 bruteForceCorrespondences(base_indices[2], base_indices[3], pairs_b) ==
203 std::vector<std::vector<int>> matches;
204 if (determineBaseMatches(base_indices, matches, pairs_a, pairs_b, ratio) ==
207 handleMatches(base_indices, matches, candidates);
208 if (!candidates.empty())
209 all_candidates[i] = candidates;
215 abort = (!candidates.empty() ? candidates[0].fitness_score < score_threshold_
219 #pragma omp flush(abort)
225 finalCompute(all_candidates);
234 template <
typename Po
intSource,
typename Po
intTarget,
typename NormalT,
typename Scalar>
239 std::srand(
static_cast<unsigned int>(std::time(
nullptr)));
246 if (!input_ || !target_) {
247 PCL_ERROR(
"[%s::initCompute] Source or target dataset not given!\n",
252 if (!target_indices_ || target_indices_->empty()) {
253 target_indices_.reset(
new std::vector<int>(
static_cast<int>(target_->size())));
255 for (
int& target_index : *target_indices_)
256 target_index = index++;
257 target_cloud_updated_ =
true;
262 if (nr_samples_ != 0) {
263 const int ss =
static_cast<int>(indices_->size());
264 const int sample_fraction_src = std::max(1,
static_cast<int>(ss / nr_samples_));
267 for (
int i = 0; i < ss; i++)
268 if (rand() % sample_fraction_src == 0)
269 source_indices_->push_back((*indices_)[i]);
272 source_indices_ = indices_;
275 if (source_normals_ && target_normals_ && source_normals_->size() == input_->size() &&
276 target_normals_->size() == target_->size())
280 if (target_cloud_updated_) {
281 tree_->setInputCloud(target_, target_indices_);
282 target_cloud_updated_ =
false;
286 const int min_iterations = 4;
287 const float diameter_fraction = 0.3f;
290 Eigen::Vector4f pt_min, pt_max;
292 diameter_ = (pt_max - pt_min).norm();
295 float max_base_diameter = diameter_ * approx_overlap_ * 2.f;
296 max_base_diameter_sqr_ = max_base_diameter * max_base_diameter;
299 if (normalize_delta_) {
300 float mean_dist = getMeanPointDensity<PointTarget>(
301 target_, *target_indices_, 0.05f * diameter_, nr_threads_);
307 if (max_iterations_ == 0) {
309 std::log(small_error_) /
310 std::log(1.0 - std::pow((
double)approx_overlap_, (
double)min_iterations));
312 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;
339 template <
typename Po
intSource,
typename Po
intTarget,
typename NormalT,
typename Scalar>
342 selectBase(std::vector<int>& base_indices,
float (&ratio)[2])
344 const float too_close_sqr = max_base_diameter_sqr_ * 0.01;
346 Eigen::VectorXf coefficients(4);
349 Eigen::Vector4f centre_pt;
350 float nearest_to_plane = FLT_MAX;
354 for (
int i = 0; i < ransac_iterations_; i++) {
356 if (selectBaseTriangle(base_indices) < 0)
359 std::vector<int> base_triple(base_indices.begin(), base_indices.end() - 1);
364 const PointTarget* pt1 = &((*target_)[base_indices[0]]);
365 const PointTarget* pt2 = &((*target_)[base_indices[1]]);
366 const PointTarget* pt3 = &((*target_)[base_indices[2]]);
368 for (
const auto& target_index : *target_indices_) {
369 const PointTarget* pt4 = &((*target_)[target_index]);
374 float d4 = (pt4->getVector3fMap() - centre_pt.head(3)).squaredNorm();
378 if (d1 < too_close_sqr || d2 < too_close_sqr || d3 < too_close_sqr ||
379 d4 < too_close_sqr || d1 > max_base_diameter_sqr_ ||
380 d2 > max_base_diameter_sqr_ || d3 > max_base_diameter_sqr_)
385 if (dist_to_plane < nearest_to_plane) {
386 base_indices[3] = target_index;
387 nearest_to_plane = dist_to_plane;
392 if (nearest_to_plane != FLT_MAX) {
395 setupBase(base_indices, ratio);
405 template <
typename Po
intSource,
typename Po
intTarget,
typename NormalT,
typename Scalar>
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++) {
419 int* index2 = &(*target_indices_)[rand() % nr_points];
420 int* index3 = &(*target_indices_)[rand() % nr_points];
423 (*target_)[*index2].getVector3fMap() - (*target_)[*index1].getVector3fMap();
425 (*target_)[*index3].getVector3fMap() - (*target_)[*index1].getVector3fMap();
427 u.cross(v).squaredNorm();
430 if (t > best_t && u.squaredNorm() < max_base_diameter_sqr_ &&
431 v.squaredNorm() < max_base_diameter_sqr_) {
433 base_indices[1] = *index2;
434 base_indices[2] = *index3;
439 return (best_t == 0.f ? -1 : 0);
443 template <
typename Po
intSource,
typename Po
intTarget,
typename NormalT,
typename Scalar>
446 setupBase(std::vector<int>& base_indices,
float (&ratio)[2])
448 float best_t = FLT_MAX;
449 const std::vector<int> copy(base_indices.begin(), base_indices.end());
450 std::vector<int> temp(base_indices.begin(), base_indices.end());
453 for (std::vector<int>::const_iterator i = copy.begin(), i_e = copy.end(); i != i_e;
455 for (std::vector<int>::const_iterator j = copy.begin(), j_e = copy.end(); j != j_e;
460 for (std::vector<int>::const_iterator k = copy.begin(), k_e = copy.end();
463 if (k == j || k == i)
466 std::vector<int>::const_iterator l = copy.begin();
467 while (l == i || l == j || l == k)
478 float t = segmentToSegmentDist(temp, ratio_temp);
481 ratio[0] = ratio_temp[0];
482 ratio[1] = ratio_temp[1];
490 template <
typename Po
intSource,
typename Po
intTarget,
typename NormalT,
typename Scalar>
496 Eigen::Vector3f u = (*target_)[base_indices[1]].getVector3fMap() -
497 (*target_)[base_indices[0]].getVector3fMap();
498 Eigen::Vector3f v = (*target_)[base_indices[3]].getVector3fMap() -
499 (*target_)[base_indices[2]].getVector3fMap();
500 Eigen::Vector3f w = (*target_)[base_indices[0]].getVector3fMap() -
501 (*target_)[base_indices[2]].getVector3fMap();
509 float D = a * c - b * b;
510 float sN = 0.f, sD = D;
511 float tN = 0.f, tD = D;
514 if (D < small_error_) {
521 sN = (b * e - c * d);
522 tN = (a * e - b * d);
557 else if ((-d + b) > a)
567 ratio[0] = (std::abs(sN) < small_error_) ? 0.f : sN / sD;
568 ratio[1] = (std::abs(tN) < small_error_) ? 0.f : tN / tD;
570 Eigen::Vector3f x = w + (ratio[0] * u) - (ratio[1] * v);
575 template <
typename Po
intSource,
typename Po
intTarget,
typename NormalT,
typename Scalar>
580 const float max_norm_diff = 0.5f * max_norm_diff_ *
M_PI / 180.f;
584 float ref_norm_angle =
585 (use_normals_ ? ((*target_normals_)[idx1].getNormalVector3fMap() -
586 (*target_normals_)[idx2].getNormalVector3fMap())
591 auto it_out = source_indices_->begin(), it_out_e = source_indices_->end() - 1;
592 auto it_in_e = source_indices_->end();
593 for (; it_out != it_out_e; it_out++) {
594 auto it_in = it_out + 1;
595 const PointSource* pt1 = &(*input_)[*it_out];
596 for (; it_in != it_in_e; it_in++) {
597 const PointSource* pt2 = &(*input_)[*it_in];
601 if (std::abs(dist - ref_dist) < max_pair_diff_) {
604 const NormalT* pt1_n = &((*source_normals_)[*it_out]);
605 const NormalT* pt2_n = &((*source_normals_)[*it_in]);
608 (pt1_n->getNormalVector3fMap() - pt2_n->getNormalVector3fMap()).norm();
610 (pt1_n->getNormalVector3fMap() + pt2_n->getNormalVector3fMap()).norm();
612 float norm_diff = std::min<float>(std::abs(norm_angle_1 - ref_norm_angle),
613 std::abs(norm_angle_2 - ref_norm_angle));
614 if (norm_diff > max_norm_diff)
625 return (pairs.empty() ? -1 : 0);
629 template <
typename Po
intSource,
typename Po
intTarget,
typename NormalT,
typename Scalar>
633 std::vector<std::vector<int>>& matches,
636 const float (&ratio)[2])
652 cloud_e->resize(pairs_a.size() * 2);
653 PointCloudSourceIterator it_pt = cloud_e->begin();
654 for (
const auto& pair : pairs_a) {
655 const PointSource* pt1 = &((*input_)[pair.index_match]);
656 const PointSource* pt2 = &((*input_)[pair.index_query]);
659 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) {
675 const PointTarget* pt1 = &((*input_)[pair.index_match]);
676 const PointTarget* pt2 = &((*input_)[pair.index_query]);
679 for (
const float& r : ratio) {
681 pt_e.x = pt1->x + r * (pt2->x - pt1->x);
682 pt_e.y = pt1->y + r * (pt2->y - pt1->y);
683 pt_e.z = pt1->z + r * (pt2->z - pt1->z);
686 tree_e->radiusSearch(pt_e, coincidation_limit_, ids, dists_sqr);
687 for (
const auto&
id : ids) {
688 std::vector<int> match_indices(4);
691 pairs_a[
static_cast<int>(std::floor((
float)(
id / 2.f)))].index_match;
693 pairs_a[
static_cast<int>(std::floor((
float)(
id / 2.f)))].index_query;
694 match_indices[2] = pair.index_match;
695 match_indices[3] = pair.index_query;
698 if (checkBaseMatch(match_indices, dist_base) < 0)
701 matches.push_back(match_indices);
707 return (!matches.empty() ? 0 : -1);
711 template <
typename Po
intSource,
typename Po
intTarget,
typename NormalT,
typename Scalar>
714 checkBaseMatch(
const std::vector<int>& match_indices,
const float (&dist_ref)[4])
726 return (std::abs(d0 - dist_ref[0]) < max_edge_diff_ &&
727 std::abs(d1 - dist_ref[1]) < max_edge_diff_ &&
728 std::abs(d2 - dist_ref[2]) < max_edge_diff_ &&
729 std::abs(d3 - dist_ref[3]) < max_edge_diff_)
735 template <
typename Po
intSource,
typename Po
intTarget,
typename NormalT,
typename Scalar>
739 std::vector<std::vector<int>>& matches,
742 candidates.resize(1);
743 float fitness_score = FLT_MAX;
746 for (
auto& match : matches) {
747 Eigen::Matrix4f transformation_temp;
752 linkMatchWithBase(base_indices, match, correspondences_temp);
755 if (validateMatch(base_indices, match, correspondences_temp, transformation_temp) <
761 if (validateTransformation(transformation_temp, fitness_score) < 0)
765 candidates[0].fitness_score = fitness_score;
766 candidates[0].transformation = transformation_temp;
767 correspondences_temp.erase(correspondences_temp.end() - 1);
768 candidates[0].correspondences = correspondences_temp;
773 template <
typename Po
intSource,
typename Po
intTarget,
typename NormalT,
typename Scalar>
777 std::vector<int>& match_indices,
781 Eigen::Vector4f centre_base{0, 0, 0, 0}, centre_match{0, 0, 0, 0};
785 PointTarget centre_pt_base;
786 centre_pt_base.x = centre_base[0];
787 centre_pt_base.y = centre_base[1];
788 centre_pt_base.z = centre_base[2];
790 PointSource centre_pt_match;
791 centre_pt_match.x = centre_match[0];
792 centre_pt_match.y = centre_match[1];
793 centre_pt_match.z = centre_match[2];
796 std::vector<int> copy = match_indices;
798 auto it_match_orig = match_indices.begin();
799 for (
auto it_base = base_indices.cbegin(), it_base_e = base_indices.cend();
800 it_base != it_base_e;
801 it_base++, it_match_orig++) {
804 float best_diff_sqr = FLT_MAX;
807 for (
const auto& match_index : copy) {
811 float diff_sqr = std::abs(dist_sqr_1 - dist_sqr_2);
813 if (diff_sqr < best_diff_sqr) {
814 best_diff_sqr = diff_sqr;
815 best_index = match_index;
821 *it_match_orig = best_index;
826 template <
typename Po
intSource,
typename Po
intTarget,
typename NormalT,
typename Scalar>
830 const std::vector<int>& match_indices,
832 Eigen::Matrix4f& transformation)
836 correspondences_temp.erase(correspondences_temp.end() - 1);
839 transformation_estimation_->estimateRigidTransformation(
840 *input_, *target_, correspondences_temp, transformation);
847 std::size_t nr_points = correspondences_temp.size();
849 for (std::size_t i = 0; i < nr_points; i++)
851 target_->points[base_indices[i]]);
854 return (mse < max_mse_ ? 0 : -1);
858 template <
typename Po
intSource,
typename Po
intTarget,
typename NormalT,
typename Scalar>
866 *input_, *source_indices_, source_transformed, transformation);
868 std::size_t nr_points = source_transformed.
size();
869 std::size_t terminate_value =
870 fitness_score > 1 ? 0
871 :
static_cast<std::size_t
>((1.f - fitness_score) * nr_points);
873 float inlier_score_temp = 0;
874 std::vector<int> ids;
875 std::vector<float> dists_sqr;
876 PointCloudSourceIterator it = source_transformed.
begin();
878 for (std::size_t i = 0; i < nr_points; it++, i++) {
880 tree_->nearestKSearch(*it, 1, ids, dists_sqr);
881 inlier_score_temp += (dists_sqr[0] < max_inlier_dist_sqr_ ? 1 : 0);
884 if (nr_points - i + inlier_score_temp < terminate_value)
889 inlier_score_temp /=
static_cast<float>(nr_points);
890 float fitness_score_temp = 1.f - inlier_score_temp;
892 if (fitness_score_temp > fitness_score)
895 fitness_score = fitness_score_temp;
900 template <
typename Po
intSource,
typename Po
intTarget,
typename NormalT,
typename Scalar>
906 int nr_candidates =
static_cast<int>(candidates.size());
908 float best_score = FLT_MAX;
909 for (
int i = 0; i < nr_candidates; i++) {
910 const float& fitness_score = candidates[i][0].fitness_score;
911 if (fitness_score < best_score) {
912 best_score = fitness_score;
918 if (!(best_index < 0)) {
919 fitness_score_ = candidates[best_index][0].fitness_score;
920 final_transformation_ = candidates[best_index][0].transformation;
921 *correspondences_ = candidates[best_index][0].correspondences;
924 converged_ = fitness_score_ < score_threshold_;
930 #endif // PCL_REGISTRATION_IMPL_IA_4PCS_H_