k-4pcs registration method as an extension of 4pcs

PointCloudLibrary · Apr 8, 2015 · be11c46 · be11c46
1 parent fc4be0e
commit be11c46
Show file tree

Hide file tree

Showing 3 changed files with 16 additions and 11 deletions.
diff --git a/registration/include/pcl/registration/ia_kfpcs.h b/registration/include/pcl/registration/ia_kfpcs.h
@@ -161,7 +161,7 @@ namespace pcl
 
 
     protected:
-      /** \cond */
+
       using PCLBase <PointSource>::deinitCompute;
       using PCLBase <PointSource>::input_;
       using PCLBase <PointSource>::indices_;
@@ -170,6 +170,7 @@ namespace pcl
       using Registration <PointSource, PointTarget, Scalar>::tree_;
       using Registration <PointSource, PointTarget, Scalar>::final_transformation_;
       using Registration <PointSource, PointTarget, Scalar>::ransac_iterations_;
+      using Registration <PointSource, PointTarget, Scalar>::correspondences_;
       using Registration <PointSource, PointTarget, Scalar>::converged_;
 
       using FPCSInitialAlignment <PointSource, PointTarget, NormalT, Scalar>::delta_;
@@ -181,7 +182,11 @@ namespace pcl
       using FPCSInitialAlignment <PointSource, PointTarget, NormalT, Scalar>::max_inlier_dist_sqr_;
       using FPCSInitialAlignment <PointSource, PointTarget, NormalT, Scalar>::diameter_;
       using FPCSInitialAlignment <PointSource, PointTarget, NormalT, Scalar>::normalize_delta_;
-      /** \endcond */
+      using FPCSInitialAlignment <PointSource, PointTarget, NormalT, Scalar>::fitness_score_;
+      using FPCSInitialAlignment <PointSource, PointTarget, NormalT, Scalar>::score_threshold_;
+      using FPCSInitialAlignment <PointSource, PointTarget, NormalT, Scalar>::linkMatchWithBase;
+      using FPCSInitialAlignment <PointSource, PointTarget, NormalT, Scalar>::validateMatch;
+
 
       /** \brief Internal computation initialization. */
       virtual bool

diff --git a/registration/include/pcl/registration/impl/ia_kfpcs.hpp b/registration/include/pcl/registration/impl/ia_kfpcs.hpp
@@ -41,8 +41,8 @@
 ///////////////////////////////////////////////////////////////////////////////////////////
 template <typename PointSource, typename PointTarget, typename NormalT, typename Scalar>
 pcl::registration::KFPCSInitialAlignment <PointSource, PointTarget, NormalT, Scalar>::KFPCSInitialAlignment () :
-  upper_trl_boundary_ (-1.f),
   lower_trl_boundary_ (-1.f),
+  upper_trl_boundary_ (-1.f),  
   lambda_ (0.5f),
   candidates_ (),
   use_trl_score_ (false),
@@ -59,7 +59,7 @@ pcl::registration::KFPCSInitialAlignment <PointSource, PointTarget, NormalT, Sca
   // due to sparse keypoint cloud, do not normalize delta with estimated point density
   if (normalize_delta_)
   {
-    PCL_WARN ("[%s::initCompute] Delta should be set according to keypoint precision! Normalization according to point cloud density is ignored.", reg_name_);
+    PCL_WARN ("[%s::initCompute] Delta should be set according to keypoint precision! Normalization according to point cloud density is ignored.", reg_name_.c_str ());
     normalize_delta_ = false;
   }
 
@@ -70,8 +70,8 @@ pcl::registration::KFPCSInitialAlignment <PointSource, PointTarget, NormalT, Sca
   max_pair_diff_ = delta_ * 1.414f; // diff between 2 points of delta_ accuracy
   coincidation_limit_ = delta_ * 2.828f; // diff between diff of 2 points
   max_edge_diff_ = delta_ * 3.f; // diff between 2 points + some inaccuracy due to quadruple orientation
-  max_mse_ = std::powf (delta_ * 4.f, 2.f); // diff between 2 points + some registration inaccuracy
-  max_inlier_dist_sqr_ = std::powf (delta_ * 8.f, 2.f); // set rel. high, because MSAC is used (residual based score function)
+  max_mse_ = powf (delta_ * 4.f, 2.f); // diff between 2 points + some registration inaccuracy
+  max_inlier_dist_sqr_ = powf (delta_ * 8.f, 2.f); // set rel. high, because MSAC is used (residual based score function)
 
   // check use of translation costs and calculate upper boundary if not set by user
   if (upper_trl_boundary_ < 0)
@@ -182,16 +182,16 @@ pcl::registration::KFPCSInitialAlignment <PointSource, PointTarget, NormalT, Sca
 {
   // reorganize candidates into single vector
   size_t total_size = 0;
-  std::vector <MatchingCandidates>::const_iterator it, it_e = candidates.cend ();
-  for (it = candidates.cbegin (); it != it_e; it++)
+  std::vector <MatchingCandidates>::const_iterator it, it_e = candidates.end ();
+  for (it = candidates.begin (); it != it_e; it++)
     total_size += it->size ();
 
   candidates_.clear ();
   candidates_.reserve (total_size);
 
   MatchingCandidates::const_iterator it_curr, it_curr_e;
-  for (it = candidates.cbegin (); it != it_e; it++)
-  for (it_curr = it->cbegin (), it_curr_e = it->cend (); it_curr != it_curr_e; it_curr++)
+  for (it = candidates.begin (); it != it_e; it++)
+  for (it_curr = it->begin (), it_curr_e = it->end (); it_curr != it_curr_e; it_curr++)
     candidates_.push_back (*it_curr);
 
   // sort acoording to score value

diff --git a/test/registration/test_kfpcs_ia.cpp b/test/registration/test_kfpcs_ia.cpp
@@ -54,7 +54,7 @@ PointCloud<PointXYZI> cloud_source, cloud_target;
 TEST (PCL, KFPCSInitialAlignment)
 {
   // create shared pointers
-  PointCloud<PointXYZ>::Ptr cloud_source_ptr, cloud_target_ptr;
+  PointCloud<PointXYZI>::Ptr cloud_source_ptr, cloud_target_ptr;
   cloud_source_ptr = cloud_source.makeShared ();
   cloud_target_ptr = cloud_target.makeShared ();