37 #ifndef PCL_REGISTRATION_IMPL_IA_KFPCS_H_
38 #define PCL_REGISTRATION_IMPL_IA_KFPCS_H_
42 namespace registration {
44 template <
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)
51 , indices_validation_(new std::vector<int>)
53 reg_name_ =
"pcl::registration::KFPCSInitialAlignment";
56 template <
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;
95 std::size_t nr_indices = indices_->size();
96 if (nr_indices < std::size_t(ransac_iterations_))
97 indices_validation_ = indices_;
99 for (
int i = 0; i < ransac_iterations_; i++)
100 indices_validation_->push_back((*indices_)[rand() % nr_indices]);
105 template <
typename Po
intSource,
typename Po
intTarget,
typename NormalT,
typename Scalar>
108 const std::vector<int>& base_indices,
109 std::vector<std::vector<int>>& matches,
115 for (
auto& match : matches) {
116 Eigen::Matrix4f transformation_temp;
118 float fitness_score =
123 linkMatchWithBase(base_indices, match, correspondences_temp);
126 if (validateMatch(base_indices, match, correspondences_temp, transformation_temp) <
132 validateTransformation(transformation_temp, fitness_score);
135 candidates.push_back(
140 template <
typename Po
intSource,
typename Po
intTarget,
typename NormalT,
typename Scalar>
148 *input_, *indices_validation_, source_transformed, transformation);
150 const std::size_t nr_points = source_transformed.
size();
151 float score_a = 0.f, score_b = 0.f;
154 std::vector<int> ids;
155 std::vector<float> dists_sqr;
156 for (PointCloudSourceIterator it = source_transformed.
begin(),
157 it_e = source_transformed.
end();
161 tree_->nearestKSearch(*it, 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());
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_;
231 template <
typename Po
intSource,
typename Po
intTarget,
typename NormalT,
typename Scalar>
239 for (MatchingCandidates::iterator it_candidate = candidates_.begin(),
240 it_e = candidates_.end();
241 it_candidate != it_e;
244 if (it_candidate->fitness_score == FLT_MAX)
250 MatchingCandidates::iterator it = candidates.begin(), it_e2 = candidates.end();
251 while (unique && it != it_e2) {
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();
255 const float translation3d = diff.block<3, 1>(0, 3).norm();
256 unique = angle3d > min_angle3d && translation3d > min_translation3d;
262 candidates.push_back(*it_candidate);
265 if (candidates.size() == n)
270 template <
typename Po
intSource,
typename Po
intTarget,
typename NormalT,
typename Scalar>
278 for (MatchingCandidates::iterator it_candidate = candidates_.begin(),
279 it_e = candidates_.end();
280 it_candidate != it_e;
283 if (it_candidate->fitness_score > t)
289 MatchingCandidates::iterator it = candidates.begin(), it_e2 = candidates.end();
290 while (unique && it != it_e2) {
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();
294 const float translation3d = diff.block<3, 1>(0, 3).norm();
295 unique = angle3d > min_angle3d && translation3d > min_translation3d;
301 candidates.push_back(*it_candidate);
308 #endif // PCL_REGISTRATION_IMPL_IA_KFPCS_H_