37 #ifndef PCL_REGISTRATION_IMPL_IA_KFPCS_H_
38 #define PCL_REGISTRATION_IMPL_IA_KFPCS_H_
44 namespace registration
47 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),
53 indices_validation_ (
new std::vector <int>)
55 reg_name_ =
"pcl::registration::KFPCSInitialAlignment";
59 template <
typename Po
intSource,
typename Po
intTarget,
typename NormalT,
typename Scalar>
bool
65 PCL_WARN (
"[%s::initCompute] Delta should be set according to keypoint precision! Normalization according to point cloud density is ignored.", reg_name_.c_str ());
66 normalize_delta_ =
false;
73 max_pair_diff_ = delta_ * 1.414f;
74 coincidation_limit_ = delta_ * 2.828f;
75 max_edge_diff_ = delta_ * 3.f;
76 max_mse_ = powf (delta_ * 4.f, 2.f);
77 max_inlier_dist_sqr_ = powf (delta_ * 8.f, 2.f);
80 if (upper_trl_boundary_ < 0)
81 upper_trl_boundary_ = diameter_ * (1.f - approx_overlap_) * 0.5f;
83 if (!(lower_trl_boundary_ < 0) && upper_trl_boundary_ > lower_trl_boundary_)
84 use_trl_score_ =
true;
89 std::size_t nr_indices = indices_->size ();
90 if (nr_indices < std::size_t (ransac_iterations_))
91 indices_validation_ = indices_;
93 for (
int i = 0; i < ransac_iterations_; i++)
94 indices_validation_->push_back ((*indices_)[rand () % nr_indices]);
100 template <
typename Po
intSource,
typename Po
intTarget,
typename NormalT,
typename Scalar>
void
102 const std::vector <int> &base_indices,
103 std::vector <std::vector <int> > &matches,
109 for (
auto &match : matches)
111 Eigen::Matrix4f transformation_temp;
113 float fitness_score = FLT_MAX;
116 linkMatchWithBase (base_indices, match, correspondences_temp);
119 if (validateMatch (base_indices, match, correspondences_temp, transformation_temp) < 0)
124 validateTransformation (transformation_temp, fitness_score);
127 candidates.push_back (
MatchingCandidate (fitness_score, correspondences_temp, transformation_temp));
132 template <
typename Po
intSource,
typename Po
intTarget,
typename NormalT,
typename Scalar>
int
134 Eigen::Matrix4f &transformation,
135 float &fitness_score)
141 const std::size_t nr_points = source_transformed.
size ();
142 float score_a = 0.f, score_b = 0.f;
145 std::vector <int> ids;
146 std::vector <float> dists_sqr;
147 for (PointCloudSourceIterator it = source_transformed.
begin (), it_e = source_transformed.
end (); it != it_e; ++it)
150 tree_->nearestKSearch (*it, 1, ids, dists_sqr);
151 score_a += (dists_sqr[0] < max_inlier_dist_sqr_ ? dists_sqr[0] : max_inlier_dist_sqr_);
154 score_a /= (max_inlier_dist_sqr_ * nr_points);
161 float trl = transformation.rightCols <1> ().head (3).norm ();
162 float trl_ratio = (trl - lower_trl_boundary_) / (upper_trl_boundary_ - lower_trl_boundary_);
164 score_b = (trl_ratio < 0.f ? 1.f : (trl_ratio > 1.f ? 0.f : 0.5f * sin (
M_PI * trl_ratio +
M_PI_2) + 0.5f));
169 float fitness_score_temp = (score_a + lambda_ * score_b) / scale;
170 if (fitness_score_temp > fitness_score)
173 fitness_score = fitness_score_temp;
178 template <
typename Po
intSource,
typename Po
intTarget,
typename NormalT,
typename Scalar>
void
180 const std::vector <MatchingCandidates > &candidates)
183 std::size_t total_size = 0;
184 for (
const auto &candidate : candidates)
185 total_size += candidate.size ();
187 candidates_.clear ();
188 candidates_.reserve (total_size);
190 for (
const auto &candidate : candidates)
191 for (
const auto &match : candidate)
192 candidates_.push_back (match);
195 std::sort (candidates_.begin (), candidates_.end (),
by_score ());
198 if (candidates_[0].fitness_score == FLT_MAX)
206 fitness_score_ = candidates_ [0].fitness_score;
207 final_transformation_ = candidates_ [0].transformation;
208 *correspondences_ = candidates_ [0].correspondences;
211 converged_ = fitness_score_ < score_threshold_;
215 template <
typename Po
intSource,
typename Po
intTarget,
typename NormalT,
typename Scalar>
void
219 float min_translation3d,
225 for (MatchingCandidates::iterator it_candidate = candidates_.begin (), it_e = candidates_.end (); it_candidate != it_e; ++it_candidate)
228 if (it_candidate->fitness_score == FLT_MAX)
233 MatchingCandidates::iterator it = candidates.begin (), it_e2 = candidates.end ();
234 while (unique && it != it_e2)
236 Eigen::Matrix4f diff = it_candidate->transformation.colPivHouseholderQr ().solve (it->transformation);
237 const float angle3d = Eigen::AngleAxisf (diff.block <3, 3> (0, 0)).angle ();
238 const float translation3d = diff.block <3, 1> (0, 3).norm ();
239 unique = angle3d > min_angle3d && translation3d > min_translation3d;
245 candidates.push_back (*it_candidate);
248 if (candidates.size () == n)
254 template <
typename Po
intSource,
typename Po
intTarget,
typename NormalT,
typename Scalar>
void
258 float min_translation3d,
264 for (MatchingCandidates::iterator it_candidate = candidates_.begin (), it_e = candidates_.end (); it_candidate != it_e; ++it_candidate)
267 if (it_candidate->fitness_score > t)
272 MatchingCandidates::iterator it = candidates.begin (), it_e2 = candidates.end ();
273 while (unique && it != it_e2)
275 Eigen::Matrix4f diff = it_candidate->transformation.colPivHouseholderQr ().solve (it->transformation);
276 const float angle3d = Eigen::AngleAxisf (diff.block <3, 3> (0, 0)).angle ();
277 const float translation3d = diff.block <3, 1> (0, 3).norm ();
278 unique = angle3d > min_angle3d && translation3d > min_translation3d;
284 candidates.push_back (*it_candidate);
FPCSInitialAlignment computes corresponding four point congruent sets as described in: "4-points cong...
KFPCSInitialAlignment computes corresponding four point congruent sets based on keypoints as describe...
void transformPointCloud(const pcl::PointCloud< PointT > &cloud_in, pcl::PointCloud< PointT > &cloud_out, const Eigen::Transform< Scalar, 3, Eigen::Affine > &transform, bool copy_all_fields)
Apply an affine transform defined by an Eigen Transform.
std::vector< MatchingCandidate, Eigen::aligned_allocator< MatchingCandidate > > MatchingCandidates
std::vector< pcl::Correspondence, Eigen::aligned_allocator< pcl::Correspondence > > Correspondences
Container for matching candidate consisting of.
Sorting of candidates based on fitness score value.