From 22555e23699435766ae179d3f5d0686115256b43 Mon Sep 17 00:00:00 2001
From: Lars Glud <larshg@gmail.com>
Date: Fri, 11 Sep 2026 13:26:13 +0200
Subject: [PATCH] Add heterogeneous correspondence estimation coverage

Exercise direct and reciprocal correspondence estimation across common XYZ-bearing point types, including PointXYZ and PointNormal. Initialize test points explicitly and verify index preservation for every source-target combination.
---
 .../test_correspondence_estimation.cpp        | 96 +++++++++++++++----
 1 file changed, 79 insertions(+), 17 deletions(-)

diff --git a/test/registration/test_correspondence_estimation.cpp b/test/registration/test_correspondence_estimation.cpp
index ce8025de0f1..82957b73d09 100644
--- a/test/registration/test_correspondence_estimation.cpp
+++ b/test/registration/test_correspondence_estimation.cpp
@@ -36,6 +36,7 @@
  */
 
 #include <pcl/test/gtest.h>
+#include <pcl/point_types.h>
 #include <pcl/registration/correspondence_estimation_normal_shooting.h>
 #include <pcl/features/normal_3d.h>
 #include <pcl/kdtree/kdtree.h>
@@ -64,22 +65,14 @@ pcl::PointXYZI makeRandomPoint()
   return {static_cast<float>(rand()), static_cast<float>(rand()), static_cast<float>(rand()), static_cast<float>(rand())};
 }
 
-template <typename PointT, typename... Args>
-PointT makePointWithParams(Args... args)
-{
-  return PointT{ args... };
-}
-
-template <>
-pcl::PointXYZ makePointWithParams(float x, float y, float z)
-{
-  return {x, y, z};
-}
-
-template <>
-pcl::PointXYZI makePointWithParams(float x, float y, float z)
+template <typename PointT>
+PointT makePointWithXYZ(float x, float y, float z)
 {
-  return {x, y, z, static_cast<float>(rand())};
+  PointT point{};
+  point.x = x;
+  point.y = y;
+  point.z = z;
+  return point;
 }
 
 }
@@ -90,8 +83,42 @@ class CorrespondenceEstimationTestSuite : public ::testing::Test { };
 using PointTypesForCorrespondenceEstimationTest = 
   ::testing::Types<std::pair<pcl::PointXYZ, pcl::PointXYZ>, std::pair<pcl::PointXYZ, pcl::PointXYZI>>;
 
+using PointTypesForCorrespondenceEstimationMatrixTest =
+  ::testing::Types<
+    std::pair<pcl::PointXYZ, pcl::PointXYZ>,
+    std::pair<pcl::PointXYZ, pcl::PointXYZI>,
+    std::pair<pcl::PointXYZ, pcl::PointNormal>,
+    std::pair<pcl::PointXYZ, pcl::PointXYZRGB>,
+    std::pair<pcl::PointXYZ, pcl::PointXYZRGBNormal>,
+    std::pair<pcl::PointXYZI, pcl::PointXYZ>,
+    std::pair<pcl::PointXYZI, pcl::PointXYZI>,
+    std::pair<pcl::PointXYZI, pcl::PointNormal>,
+    std::pair<pcl::PointXYZI, pcl::PointXYZRGB>,
+    std::pair<pcl::PointXYZI, pcl::PointXYZRGBNormal>,
+    std::pair<pcl::PointNormal, pcl::PointXYZ>,
+    std::pair<pcl::PointNormal, pcl::PointXYZI>,
+    std::pair<pcl::PointNormal, pcl::PointNormal>,
+    std::pair<pcl::PointNormal, pcl::PointXYZRGB>,
+    std::pair<pcl::PointNormal, pcl::PointXYZRGBNormal>,
+    std::pair<pcl::PointXYZRGB, pcl::PointXYZ>,
+    std::pair<pcl::PointXYZRGB, pcl::PointXYZI>,
+    std::pair<pcl::PointXYZRGB, pcl::PointNormal>,
+    std::pair<pcl::PointXYZRGB, pcl::PointXYZRGB>,
+    std::pair<pcl::PointXYZRGB, pcl::PointXYZRGBNormal>,
+    std::pair<pcl::PointXYZRGBNormal, pcl::PointXYZ>,
+    std::pair<pcl::PointXYZRGBNormal, pcl::PointXYZI>,
+    std::pair<pcl::PointXYZRGBNormal, pcl::PointNormal>,
+    std::pair<pcl::PointXYZRGBNormal, pcl::PointXYZRGB>,
+    std::pair<pcl::PointXYZRGBNormal, pcl::PointXYZRGBNormal>>;
+
 TYPED_TEST_SUITE(CorrespondenceEstimationTestSuite, PointTypesForCorrespondenceEstimationTest);
 
+template <typename T>
+class CorrespondenceEstimationMatrixTest : public ::testing::Test { };
+
+TYPED_TEST_SUITE(CorrespondenceEstimationMatrixTest,
+                 PointTypesForCorrespondenceEstimationMatrixTest);
+
 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 TYPED_TEST(CorrespondenceEstimationTestSuite, CorrespondenceEstimationNormalShooting)
 {
@@ -106,8 +133,8 @@ TYPED_TEST(CorrespondenceEstimationTestSuite, CorrespondenceEstimationNormalShoo
   {
     for (std::size_t j = 0; j < 25; ++j)
     {
-      cloud1->push_back(makePointWithParams<PointSource>(i * 0.2f, 0.f, j * 0.2f));
-      cloud2->push_back(makePointWithParams<PointTarget>(i * 0.2f, 2.f, j * 0.2f)); // Ideally this should be the corresponding point to the point defined in the previous line
+      cloud1->push_back(makePointWithXYZ<PointSource>(i * 0.2f, 0.f, j * 0.2f));
+      cloud2->push_back(makePointWithXYZ<PointTarget>(i * 0.2f, 2.f, j * 0.2f)); // Ideally this should be the corresponding point to the point defined in the previous line
     }
   }
         
@@ -136,6 +163,41 @@ TYPED_TEST(CorrespondenceEstimationTestSuite, CorrespondenceEstimationNormalShoo
   }
 }
 
+//////////////////////////////////////////////////////////////////////////////////////
+TYPED_TEST(CorrespondenceEstimationMatrixTest, DifferentPointTypes)
+{
+  using PointSource = typename TypeParam::first_type;
+  using PointTarget = typename TypeParam::second_type;
+
+  auto source = pcl::make_shared<pcl::PointCloud<PointSource>>();
+  auto target = pcl::make_shared<pcl::PointCloud<PointTarget>>();
+  for (std::size_t i = 0; i < 4; ++i) {
+    const float coordinate = static_cast<float>(i);
+    source->push_back(makePointWithXYZ<PointSource>(coordinate, 0.f, 0.f));
+    target->push_back(makePointWithXYZ<PointTarget>(coordinate, 0.f, 0.f));
+  }
+
+  pcl::registration::CorrespondenceEstimation<PointSource, PointTarget> ce;
+  ce.setInputSource(source);
+  ce.setInputTarget(target);
+
+  pcl::Correspondences correspondences;
+  ce.determineCorrespondences(correspondences);
+  ASSERT_EQ(correspondences.size(), source->size());
+  for (std::size_t i = 0; i < correspondences.size(); ++i) {
+    EXPECT_EQ(correspondences[i].index_query, static_cast<int>(i));
+    EXPECT_EQ(correspondences[i].index_match, static_cast<int>(i));
+  }
+
+  pcl::Correspondences reciprocal_correspondences;
+  ce.determineReciprocalCorrespondences(reciprocal_correspondences);
+  ASSERT_EQ(reciprocal_correspondences.size(), source->size());
+  for (std::size_t i = 0; i < reciprocal_correspondences.size(); ++i) {
+    EXPECT_EQ(reciprocal_correspondences[i].index_query, static_cast<int>(i));
+    EXPECT_EQ(reciprocal_correspondences[i].index_match, static_cast<int>(i));
+  }
+}
+
 //////////////////////////////////////////////////////////////////////////////////////
 TYPED_TEST (CorrespondenceEstimationTestSuite, CorrespondenceEstimationSetSearchMethod)
 {