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()).

Author: mvieth

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);