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>
49template <
typename Po
intT>
56 const std::size_t s = cloud.size();
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++) {
86template <
typename Po
intT>
94 const std::size_t s = indices.size();
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++) {
132template <
typename Po
intSource,
typename Po
intTarget,
typename NormalT,
typename Scalar>
138, approx_overlap_(0.5f)
142, max_norm_diff_(90.f)
146, max_base_diameter_sqr_()
148, normalize_delta_(
true)
151, coincidation_limit_()
153, max_inlier_dist_sqr_()
154, small_error_(0.00001f)
156 reg_name_ =
"pcl::registration::FPCSInitialAlignment";
164template <
typename Po
intSource,
typename Po
intTarget,
typename NormalT,
typename Scalar>
172 final_transformation_ =
guess;
177#pragma omp parallel default(none) shared(abort, all_candidates, timer) \
178 num_threads(nr_threads_)
181 const unsigned int seed =
184 PCL_DEBUG(
"[%s::computeTransformation] Using seed=%u\n", reg_name_.c_str(), seed);
185#pragma omp for schedule(dynamic)
187 for (
int i = 0; i < max_iterations_; i++) {
188#pragma omp flush(abort)
206 std::vector<pcl::Indices>
matches;
222#pragma omp flush(abort)
237template <
typename Po
intSource,
typename Po
intTarget,
typename NormalT,
typename Scalar>
242 const unsigned int seed = std::time(
nullptr);
244 PCL_DEBUG(
"[%s::initCompute] Using seed=%u\n", reg_name_.c_str(), seed);
251 if (!input_ || !target_) {
252 PCL_ERROR(
"[%s::initCompute] Source or target dataset not given!\n",
257 if (!target_indices_ || target_indices_->empty()) {
258 target_indices_.reset(
new pcl::Indices(target_->size()));
262 target_cloud_updated_ =
true;
267 if (nr_samples_ != 0) {
268 const int ss =
static_cast<int>(indices_->size());
272 for (
int i = 0; i <
ss; i++)
274 source_indices_->push_back((*indices_)[i]);
277 source_indices_ = indices_;
280 if (source_normals_ && target_normals_ && source_normals_->size() == input_->size() &&
281 target_normals_->size() == target_->size())
285 if (target_cloud_updated_) {
286 tree_->setInputCloud(target_, target_indices_);
287 target_cloud_updated_ =
false;
304 if (normalize_delta_) {
306 target_, *target_indices_, 0.05f * diameter_, nr_threads_);
312 if (max_iterations_ == 0) {
314 std::log(small_error_) /
315 std::log(1.0 - std::pow((
double)approx_overlap_, (
double)
min_iterations));
322 score_threshold_ = 1.f - approx_overlap_;
324 if (max_iterations_ < 4)
327 if (max_runtime_ < 1)
331 max_pair_diff_ = delta_ * 2.f;
332 max_edge_diff_ = delta_ * 4.f;
333 coincidation_limit_ = delta_ * 2.f;
334 max_mse_ =
powf(delta_ * 2.f, 2.f);
335 max_inlier_dist_sqr_ =
powf(delta_ * 2.f, 2.f);
344template <
typename Po
intSource,
typename Po
intTarget,
typename NormalT,
typename Scalar>
351 Eigen::VectorXf coefficients(4);
353 plane.setIndices(target_indices_);
359 for (
int i = 0; i < ransac_iterations_; i++) {
385 d2 > max_base_diameter_sqr_ ||
d3 > max_base_diameter_sqr_)
410template <
typename Po
intSource,
typename Po
intTarget,
typename NormalT,
typename Scalar>
415 const auto nr_points = target_indices_->
size();
423 for (
int i = 0; i < ransac_iterations_; i++) {
424 auto*
index2 = &(*target_indices_)[
rand() % nr_points];
425 auto*
index3 = &(*target_indices_)[
rand() % nr_points];
428 (*target_)[*
index2].getVector3fMap() - (*target_)[*
index1].getVector3fMap();
430 (*target_)[*
index3].getVector3fMap() - (*target_)[*
index1].getVector3fMap();
432 u.cross(v).squaredNorm();
435 if (
t >
best_t && u.squaredNorm() < max_base_diameter_sqr_ &&
436 v.squaredNorm() < max_base_diameter_sqr_) {
444 return (
best_t == 0.f ? -1 : 0);
448template <
typename Po
intSource,
typename Po
intTarget,
typename NormalT,
typename Scalar>
458 for (
auto i = copy.begin(),
i_e = copy.end(); i !=
i_e; ++i)
459 for (
auto j = copy.begin(),
j_e = copy.end(); j !=
j_e; ++j) {
463 for (
auto k = copy.begin(),
k_e = copy.end(); k !=
k_e; ++k) {
464 if (k == j || k == i)
467 auto l = copy.begin();
468 while (l == i || l == j || l == k)
491template <
typename Po
intSource,
typename Po
intTarget,
typename NormalT,
typename Scalar>
497 Eigen::Vector3f u = (*target_)[
base_indices[1]].getVector3fMap() -
499 Eigen::Vector3f v = (*target_)[
base_indices[3]].getVector3fMap() -
501 Eigen::Vector3f w = (*target_)[
base_indices[0]].getVector3fMap() -
510 float D = a *
c - b * b;
511 float sN = 0.f,
sD =
D;
512 float tN = 0.f,
tD =
D;
515 if (
D < small_error_) {
522 sN = (b *
e -
c * d);
523 tN = (a *
e - b * d);
558 else if ((-d + b) > a)
568 ratio[0] = (std::abs(
sN) < small_error_) ? 0.f :
sN /
sD;
569 ratio[1] = (std::abs(
tN) < small_error_) ? 0.f :
tN /
tD;
571 Eigen::Vector3f x = w + (
ratio[0] * u) - (
ratio[1] * v);
576template <
typename Po
intSource,
typename Po
intTarget,
typename NormalT,
typename Scalar>
586 (use_normals_ ? ((*target_normals_)[
idx1].getNormalVector3fMap() -
587 (*target_normals_)[
idx2].getNormalVector3fMap())
592 auto it_out = source_indices_->begin(),
it_out_e = source_indices_->end() - 1;
593 auto it_in_e = source_indices_->end();
596 const PointSource*
pt1 = &(*input_)[*
it_out];
598 const PointSource*
pt2 = &(*input_)[*
it_in];
609 (
pt1_n->getNormalVector3fMap() -
pt2_n->getNormalVector3fMap()).norm();
611 (
pt1_n->getNormalVector3fMap() +
pt2_n->getNormalVector3fMap()).norm();
626 return (
pairs.empty() ? -1 : 0);
630template <
typename Po
intSource,
typename Po
intTarget,
typename NormalT,
typename Scalar>
634 std::vector<pcl::Indices>&
matches,
637 const float (&
ratio)[2])
655 for (
const auto& pair :
pairs_a) {
656 const PointSource*
pt1 = &((*input_)[pair.index_match]);
657 const PointSource*
pt2 = &((*input_)[pair.index_query]);
660 for (
int i = 0; i < 2; i++,
it_pt++) {
675 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) {
688 for (
const auto&
id :
ids) {
692 pairs_a[
static_cast<int>(std::floor((
float)(
id / 2.f)))].index_match;
694 pairs_a[
static_cast<int>(std::floor((
float)(
id / 2.f)))].index_query;
708 return (!
matches.empty() ? 0 : -1);
712template <
typename Po
intSource,
typename Po
intTarget,
typename NormalT,
typename Scalar>
727 return (std::abs(
d0 -
dist_ref[0]) < max_edge_diff_ &&
728 std::abs(
d1 -
dist_ref[1]) < max_edge_diff_ &&
729 std::abs(
d2 -
dist_ref[2]) < max_edge_diff_ &&
736template <
typename Po
intSource,
typename Po
intTarget,
typename NormalT,
typename Scalar>
740 std::vector<pcl::Indices>&
matches,
774template <
typename Po
intSource,
typename Po
intTarget,
typename NormalT,
typename Scalar>
827template <
typename Po
intSource,
typename Po
intTarget,
typename NormalT,
typename Scalar>
833 Eigen::Matrix4f& transformation)
840 transformation_estimation_->estimateRigidTransformation(
850 for (std::size_t i = 0; i < nr_points; i++)
855 return (
mse < max_mse_ ? 0 : -1);
859template <
typename Po
intSource,
typename Po
intTarget,
typename NormalT,
typename Scalar>
871 fitness_score > 1 ? 0
872 :
static_cast<std::size_t
>((1.f - fitness_score) * nr_points);
879 for (std::size_t i = 0; i < nr_points;
it++, i++) {
901template <
typename Po
intSource,
typename Po
intTarget,
typename NormalT,
typename Scalar>
911 const float& fitness_score =
candidates[i][0].fitness_score;
925 converged_ = fitness_score_ < score_threshold_;
Iterator class for point clouds with or without given indices.
std::size_t size() const
Size of the range the iterator is going through.
shared_ptr< const PointCloud< PointT > > ConstPtr
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.
virtual void finalCompute(const std::vector< MatchingCandidates > &candidates)
Final computation of best match out of vector of best matches.
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...
int selectBaseTriangle(pcl::Indices &base_indices)
Select randomly a triplet of points with large point-to-point distances.
virtual int bruteForceCorrespondences(int idx1, int idx2, pcl::Correspondences &pairs)
Search for corresponding point pairs given the distance between two base points.
int selectBase(pcl::Indices &base_indices, float(&ratio)[2])
Select an approximately coplanar set of four points from the source cloud.
virtual int validateTransformation(Eigen::Matrix4f &transformation, float &fitness_score)
Validate the transformation by calculating the number of inliers after transforming the source cloud.
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...
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...
virtual void handleMatches(const pcl::Indices &base_indices, std::vector< pcl::Indices > &matches, MatchingCandidates &candidates)
Method to handle current candidate matches.
int checkBaseMatch(const pcl::Indices &match_indices, const float(&ds)[4])
Check if outer rectangle distance of matched points fit with the base rectangle.
float segmentToSegmentDist(const pcl::Indices &base_indices, float(&ratio)[2])
Calculate intersection ratios and segment to segment distances of base diagonals.
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...
FPCSInitialAlignment()
Constructor.
void computeTransformation(PointCloudSource &output, const Eigen::Matrix4f &guess) override
Rigid transformation computation method.
virtual bool initCompute()
Internal computation initialization.
search::KdTree is a wrapper class which inherits the pcl::KdTree class for performing search function...
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.
void setInputCloud(const PointCloudConstPtr &cloud, const IndicesConstPtr &indices=IndicesConstPtr()) override
Provide a pointer to the input dataset.
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.
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 > ¢roid)
Compute the 3D (X-Y-Z) centroid of a set of points and return it as a 3D vector.
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.
float squaredEuclideanDistance(const PointType1 &p1, const PointType2 &p2)
Calculate the squared euclidean distance between the two given points.
std::vector< pcl::Correspondence, Eigen::aligned_allocator< pcl::Correspondence > > Correspondences
shared_ptr< Indices > IndicesPtr
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.
IndicesAllocator<> Indices
Type used for indices in PCL.
float euclideanDistance(const PointType1 &p1, const PointType2 &p2)
Calculate the euclidean distance between the two given points.
Correspondence represents a match between two entities (e.g., points, descriptors,...
A point structure representing normal coordinates and the surface curvature estimate.