37 #ifndef PCL_REGISTRATION_IMPL_IA_KFPCS_H_
38 #define PCL_REGISTRATION_IMPL_IA_KFPCS_H_
44 namespace registration {
46 template <
typename Po
intSource,
typename Po
intTarget,
typename NormalT,
typename Scalar>
49 : lower_trl_boundary_(-1.f)
50 , upper_trl_boundary_(-1.f)
52 , use_trl_score_(false)
55 reg_name_ =
"pcl::registration::KFPCSInitialAlignment";
58 template <
typename Po
intSource,
typename Po
intTarget,
typename NormalT,
typename Scalar>
63 if (normalize_delta_) {
64 PCL_WARN(
"[%s::initCompute] Delta should be set according to keypoint precision! "
65 "Normalization according to point cloud density is ignored.\n",
67 normalize_delta_ =
false;
75 max_pair_diff_ = delta_ * 1.414f;
76 coincidation_limit_ = delta_ * 2.828f;
81 powf(delta_ * 4.f, 2.f);
82 max_inlier_dist_sqr_ =
87 if (upper_trl_boundary_ < 0)
88 upper_trl_boundary_ = diameter_ * (1.f - approx_overlap_) * 0.5f;
90 if (!(lower_trl_boundary_ < 0) && upper_trl_boundary_ > lower_trl_boundary_)
91 use_trl_score_ =
true;
97 std::size_t nr_indices = indices_->size();
98 if (nr_indices <
static_cast<std::size_t
>(ransac_iterations_))
99 indices_validation_ = indices_;
101 for (
int i = 0; i < ransac_iterations_; i++)
102 indices_validation_->push_back((*indices_)[rand() % nr_indices]);
107 template <
typename Po
intSource,
typename Po
intTarget,
typename NormalT,
typename Scalar>
111 std::vector<pcl::Indices>& matches,
117 for (
auto& match : matches) {
118 Eigen::Matrix4f transformation_temp;
120 float fitness_score =
121 std::numeric_limits<float>::max();
126 linkMatchWithBase(base_indices, match, correspondences_temp);
129 if (validateMatch(base_indices, match, correspondences_temp, transformation_temp) <
135 validateTransformation(transformation_temp, fitness_score);
138 candidates.push_back(
143 template <
typename Po
intSource,
typename Po
intTarget,
typename NormalT,
typename Scalar>
151 *input_, *indices_validation_, source_transformed, transformation);
153 const std::size_t nr_points = source_transformed.
size();
154 float score_a = 0.f, score_b = 0.f;
158 std::vector<float> dists_sqr;
159 for (
const auto& source : source_transformed) {
161 tree_->nearestKSearch(source, 1, ids, dists_sqr);
162 score_a += (dists_sqr[0] < max_inlier_dist_sqr_ ? dists_sqr[0]
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_);
178 (trl_ratio < 0.f ? 1.f
179 : (trl_ratio > 1.f ? 0.f
186 float fitness_score_temp = (score_a + lambda_ * score_b) / scale;
187 if (fitness_score_temp > fitness_score)
190 fitness_score = fitness_score_temp;
194 template <
typename Po
intSource,
typename Po
intTarget,
typename NormalT,
typename Scalar>
197 const std::vector<MatchingCandidates>& candidates)
200 std::size_t total_size = 0;
201 for (
const auto& candidate : candidates)
202 total_size += candidate.size();
205 candidates_.reserve(total_size);
207 for (
const auto& candidate : candidates)
208 for (
const auto& match : candidate)
209 candidates_.push_back(match);
212 std::sort(candidates_.begin(), candidates_.end(),
by_score());
216 if (candidates_[0].fitness_score == std::numeric_limits<float>::max()) {
224 fitness_score_ = candidates_[0].fitness_score;
225 final_transformation_ = candidates_[0].transformation;
226 *correspondences_ = candidates_[0].correspondences;
229 converged_ = fitness_score_ < score_threshold_;
232 template <
typename Po
intSource,
typename Po
intTarget,
typename NormalT,
typename Scalar>
240 for (
const auto& candidate : candidates_) {
242 if (candidate.fitness_score == std::numeric_limits<float>::max())
248 for (
const auto& c2 : candidates) {
249 Eigen::Matrix4f diff =
250 candidate.transformation.colPivHouseholderQr().solve(c2.transformation);
251 const float angle3d = Eigen::AngleAxisf(diff.block<3, 3>(0, 0)).angle();
252 const float translation3d = diff.block<3, 1>(0, 3).norm();
253 unique = angle3d > min_angle3d && translation3d > min_translation3d;
261 candidates.push_back(candidate);
264 if (candidates.size() == n)
269 template <
typename Po
intSource,
typename Po
intTarget,
typename NormalT,
typename Scalar>
277 for (
const auto& candidate : candidates_) {
279 if (candidate.fitness_score > t)
285 for (
const auto& c2 : candidates) {
286 Eigen::Matrix4f diff =
287 candidate.transformation.colPivHouseholderQr().solve(c2.transformation);
288 const float angle3d = Eigen::AngleAxisf(diff.block<3, 3>(0, 0)).angle();
289 const float translation3d = diff.block<3, 1>(0, 3).norm();
290 unique = angle3d > min_angle3d && translation3d > min_translation3d;
298 candidates.push_back(candidate);
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.