38 #ifndef PCL_REGISTRATION_IMPL_IA_KFPCS_H_ 39 #define PCL_REGISTRATION_IMPL_IA_KFPCS_H_ 42 template <
typename Po
intSource,
typename Po
intTarget,
typename NormalT,
typename Scalar>
44 lower_trl_boundary_ (-1.f),
45 upper_trl_boundary_ (-1.f),
48 use_trl_score_ (
false),
49 indices_validation_ (
new std::vector <int>)
51 reg_name_ =
"pcl::registration::KFPCSInitialAlignment";
56 template <
typename Po
intSource,
typename Po
intTarget,
typename NormalT,
typename Scalar>
bool 62 PCL_WARN (
"[%s::initCompute] Delta should be set according to keypoint precision! Normalization according to point cloud density is ignored.", reg_name_.c_str ());
63 normalize_delta_ =
false;
70 max_pair_diff_ = delta_ * 1.414f;
71 coincidation_limit_ = delta_ * 2.828f;
72 max_edge_diff_ = delta_ * 3.f;
73 max_mse_ = powf (delta_ * 4.f, 2.f);
74 max_inlier_dist_sqr_ = powf (delta_ * 8.f, 2.f);
77 if (upper_trl_boundary_ < 0)
78 upper_trl_boundary_ = diameter_ * (1.f - approx_overlap_) * 0.5f;
80 if (!(lower_trl_boundary_ < 0) && upper_trl_boundary_ > lower_trl_boundary_)
81 use_trl_score_ =
true;
86 std::size_t nr_indices = indices_->size ();
87 if (nr_indices < ransac_iterations_)
88 indices_validation_ = indices_;
90 for (
int i = 0; i < ransac_iterations_; i++)
91 indices_validation_->push_back ((*indices_)[rand () % nr_indices]);
98 template <
typename Po
intSource,
typename Po
intTarget,
typename NormalT,
typename Scalar>
void 100 const std::vector <int> &base_indices,
101 std::vector <std::vector <int> > &matches,
102 MatchingCandidates &candidates)
105 float fitness_score = FLT_MAX;
108 for (std::vector <std::vector <int> >::iterator match_indices = matches.begin (), it_e = matches.end (); match_indices != it_e; match_indices++)
110 Eigen::Matrix4f transformation_temp;
112 fitness_score = FLT_MAX;
115 linkMatchWithBase (base_indices, *match_indices, correspondences_temp);
118 if (validateMatch (base_indices, *match_indices, correspondences_temp, transformation_temp) < 0)
123 validateTransformation (transformation_temp, fitness_score);
126 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;
179 template <
typename Po
intSource,
typename Po
intTarget,
typename NormalT,
typename Scalar>
void 181 const std::vector <MatchingCandidates > &candidates)
184 size_t total_size = 0;
185 std::vector <MatchingCandidates>::const_iterator it, it_e = candidates.end ();
186 for (it = candidates.begin (); it != it_e; it++)
187 total_size += it->size ();
189 candidates_.clear ();
190 candidates_.reserve (total_size);
192 MatchingCandidates::const_iterator it_curr, it_curr_e;
193 for (it = candidates.begin (); it != it_e; it++)
194 for (it_curr = it->begin (), it_curr_e = it->end (); it_curr != it_curr_e; it_curr++)
195 candidates_.push_back (*it_curr);
198 std::sort (candidates_.begin (), candidates_.end (),
by_score ());
201 if (candidates_[0].fitness_score == FLT_MAX)
209 fitness_score_ = candidates_ [0].fitness_score;
210 final_transformation_ = candidates_ [0].transformation;
211 *correspondences_ = candidates_ [0].correspondences;
214 converged_ = fitness_score_ < score_threshold_;
218 template <
typename Po
intSource,
typename Po
intTarget,
typename NormalT,
typename Scalar>
void 222 float min_translation3d,
223 MatchingCandidates &candidates)
228 for (MatchingCandidates::iterator it_candidate = candidates_.begin (), it_e = candidates_.end (); it_candidate != it_e; it_candidate++)
231 if (it_candidate->fitness_score == FLT_MAX)
236 MatchingCandidates::iterator it = candidates.begin (), it_e2 = candidates.end ();
237 while (unique && it != it_e2)
239 Eigen::Matrix4f diff = it_candidate->transformation.colPivHouseholderQr ().solve (it->transformation);
240 const float angle3d = Eigen::AngleAxisf (diff.block <3, 3> (0, 0)).angle ();
241 const float translation3d = diff.block <3, 1> (0, 3).norm ();
242 unique = angle3d > min_angle3d && translation3d > min_translation3d;
248 candidates.push_back (*it_candidate);
251 if (candidates.size () == n)
257 template <
typename Po
intSource,
typename Po
intTarget,
typename NormalT,
typename Scalar>
void 261 float min_translation3d,
262 MatchingCandidates &candidates)
267 for (MatchingCandidates::iterator it_candidate = candidates_.begin (), it_e = candidates_.end (); it_candidate != it_e; it_candidate++)
270 if (it_candidate->fitness_score > t)
275 MatchingCandidates::iterator it = candidates.begin (), it_e2 = candidates.end ();
276 while (unique && it != it_e2)
278 Eigen::Matrix4f diff = it_candidate->transformation.colPivHouseholderQr ().solve (it->transformation);
279 const float angle3d = Eigen::AngleAxisf (diff.block <3, 3> (0, 0)).angle ();
280 const float translation3d = diff.block <3, 1> (0, 3).norm ();
281 unique = angle3d > min_angle3d && translation3d > min_translation3d;
287 candidates.push_back (*it_candidate);
293 #endif // PCL_REGISTRATION_IMPL_IA_KFPCS_H_
Sorting of candidates based on fitness score value.
FPCSInitialAlignment computes corresponding four point congruent sets as described in: "4-points cong...
Container for matching candidate consisting of.
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=true)
Apply an affine transform defined by an Eigen Transform.
std::vector< pcl::Correspondence, Eigen::aligned_allocator< pcl::Correspondence > > Correspondences
KFPCSInitialAlignment computes corresponding four point congruent sets based on keypoints as describe...