Added util3d_registration tests

corelib/include/rtabmap/core/util3d_registration.h

@@ -45,10 +45,59 @@ int RTABMAP_CORE_EXPORT getCorrespondencesCount(const pcl::PointCloud<pcl::Point
 							const pcl::PointCloud<pcl::PointXYZ>::ConstPtr & cloud_target,
 							float maxDistance);
 
+/**
+ * @brief Estimates the rigid 3D transformation between two point clouds using SVD.
+ *
+ * This function computes the transformation (rotation and translation) that best aligns
+ * `cloud2` to `cloud1` using Singular Value Decomposition (SVD) based on point correspondences.
+ * It assumes a one-to-one correspondence between points in the two clouds.
+ *
+ * Internally, it uses PCL's `TransformationEstimationSVD` to compute the 4x4 transformation matrix,
+ * which is then converted to a `Transform` object.
+ *
+ * @param cloud1 Target point cloud (reference frame).
+ * @param cloud2 Source point cloud to be aligned with `cloud1`.
+ *               It must have the same number of points as `cloud1`, and the points should
+ *               correspond to each other by index.
+ *
+ * @return A `Transform` representing the rigid-body transformation from `cloud1` to `cloud2`.
+ *
+ * @note This function does not perform any outlier rejection or correspondence estimation—
+ *       it assumes that the input clouds are already matched appropriately.
+ */
 Transform RTABMAP_CORE_EXPORT transformFromXYZCorrespondencesSVD(
 	const pcl::PointCloud<pcl::PointXYZ> & cloud1,
 	const pcl::PointCloud<pcl::PointXYZ> & cloud2);
 
+/**
+ * @brief Estimates a rigid transformation between two point clouds using RANSAC with optional refinement.
+ *
+ * This function finds a 3D rigid-body transform from `cloud1` to `cloud2` using one-to-one point correspondences.
+ * It applies a RANSAC-based outlier rejection and optionally refines the transformation with iterative model optimization.
+ *
+ * It also optionally returns the inlier indices used to compute the final model and an approximate 6x6 covariance matrix
+ * of the transform.
+ *
+ * @param cloud1         Target point cloud (reference frame). Must contain at least 3 points and match `cloud2` in size.
+ * @param cloud2         Source point cloud to align to `cloud1`. Must be the same size as `cloud1`.
+ * @param inlierThreshold Maximum Euclidean distance (in meters) between corresponding points for them to be considered inliers.
+ * @param iterations     Number of RANSAC iterations to perform.
+ * @param refineIterations Number of refinement steps to perform after the initial RANSAC.
+ *                         If set to 0, no refinement is done.
+ * @param refineSigma    Multiplier for standard deviation used to adjust the inlier threshold during refinement.
+ * @param inliersOut     Optional pointer to a vector that will receive the indices of the inlier correspondences.
+ * @param covariance     Optional pointer to a 6x6 covariance matrix of the estimated transform (as `CV_64FC1`).
+ *                       Will be identity if set and no inliers are found.
+ *
+ * @return A `Transform` representing the estimated pose from `cloud1` to `cloud2`.
+ *         If no valid model is found, the returned transform will be identity.
+ *
+ * @note This function assumes a one-to-one correspondence between points in the two clouds
+ *       (e.g., index `i` in `cloud1` corresponds to index `i` in `cloud2`).
+ * @note If fewer than 3 points are provided or point counts do not match, the identity transform is returned.
+ *
+ * @warning Inlier refinement is sensitive to oscillation and may stop early if alternating inlier counts are detected.
+ */
 Transform RTABMAP_CORE_EXPORT transformFromXYZCorrespondences(
 		const pcl::PointCloud<pcl::PointXYZ>::ConstPtr & cloud1,
 		const pcl::PointCloud<pcl::PointXYZ>::ConstPtr & cloud2,
@@ -59,6 +108,36 @@ Transform RTABMAP_CORE_EXPORT transformFromXYZCorrespondences(
 		std::vector<int> * inliers = 0,
 		cv::Mat * variance = 0);
 
+/**
+ * @defgroup ComputeVarianceAndCorrespondences Compute Variance and Correspondences of Two Point Clouds
+ * @brief Computes the geometric variance and number of valid correspondences between two point clouds.
+ *
+ * This function estimates correspondences between two point clouds and computes a variance
+ * measure (based on squared median error) for the inlier correspondences. Optionally filters correspondences by 
+ * normal alignment angle if the point cloud has normals.
+ *
+ * The correspondence estimation uses PCL's reciprocal correspondence logic by default. The target and source clouds are automatically
+ * chosen based on their sizes (the larger becomes the target to ensure optimal matching behavior).
+ *
+ * @param cloudA First input point cloud (used interchangeably with cloudB for matching).
+ * @param cloudB Second input point cloud.
+ * @param maxCorrespondenceDistance Maximum allowable Euclidean distance between correspondences.
+ * @param maxCorrespondenceAngle Maximum angle (in radians) between normals for correspondences to be considered valid.
+ *                                If ≤ 0, angle filtering is disabled.
+ * @param[out] variance Output variance value (estimated using 2.1981 × median squared distance of inliers).
+ * @param[out] correspondencesOut Output number of correspondences that passed all filters (distance and optional angle).
+ * @param reciprocal If true, use reciprocal correspondences.
+ *
+ * @note This function chooses the target/source roles based on cloud size (the larger becomes the target).
+ * @note The normal comparison is only applied if `maxCorrespondenceAngle > 0`.
+ * @note The computed `variance` is based on a robust estimator using the median squared distance of correspondences.
+ *
+ * @see pcl::registration::CorrespondenceEstimation
+ */
+/**
+ * @ingroup ComputeVarianceAndCorrespondences
+ * @brief Compute with variance and correspondences of `pcl::PointNormal` point cloud type.
+ */
 void RTABMAP_CORE_EXPORT computeVarianceAndCorrespondences(
 		const pcl::PointCloud<pcl::PointNormal>::ConstPtr & cloudA,
 		const pcl::PointCloud<pcl::PointNormal>::ConstPtr & cloudB,
@@ -67,6 +146,10 @@ void RTABMAP_CORE_EXPORT computeVarianceAndCorrespondences(
 		double & variance,
 		int & correspondencesOut,
 		bool reciprocal);
+/**
+ * @ingroup ComputeVarianceAndCorrespondences
+ * @brief Compute with variance and correspondences of `pcl::PointXYZINormal` point cloud type.
+ */
 void RTABMAP_CORE_EXPORT computeVarianceAndCorrespondences(
 		const pcl::PointCloud<pcl::PointXYZINormal>::ConstPtr & cloudA,
 		const pcl::PointCloud<pcl::PointXYZINormal>::ConstPtr & cloudB,
@@ -75,6 +158,10 @@ void RTABMAP_CORE_EXPORT computeVarianceAndCorrespondences(
 		double & variance,
 		int & correspondencesOut,
 		bool reciprocal);
+/**
+ * @ingroup ComputeVarianceAndCorrespondences
+ * @brief Compute with variance and correspondences of `pcl::PointXYZ` point cloud type.
+ */
 void RTABMAP_CORE_EXPORT computeVarianceAndCorrespondences(
 		const pcl::PointCloud<pcl::PointXYZ>::ConstPtr & cloudA,
 		const pcl::PointCloud<pcl::PointXYZ>::ConstPtr & cloudB,
@@ -82,6 +169,10 @@ void RTABMAP_CORE_EXPORT computeVarianceAndCorrespondences(
 		double & variance,
 		int & correspondencesOut,
 		bool reciprocal);
+/**
+ * @ingroup ComputeVarianceAndCorrespondences
+ * @brief Compute with variance and correspondences of `pcl::PointXYZI` point cloud type.
+ */
 void RTABMAP_CORE_EXPORT computeVarianceAndCorrespondences(
 		const pcl::PointCloud<pcl::PointXYZI>::ConstPtr & cloudA,
 		const pcl::PointCloud<pcl::PointXYZI>::ConstPtr & cloudB,
@@ -90,6 +181,31 @@ void RTABMAP_CORE_EXPORT computeVarianceAndCorrespondences(
 		int & correspondencesOut,
 		bool reciprocal);
 
+/**
+ * @brief Performs Iterative Closest Point (ICP) alignment between two point clouds and returns the resulting transform.
+ *
+ * This function aligns the `cloud_source` to the `cloud_target` using PCL's ICP algorithm. It optionally supports 2D ICP,
+ * which constrains the estimated transformation to the XY-plane with rotation about the Z-axis.
+ *
+ * The result is returned as a `Transform` representing the transformation from source to target.
+ * The aligned version of the source cloud is written into `cloud_source_registered`.
+ *
+ * @param cloud_source The input source point cloud to align.
+ * @param cloud_target The input target point cloud to align to.
+ * @param maxCorrespondenceDistance Maximum distance threshold for point correspondences.
+ * @param maximumIterations Maximum number of ICP iterations to perform.
+ * @param[out] hasConverged Set to true if ICP converged to a solution; false otherwise.
+ * @param[out] cloud_source_registered Output point cloud containing the source aligned to the target.
+ * @param epsilon Convergence threshold for transformation changes between iterations (applied as squared value).
+ * @param icp2D If true, enforces 2D ICP using only XY translation and Z rotation (ignores Z and X/Y rotation).
+ *
+ * @return Transform The estimated transformation from `cloud_source` to `cloud_target`.
+ *
+ * @note All input points in both clouds must be finite (i.e., no NaNs or infinite values).
+ *
+ * @see pcl::IterativeClosestPoint
+ * @see pcl::registration::TransformationEstimation2D
+ */
 Transform RTABMAP_CORE_EXPORT icp(
 		const pcl::PointCloud<pcl::PointXYZ>::ConstPtr & cloud_source,
 		const pcl::PointCloud<pcl::PointXYZ>::ConstPtr & cloud_target,
@@ -99,6 +215,10 @@ Transform RTABMAP_CORE_EXPORT icp(
 		pcl::PointCloud<pcl::PointXYZ> & cloud_source_registered,
 		float epsilon = 0.0f,
 		bool icp2D = false);
+/**
+ * @brief Performs Iterative Closest Point (ICP) alignment between two point clouds and returns the resulting transform.
+ * @see util3d::icp()
+ */
 Transform RTABMAP_CORE_EXPORT icp(
 		const pcl::PointCloud<pcl::PointXYZI>::ConstPtr & cloud_source,
 		const pcl::PointCloud<pcl::PointXYZI>::ConstPtr & cloud_target,
@@ -109,6 +229,31 @@ Transform RTABMAP_CORE_EXPORT icp(
 		float epsilon = 0.0f,
 		bool icp2D = false);
 
+/**
+ * @brief Performs Iterative Closest Point (ICP) alignment using a point-to-plane error metric.
+ *
+ * This function aligns a source point cloud to a target point cloud using PCL's
+ * point-to-plane ICP implementation with a linear least squares estimator. It returns
+ * the estimated transformation from the source to the target.
+ *
+ * Optionally, if `icp2D` is true, the resulting transformation is projected to 3DoF (XY translation and rotation about Z).
+ *
+ * @param cloud_source Input source point cloud with normals.
+ * @param cloud_target Input target point cloud with normals.
+ * @param maxCorrespondenceDistance Maximum distance for considering point correspondences.
+ * @param maximumIterations Maximum number of ICP iterations to perform.
+ * @param[out] hasConverged Set to true if the ICP algorithm successfully converged.
+ * @param[out] cloud_source_registered Output cloud representing the aligned source.
+ * @param epsilon Convergence threshold for the transformation change (used as squared value).
+ * @param icp2D If true, the result is projected to 2D (XY + Yaw only).
+ *
+ * @return The transformation from the source to the target cloud.
+ *
+ * @note All points and normals in both input clouds must be finite (no NaNs or infinities).
+ *
+ * @see pcl::IterativeClosestPoint
+ * @see pcl::registration::TransformationEstimationPointToPlaneLLS
+ */
 Transform RTABMAP_CORE_EXPORT icpPointToPlane(
 		const pcl::PointCloud<pcl::PointNormal>::ConstPtr & cloud_source,
 		const pcl::PointCloud<pcl::PointNormal>::ConstPtr & cloud_target,
@@ -118,6 +263,10 @@ Transform RTABMAP_CORE_EXPORT icpPointToPlane(
 		pcl::PointCloud<pcl::PointNormal> & cloud_source_registered,
 		float epsilon = 0.0f,
 		bool icp2D = false);
+/**
+ * @briefPerforms Iterative Closest Point (ICP) alignment using a point-to-plane error metric.
+ * @see util3d::icpPointToPlane()
+ */
 Transform RTABMAP_CORE_EXPORT icpPointToPlane(
 		const pcl::PointCloud<pcl::PointXYZINormal>::ConstPtr & cloud_source,
 		const pcl::PointCloud<pcl::PointXYZINormal>::ConstPtr & cloud_target,

corelib/src/util3d_registration.cpp

@@ -51,6 +51,7 @@ Transform transformFromXYZCorrespondencesSVD(
 	const pcl::PointCloud<pcl::PointXYZ> & cloud1,
 	const pcl::PointCloud<pcl::PointXYZ> & cloud2)
 {
+	UASSERT(cloud1.size() == cloud2.size());
 	pcl::registration::TransformationEstimationSVD<pcl::PointXYZ, pcl::PointXYZ> svd;
 
 	// Perform the alignment
@@ -256,7 +257,13 @@ void computeVarianceAndCorrespondencesImpl(
 	est->setInputTarget(target);
 	est->setInputSource(source);
 	pcl::Correspondences correspondences;
-	est->determineReciprocalCorrespondences(correspondences, maxCorrespondenceDistance);
+	if(reciprocal) {
+		est->determineReciprocalCorrespondences(correspondences, maxCorrespondenceDistance);
+	}
+	else {
+		est->determineCorrespondences(correspondences, maxCorrespondenceDistance);
+	}
+
 
 	if(correspondences.size())
 	{
@@ -340,7 +347,12 @@ void computeVarianceAndCorrespondencesImpl(
 	est->setInputTarget(cloudA->size()>cloudB->size()?cloudA:cloudB);
 	est->setInputSource(cloudA->size()>cloudB->size()?cloudB:cloudA);
 	pcl::Correspondences correspondences;
-	est->determineReciprocalCorrespondences(correspondences, maxCorrespondenceDistance);
+	if(reciprocal) {
+		est->determineReciprocalCorrespondences(correspondences, maxCorrespondenceDistance);
+	}
+	else {
+		est->determineCorrespondences(correspondences, maxCorrespondenceDistance);
+	}
 
 	if(correspondences.size()>=3)
 	{
@@ -411,7 +423,7 @@ Transform icpImpl(const typename pcl::PointCloud<PointT>::ConstPtr & cloud_sourc
 	// Set the transformation epsilon (criterion 2)
 	icp.setTransformationEpsilon (epsilon*epsilon);
 	// Set the euclidean distance difference epsilon (criterion 3)
-	//icp.setEuclideanFitnessEpsilon (1);
+	//icp.setEuclideanFitnessEpsilon (-std::numeric_limits<double>::max());
 	//icp.setRANSACOutlierRejectionThreshold(maxCorrespondenceDistance);
 
 	// Perform the alignment
@@ -486,6 +498,7 @@ Transform icpPointToPlaneImpl(
 	{
 		// FIXME probably an estimation approach already 2D like in icp() version above exists.
 		t = t.to3DoF();
+		pcl::transformPointCloudWithNormals(*cloud_source, cloud_source_registered, t.toEigen4f());
 	}
 
 	return t;

corelib/test/CMakeLists.txt

@@ -19,4 +19,9 @@ gtest_discover_tests(test_util3d_transforms)
 #util3d_filtering.h
 add_executable(test_util3d_filtering test_util3d_filtering.cpp)
 target_link_libraries(test_util3d_filtering gtest_main rtabmap_core)
-gtest_discover_tests(test_util3d_filtering)
+gtest_discover_tests(test_util3d_filtering)
+
+#util3d_registration.h
+add_executable(test_util3d_registration test_util3d_registration.cpp)
+target_link_libraries(test_util3d_registration gtest_main rtabmap_core)
+gtest_discover_tests(test_util3d_registration)

corelib/test/test_util3d_registration.cpp

@@ -0,0 +1,440 @@
+#include "gtest/gtest.h"
+#include "rtabmap/core/util3d_registration.h"
+#include "rtabmap/core/util3d_transforms.h"
+#include "rtabmap/core/util3d_surface.h"
+#include "rtabmap/utilite/UException.h"
+#include <pcl/common/impl/angles.hpp>
+#include <pcl/common/io.h>
+#include <pcl/io/pcd_io.h>
+
+using namespace rtabmap;
+
+TEST(Util3DRegistration, transformFromXYZCorrespondencesSVDIdentityTransform)
+{
+    pcl::PointCloud<pcl::PointXYZ> cloud1, cloud2;
+    cloud1.push_back(pcl::PointXYZ(1, 2, 3));
+    cloud1.push_back(pcl::PointXYZ(4, 5, 6));
+    cloud1.push_back(pcl::PointXYZ(7, 8, 9));
+    cloud1.push_back(pcl::PointXYZ(10, 11, 12));
+    cloud1.push_back(pcl::PointXYZ(1, 6, 12));
+    cloud2 = cloud1; // Exact same points
+
+    Transform result = util3d::transformFromXYZCorrespondencesSVD(cloud1, cloud2);
+    Transform identity = Transform::getIdentity();
+
+    EXPECT_LT(result.getDistance(identity), 0.001f);
+    EXPECT_LT(result.getAngle(identity), 0.001f);
+}
+
+TEST(Util3DRegistration, transformFromXYZCorrespondencesSVDTranslationOnly)
+{
+    pcl::PointCloud<pcl::PointXYZ> cloud1, cloud2;
+    cloud1.push_back(pcl::PointXYZ(0, 0, 0));
+    cloud1.push_back(pcl::PointXYZ(1, 0, 0));
+    cloud1.push_back(pcl::PointXYZ(0, 1, 0));
+
+    cloud2.push_back(pcl::PointXYZ(1, 2, 3));
+    cloud2.push_back(pcl::PointXYZ(2, 2, 3));
+    cloud2.push_back(pcl::PointXYZ(1, 3, 3));
+
+    Transform result = util3d::transformFromXYZCorrespondencesSVD(cloud1, cloud2);
+
+    EXPECT_LT(result.getAngle(Transform::getIdentity()), 0.001f);
+    EXPECT_NEAR(result.x(), 1.0f, 1e-4f);
+    EXPECT_NEAR(result.y(), 2.0f, 1e-4f);
+    EXPECT_NEAR(result.z(), 3.0f, 1e-4f);
+}
+
+TEST(Util3DRegistration, transformFromXYZCorrespondencesSVDRotationAndTranslation)
+{
+    pcl::PointCloud<pcl::PointXYZ> cloud1, cloud2;
+
+    // Original triangle
+    cloud1.push_back(pcl::PointXYZ(1, 0, 0));
+    cloud1.push_back(pcl::PointXYZ(0, 1, 0));
+    cloud1.push_back(pcl::PointXYZ(0, 0, 1));
+
+    // Apply known rotation (90° about Z) and translation (1, 2, 3)
+    Eigen::Matrix3f R;
+    R = Eigen::AngleAxisf(M_PI_2, Eigen::Vector3f::UnitZ());
+    Eigen::Vector3f t(1, 2, 3);
+
+    for (const auto & pt : cloud1.points)
+    {
+        Eigen::Vector3f p(pt.x, pt.y, pt.z);
+        p = R * p + t;
+        cloud2.push_back(pcl::PointXYZ(p[0], p[1], p[2]));
+    }
+
+    Transform result = util3d::transformFromXYZCorrespondencesSVD(cloud1, cloud2);
+
+    Eigen::Matrix4f expected = Eigen::Matrix4f::Identity();
+    expected.block<3,3>(0,0) = R;
+    expected.block<3,1>(0,3) = t;
+    Transform expected_t = Transform::fromEigen4f(expected);
+
+    EXPECT_LT(result.getAngle(expected_t), 0.001f);
+    EXPECT_NEAR(result.x(), expected_t.x(), 1e-4f);
+    EXPECT_NEAR(result.y(), expected_t.y(), 1e-4f);
+    EXPECT_NEAR(result.z(), expected_t.z(), 1e-4f);
+}
+
+TEST(Util3DRegistration, transformFromXYZCorrespondencesSVDMismatchedSizes)
+{
+    pcl::PointCloud<pcl::PointXYZ> cloud1, cloud2;
+    cloud1.push_back(pcl::PointXYZ(0, 0, 0));
+    cloud1.push_back(pcl::PointXYZ(1, 1, 1));
+    cloud2.push_back(pcl::PointXYZ(0, 0, 0)); // Only 1 point
+
+    EXPECT_THROW(util3d::transformFromXYZCorrespondencesSVD(cloud1, cloud2), UException);
+}
+
+TEST(Util3DRegistration, transformFromXYZCorrespondencesIdentityTransform)
+{
+    auto cloud1 = std::make_shared<pcl::PointCloud<pcl::PointXYZ>>();
+    cloud1->push_back(pcl::PointXYZ(1, 0, 0));
+    cloud1->push_back(pcl::PointXYZ(0, 1, 0));
+    cloud1->push_back(pcl::PointXYZ(0, 0, 1));
+
+    auto cloud2 = std::make_shared<pcl::PointCloud<pcl::PointXYZ>>(*cloud1);
+
+    std::vector<int> inliers;
+    cv::Mat covariance;
+
+    Transform result = util3d::transformFromXYZCorrespondences(
+        cloud1, cloud2, 0.01, 100, 0, 1.0, &inliers, &covariance);
+
+    Transform identity = Transform::getIdentity();
+    EXPECT_LT(result.getDistance(identity), 0.001f);
+    EXPECT_LT(result.getAngle(identity), 0.001f);
+    EXPECT_EQ(inliers.size(), 3);
+    EXPECT_EQ(covariance.rows, 6);
+    EXPECT_EQ(covariance.cols, 6);
+}
+
+TEST(Util3DRegistration, transformFromXYZCorrespondencesTranslatedCloud)
+{
+    auto cloud1 = std::make_shared<pcl::PointCloud<pcl::PointXYZ>>();
+    auto cloud2 = std::make_shared<pcl::PointCloud<pcl::PointXYZ>>();
+    Eigen::Vector3f t(1.0f, 2.0f, 3.0f);
+
+    for (int i = 0; i < 3; ++i)
+    {
+        pcl::PointXYZ p(i, i * 2, i * 3);
+        cloud1->push_back(p);
+        cloud2->push_back(pcl::PointXYZ(p.x + t.x(), p.y + t.y(), p.z + t.z()));
+    }
+
+    std::vector<int> inliers;
+    cv::Mat covariance;
+
+    Transform result = util3d::transformFromXYZCorrespondences(
+        cloud1, cloud2, 0.1, 100, 0, 1.0, &inliers, &covariance);
+
+    EXPECT_LT(result.getAngle(Transform::getIdentity()), 0.001f);
+    EXPECT_NEAR(result.x(), t.x(), 1e-4f);
+    EXPECT_NEAR(result.y(), t.y(), 1e-4f);
+    EXPECT_NEAR(result.z(), t.z(), 1e-4f);
+    EXPECT_EQ(inliers.size(), 3);
+}
+
+TEST(Util3DRegistration, transformFromXYZCorrespondencesTooFewPoints)
+{
+    auto cloud1 = std::make_shared<pcl::PointCloud<pcl::PointXYZ>>();
+    auto cloud2 = std::make_shared<pcl::PointCloud<pcl::PointXYZ>>();
+
+    cloud1->push_back(pcl::PointXYZ(0, 0, 0));
+    cloud2->push_back(pcl::PointXYZ(1, 1, 1));
+
+    Transform result = util3d::transformFromXYZCorrespondences(
+        cloud1, cloud2, 0.1, 100, 0, 1.0, nullptr, nullptr);
+
+    EXPECT_TRUE(result.isNull());
+}
+
+TEST(Util3DRegistration, transformFromXYZCorrespondencesMismatchedPointCounts)
+{
+    auto cloud1 = std::make_shared<pcl::PointCloud<pcl::PointXYZ>>();
+    auto cloud2 = std::make_shared<pcl::PointCloud<pcl::PointXYZ>>();
+
+    cloud1->push_back(pcl::PointXYZ(0, 0, 0));
+    cloud1->push_back(pcl::PointXYZ(1, 0, 0));
+    cloud1->push_back(pcl::PointXYZ(0, 1, 0));
+
+    cloud2->push_back(pcl::PointXYZ(0, 0, 0)); // Only one point
+
+    Transform result = util3d::transformFromXYZCorrespondences(
+        cloud1, cloud2, 0.1, 100, 0, 1.0, nullptr, nullptr);
+
+    EXPECT_TRUE(result.isNull());
+}
+
+TEST(Util3DRegistration, computeVarianceAndCorrespondencesPerfectMatchNoAngleCheck)
+{
+    auto cloud1 = std::make_shared<pcl::PointCloud<pcl::PointNormal> >();
+    auto cloud2 = std::make_shared<pcl::PointCloud<pcl::PointNormal> >();
+
+    for (int i = 0; i < 5; ++i)
+    {
+        pcl::PointNormal pt;
+        pt.x = i; pt.y = i; pt.z = i;
+        pt.normal_x = 1; pt.normal_y = 0; pt.normal_z = 0;
+        cloud1->push_back(pt);
+        cloud2->push_back(pt); // identical
+    }
+
+    double variance = -1.0;
+    int correspondences = -1;
+    util3d::computeVarianceAndCorrespondences(
+        cloud1, cloud2, 0.1, -1.0, variance, correspondences, true);
+
+    EXPECT_EQ(correspondences, 5);
+    EXPECT_DOUBLE_EQ(variance, 0.0);
+}
+
+TEST(Util3DRegistration, computeVarianceAndCorrespondencesNormalMismatchFilteredByAngle)
+{
+    auto cloud1 = std::make_shared<pcl::PointCloud<pcl::PointNormal> >();
+    auto cloud2 = std::make_shared<pcl::PointCloud<pcl::PointNormal> >();
+
+    for (int i = 0; i < 5; ++i)
+    {
+        pcl::PointNormal a, b;
+        a.x = b.x = i; a.y = b.y = i; a.z = b.z = i;
+
+        a.normal_x = 1; a.normal_y = 0; a.normal_z = 0;
+        b.normal_x = 0; b.normal_y = 1; b.normal_z = 0; // orthogonal normals
+
+        cloud1->push_back(a);
+        cloud2->push_back(b);
+    }
+
+    double variance = -1.0;
+    int correspondences = -1;
+
+    util3d::computeVarianceAndCorrespondences(
+        cloud1, cloud2, 0.1, pcl::deg2rad(45.0), variance, correspondences, true);
+
+    EXPECT_EQ(correspondences, 0);
+    EXPECT_DOUBLE_EQ(variance, 1.0); // untouched default value
+}
+
+TEST(Util3DRegistration, computeVarianceAndCorrespondencesAnglePassWithLargeThreshold)
+{
+    auto cloud1 = std::make_shared<pcl::PointCloud<pcl::PointNormal> >();
+    auto cloud2 = std::make_shared<pcl::PointCloud<pcl::PointNormal> >();
+
+    for (int i = 0; i < 5; ++i)
+    {
+        pcl::PointNormal a, b;
+        a.x = b.x = i; a.y = b.y = i; a.z = b.z = i;
+
+        a.normal_x = 1; a.normal_y = 0; a.normal_z = 0;
+        b.normal_x = 0.7f; b.normal_y = 0.7f; b.normal_z = 0;
+
+        cloud1->push_back(a);
+        cloud2->push_back(b);
+    }
+
+    double variance = -1.0;
+    int correspondences = -1;
+
+    util3d::computeVarianceAndCorrespondences(
+        cloud1, cloud2, 0.1, pcl::deg2rad(90.0), variance, correspondences, true);
+
+    EXPECT_EQ(correspondences, 5);
+    EXPECT_GE(variance, 0.0);
+}
+
+TEST(Util3DRegistration, computeVarianceAndCorrespondencesNoCorrespondencesDueToDistance)
+{
+    auto cloud1 = std::make_shared<pcl::PointCloud<pcl::PointNormal> >();
+    auto cloud2 = std::make_shared<pcl::PointCloud<pcl::PointNormal> >();
+
+    for (int i = 0; i < 5; ++i)
+    {
+        pcl::PointNormal pt1, pt2;
+        pt1.x = pt2.x = i;
+        pt1.y = pt2.y = i;
+        pt1.z = pt2.z = i;
+
+        pt1.normal_x = pt2.normal_x = 1;
+        pt1.normal_y = pt2.normal_y = 0;
+        pt1.normal_z = pt2.normal_z = 0;
+
+        pt2.x += 100; // make them too far apart
+
+        cloud1->push_back(pt1);
+        cloud2->push_back(pt2);
+    }
+
+    double variance = -1.0;
+    int correspondences = -1;
+
+    util3d::computeVarianceAndCorrespondences(
+        cloud1, cloud2, 0.5, 0.0, variance, correspondences, true);
+
+    EXPECT_EQ(correspondences, 0);
+    EXPECT_DOUBLE_EQ(variance, 1.0); // default untouched
+}
+
+TEST(Util3DRegistration, icpIdentityTransformConverges)
+{
+    auto cloud_source = std::make_shared<pcl::PointCloud<pcl::PointXYZ> >();
+    for (float i = 0; i < 5; ++i)
+    {
+        cloud_source->emplace_back(i, i * 2.0f, 0.0f);
+    }
+
+    auto cloud_target = std::make_shared<pcl::PointCloud<pcl::PointXYZ> >(*cloud_source); // identical
+
+    bool hasConverged = false;
+    pcl::PointCloud<pcl::PointXYZ> aligned;
+    Transform result = util3d::icp(
+        cloud_source, cloud_target,
+        0.1,             // max correspondence distance
+        50,              // max iterations
+        hasConverged,
+        aligned,
+        1e-6f,           // epsilon
+        false            // 3D ICP
+    );
+
+    EXPECT_TRUE(hasConverged);
+    // ICP should return identity transform for identical clouds
+    Eigen::Matrix4f identity = Eigen::Matrix4f::Identity();
+    EXPECT_TRUE(result.toEigen4f().isApprox(identity, 1e-4));
+}
+
+TEST(Util3DRegistration, icpTranslatedTransformConverges)
+{
+    auto cloud_source = std::make_shared<pcl::PointCloud<pcl::PointXYZ> >();
+    for (float i = 0; i < 5; ++i)
+    {
+        cloud_source->emplace_back(i, i * 2.0f, 0);
+    }
+
+    // Translate the cloud
+    Transform transformGT(0.025, 0, 0.0f ,0,0,0);
+    auto cloud_target = util3d::transformPointCloud(cloud_source, transformGT);
+
+    bool hasConverged = false;
+    pcl::PointCloud<pcl::PointXYZ> aligned;
+    Transform result = util3d::icp(
+        cloud_source, cloud_target,
+        0.05, 100, hasConverged, aligned,
+        1e-6f,
+        false
+    );
+
+    EXPECT_TRUE(hasConverged);
+
+    std::cout << result << std::endl;
+    std::cout << transformGT << std::endl;
+
+    Eigen::Matrix4f estimated = result.toEigen4f();
+    Eigen::Matrix4f expected = transformGT.toEigen4f();
+    EXPECT_TRUE(estimated.isApprox(expected, 1e-2));
+}
+
+TEST(Util3DRegistration, icp2DAlignsFlatClouds)
+{
+
+    pcl::console::setVerbosityLevel(pcl::console::L_DEBUG);
+    auto cloud_source = std::make_shared<pcl::PointCloud<pcl::PointXYZ>>();
+    for (float x = 0; x < 5; ++x)
+    {
+        for (float y = 0; y < 5; ++y)
+        {
+            if(y == 0 || y == 2 || y == 4 || x==0 || x==2 || x== 4){
+                cloud_source->emplace_back(x*0.05, y*0.05, (int)x%2==0?0.01:-0.01);
+            }
+        }
+    }
+
+    Transform transformGT(0.075, 0.05, 0.01f, 0,0, M_PI / 8);
+
+    auto cloud_target = util3d::transformPointCloud(cloud_source, transformGT);
+
+    bool hasConverged = false;
+    pcl::PointCloud<pcl::PointXYZ> aligned;
+    Transform result = util3d::icp(
+        cloud_source, cloud_target,
+        0.15, 100, hasConverged, aligned,
+        1e-6f,
+        true // use ICP 2D
+    );
+
+    transformGT.z() = 0;
+
+    EXPECT_TRUE(hasConverged);
+    Eigen::Matrix4f estimated = result.toEigen4f();
+    Eigen::Matrix4f expected = transformGT.toEigen4f();
+    EXPECT_TRUE(estimated.isApprox(expected, 1e-4));
+}
+
+TEST(Util3DRegistration, icpPointToPlaneAlignsTranslatedPlane)
+{
+
+
+    // Create a plane point cloud
+    auto cloud_source_raw = std::make_shared<pcl::PointCloud<pcl::PointXYZ> >();
+    for (float x = -0.5f; x <= 0.5f; x += 0.1f)
+    {
+        for (float y = -0.5f; y <= 0.5f; y += 0.1f)
+        {
+            cloud_source_raw->emplace_back(x, y, int(x*10)%2==0&&int(y*10)%2==0?0.01:-0.01);
+        }
+    }
+
+    // Compute normals
+    auto normals = util3d::computeNormals(cloud_source_raw, 20, 0, Eigen::Vector3f(0,0,1));
+    pcl::PointCloud<pcl::PointNormal>::Ptr cloud_source(new pcl::PointCloud<pcl::PointNormal>);
+    pcl::concatenateFields(*cloud_source_raw, *normals, *cloud_source);
+
+    // Apply known transformation
+    Transform gtTransform(0.2f, -0.1f, 0.05f, 0.1f, 0, 0.1f);
+
+    auto cloud_target = util3d::transformPointCloud(cloud_source, gtTransform);
+
+    bool hasConverged = false;
+    pcl::PointCloud<pcl::PointNormal> aligned;
+    Transform estimated = util3d::icpPointToPlane(
+        cloud_source,
+        cloud_target,
+        0.2,   // maxCorrespondenceDistance
+        50,    // iterations
+        hasConverged,
+        aligned,
+        1e-6f, // epsilon
+        false  // icp2D
+    );
+
+    std::cout << gtTransform << std::endl;
+    std::cout << estimated << std::endl;
+
+    EXPECT_TRUE(hasConverged);
+    Eigen::Matrix4f estimatedMatrix = estimated.toEigen4f();
+    Eigen::Matrix4f expectedMatrix = gtTransform.toEigen4f();
+    EXPECT_TRUE(estimatedMatrix.isApprox(expectedMatrix, 1e-4));
+
+    hasConverged = false;
+    estimated = util3d::icpPointToPlane(
+        cloud_source,
+        cloud_target,
+        0.2,   // maxCorrespondenceDistance
+        50,    // iterations
+        hasConverged,
+        aligned,
+        1e-6f, // epsilon
+        true  // icp2D
+    );
+
+    // remove z and roll
+    gtTransform = Transform(0.2f, -0.1f, 0, 0, 0, 0.1f);
+
+    EXPECT_TRUE(hasConverged);
+    estimatedMatrix = estimated.toEigen4f();
+    expectedMatrix = gtTransform.toEigen4f();
+    EXPECT_TRUE(estimatedMatrix.isApprox(expectedMatrix, 1e-4));
+}