37#ifndef PCL_REGISTRATION_IMPL_IA_KFPCS_H_
38#define PCL_REGISTRATION_IMPL_IA_KFPCS_H_
42namespace registration {
44template <
typename Po
intSource,
typename Po
intTarget,
typename NormalT,
typename Scalar>
47: lower_trl_boundary_(-1.f)
48, upper_trl_boundary_(-1.f)
50, use_trl_score_(
false)
53 reg_name_ =
"pcl::registration::KFPCSInitialAlignment";
56template <
typename Po
intSource,
typename Po
intTarget,
typename NormalT,
typename Scalar>
61 if (normalize_delta_) {
62 PCL_WARN(
"[%s::initCompute] Delta should be set according to keypoint precision! "
63 "Normalization according to point cloud density is ignored.\n",
65 normalize_delta_ =
false;
73 max_pair_diff_ = delta_ * 1.414f;
74 coincidation_limit_ = delta_ * 2.828f;
79 powf(delta_ * 4.f, 2.f);
80 max_inlier_dist_sqr_ =
85 if (upper_trl_boundary_ < 0)
86 upper_trl_boundary_ = diameter_ * (1.f - approx_overlap_) * 0.5f;
88 if (!(lower_trl_boundary_ < 0) && upper_trl_boundary_ > lower_trl_boundary_)
89 use_trl_score_ =
true;
96 if (
nr_indices < std::size_t(ransac_iterations_))
97 indices_validation_ = indices_;
99 for (
int i = 0; i < ransac_iterations_; i++)
105template <
typename Po
intSource,
typename Po
intTarget,
typename NormalT,
typename Scalar>
109 std::vector<pcl::Indices>&
matches,
118 float fitness_score =
140template <
typename Po
intSource,
typename Po
intTarget,
typename NormalT,
typename Scalar>
163 : max_inlier_dist_sqr_);
166 score_a /= (max_inlier_dist_sqr_ * nr_points);
172 if (use_trl_score_) {
173 float trl = transformation.rightCols<1>().
head(3).norm();
175 (
trl - lower_trl_boundary_) / (upper_trl_boundary_ - lower_trl_boundary_);
194template <
typename Po
intSource,
typename Po
intTarget,
typename NormalT,
typename Scalar>
197 const std::vector<MatchingCandidates>&
candidates)
209 candidates_.push_back(
match);
212 std::sort(candidates_.begin(), candidates_.end(),
by_score());
215 if (candidates_[0].fitness_score ==
FLT_MAX) {
223 fitness_score_ = candidates_[0].fitness_score;
224 final_transformation_ = candidates_[0].transformation;
225 *correspondences_ = candidates_[0].correspondences;
228 converged_ = fitness_score_ < score_threshold_;
231template <
typename Po
intSource,
typename Po
intTarget,
typename NormalT,
typename Scalar>
239 for (MatchingCandidates::iterator
it_candidate = candidates_.begin(),
240 it_e = candidates_.end();
252 Eigen::Matrix4f
diff =
253 it_candidate->transformation.colPivHouseholderQr().solve(
it->transformation);
254 const float angle3d = Eigen::AngleAxisf(
diff.block<3, 3>(0, 0)).angle();
270template <
typename Po
intSource,
typename Po
intTarget,
typename NormalT,
typename Scalar>
278 for (MatchingCandidates::iterator
it_candidate = candidates_.begin(),
279 it_e = candidates_.end();
291 Eigen::Matrix4f
diff =
292 it_candidate->transformation.colPivHouseholderQr().solve(
it->transformation);
293 const float angle3d = Eigen::AngleAxisf(
diff.block<3, 3>(0, 0)).angle();
Iterator class for point clouds with or without given indices.
std::size_t size() const
Size of the range the iterator is going through.
std::string reg_name_
The registration method name.
virtual bool initCompute()
Internal computation initialization.
void getTBestCandidates(float t, float min_angle3d, float min_translation3d, MatchingCandidates &candidates)
Get all unique candidate matches with fitness scores above a threshold t.
void finalCompute(const std::vector< MatchingCandidates > &candidates) override
Final computation of best match out of vector of matches.
void handleMatches(const pcl::Indices &base_indices, std::vector< pcl::Indices > &matches, MatchingCandidates &candidates) override
Method to handle current candidate matches.
KFPCSInitialAlignment()
Constructor.
void getNBestCandidates(int n, float min_angle3d, float min_translation3d, MatchingCandidates &candidates)
Get the N best unique candidate matches according to their fitness score.
int validateTransformation(Eigen::Matrix4f &transformation, float &fitness_score) override
Validate the transformation by calculating the score value after transforming the input source cloud.
bool initCompute() override
Internal computation initialization.
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.
std::vector< MatchingCandidate, Eigen::aligned_allocator< MatchingCandidate > > MatchingCandidates
std::vector< pcl::Correspondence, Eigen::aligned_allocator< pcl::Correspondence > > Correspondences
IndicesAllocator<> Indices
Type used for indices in PCL.
Container for matching candidate consisting of.
Sorting of candidates based on fitness score value.