37 #ifndef PCL_REGISTRATION_IMPL_IA_KFPCS_H_
38 #define PCL_REGISTRATION_IMPL_IA_KFPCS_H_
41 template <
typename Po
intSource,
typename Po
intTarget,
typename NormalT,
typename Scalar>
43 lower_trl_boundary_ (-1.f),
44 upper_trl_boundary_ (-1.f),
46 use_trl_score_ (
false),
47 indices_validation_ (
new std::vector <int>)
49 reg_name_ =
"pcl::registration::KFPCSInitialAlignment";
54 template <
typename Po
intSource,
typename Po
intTarget,
typename NormalT,
typename Scalar>
bool
60 PCL_WARN (
"[%s::initCompute] Delta should be set according to keypoint precision! Normalization according to point cloud density is ignored.", reg_name_.c_str ());
61 normalize_delta_ =
false;
68 max_pair_diff_ = delta_ * 1.414f;
69 coincidation_limit_ = delta_ * 2.828f;
70 max_edge_diff_ = delta_ * 3.f;
71 max_mse_ = powf (delta_ * 4.f, 2.f);
72 max_inlier_dist_sqr_ = powf (delta_ * 8.f, 2.f);
75 if (upper_trl_boundary_ < 0)
76 upper_trl_boundary_ = diameter_ * (1.f - approx_overlap_) * 0.5f;
78 if (!(lower_trl_boundary_ < 0) && upper_trl_boundary_ > lower_trl_boundary_)
79 use_trl_score_ =
true;
84 std::size_t nr_indices = indices_->size ();
85 if (nr_indices < std::size_t (ransac_iterations_))
86 indices_validation_ = indices_;
88 for (
int i = 0; i < ransac_iterations_; i++)
89 indices_validation_->push_back ((*indices_)[rand () % nr_indices]);
96 template <
typename Po
intSource,
typename Po
intTarget,
typename NormalT,
typename Scalar>
void
98 const std::vector <int> &base_indices,
99 std::vector <std::vector <int> > &matches,
103 float fitness_score = FLT_MAX;
106 for (
auto &match : matches)
108 Eigen::Matrix4f transformation_temp;
110 fitness_score = FLT_MAX;
113 linkMatchWithBase (base_indices, match, correspondences_temp);
116 if (validateMatch (base_indices, match, correspondences_temp, transformation_temp) < 0)
121 validateTransformation (transformation_temp, fitness_score);
124 candidates.push_back (
MatchingCandidate (fitness_score, correspondences_temp, transformation_temp));
130 template <
typename Po
intSource,
typename Po
intTarget,
typename NormalT,
typename Scalar>
int
132 Eigen::Matrix4f &transformation,
133 float &fitness_score)
139 const std::size_t nr_points = source_transformed.
size ();
140 float score_a = 0.f, score_b = 0.f;
143 std::vector <int> ids;
144 std::vector <float> dists_sqr;
145 for (PointCloudSourceIterator it = source_transformed.
begin (), it_e = source_transformed.
end (); it != it_e; ++it)
148 tree_->nearestKSearch (*it, 1, ids, dists_sqr);
149 score_a += (dists_sqr[0] < max_inlier_dist_sqr_ ? dists_sqr[0] : max_inlier_dist_sqr_);
152 score_a /= (max_inlier_dist_sqr_ * nr_points);
159 float trl = transformation.rightCols <1> ().head (3).norm ();
160 float trl_ratio = (trl - lower_trl_boundary_) / (upper_trl_boundary_ - lower_trl_boundary_);
162 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));
167 float fitness_score_temp = (score_a + lambda_ * score_b) / scale;
168 if (fitness_score_temp > fitness_score)
171 fitness_score = fitness_score_temp;
177 template <
typename Po
intSource,
typename Po
intTarget,
typename NormalT,
typename Scalar>
void
179 const std::vector <MatchingCandidates > &candidates)
182 std::size_t total_size = 0;
183 for (
const auto &candidate : candidates)
184 total_size += candidate.size ();
186 candidates_.clear ();
187 candidates_.reserve (total_size);
189 for (
const auto &candidate : candidates)
190 for (
const auto &match : candidate)
191 candidates_.push_back (match);
194 std::sort (candidates_.begin (), candidates_.end (),
by_score ());
197 if (candidates_[0].fitness_score == FLT_MAX)
205 fitness_score_ = candidates_ [0].fitness_score;
206 final_transformation_ = candidates_ [0].transformation;
207 *correspondences_ = candidates_ [0].correspondences;
210 converged_ = fitness_score_ < score_threshold_;
214 template <
typename Po
intSource,
typename Po
intTarget,
typename NormalT,
typename Scalar>
void
218 float min_translation3d,
224 for (MatchingCandidates::iterator it_candidate = candidates_.begin (), it_e = candidates_.end (); it_candidate != it_e; it_candidate++)
227 if (it_candidate->fitness_score == FLT_MAX)
232 MatchingCandidates::iterator it = candidates.begin (), it_e2 = candidates.end ();
233 while (unique && it != it_e2)
235 Eigen::Matrix4f diff = it_candidate->transformation.colPivHouseholderQr ().solve (it->transformation);
236 const float angle3d = Eigen::AngleAxisf (diff.block <3, 3> (0, 0)).angle ();
237 const float translation3d = diff.block <3, 1> (0, 3).norm ();
238 unique = angle3d > min_angle3d && translation3d > min_translation3d;
244 candidates.push_back (*it_candidate);
247 if (candidates.size () == n)
253 template <
typename Po
intSource,
typename Po
intTarget,
typename NormalT,
typename Scalar>
void
257 float min_translation3d,
263 for (MatchingCandidates::iterator it_candidate = candidates_.begin (), it_e = candidates_.end (); it_candidate != it_e; it_candidate++)
266 if (it_candidate->fitness_score > t)
271 MatchingCandidates::iterator it = candidates.begin (), it_e2 = candidates.end ();
272 while (unique && it != it_e2)
274 Eigen::Matrix4f diff = it_candidate->transformation.colPivHouseholderQr ().solve (it->transformation);
275 const float angle3d = Eigen::AngleAxisf (diff.block <3, 3> (0, 0)).angle ();
276 const float translation3d = diff.block <3, 1> (0, 3).norm ();
277 unique = angle3d > min_angle3d && translation3d > min_translation3d;
283 candidates.push_back (*it_candidate);
289 #endif // PCL_REGISTRATION_IMPL_IA_KFPCS_H_