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),
47 use_trl_score_ (
false),
48 indices_validation_ (
new std::vector <int>)
50 reg_name_ =
"pcl::registration::KFPCSInitialAlignment";
55 template <
typename Po
intSource,
typename Po
intTarget,
typename NormalT,
typename Scalar>
bool 61 PCL_WARN (
"[%s::initCompute] Delta should be set according to keypoint precision! Normalization according to point cloud density is ignored.", reg_name_.c_str ());
62 normalize_delta_ =
false;
69 max_pair_diff_ = delta_ * 1.414f;
70 coincidation_limit_ = delta_ * 2.828f;
71 max_edge_diff_ = delta_ * 3.f;
72 max_mse_ = powf (delta_ * 4.f, 2.f);
73 max_inlier_dist_sqr_ = powf (delta_ * 8.f, 2.f);
76 if (upper_trl_boundary_ < 0)
77 upper_trl_boundary_ = diameter_ * (1.f - approx_overlap_) * 0.5f;
79 if (!(lower_trl_boundary_ < 0) && upper_trl_boundary_ > lower_trl_boundary_)
80 use_trl_score_ =
true;
85 std::size_t nr_indices = indices_->size ();
86 if (nr_indices <
size_t (ransac_iterations_))
87 indices_validation_ = indices_;
89 for (
int i = 0; i < ransac_iterations_; i++)
90 indices_validation_->push_back ((*indices_)[rand () % nr_indices]);
97 template <
typename Po
intSource,
typename Po
intTarget,
typename NormalT,
typename Scalar>
void 99 const std::vector <int> &base_indices,
100 std::vector <std::vector <int> > &matches,
104 float fitness_score = FLT_MAX;
107 for (std::vector <std::vector <int> >::iterator match_indices = matches.begin (), it_e = matches.end (); match_indices != it_e; match_indices++)
109 Eigen::Matrix4f transformation_temp;
111 fitness_score = FLT_MAX;
114 linkMatchWithBase (base_indices, *match_indices, correspondences_temp);
117 if (validateMatch (base_indices, *match_indices, correspondences_temp, transformation_temp) < 0)
122 validateTransformation (transformation_temp, fitness_score);
125 candidates.push_back (
MatchingCandidate (fitness_score, correspondences_temp, transformation_temp));
131 template <
typename Po
intSource,
typename Po
intTarget,
typename NormalT,
typename Scalar>
int 133 Eigen::Matrix4f &transformation,
134 float &fitness_score)
140 const std::size_t nr_points = source_transformed.
size ();
141 float score_a = 0.f, score_b = 0.f;
144 std::vector <int> ids;
145 std::vector <float> dists_sqr;
146 for (PointCloudSourceIterator it = source_transformed.
begin (), it_e = source_transformed.
end (); it != it_e; ++it)
149 tree_->nearestKSearch (*it, 1, ids, dists_sqr);
150 score_a += (dists_sqr[0] < max_inlier_dist_sqr_ ? dists_sqr[0] : max_inlier_dist_sqr_);
153 score_a /= (max_inlier_dist_sqr_ * nr_points);
160 float trl = transformation.rightCols <1> ().head (3).norm ();
161 float trl_ratio = (trl - lower_trl_boundary_) / (upper_trl_boundary_ - lower_trl_boundary_);
163 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));
168 float fitness_score_temp = (score_a + lambda_ * score_b) / scale;
169 if (fitness_score_temp > fitness_score)
172 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 size_t total_size = 0;
184 std::vector <MatchingCandidates>::const_iterator it, it_e = candidates.end ();
185 for (it = candidates.begin (); it != it_e; it++)
186 total_size += it->size ();
188 candidates_.clear ();
189 candidates_.reserve (total_size);
191 MatchingCandidates::const_iterator it_curr, it_curr_e;
192 for (it = candidates.begin (); it != it_e; it++)
193 for (it_curr = it->begin (), it_curr_e = it->end (); it_curr != it_curr_e; it_curr++)
194 candidates_.push_back (*it_curr);
197 std::sort (candidates_.begin (), candidates_.end (),
by_score ());
200 if (candidates_[0].fitness_score == FLT_MAX)
208 fitness_score_ = candidates_ [0].fitness_score;
209 final_transformation_ = candidates_ [0].transformation;
210 *correspondences_ = candidates_ [0].correspondences;
213 converged_ = fitness_score_ < score_threshold_;
217 template <
typename Po
intSource,
typename Po
intTarget,
typename NormalT,
typename Scalar>
void 221 float min_translation3d,
227 for (MatchingCandidates::iterator it_candidate = candidates_.begin (), it_e = candidates_.end (); it_candidate != it_e; it_candidate++)
230 if (it_candidate->fitness_score == FLT_MAX)
235 MatchingCandidates::iterator it = candidates.begin (), it_e2 = candidates.end ();
236 while (unique && it != it_e2)
238 Eigen::Matrix4f diff = it_candidate->transformation.colPivHouseholderQr ().solve (it->transformation);
239 const float angle3d = Eigen::AngleAxisf (diff.block <3, 3> (0, 0)).angle ();
240 const float translation3d = diff.block <3, 1> (0, 3).norm ();
241 unique = angle3d > min_angle3d && translation3d > min_translation3d;
247 candidates.push_back (*it_candidate);
250 if (candidates.size () == n)
256 template <
typename Po
intSource,
typename Po
intTarget,
typename NormalT,
typename Scalar>
void 260 float min_translation3d,
266 for (MatchingCandidates::iterator it_candidate = candidates_.begin (), it_e = candidates_.end (); it_candidate != it_e; it_candidate++)
269 if (it_candidate->fitness_score > t)
274 MatchingCandidates::iterator it = candidates.begin (), it_e2 = candidates.end ();
275 while (unique && it != it_e2)
277 Eigen::Matrix4f diff = it_candidate->transformation.colPivHouseholderQr ().solve (it->transformation);
278 const float angle3d = Eigen::AngleAxisf (diff.block <3, 3> (0, 0)).angle ();
279 const float translation3d = diff.block <3, 1> (0, 3).norm ();
280 unique = angle3d > min_angle3d && translation3d > min_translation3d;
286 candidates.push_back (*it_candidate);
292 #endif // PCL_REGISTRATION_IMPL_IA_KFPCS_H_ std::vector< MatchingCandidate, Eigen::aligned_allocator< MatchingCandidate > > MatchingCandidates
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...