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>
51 reg_name_ =
"pcl::registration::KFPCSInitialAlignment";
54 template <
typename Po
intSource,
typename Po
intTarget,
typename NormalT,
typename Scalar>
59 if (normalize_delta_) {
60 PCL_WARN(
"[%s::initCompute] Delta should be set according to keypoint precision! "
61 "Normalization according to point cloud density is ignored.\n",
63 normalize_delta_ =
false;
71 max_pair_diff_ = delta_ * 1.414f;
72 coincidation_limit_ = delta_ * 2.828f;
77 powf(delta_ * 4.f, 2.f);
78 max_inlier_dist_sqr_ =
83 if (upper_trl_boundary_ < 0)
84 upper_trl_boundary_ = diameter_ * (1.f - approx_overlap_) * 0.5f;
86 if (!(lower_trl_boundary_ < 0) && upper_trl_boundary_ > lower_trl_boundary_)
87 use_trl_score_ =
true;
93 if (indices_->size() <=
static_cast<std::size_t
>(ransac_iterations_) ||
94 ransac_iterations_ <= 0)
95 indices_validation_ = indices_;
101 random_sampling.
setSample(ransac_iterations_);
102 random_sampling.
filter(*indices_validation_);
105 PCL_DEBUG(
"[%s::initCompute] delta_=%g, max_inlier_dist_sqr_=%g, "
106 "coincidation_limit_=%g, max_edge_diff_=%g, max_pair_diff_=%g\n",
109 max_inlier_dist_sqr_,
116 template <
typename Po
intSource,
typename Po
intTarget,
typename NormalT,
typename Scalar>
120 std::vector<pcl::Indices>& matches,
126 for (
auto& match : matches) {
127 Eigen::Matrix4f transformation_temp;
129 float fitness_score =
130 std::numeric_limits<float>::max();
139 if (validateMatch(base_indices, match, correspondences_temp, transformation_temp) <
145 validateTransformation(transformation_temp, fitness_score);
148 candidates.push_back(
153 if (!candidates.empty()) {
154 auto best_candidate = candidates.begin();
155 for (
auto iter = candidates.begin(); iter < candidates.end(); ++iter)
156 if (iter->fitness_score < best_candidate->fitness_score)
157 best_candidate = iter;
158 if (best_candidate != candidates.begin())
159 std::swap(*best_candidate, *candidates.begin());
163 template <
typename Po
intSource,
typename Po
intTarget,
typename NormalT,
typename Scalar>
171 *input_, *indices_validation_, source_transformed, transformation);
173 const std::size_t nr_points = source_transformed.
size();
174 float score_a = 0.f, score_b = 0.f;
178 std::vector<float> dists_sqr(1);
179 for (
const auto& source : source_transformed) {
181 tree_->nearestKSearch(source, 1, ids, dists_sqr);
182 score_a += (dists_sqr[0] < max_inlier_dist_sqr_ ? dists_sqr[0]
183 : max_inlier_dist_sqr_);
186 score_a /= (max_inlier_dist_sqr_ * nr_points);
192 if (use_trl_score_) {
193 float trl = transformation.rightCols<1>().head<3>().norm();
195 (trl - lower_trl_boundary_) / (upper_trl_boundary_ - lower_trl_boundary_);
198 (trl_ratio < 0.f ? 1.f
199 : (trl_ratio > 1.f ? 0.f
206 float fitness_score_temp = (score_a + lambda_ * score_b) / scale;
207 if (fitness_score_temp > fitness_score)
210 fitness_score = fitness_score_temp;
214 template <
typename Po
intSource,
typename Po
intTarget,
typename NormalT,
typename Scalar>
217 const std::vector<MatchingCandidates>& candidates)
220 std::size_t total_size = 0;
221 for (
const auto& candidate : candidates)
222 total_size += candidate.size();
225 candidates_.reserve(total_size);
227 for (
const auto& candidate : candidates)
228 for (
const auto& match : candidate)
229 candidates_.push_back(match);
232 std::sort(candidates_.begin(), candidates_.end(),
by_score());
236 if (candidates_[0].fitness_score == std::numeric_limits<float>::max()) {
244 fitness_score_ = candidates_[0].fitness_score;
245 final_transformation_ = candidates_[0].transformation;
246 *correspondences_ = candidates_[0].correspondences;
247 PCL_DEBUG(
"[%s::finalCompute] best score is %g, out of %zu candidate solutions.\n",
253 converged_ = fitness_score_ < score_threshold_;
256 template <
typename Po
intSource,
typename Po
intTarget,
typename NormalT,
typename Scalar>
264 for (
const auto& candidate : candidates_) {
266 if (candidate.fitness_score == std::numeric_limits<float>::max())
272 for (
const auto& c2 : candidates) {
273 Eigen::Matrix4f diff =
274 candidate.transformation.colPivHouseholderQr().solve(c2.transformation);
275 const float angle3d = Eigen::AngleAxisf(diff.topLeftCorner<3, 3>()).angle();
276 const float translation3d = diff.block<3, 1>(0, 3).norm();
277 unique = angle3d > min_angle3d && translation3d > min_translation3d;
285 candidates.push_back(candidate);
288 if (candidates.size() == n)
293 template <
typename Po
intSource,
typename Po
intTarget,
typename NormalT,
typename Scalar>
301 for (
const auto& candidate : candidates_) {
303 if (candidate.fitness_score > t)
309 for (
const auto& c2 : candidates) {
310 Eigen::Matrix4f diff =
311 candidate.transformation.colPivHouseholderQr().solve(c2.transformation);
312 const float angle3d = Eigen::AngleAxisf(diff.topLeftCorner<3, 3>()).angle();
313 const float translation3d = diff.block<3, 1>(0, 3).norm();
314 unique = angle3d > min_angle3d && translation3d > min_translation3d;
322 candidates.push_back(candidate);
void filter(Indices &indices)
Calls the filtering method and returns the filtered point cloud indices.
virtual void setInputCloud(const PointCloudConstPtr &cloud)
Provide a pointer to the input dataset.
virtual void setIndices(const IndicesPtr &indices)
Provide a pointer to the vector of indices that represents the input data.
RandomSample applies a random sampling with uniform probability.
void setSample(unsigned int sample)
Set number of indices to be sampled.
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.
Correspondence represents a match between two entities (e.g., points, descriptors,...
Container for matching candidate consisting of.
Sorting of candidates based on fitness score value.