39 #ifndef PCL_REGISTRATION_IMPL_IA_FPCS_H_ 40 #define PCL_REGISTRATION_IMPL_IA_FPCS_H_ 42 #include <pcl/registration/ia_fpcs.h> 43 #include <pcl/common/time.h> 44 #include <pcl/common/distances.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 \ 65 reduction (+:mean_dist, num) \ 66 private (ids, dists_sqr) shared (tree, cloud) \ 67 default (none)num_threads (nr_threads) 70 for (
int i = 0; i < 1000; i++)
73 if (dists_sqr[1] < max_dist_sqr)
75 mean_dist += std::sqrt (dists_sqr[1]);
80 return (mean_dist / num);
85 template <
typename Po
intT>
inline float 87 float max_dist,
int nr_threads)
89 const float max_dist_sqr = max_dist * max_dist;
90 const std::size_t s = indices.size ();
95 float mean_dist = 0.f;
97 std::vector <int> ids (2);
98 std::vector <float> dists_sqr (2);
101 #pragma omp parallel for \ 102 reduction (+:mean_dist, num) \ 103 private (ids, dists_sqr) shared (tree, cloud, indices) \ 104 default (none)num_threads (nr_threads) 107 for (
int i = 0; i < 1000; i++)
110 if (dists_sqr[1] < max_dist_sqr)
112 mean_dist += std::sqrt (dists_sqr[1]);
117 return (mean_dist / num);
122 template <
typename Po
intSource,
typename Po
intTarget,
typename NormalT,
typename Scalar>
127 approx_overlap_ (0.5f),
129 score_threshold_ (FLT_MAX),
131 max_norm_diff_ (90.f),
133 fitness_score_ (FLT_MAX),
135 max_base_diameter_sqr_ (),
136 use_normals_ (
false),
137 normalize_delta_ (
true),
140 coincidation_limit_ (),
142 max_inlier_dist_sqr_ (),
143 small_error_ (0.00001f)
145 reg_name_ =
"pcl::registration::FPCSInitialAlignment";
147 ransac_iterations_ = 1000;
153 template <
typename Po
intSource,
typename Po
intTarget,
typename NormalT,
typename Scalar>
void 156 const Eigen::Matrix4f &guess)
161 final_transformation_ = guess;
163 std::vector <MatchingCandidates> all_candidates (max_iterations_);
167 #pragma omp parallel num_threads (nr_threads_) 171 std::srand (static_cast <unsigned int> (std::time (NULL)) ^ omp_get_thread_num ());
172 #pragma omp for schedule (dynamic) 174 for (
int i = 0; i < max_iterations_; i++)
178 #pragma omp flush (abort) 182 std::vector <int> base_indices (4);
184 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.size () != 0)
203 all_candidates[i] = candidates;
209 abort = (candidates.size () > 0 ? candidates[0].fitness_score < score_threshold_ : abort);
214 #pragma omp flush (abort) 222 finalCompute (all_candidates);
232 template <
typename Po
intSource,
typename Po
intTarget,
typename NormalT,
typename Scalar>
bool 235 std::srand (static_cast <unsigned int> (std::time (NULL)));
242 if (!input_ || !target_)
244 PCL_ERROR (
"[%s::initCompute] Source or target dataset not given!\n", reg_name_.c_str ());
248 if (!target_indices_ || target_indices_->size () == 0)
250 target_indices_.reset (
new std::vector <int> (static_cast <int> (target_->size ())));
252 for (std::vector <int>::iterator it = target_indices_->begin (), it_e = target_indices_->end (); it != it_e; it++)
254 target_cloud_updated_ =
true;
258 if (nr_samples_ != 0)
260 const int ss = static_cast <
int> (indices_->size ());
261 const int sample_fraction_src = std::max (1, static_cast <int> (ss / nr_samples_));
264 for (
int i = 0; i < ss; i++)
265 if (rand () % sample_fraction_src == 0)
266 source_indices_->push_back ((*indices_) [i]);
269 source_indices_ = indices_;
272 if (source_normals_ && target_normals_ && source_normals_->size () == input_->size () && target_normals_->size () == target_->size ())
276 if (target_cloud_updated_)
278 tree_->setInputCloud (target_, target_indices_);
279 target_cloud_updated_ =
false;
283 const int min_iterations = 4;
284 const float diameter_fraction = 0.3f;
287 Eigen::Vector4f pt_min, pt_max;
289 diameter_ = (pt_max - pt_min).norm ();
292 float max_base_diameter = diameter_* approx_overlap_ * 2.f;
293 max_base_diameter_sqr_ = max_base_diameter * max_base_diameter;
296 if (normalize_delta_)
298 float mean_dist = getMeanPointDensity <PointTarget> (target_, *target_indices_, 0.05f * diameter_, nr_threads_);
303 if (max_iterations_ == 0)
305 float first_est = std::log (small_error_) / std::log (1.0 - std::pow ((
double) approx_overlap_, (
double) min_iterations));
306 max_iterations_ = static_cast <
int> (first_est / (diameter_fraction * approx_overlap_ * 2.f));
310 if (score_threshold_ == FLT_MAX)
311 score_threshold_ = 1.f - approx_overlap_;
313 if (max_iterations_ < 4)
316 if (max_runtime_ < 1)
317 max_runtime_ = INT_MAX;
320 max_pair_diff_ = delta_ * 2.f;
321 max_edge_diff_ = delta_ * 4.f;
322 coincidation_limit_ = delta_ * 2.f;
323 max_mse_ = powf (delta_* 2.f, 2.f);
324 max_inlier_dist_sqr_ = powf (delta_ * 2.f, 2.f);
327 fitness_score_ = FLT_MAX;
334 template <
typename Po
intSource,
typename Po
intTarget,
typename NormalT,
typename Scalar>
int 336 std::vector <int> &base_indices,
339 const float too_close_sqr = max_base_diameter_sqr_*0.01;
341 Eigen::VectorXf coefficients (4);
344 Eigen::Vector4f centre_pt;
345 float nearest_to_plane = FLT_MAX;
348 for (
int i = 0; i < ransac_iterations_; i++)
351 if (selectBaseTriangle (base_indices) < 0)
354 std::vector <int> base_triple (base_indices.begin (), base_indices.end () - 1);
359 const PointTarget *pt1 = &(target_->points[base_indices[0]]);
360 const PointTarget *pt2 = &(target_->points[base_indices[1]]);
361 const PointTarget *pt3 = &(target_->points[base_indices[2]]);
363 for (std::vector <int>::iterator it = target_indices_->begin (), it_e = target_indices_->end (); it != it_e; it++)
365 const PointTarget *pt4 = &(target_->points[*it]);
370 float d4 = (pt4->getVector3fMap () - centre_pt.head (3)).squaredNorm ();
373 if (d1 < too_close_sqr || d2 < too_close_sqr || d3 < too_close_sqr || d4 < too_close_sqr ||
374 d1 > max_base_diameter_sqr_ || d2 > max_base_diameter_sqr_ || d3 > max_base_diameter_sqr_)
379 if (dist_to_plane < nearest_to_plane)
381 base_indices[3] = *it;
382 nearest_to_plane = dist_to_plane;
387 if (nearest_to_plane != FLT_MAX)
390 setupBase (base_indices, ratio);
401 template <
typename Po
intSource,
typename Po
intTarget,
typename NormalT,
typename Scalar>
int 404 int nr_points = static_cast <
int> (target_indices_->size ());
408 base_indices[0] = (*target_indices_)[rand () % nr_points];
409 int *index1 = &base_indices[0];
412 for (
int i = 0; i < ransac_iterations_; i++)
414 int *index2 = &(*target_indices_)[rand () % nr_points];
415 int *index3 = &(*target_indices_)[rand () % nr_points];
417 Eigen::Vector3f u = target_->points[*index2].getVector3fMap () - target_->points[*index1].getVector3fMap ();
418 Eigen::Vector3f v = target_->points[*index3].getVector3fMap () - target_->points[*index1].getVector3fMap ();
419 float t = u.cross (v).squaredNorm ();
422 if (t > best_t && u.squaredNorm () < max_base_diameter_sqr_ && v.squaredNorm () < max_base_diameter_sqr_)
425 base_indices[1] = *index2;
426 base_indices[2] = *index3;
431 return (best_t == 0.f ? -1 : 0);
436 template <
typename Po
intSource,
typename Po
intTarget,
typename NormalT,
typename Scalar>
void 438 std::vector <int> &base_indices,
441 float best_t = FLT_MAX;
442 const std::vector <int> copy (base_indices.begin (), base_indices.end ());
443 std::vector <int> temp (base_indices.begin (), base_indices.end ());
446 for (std::vector <int>::const_iterator i = copy.begin (), i_e = copy.end (); i != i_e; i++)
447 for (std::vector <int>::const_iterator j = copy.begin (), j_e = copy.end (); j != j_e; j++)
452 for (std::vector <int>::const_iterator k = copy.begin (), k_e = copy.end (); k != k_e; k++)
454 if (k == j || k == i)
457 std::vector <int>::const_iterator l = copy.begin ();
458 while (l == i || l == j || l == k)
468 float t = segmentToSegmentDist (temp, ratio_temp);
472 ratio[0] = ratio_temp[0];
473 ratio[1] = ratio_temp[1];
482 template <
typename Po
intSource,
typename Po
intTarget,
typename NormalT,
typename Scalar>
float 484 const std::vector <int> &base_indices,
488 Eigen::Vector3f u = target_->points[base_indices[1]].getVector3fMap () - target_->points[base_indices[0]].getVector3fMap ();
489 Eigen::Vector3f v = target_->points[base_indices[3]].getVector3fMap () - target_->points[base_indices[2]].getVector3fMap ();
490 Eigen::Vector3f w = target_->points[base_indices[0]].getVector3fMap () - target_->points[base_indices[2]].getVector3fMap ();
498 float D = a * c - b * b;
499 float sN = 0.f, sD = D;
500 float tN = 0.f, tD = D;
503 if (D < small_error_)
512 sN = (b * e - c * d);
513 tN = (a * e - b * d);
553 else if ((-d + b) > a)
564 ratio[0] = (std::abs (sN) < small_error_) ? 0.f : sN / sD;
565 ratio[1] = (std::abs (tN) < small_error_) ? 0.f : tN / tD;
567 Eigen::Vector3f x = w + (ratio[0] * u) - (ratio[1] * v);
573 template <
typename Po
intSource,
typename Po
intTarget,
typename NormalT,
typename Scalar>
int 579 const float max_norm_diff = 0.5f * max_norm_diff_ * M_PI / 180.f;
583 float ref_norm_angle = (use_normals_ ? (target_normals_->points[idx1].getNormalVector3fMap () -
584 target_normals_->points[idx2].getNormalVector3fMap ()).norm () : 0.f);
587 std::vector <int>::iterator it_out = source_indices_->begin (), it_out_e = source_indices_->end () - 1;
588 std::vector <int>::iterator it_in, it_in_e = source_indices_->end ();
589 for ( ; it_out != it_out_e; it_out++)
592 const PointSource *pt1 = &(*input_)[*it_out];
593 for ( ; it_in != it_in_e; it_in++)
595 const PointSource *pt2 = &(*input_)[*it_in];
599 if (std::abs(dist - ref_dist) < max_pair_diff_)
604 const NormalT *pt1_n = &(source_normals_->points[*it_out]);
605 const NormalT *pt2_n = &(source_normals_->points[*it_in]);
607 float norm_angle_1 = (pt1_n->getNormalVector3fMap () - pt2_n->getNormalVector3fMap ()).norm ();
608 float norm_angle_2 = (pt1_n->getNormalVector3fMap () + pt2_n->getNormalVector3fMap ()).norm ();
610 float norm_diff = std::min <float> (std::abs (norm_angle_1 - ref_norm_angle), std::abs (norm_angle_2 - ref_norm_angle));
611 if (norm_diff > max_norm_diff)
622 return (pairs.size () == 0 ? -1 : 0);
627 template <
typename Po
intSource,
typename Po
intTarget,
typename NormalT,
typename Scalar>
int 629 const std::vector <int> &base_indices,
630 std::vector <std::vector <int> > &matches,
633 const float (&ratio)[2])
637 dist_base[0] =
pcl::euclideanDistance (target_->points[base_indices[0]], target_->points[base_indices[2]]);
638 dist_base[1] =
pcl::euclideanDistance (target_->points[base_indices[0]], target_->points[base_indices[3]]);
639 dist_base[2] =
pcl::euclideanDistance (target_->points[base_indices[1]], target_->points[base_indices[2]]);
640 dist_base[3] =
pcl::euclideanDistance (target_->points[base_indices[1]], target_->points[base_indices[3]]);
644 cloud_e->resize (pairs_a.size () * 2);
645 PointCloudSourceIterator it_pt = cloud_e->begin ();
646 for (pcl::Correspondences::const_iterator it_pair = pairs_a.begin (), it_pair_e = pairs_a.end () ; it_pair != it_pair_e; it_pair++)
648 const PointSource *pt1 = &(input_->points[it_pair->index_match]);
649 const PointSource *pt2 = &(input_->points[it_pair->index_query]);
652 for (
int i = 0; i < 2; i++, it_pt++)
654 it_pt->x = pt1->x + ratio[i] * (pt2->x - pt1->x);
655 it_pt->y = pt1->y + ratio[i] * (pt2->y - pt1->y);
656 it_pt->z = pt1->z + ratio[i] * (pt2->z - pt1->z);
662 tree_e->setInputCloud (cloud_e);
664 std::vector <int> ids;
665 std::vector <float> dists_sqr;
668 for (pcl::Correspondences::const_iterator it_pair = pairs_b.begin (), it_pair_e = pairs_b.end () ; it_pair != it_pair_e; it_pair++)
670 const PointTarget *pt1 = &(input_->points[it_pair->index_match]);
671 const PointTarget *pt2 = &(input_->points[it_pair->index_query]);
674 for (
int i = 0; i < 2; i++)
677 pt_e.x = pt1->x + ratio[i] * (pt2->x - pt1->x);
678 pt_e.y = pt1->y + ratio[i] * (pt2->y - pt1->y);
679 pt_e.z = pt1->z + ratio[i] * (pt2->z - pt1->z);
682 tree_e->radiusSearch (pt_e, coincidation_limit_, ids, dists_sqr);
683 for (std::vector <int>::iterator it = ids.begin (), it_e = ids.end (); it != it_e; it++)
685 std::vector <int> match_indices (4);
687 match_indices[0] = pairs_a[static_cast <
int> (std::floor ((
float)(*it/2.f)))].index_match;
688 match_indices[1] = pairs_a[static_cast <
int> (std::floor ((
float)(*it/2.f)))].index_query;
689 match_indices[2] = it_pair->index_match;
690 match_indices[3] = it_pair->index_query;
693 if (checkBaseMatch (match_indices, dist_base) < 0)
696 matches.push_back (match_indices);
702 return (matches.size () > 0 ? 0 : -1);
707 template <
typename Po
intSource,
typename Po
intTarget,
typename NormalT,
typename Scalar>
int 709 const std::vector <int> &match_indices,
710 const float (&dist_ref)[4])
718 return (std::abs (d0 - dist_ref[0]) < max_edge_diff_ && std::abs (d1 - dist_ref[1]) < max_edge_diff_ &&
719 std::abs (d2 - dist_ref[2]) < max_edge_diff_ && std::abs (d3 - dist_ref[3]) < max_edge_diff_) ? 0 : -1;
724 template <
typename Po
intSource,
typename Po
intTarget,
typename NormalT,
typename Scalar>
void 726 const std::vector <int> &base_indices,
727 std::vector <std::vector <int> > &matches,
730 candidates.resize (1);
731 float fitness_score = FLT_MAX;
734 for (std::vector <std::vector <int> >::iterator match_indices = matches.begin (), it_e = matches.end (); match_indices != it_e; match_indices++)
736 Eigen::Matrix4f transformation_temp;
740 linkMatchWithBase (base_indices, *match_indices, correspondences_temp);
743 if (validateMatch (base_indices, *match_indices, correspondences_temp, transformation_temp) < 0)
747 if (validateTransformation (transformation_temp, fitness_score) < 0)
751 candidates[0].fitness_score = fitness_score;
752 candidates [0].transformation = transformation_temp;
753 correspondences_temp.erase (correspondences_temp.end () - 1);
754 candidates[0].correspondences = correspondences_temp;
760 template <
typename Po
intSource,
typename Po
intTarget,
typename NormalT,
typename Scalar>
void 762 const std::vector <int> &base_indices,
763 std::vector <int> &match_indices,
767 Eigen::Vector4f centre_base, centre_match;
771 PointTarget centre_pt_base;
772 centre_pt_base.x = centre_base[0];
773 centre_pt_base.y = centre_base[1];
774 centre_pt_base.z = centre_base[2];
776 PointSource centre_pt_match;
777 centre_pt_match.x = centre_match[0];
778 centre_pt_match.y = centre_match[1];
779 centre_pt_match.z = centre_match[2];
782 std::vector <int> copy = match_indices;
784 std::vector <int>::const_iterator it_base = base_indices.begin (), it_base_e = base_indices.end ();
785 std::vector <int>::iterator it_match, it_match_e = copy.end ();
786 std::vector <int>::iterator it_match_orig = match_indices.begin ();
787 for (; it_base != it_base_e; it_base++, it_match_orig++)
790 float best_diff_sqr = FLT_MAX;
793 for (it_match = copy.begin (); it_match != it_match_e; it_match++)
797 float diff_sqr = std::abs(dist_sqr_1 - dist_sqr_2);
799 if (diff_sqr < best_diff_sqr)
801 best_diff_sqr = diff_sqr;
802 best_index = *it_match;
808 *it_match_orig = best_index;
814 template <
typename Po
intSource,
typename Po
intTarget,
typename NormalT,
typename Scalar>
int 816 const std::vector <int> &base_indices,
817 const std::vector <int> &match_indices,
819 Eigen::Matrix4f &transformation)
823 correspondences_temp.erase (correspondences_temp.end () - 1);
826 transformation_estimation_->estimateRigidTransformation (*input_, *target_, correspondences_temp, transformation);
833 std::size_t nr_points = correspondences_temp.size ();
835 for (std::size_t i = 0; i < nr_points; i++)
839 return (mse < max_mse_ ? 0 : -1);
844 template <
typename Po
intSource,
typename Po
intTarget,
typename NormalT,
typename Scalar>
int 846 Eigen::Matrix4f &transformation,
847 float &fitness_score)
853 std::size_t nr_points = source_transformed.
size ();
854 std::size_t terminate_value = fitness_score > 1 ? 0 : static_cast <std::size_t> ((1.f - fitness_score) * nr_points);
856 float inlier_score_temp = 0;
857 std::vector <int> ids;
858 std::vector <float> dists_sqr;
859 PointCloudSourceIterator it = source_transformed.
begin ();
861 for (std::size_t i = 0; i < nr_points; it++, i++)
864 tree_->nearestKSearch (*it, 1, ids, dists_sqr);
865 inlier_score_temp += (dists_sqr[0] < max_inlier_dist_sqr_ ? 1 : 0);
868 if (nr_points - i + inlier_score_temp < terminate_value)
873 inlier_score_temp /= static_cast <
float> (nr_points);
874 float fitness_score_temp = 1.f - inlier_score_temp;
876 if (fitness_score_temp > fitness_score)
879 fitness_score = fitness_score_temp;
885 template <
typename Po
intSource,
typename Po
intTarget,
typename NormalT,
typename Scalar>
void 887 const std::vector <MatchingCandidates > &candidates)
890 int nr_candidates = static_cast <
int> (candidates.size ());
892 float best_score = FLT_MAX;
893 for (
int i = 0; i < nr_candidates; i++)
895 const float &fitness_score = candidates [i][0].fitness_score;
896 if (fitness_score < best_score)
898 best_score = fitness_score;
904 if (!(best_index < 0))
906 fitness_score_ = candidates [best_index][0].fitness_score;
907 final_transformation_ = candidates [best_index][0].transformation;
908 *correspondences_ = candidates [best_index][0].correspondences;
911 converged_ = fitness_score_ < score_threshold_;
917 #endif // PCL_REGISTRATION_IMPL_IA_4PCS_H_ A point structure representing normal coordinates and the surface curvature estimate.
std::vector< MatchingCandidate, Eigen::aligned_allocator< MatchingCandidate > > MatchingCandidates
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
FPCSInitialAlignment computes corresponding four point congruent sets as described in: "4-points cong...
boost::shared_ptr< std::vector< int > > IndicesPtr
void setIndices(const boost::shared_ptr< std::vector< int > > &indices)
Provide a pointer to the vector of indices that represents the input data.
float squaredEuclideanDistance(const PointType1 &p1, const PointType2 &p2)
Calculate the squared euclidean distance between the two given points.
Correspondence represents a match between two entities (e.g., points, descriptors, etc).
float euclideanDistance(const PointType1 &p1, const PointType2 &p2)
Calculate the euclidean distance between the two given points.
PointCloudSource::Ptr PointCloudSourcePtr
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.
bool computeModelCoefficients(const std::vector< int > &samples, Eigen::VectorXf &model_coefficients)
Check whether the given index samples can form a valid plane model, compute the model coefficients fr...
void transformPointCloud(const pcl::PointCloud< PointT > &cloud_in, pcl::PointCloud< PointT > &cloud_out, const Eigen::Transform< Scalar, 3, Eigen::Affine > &transform, bool copy_all_fields=true)
Apply an affine transform defined by an Eigen Transform.
std::vector< pcl::Correspondence, Eigen::aligned_allocator< pcl::Correspondence > > Correspondences
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...
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.
boost::shared_ptr< const PointCloud< PointT > > ConstPtr
double getTimeSeconds()
Retrieve the time in seconds spent since the last call to reset().
void setInputCloud(const PointCloudConstPtr &cloud, const IndicesConstPtr &indices=IndicesConstPtr())
Provide a pointer to the input dataset.
KdTreeReciprocal::Ptr KdTreeReciprocalPtr
unsigned int compute3DCentroid(ConstCloudIterator< PointT > &cloud_iterator, Eigen::Matrix< Scalar, 4, 1 > ¢roid)
Compute the 3D (X-Y-Z) centroid of a set of points and return it as a 3D vector.
int nearestKSearch(const PointT &point, int k, std::vector< int > &k_indices, std::vector< float > &k_sqr_distances) const
Search for the k-nearest neighbors for the given query point.
SampleConsensusModelPlane defines a model for 3D plane segmentation.