From ade277885735a6f6dd4f5f34e9995f8c7c655fc9 Mon Sep 17 00:00:00 2001
From: Markus Vieth <mvieth@techfak.uni-bielefeld.de>
Date: Sat, 10 May 2025 17:15:30 +0200
Subject: [PATCH] Make GICP use CorrespondenceEstimation The main benefit is
 that CorrespondenceEstimation is parallelized, so GICP can benefit from that.
 But even with 1 thread, the new code is slightly faster due to the improved
 way of computing the mahalanobis matrix (invert3x3SymMatrix uses the fact
 that the matrix is symmetric and is thus faster than the general-case
 .inverse()).

---
 .../include/pcl/registration/impl/gicp.hpp    | 54 ++++++++-----------
 1 file changed, 23 insertions(+), 31 deletions(-)

diff --git a/registration/include/pcl/registration/impl/gicp.hpp b/registration/include/pcl/registration/impl/gicp.hpp
index 0132923d87c..78490b66370 100644
--- a/registration/include/pcl/registration/impl/gicp.hpp
+++ b/registration/include/pcl/registration/impl/gicp.hpp
@@ -790,11 +790,20 @@ GeneralizedIterativeClosestPoint<PointSource, PointTarget, Scalar>::
   base_transformation_ = Matrix4::Identity();
   nr_iterations_ = 0;
   converged_ = false;
-  double dist_threshold = corr_dist_threshold_ * corr_dist_threshold_;
   pcl::Indices nn_indices(1);
   std::vector<float> nn_dists(1);
 
   pcl::transformPointCloud(output, output, guess);
+  pcl::registration::CorrespondenceEstimation<PointSource, PointTarget, Scalar>
+      corr_estimation;
+  corr_estimation.setNumberOfThreads(threads_);
+  // setSearchMethodSource is not necessary because we do not use
+  // determineReciprocalCorrespondences
+  corr_estimation.setSearchMethodTarget(this->getSearchMethodTarget());
+  corr_estimation.setInputTarget(target_);
+  auto output_transformed = pcl::make_shared<PointCloudSource>();
+  output_transformed->resize(output.size());
+  pcl::Correspondences correspondences;
 
   while (!converged_) {
     std::size_t cnt = 0;
@@ -811,36 +820,19 @@ GeneralizedIterativeClosestPoint<PointSource, PointTarget, Scalar>::
 
     Eigen::Matrix3d R = transform_R.topLeftCorner<3, 3>();
 
-    for (std::size_t i = 0; i < N; i++) {
-      PointSource query = output[i];
-      query.getVector4fMap() =
-          transformation_.template cast<float>() * query.getVector4fMap();
-
-      if (!searchForNeighbors(query, nn_indices, nn_dists)) {
-        PCL_ERROR("[pcl::%s::computeTransformation] Unable to find a nearest neighbor "
-                  "in the target dataset for point %d in the source!\n",
-                  getClassName().c_str(),
-                  (*indices_)[i]);
-        return;
-      }
-
-      // Check if the distance to the nearest neighbor is smaller than the user imposed
-      // threshold
-      if (nn_dists[0] < dist_threshold) {
-        Eigen::Matrix3d& C1 = (*input_covariances_)[i];
-        Eigen::Matrix3d& C2 = (*target_covariances_)[nn_indices[0]];
-        Eigen::Matrix3d& M = mahalanobis_[i];
-        // M = R*C1
-        M = R * C1;
-        // temp = M*R' + C2 = R*C1*R' + C2
-        Eigen::Matrix3d temp = M * R.transpose();
-        temp += C2;
-        // M = temp^-1
-        M = temp.inverse();
-        source_indices[cnt] = static_cast<int>(i);
-        target_indices[cnt] = nn_indices[0];
-        cnt++;
-      }
+    transformPointCloud(
+        output, *output_transformed, transformation_.template cast<float>(), false);
+    corr_estimation.setInputSource(output_transformed);
+    corr_estimation.determineCorrespondences(correspondences, corr_dist_threshold_);
+    cnt = 0;
+    for (const auto& corr : correspondences) {
+      source_indices[cnt] = corr.index_query;
+      target_indices[cnt] = corr.index_match;
+      const Eigen::Matrix3d& C1 = (*input_covariances_)[corr.index_query];
+      const Eigen::Matrix3d& C2 = (*target_covariances_)[corr.index_match];
+      pcl::invert3x3SymMatrix<Eigen::Matrix3d>(R * C1 * R.transpose() + C2,
+                                               mahalanobis_[corr.index_query]);
+      ++cnt;
     }
     // Resize to the actual number of valid correspondences
     source_indices.resize(cnt);