Fixes and improvements for FPCS and KFPCS (PointCloudLibrary#6073)

* Fixes and improvements for FPCS and KFPCS - Replace bad random sampling: previously, the random sampling was not guaranteed to give the user-specified number of samples, was not guaranteed to choose all points with the same probability, and might choose a point more than once. The new approach works similar to the RandomSample class - Number of threads can now be set to 0 to use automatic setting - linkMatchWithBase is not used any more. It has a bug that some indices are duplicated and others dropped. It is not necessary to use it any way because match and base_indices are already correctly ordered - ids and dists_sqr are now created with the correct size (no resize necessary later) - Add debug prints, improve documentation - fpcs and kfpcs tests: set fine tuned score threshold, early termination if a good solution is found. - kfpcs test: set higher maximum number of iterations (was previously automatically estimated as 19). This should fix the random (rare) failures on Azure pipelines * Fix for early termination in KFPCS * Use RandomSample, deprecate linkMatchWithBase
mvieth · Jul 5, 2024 · 3c550ae · 3c550ae
1 parent b0b2519
commit 3c550ae
Show file tree

Hide file tree

Showing 8 changed files with 134 additions and 44 deletions.
diff --git a/registration/include/pcl/registration/ia_fpcs.h b/registration/include/pcl/registration/ia_fpcs.h
@@ -415,6 +415,7 @@ class FPCSInitialAlignment : public Registration<PointSource, PointTarget, Scala
    * \param[in] match_indices indices of match M
    * \param[out] correspondences resulting correspondences
    */
+  PCL_DEPRECATED(1, 18, "this function has a bug and is generally not needed")
   virtual void
   linkMatchWithBase(const pcl::Indices& base_indices,
                     pcl::Indices& match_indices,

diff --git a/registration/include/pcl/registration/ia_kfpcs.h b/registration/include/pcl/registration/ia_kfpcs.h
@@ -44,8 +44,18 @@ namespace registration {
  * on keypoints as described in: "Markerless point cloud registration with
  * keypoint-based 4-points congruent sets", Pascal Theiler, Jan Dirk Wegner, Konrad
  * Schindler. ISPRS Annals II-5/W2, 2013. Presented at ISPRS Workshop Laser Scanning,
- * Antalya, Turkey, 2013. \note Method has since been improved and some variations to
- * the paper exist. \author P.W.Theiler \ingroup registration
+ * Antalya, Turkey, 2013.
+ * \note Method has since been improved and some variations to the paper exist.
+ *
+ * The main differences to FPCSInitialAlignment are:
+ * <ol>
+ *   <li> KFPCSInitialAlignment stores all solution candidates instead of only the best
+ * one
+ *   <li> KFPCSInitialAlignment uses an MSAC approach to score candidates instead of
+ * counting inliers
+ * </ol>
+ * \author P.W.Theiler
+ * \ingroup registration
  */
 template <typename PointSource,
           typename PointTarget,
@@ -187,8 +197,6 @@ class KFPCSInitialAlignment
   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. */

diff --git a/registration/include/pcl/registration/impl/ia_fpcs.hpp b/registration/include/pcl/registration/impl/ia_fpcs.hpp
@@ -41,6 +41,7 @@
 #include <pcl/common/distances.h>
 #include <pcl/common/time.h>
 #include <pcl/common/utils.h>
+#include <pcl/filters/random_sample.h>
 #include <pcl/registration/ia_fpcs.h>
 #include <pcl/registration/transformation_estimation_3point.h>
 #include <pcl/sample_consensus/sac_model_plane.h>
@@ -189,9 +190,19 @@ pcl::registration::FPCSInitialAlignment<PointSource, PointTarget, NormalT, Scala
         }
 
         // check terminate early (time or fitness_score threshold reached)
-        abort = (!candidates.empty() ? candidates[0].fitness_score < score_threshold_
-                                     : abort);
-        abort = (abort ? abort : timer.getTimeSeconds() > max_runtime_);
+        if (!candidates.empty() && candidates[0].fitness_score < score_threshold_) {
+          PCL_DEBUG("[%s::computeTransformation] Terminating because fitness score "
+                    "(%g) is below threshold (%g).\n",
+                    reg_name_.c_str(),
+                    candidates[0].fitness_score,
+                    score_threshold_);
+          abort = true;
+        }
+        else if (timer.getTimeSeconds() > max_runtime_) {
+          PCL_DEBUG("[%s::computeTransformation] Terminating because time exceeded.\n",
+                    reg_name_.c_str());
+          abort = true;
+        }
 
 #pragma omp flush(abort)
       }
@@ -238,14 +249,14 @@ pcl::registration::FPCSInitialAlignment<PointSource, PointTarget, NormalT, Scala
 
   // if a sample size for the point clouds is given; preferably no sampling of target
   // cloud
-  if (nr_samples_ != 0) {
-    const int ss = static_cast<int>(indices_->size());
-    const int sample_fraction_src = std::max(1, static_cast<int>(ss / nr_samples_));
-
+  if (nr_samples_ > 0 && static_cast<std::size_t>(nr_samples_) < indices_->size()) {
     source_indices_ = pcl::IndicesPtr(new pcl::Indices);
-    for (int i = 0; i < ss; i++)
-      if (rand() % sample_fraction_src == 0)
-        source_indices_->push_back((*indices_)[i]);
+    pcl::RandomSample<PointSource> random_sampling;
+    random_sampling.setInputCloud(input_);
+    random_sampling.setIndices(indices_);
+    random_sampling.setSample(nr_samples_);
+    random_sampling.setSeed(seed);
+    random_sampling.filter(*source_indices_);
   }
   else
     source_indices_ = indices_;
@@ -274,6 +285,15 @@ pcl::registration::FPCSInitialAlignment<PointSource, PointTarget, NormalT, Scala
   float max_base_diameter = diameter_ * approx_overlap_ * 2.f;
   max_base_diameter_sqr_ = max_base_diameter * max_base_diameter;
 
+#ifdef _OPENMP
+  if (nr_threads_ < 1) {
+    nr_threads_ = omp_get_num_procs();
+    PCL_DEBUG("[%s::initCompute] Setting number of threads to %i.\n",
+              reg_name_.c_str(),
+              nr_threads_);
+  }
+#endif // _OPENMP
+
   // normalize the delta
   if (normalize_delta_) {
     float mean_dist = getMeanPointDensity<PointTarget>(
@@ -289,6 +309,9 @@ pcl::registration::FPCSInitialAlignment<PointSource, PointTarget, NormalT, Scala
                                               static_cast<double>(min_iterations)));
     max_iterations_ =
         static_cast<int>(first_est / (diameter_fraction * approx_overlap_ * 2.f));
+    PCL_DEBUG("[%s::initCompute] Estimated max iterations as %i\n",
+              reg_name_.c_str(),
+              max_iterations_);
   }
 
   // set further parameter
@@ -307,6 +330,14 @@ pcl::registration::FPCSInitialAlignment<PointSource, PointTarget, NormalT, Scala
   coincidation_limit_ = delta_ * 2.f; // EDITED: originally std::sqrt (delta_ * 2.f)
   max_mse_ = powf(delta_ * 2.f, 2.f);
   max_inlier_dist_sqr_ = powf(delta_ * 2.f, 2.f);
+  PCL_DEBUG("[%s::initCompute] delta_=%g, max_inlier_dist_sqr_=%g, "
+            "coincidation_limit_=%g, max_edge_diff_=%g, max_pair_diff_=%g\n",
+            reg_name_.c_str(),
+            delta_,
+            max_inlier_dist_sqr_,
+            coincidation_limit_,
+            max_edge_diff_,
+            max_pair_diff_);
 
   // reset fitness_score
   fitness_score_ = std::numeric_limits<float>::max();
@@ -668,6 +699,11 @@ pcl::registration::FPCSInitialAlignment<PointSource, PointTarget, NormalT, Scala
             pairs_a[static_cast<int>(std::floor((id / 2.f)))].index_query;
         match_indices[2] = pair.index_match;
         match_indices[3] = pair.index_query;
+        if (match_indices[0] == match_indices[2] ||
+            match_indices[0] == match_indices[3] ||
+            match_indices[1] == match_indices[2] ||
+            match_indices[1] == match_indices[3])
+          continue;
 
         // EDITED: added coarse check of match based on edge length (due to rigid-body )
         if (checkBaseMatch(match_indices, dist_base) < 0)
@@ -679,7 +715,16 @@ pcl::registration::FPCSInitialAlignment<PointSource, PointTarget, NormalT, Scala
   }
 
   // return unsuccessful if no match was found
-  return (!matches.empty() ? 0 : -1);
+  if (matches.empty()) {
+    PCL_DEBUG("[%s::determineBaseMatches] no matches found\n", reg_name_.c_str());
+    return -1;
+  }
+  else {
+    PCL_DEBUG("[%s::determineBaseMatches] found %zu matches\n",
+              reg_name_.c_str(),
+              matches.size());
+    return 0;
+  }
 }
 
 ///////////////////////////////////////////////////////////////////////////////////////////
@@ -721,10 +766,10 @@ pcl::registration::FPCSInitialAlignment<PointSource, PointTarget, NormalT, Scala
   for (auto& match : matches) {
     Eigen::Matrix4f transformation_temp;
     pcl::Correspondences correspondences_temp;
-
-    // determine correspondences between base and match according to their distance to
-    // centroid
-    linkMatchWithBase(base_indices, match, correspondences_temp);
+    correspondences_temp.push_back(pcl::Correspondence(match[0], base_indices[0], 0.0));
+    correspondences_temp.push_back(pcl::Correspondence(match[1], base_indices[1], 0.0));
+    correspondences_temp.push_back(pcl::Correspondence(match[2], base_indices[2], 0.0));
+    correspondences_temp.push_back(pcl::Correspondence(match[3], base_indices[3], 0.0));
 
     // check match based on residuals of the corresponding points after
     if (validateMatch(base_indices, match, correspondences_temp, transformation_temp) <
@@ -846,8 +891,8 @@ pcl::registration::FPCSInitialAlignment<PointSource, PointTarget, NormalT, Scala
                         : static_cast<std::size_t>((1.f - fitness_score) * nr_points);
 
   float inlier_score_temp = 0;
-  pcl::Indices ids;
-  std::vector<float> dists_sqr;
+  pcl::Indices ids(1);
+  std::vector<float> dists_sqr(1);
   auto it = source_transformed.begin();
 
   for (std::size_t i = 0; i < nr_points; it++, i++) {
@@ -888,6 +933,12 @@ pcl::registration::FPCSInitialAlignment<PointSource, PointTarget, NormalT, Scala
       best_index = i;
     }
   }
+  PCL_DEBUG(
+      "[%s::finalCompute] best score is %g (iteration %i), out of %zu iterations.\n",
+      reg_name_.c_str(),
+      best_score,
+      best_index,
+      candidates.size());
 
   // check if a valid candidate was available
   if (!(best_index < 0)) {

diff --git a/registration/include/pcl/registration/impl/ia_kfpcs.hpp b/registration/include/pcl/registration/impl/ia_kfpcs.hpp
@@ -90,13 +90,26 @@ KFPCSInitialAlignment<PointSource, PointTarget, NormalT, Scalar>::initCompute()
 
   // generate a subset of indices of size ransac_iterations_ on which to evaluate
   // candidates on
-  std::size_t nr_indices = indices_->size();
-  if (nr_indices < static_cast<std::size_t>(ransac_iterations_))
+  if (indices_->size() <= static_cast<std::size_t>(ransac_iterations_) ||
+      ransac_iterations_ <= 0)
     indices_validation_ = indices_;
-  else
-    for (int i = 0; i < ransac_iterations_; i++)
-      indices_validation_->push_back((*indices_)[rand() % nr_indices]);
+  else {
+    indices_validation_.reset(new pcl::Indices);
+    pcl::RandomSample<PointSource> random_sampling;
+    random_sampling.setInputCloud(input_);
+    random_sampling.setIndices(indices_);
+    random_sampling.setSample(ransac_iterations_);
+    random_sampling.filter(*indices_validation_);
+  }
 
+  PCL_DEBUG("[%s::initCompute] delta_=%g, max_inlier_dist_sqr_=%g, "
+            "coincidation_limit_=%g, max_edge_diff_=%g, max_pair_diff_=%g\n",
+            reg_name_.c_str(),
+            delta_,
+            max_inlier_dist_sqr_,
+            coincidation_limit_,
+            max_edge_diff_,
+            max_pair_diff_);
   return (true);
 }
 
@@ -117,9 +130,10 @@ KFPCSInitialAlignment<PointSource, PointTarget, NormalT, Scalar>::handleMatches(
         std::numeric_limits<float>::max(); // reset to std::numeric_limits<float>::max()
                                            // to accept all candidates and not only best
 
-    // determine correspondences between base and match according to their distance to
-    // centroid
-    linkMatchWithBase(base_indices, match, correspondences_temp);
+    correspondences_temp.push_back(pcl::Correspondence(match[0], base_indices[0], 0.0));
+    correspondences_temp.push_back(pcl::Correspondence(match[1], base_indices[1], 0.0));
+    correspondences_temp.push_back(pcl::Correspondence(match[2], base_indices[2], 0.0));
+    correspondences_temp.push_back(pcl::Correspondence(match[3], base_indices[3], 0.0));
 
     // check match based on residuals of the corresponding points after transformation
     if (validateMatch(base_indices, match, correspondences_temp, transformation_temp) <
@@ -134,6 +148,16 @@ KFPCSInitialAlignment<PointSource, PointTarget, NormalT, Scalar>::handleMatches(
     candidates.push_back(
         MatchingCandidate(fitness_score, correspondences_temp, transformation_temp));
   }
+  // make sure that candidate with best fitness score is at the front, for early
+  // termination check
+  if (!candidates.empty()) {
+    auto best_candidate = candidates.begin();
+    for (auto iter = candidates.begin(); iter < candidates.end(); ++iter)
+      if (iter->fitness_score < best_candidate->fitness_score)
+        best_candidate = iter;
+    if (best_candidate != candidates.begin())
+      std::swap(*best_candidate, *candidates.begin());
+  }
 }
 
 template <typename PointSource, typename PointTarget, typename NormalT, typename Scalar>
@@ -150,8 +174,8 @@ KFPCSInitialAlignment<PointSource, PointTarget, NormalT, Scalar>::
   float score_a = 0.f, score_b = 0.f;
 
   // residual costs based on mse
-  pcl::Indices ids;
-  std::vector<float> dists_sqr;
+  pcl::Indices ids(1);
+  std::vector<float> dists_sqr(1);
   for (const auto& source : source_transformed) {
     // search for nearest point using kd tree search
     tree_->nearestKSearch(source, 1, ids, dists_sqr);
@@ -220,6 +244,10 @@ KFPCSInitialAlignment<PointSource, PointTarget, NormalT, Scalar>::finalCompute(
   fitness_score_ = candidates_[0].fitness_score;
   final_transformation_ = candidates_[0].transformation;
   *correspondences_ = candidates_[0].correspondences;
+  PCL_DEBUG("[%s::finalCompute] best score is %g, out of %zu candidate solutions.\n",
+            reg_name_.c_str(),
+            fitness_score_,
+            candidates_.size());
 
   // here we define convergence if resulting score is above threshold
   converged_ = fitness_score_ < score_threshold_;

diff --git a/test/registration/test_fpcs_ia.cpp b/test/registration/test_fpcs_ia.cpp
@@ -73,17 +73,18 @@ TEST (PCL, FPCSInitialAlignment)
   fpcs_ia.setNumberOfThreads (nr_threads);
   fpcs_ia.setApproxOverlap (approx_overlap);
   fpcs_ia.setDelta (delta, true);
+  fpcs_ia.setScoreThreshold (0.025); // if score is below this threshold, fpcs can stop because the solution is very good
   fpcs_ia.setNumberOfSamples (nr_samples);
 
   // align
   fpcs_ia.align (source_aligned);
   EXPECT_EQ (source_aligned.size (), cloud_source.size ());
 
   // check for correct coarse transformation matrix
-  //Eigen::Matrix4f transform_res_from_fpcs = fpcs_ia.getFinalTransformation ();
-  //for (int i = 0; i < 4; ++i)
-  //  for (int j = 0; j < 4; ++j)
-  //    EXPECT_NEAR (transform_res_from_fpcs (i,j), transform_from_fpcs[i][j], 0.5);
+  Eigen::Matrix4f transform_res_from_fpcs = fpcs_ia.getFinalTransformation ();
+  for (int i = 0; i < 4; ++i)
+    for (int j = 0; j < 4; ++j)
+      EXPECT_NEAR (transform_res_from_fpcs (i,j), transform_from_fpcs[i][j], 0.25);
 }
 
 

diff --git a/test/registration/test_fpcs_ia_data.h b/test/registration/test_fpcs_ia_data.h
@@ -6,7 +6,7 @@ constexpr float delta = 1.f;
 constexpr int nr_samples = 100;
 
 const float transform_from_fpcs [4][4] = {
-  { -0.0019f, 0.8266f, -0.5628f, -0.0378f },
+  { -0.0019f, 0.8266f, -0.5628f, 0.0378f },
   { -0.9999f, -0.0094f, -0.0104f, 0.9997f },
   { -0.0139f, 0.5627f, 0.8265f, 0.0521f },
   { 0.f, 0.f, 0.f, 1.f }

diff --git a/test/registration/test_kfpcs_ia.cpp b/test/registration/test_kfpcs_ia.cpp
@@ -49,28 +49,29 @@ using namespace pcl;
 using namespace pcl::io;
 using namespace pcl::registration;
 
-PointCloud<PointXYZI> cloud_source, cloud_target;
+PointCloud<PointXYZ> cloud_source, cloud_target;
 
 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 TEST (PCL, KFPCSInitialAlignment)
 {
   const auto previous_verbosity_level = pcl::console::getVerbosityLevel();
   pcl::console::setVerbosityLevel(pcl::console::L_VERBOSE);
   // create shared pointers
-  PointCloud<PointXYZI>::Ptr cloud_source_ptr, cloud_target_ptr;
+  PointCloud<PointXYZ>::Ptr cloud_source_ptr, cloud_target_ptr;
   cloud_source_ptr = cloud_source.makeShared ();
   cloud_target_ptr = cloud_target.makeShared ();
 
   // initialize k-fpcs
-  PointCloud <PointXYZI> cloud_source_aligned;
-  KFPCSInitialAlignment <PointXYZI, PointXYZI> kfpcs_ia;
+  PointCloud <PointXYZ> cloud_source_aligned;
+  KFPCSInitialAlignment <PointXYZ, PointXYZ> kfpcs_ia;
   kfpcs_ia.setInputSource (cloud_source_ptr);
   kfpcs_ia.setInputTarget (cloud_target_ptr);
 
   //kfpcs_ia.setNumberOfThreads (nr_threads);
   kfpcs_ia.setApproxOverlap (approx_overlap);
   kfpcs_ia.setDelta (voxel_size, false);
   kfpcs_ia.setScoreThreshold (abort_score);
+  kfpcs_ia.setMaximumIterations (100);
 
   // repeat alignment 2 times to increase probability to ~99.99%
   constexpr float max_angle3d = 0.1745f, max_translation3d = 1.f;
@@ -107,19 +108,19 @@ main (int argc, char** argv)
 {
   if (argc < 3)
   {
-    std::cerr << "No test files given. Please download `bun0.pcd` and `bun4.pcd` pass their path to the test." << std::endl;
+    std::cerr << "No test files given. Please download `office1_keypoints.pcd` and `office2_keypoints.pcd` pass their path to the test." << std::endl;
     return (-1);
   }
 
   // Input
   if (loadPCDFile (argv[1], cloud_source) < 0)
   {
-    std::cerr << "Failed to read test file. Please download `bun0.pcd` and pass its path to the test." << std::endl;
+    std::cerr << "Failed to read test file. Please download `office1_keypoints.pcd` and pass its path to the test." << std::endl;
     return (-1);
   }
   if (loadPCDFile (argv[2], cloud_target) < 0)
   {
-    std::cerr << "Failed to read test file. Please download `bun4.pcd` and pass its path to the test." << std::endl;
+    std::cerr << "Failed to read test file. Please download `office2_keypoints.pcd` and pass its path to the test." << std::endl;
     return (-1);
   }
 

diff --git a/test/registration/test_kfpcs_ia_data.h b/test/registration/test_kfpcs_ia_data.h
@@ -3,7 +3,7 @@
 constexpr int nr_threads = 1;
 constexpr float voxel_size = 0.1f;
 constexpr float approx_overlap = 0.9f;
-constexpr float abort_score = 0.0f;
+constexpr float abort_score = 0.4f;
 
 const float transformation_office1_office2 [4][4] = {
   { -0.6946f, -0.7194f, -0.0051f, -3.6352f },