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 std::size_t nr_indices = indices_->size();
94 if (nr_indices <
static_cast<std::size_t
>(ransac_iterations_))
95 indices_validation_ = indices_;
97 for (
int i = 0; i < ransac_iterations_; i++)
98 indices_validation_->push_back((*indices_)[rand() % nr_indices]);
103 template <
typename Po
intSource,
typename Po
intTarget,
typename NormalT,
typename Scalar>
107 std::vector<pcl::Indices>& matches,
113 for (
auto& match : matches) {
114 Eigen::Matrix4f transformation_temp;
116 float fitness_score =
117 std::numeric_limits<float>::max();
122 linkMatchWithBase(base_indices, match, correspondences_temp);
125 if (validateMatch(base_indices, match, correspondences_temp, transformation_temp) <
131 validateTransformation(transformation_temp, fitness_score);
134 candidates.push_back(
139 template <
typename Po
intSource,
typename Po
intTarget,
typename NormalT,
typename Scalar>
147 *input_, *indices_validation_, source_transformed, transformation);
149 const std::size_t nr_points = source_transformed.
size();
150 float score_a = 0.f, score_b = 0.f;
154 std::vector<float> dists_sqr;
155 for (
const auto& source : source_transformed) {
157 tree_->nearestKSearch(source, 1, ids, dists_sqr);
158 score_a += (dists_sqr[0] < max_inlier_dist_sqr_ ? dists_sqr[0]
159 : max_inlier_dist_sqr_);
162 score_a /= (max_inlier_dist_sqr_ * nr_points);
168 if (use_trl_score_) {
169 float trl = transformation.rightCols<1>().head<3>().norm();
171 (trl - lower_trl_boundary_) / (upper_trl_boundary_ - lower_trl_boundary_);
174 (trl_ratio < 0.f ? 1.f
175 : (trl_ratio > 1.f ? 0.f
182 float fitness_score_temp = (score_a + lambda_ * score_b) / scale;
183 if (fitness_score_temp > fitness_score)
186 fitness_score = fitness_score_temp;
190 template <
typename Po
intSource,
typename Po
intTarget,
typename NormalT,
typename Scalar>
193 const std::vector<MatchingCandidates>& candidates)
196 std::size_t total_size = 0;
197 for (
const auto& candidate : candidates)
198 total_size += candidate.size();
201 candidates_.reserve(total_size);
203 for (
const auto& candidate : candidates)
204 for (
const auto& match : candidate)
205 candidates_.push_back(match);
208 std::sort(candidates_.begin(), candidates_.end(),
by_score());
212 if (candidates_[0].fitness_score == std::numeric_limits<float>::max()) {
220 fitness_score_ = candidates_[0].fitness_score;
221 final_transformation_ = candidates_[0].transformation;
222 *correspondences_ = candidates_[0].correspondences;
225 converged_ = fitness_score_ < score_threshold_;
228 template <
typename Po
intSource,
typename Po
intTarget,
typename NormalT,
typename Scalar>
236 for (
const auto& candidate : candidates_) {
238 if (candidate.fitness_score == std::numeric_limits<float>::max())
244 for (
const auto& c2 : candidates) {
245 Eigen::Matrix4f diff =
246 candidate.transformation.colPivHouseholderQr().solve(c2.transformation);
247 const float angle3d = Eigen::AngleAxisf(diff.topLeftCorner<3, 3>()).angle();
248 const float translation3d = diff.block<3, 1>(0, 3).norm();
249 unique = angle3d > min_angle3d && translation3d > min_translation3d;
257 candidates.push_back(candidate);
260 if (candidates.size() == n)
265 template <
typename Po
intSource,
typename Po
intTarget,
typename NormalT,
typename Scalar>
273 for (
const auto& candidate : candidates_) {
275 if (candidate.fitness_score > t)
281 for (
const auto& c2 : candidates) {
282 Eigen::Matrix4f diff =
283 candidate.transformation.colPivHouseholderQr().solve(c2.transformation);
284 const float angle3d = Eigen::AngleAxisf(diff.topLeftCorner<3, 3>()).angle();
285 const float translation3d = diff.block<3, 1>(0, 3).norm();
286 unique = angle3d > min_angle3d && translation3d > min_translation3d;
294 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.