util3d_transforms.h: Added doc and tests

corelib/include/rtabmap/core/util3d_transforms.h

@@ -42,75 +42,207 @@ namespace rtabmap
 namespace util3d
 {
 
+/**
+ * @brief Applies a 3D transform to all points (and normals if present) in a LaserScan.
+ *
+ * This function transforms each point in the input `LaserScan` using the specified `Transform`.
+ * The transformation is applied on a cloned copy of the scan data, preserving the original.
+ * It supports both 2D and 3D scans, and if normals are present, they are also properly transformed.
+ *
+ * The transformation is only applied if it is neither null nor the identity transform.
+ *
+ * @param laserScan The input `LaserScan` object containing scan data to be transformed.
+ * @param transform A `Transform` representing the spatial transformation to apply (translation + rotation).
+ *                  Can be 3DoF or 6DoF depending on the scan dimensionality.
+ *
+ * @return A new `LaserScan` object with transformed points (and optionally normals), and the same metadata
+ *         such as range limits, angle information, format, and local transform as the input scan.
+ *
+ * ### Behavior:
+ * - If the transform is null or identity, the scan is returned unchanged.
+ * - If the scan has normals (e.g., format is `kXYZNormal`, `kXYZINormal`, etc.), both positions and normals are transformed.
+ * - If the scan has no normals, only point positions are transformed.
+ * - Angle-based scans (2D with valid angle increment) retain angular properties in the returned object.
+ * - The `localTransform` of the original scan is preserved in the returned scan.
+ *
+ * ### Supported Formats:
+ * This function works with all valid formats defined by `LaserScan::Format`, including:
+ * - 2D: `kXY`, `kXYI`, `kXYNormal`, `kXYINormal`
+ * - 3D: `kXYZ`, `kXYZI`, `kXYZNormal`, `kXYZINormal`, `kXYZRGB`, `kXYZRGBNormal`, `kXYZIT`
+ *
+ * @see LaserScan, Transform, util3d::transformPoint()
+ */
 LaserScan RTABMAP_CORE_EXPORT transformLaserScan(
 		const LaserScan & laserScan,
 		const Transform & transform);
 
+/**
+ * @defgroup TransformPointcloud Transform PCL Point Clouds
+ * @brief Transforms various PCL point cloud types using the given transform.
+ * 
+ * - For types with normals, the normals are transformed appriopriatly.
+ * - If indices are provided, the returned point cloud contains only points matching the indices.
+ * 
+ * @param cloud The input point cloud.
+ * @param indices Optional subset of indices of the points to transform.
+ * @param transform The transform to apply.
+ * @return A new point cloud with the transform applied.
+ */
+/**
+ * @ingroup TransformPointcloud
+ * @brief Transforms `pcl::PointXYZ` point cloud type.
+ */
 pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_CORE_EXPORT transformPointCloud(
 		const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
 		const Transform & transform);
+/**
+ * @ingroup TransformPointcloud
+ * @brief Transforms `pcl::PointXYZI` point cloud type.
+ */
 pcl::PointCloud<pcl::PointXYZI>::Ptr RTABMAP_CORE_EXPORT transformPointCloud(
 		const pcl::PointCloud<pcl::PointXYZI>::Ptr & cloud,
 		const Transform & transform);
+/**
+ * @ingroup TransformPointcloud
+ * @brief Transforms `pcl::PointXYZRGB` point cloud type.
+ */
 pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_CORE_EXPORT transformPointCloud(
 		const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
 		const Transform & transform);
+/**
+ * @ingroup TransformPointcloud
+ * @brief Transforms `pcl::PointNormal` point cloud type.
+ */
 pcl::PointCloud<pcl::PointNormal>::Ptr RTABMAP_CORE_EXPORT transformPointCloud(
 		const pcl::PointCloud<pcl::PointNormal>::Ptr & cloud,
 		const Transform & transform);
+/**
+ * @ingroup TransformPointcloud
+ * @brief Transforms `pcl::PointXYZRGBNormal` point cloud type.
+ */
 pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr RTABMAP_CORE_EXPORT transformPointCloud(
 		const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud,
 		const Transform & transform);
+/**
+ * @ingroup TransformPointcloud
+ * @brief Transforms `pcl::PointXYZINormal` point cloud type.
+ */
 pcl::PointCloud<pcl::PointXYZINormal>::Ptr RTABMAP_CORE_EXPORT transformPointCloud(
 		const pcl::PointCloud<pcl::PointXYZINormal>::Ptr & cloud,
 		const Transform & transform);
 
+/**
+ * @ingroup TransformPointcloud
+ * @brief Transforms `pcl::PointXYZ` point cloud type with specified indices.
+ */
 pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_CORE_EXPORT transformPointCloud(
 		const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
 		const pcl::IndicesPtr & indices,
 		const Transform & transform);
+/**
+ * @ingroup TransformPointcloud
+ * @brief Transforms `pcl::PointXYZI` point cloud type with specified indices.
+ */
 pcl::PointCloud<pcl::PointXYZI>::Ptr RTABMAP_CORE_EXPORT transformPointCloud(
 		const pcl::PointCloud<pcl::PointXYZI>::Ptr & cloud,
 		const pcl::IndicesPtr & indices,
 		const Transform & transform);
+/**
+ * @ingroup TransformPointcloud
+ * @brief Transforms `pcl::PointXYZRGB` point cloud type with specified indices.
+ */
 pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_CORE_EXPORT transformPointCloud(
 		const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
 		const pcl::IndicesPtr & indices,
 		const Transform & transform);
+/**
+ * @ingroup TransformPointcloud
+ * @brief Transforms `pcl::PointNormal` point cloud type with specified indices.
+ */
 pcl::PointCloud<pcl::PointNormal>::Ptr RTABMAP_CORE_EXPORT transformPointCloud(
 		const pcl::PointCloud<pcl::PointNormal>::Ptr & cloud,
 		const pcl::IndicesPtr & indices,
 		const Transform & transform);
+/**
+ * @ingroup TransformPointcloud
+ * @brief Transforms `pcl::PointXYZRGBNormal` point cloud type with specified indices.
+ */
 pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr RTABMAP_CORE_EXPORT transformPointCloud(
 		const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud,
 		const pcl::IndicesPtr & indices,
 		const Transform & transform);
+/**
+ * @ingroup TransformPointcloud
+ * @brief Transforms `pcl::PointXYZINormal` point cloud type with specified indices.
+ */
 pcl::PointCloud<pcl::PointXYZINormal>::Ptr RTABMAP_CORE_EXPORT transformPointCloud(
 		const pcl::PointCloud<pcl::PointXYZINormal>::Ptr & cloud,
 		const pcl::IndicesPtr & indices,
 		const Transform & transform);
 
+/**
+ * @defgroup TransformPoint Transform a single point
+ * @brief Transforms various point types using the given transform.
+ * 
+ * - For types with normal, the normal is transformed appriopriatly.
+ * 
+ * @param pt The input point.
+ * @param transform The transform to apply.
+ * @return A new point with the transform applied.
+ */
+/**
+ * @ingroup TransformPoint
+ * @brief Transforms `cv::Point3f` point type.
+ */
 cv::Point3f RTABMAP_CORE_EXPORT transformPoint(
 		const cv::Point3f & pt,
 		const Transform & transform);
+/**
+ * @ingroup TransformPoint
+ * @brief Transforms `cv::Point3d` point type.
+ */
 cv::Point3d RTABMAP_CORE_EXPORT transformPoint(
 		const cv::Point3d & pt,
 		const Transform & transform);
+/**
+ * @ingroup TransformPoint
+ * @brief Transforms `pcl::PointXYZ` point type.
+ */
 pcl::PointXYZ RTABMAP_CORE_EXPORT transformPoint(
 		const pcl::PointXYZ & pt,
 		const Transform & transform);
+/**
+ * @ingroup TransformPoint
+ * @brief Transforms `pcl::PointXYZI` point type.
+ */
 pcl::PointXYZI RTABMAP_CORE_EXPORT transformPoint(
 		const pcl::PointXYZI & pt,
 		const Transform & transform);
+/**
+ * @ingroup TransformPoint
+ * @brief Transforms `pcl::PointXYZRGB` point type.
+ */
 pcl::PointXYZRGB RTABMAP_CORE_EXPORT transformPoint(
 		const pcl::PointXYZRGB & pt,
 		const Transform & transform);
+/**
+ * @ingroup TransformPoint
+ * @brief Transforms `pcl::PointNormal` point type.
+ */
 pcl::PointNormal RTABMAP_CORE_EXPORT transformPoint(
 		const pcl::PointNormal & point,
 		const Transform & transform);
+/**
+ * @ingroup TransformPoint
+ * @brief Transforms `pcl::PointXYZRGBNormal` point type.
+ */
 pcl::PointXYZRGBNormal RTABMAP_CORE_EXPORT transformPoint(
 		const pcl::PointXYZRGBNormal & point,
 		const Transform & transform);
+/**
+ * @ingroup TransformPoint
+ * @brief Transforms `pcl::PointXYZINormal` point type.
+ */
 pcl::PointXYZINormal RTABMAP_CORE_EXPORT transformPoint(
 		const pcl::PointXYZINormal & point,
 		const Transform & transform);

corelib/test/CMakeLists.txt

@@ -9,4 +9,9 @@ gtest_discover_tests(test_util2d)
 #util3d.h
 add_executable(test_util3d test_util3d.cpp)
 target_link_libraries(test_util3d gtest_main rtabmap_core)
-gtest_discover_tests(test_util3d)
+gtest_discover_tests(test_util3d)
+
+#util3d_transforms.h
+add_executable(test_util3d_transforms test_util3d_transforms.cpp)
+target_link_libraries(test_util3d_transforms gtest_main rtabmap_core)
+gtest_discover_tests(test_util3d_transforms)

corelib/test/test_util3d_transforms.cpp

@@ -0,0 +1,376 @@
+#include "gtest/gtest.h"
+#include "rtabmap/core/util3d_transforms.h"
+#include "rtabmap/utilite/UException.h"
+
+using namespace rtabmap;
+
+TEST(Util3DTransforms, transformLaserScanBasicXYZTransform)
+{
+    // Create a simple 3D laser scan with 3 points
+    cv::Mat scanData(1, 3, CV_32FC3); // 3D points (X,Y,Z)
+    scanData.at<cv::Vec3f>(0,0) = cv::Vec3f(1.0f, 0.0f, 0.0f);
+    scanData.at<cv::Vec3f>(0,1) = cv::Vec3f(0.0f, 1.0f, 0.0f);
+    scanData.at<cv::Vec3f>(0,2) = cv::Vec3f(0.0f, 0.0f, 1.0f);
+
+    LaserScan scan(scanData, 0, 10.0f, LaserScan::kXYZ, Transform::getIdentity());
+
+    // Create a transform: translate (1, 2, 3) and rotate 90 degrees about Z
+    Transform transform(0, -1, 0, 1,
+                        1,  0, 0, 2,
+                        0,  0, 1, 3);
+
+    // Apply transform
+    LaserScan result = util3d::transformLaserScan(scan, transform);
+
+    // Expected points after transformation
+    std::vector<Eigen::Vector3f> expectedPoints = {
+        Eigen::Vector3f(1.0f, 3.0f, 3.0f), // Rotated (1,0,0) + translated
+        Eigen::Vector3f(0.0f, 2.0f, 3.0f), // Rotated (0,1,0) + translated
+        Eigen::Vector3f(1.0f, 2.0f, 4.0f)  // Rotated (0,0,1) + translated
+    };
+
+    ASSERT_EQ(result.size(), 3);
+    for (int i = 0; i < 3; ++i)
+    {
+        EXPECT_NEAR(result.field(i, 0), expectedPoints[i].x(), 1e-5f);
+        EXPECT_NEAR(result.field(i, 1), expectedPoints[i].y(), 1e-5f);
+        EXPECT_NEAR(result.field(i, 2), expectedPoints[i].z(), 1e-5f);
+    }
+}
+
+TEST(Util3DTransforms, transformLaserScanXYZNormalTransform)
+{
+    // One point: position (1, 0, 0), normal (0, 1, 0)
+    cv::Mat scanData(1, 1, CV_32FC(6)); // XYZ + normal_x, normal_y, normal_z
+    float* ptr = scanData.ptr<float>(0, 0);
+    ptr[0] = 1.0f; // x
+    ptr[1] = 0.0f; // y
+    ptr[2] = 0.0f; // z
+    ptr[3] = 0.0f; // normal_x
+    ptr[4] = 1.0f; // normal_y
+    ptr[5] = 0.0f; // normal_z
+
+    LaserScan scan(scanData, 0, 10.0f, LaserScan::kXYZNormal, Transform::getIdentity());
+
+    // Transform: rotate 90 deg around Z axis + translate (0, 1, 0)
+    Transform transform(0, -1, 0, 1,
+                        1,  0, 0, 2,
+                        0,  0, 1, 3);
+
+    // Apply transform
+    LaserScan result = util3d::transformLaserScan(scan, transform);
+
+    ASSERT_EQ(result.size(), 1);
+    const float* out = result.data().ptr<float>(0, 0);
+
+    // Expected:
+    // Point (1,0,0) → rotated to (0,1,0) + translated → (1,2,3)
+    // Normal (0,1,0) → rotated to (-1,0,0) (normals are rotated, not translated)
+    EXPECT_NEAR(out[0], 1.0f, 1e-5);  // x
+    EXPECT_NEAR(out[1], 3.0f, 1e-5);  // y
+    EXPECT_NEAR(out[2], 3.0f, 1e-5);  // z
+
+    EXPECT_NEAR(out[3], -1.0f, 1e-5); // normal_x
+    EXPECT_NEAR(out[4],  0.0f, 1e-5); // normal_y
+    EXPECT_NEAR(out[5],  0.0f, 1e-5); // normal_z
+}
+
+TEST(Util3DTransforms, transformPointCloudXYZ)
+{
+    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
+    pcl::PointXYZ pt(1.0f, 2.0f, 3.0f);
+    cloud->push_back(pt);
+
+    Transform tf(1, 0, 0, 1,
+                 0, 1, 0, 2,
+                 0, 0, 1, 3); // Translate (1,2,3)
+
+    auto result = util3d::transformPointCloud(cloud, tf);
+
+    ASSERT_EQ(result->size(), 1);
+    EXPECT_FLOAT_EQ(result->at(0).x, 2.0f);
+    EXPECT_FLOAT_EQ(result->at(0).y, 4.0f);
+    EXPECT_FLOAT_EQ(result->at(0).z, 6.0f);
+
+    // test indices
+    pcl::PointXYZ pt2(4.0f, 5.0f, 6.0f);
+    cloud->push_back(pt2);
+    pcl::IndicesPtr indices(new pcl::Indices(1,1)); // will transform only second point
+    result = util3d::transformPointCloud(cloud, indices, tf);
+    ASSERT_EQ(result->size(), 1);
+    EXPECT_FLOAT_EQ(result->at(0).x, 5.0f);
+    EXPECT_FLOAT_EQ(result->at(0).y, 7.0f);
+    EXPECT_FLOAT_EQ(result->at(0).z, 9.0f);
+}
+
+TEST(Util3DTransforms, transformPointCloudXYZI)
+{
+    pcl::PointCloud<pcl::PointXYZI>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZI>);
+    pcl::PointXYZI pt;
+    pt.x = 1; pt.y = 1; pt.z = 1; pt.intensity = 42.0f;
+    cloud->push_back(pt);
+
+    Transform tf(1, 0, 0, 0,
+                 0, 1, 0, 0,
+                 0, 0, 1, 1); // Translate (0,0,1)
+
+    auto result = util3d::transformPointCloud(cloud, tf);
+    ASSERT_EQ(result->size(), 1);
+    EXPECT_FLOAT_EQ(result->at(0).z, 2.0f);
+    EXPECT_FLOAT_EQ(result->at(0).intensity, 42.0f);
+    
+    // test indices
+    pt.intensity = 52.0f;
+    cloud->push_back(pt);
+    pcl::IndicesPtr indices(new pcl::Indices(1,1)); // will transform only second point
+    result = util3d::transformPointCloud(cloud, indices, tf);
+    ASSERT_EQ(result->size(), 1);
+    EXPECT_FLOAT_EQ(result->at(0).intensity, 52.0f);
+}
+
+TEST(Util3DTransforms, transformPointCloudXYZRGB)
+{
+    pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
+    pcl::PointXYZRGB pt;
+    pt.x = 0; pt.y = 0; pt.z = 0;
+    pt.r = 255; pt.g = 100; pt.b = 50;
+    cloud->push_back(pt);
+
+    Transform tf(1, 0, 0, 0,
+                 0, 1, 0, 0,
+                 0, 0, 1, 0);
+
+    auto result = util3d::transformPointCloud(cloud, tf);
+    ASSERT_EQ(result->size(), 1);
+    EXPECT_EQ(result->at(0).r, 255);
+    EXPECT_EQ(result->at(0).g, 100);
+    EXPECT_EQ(result->at(0).b, 50);
+
+    // test indices
+    pt.r = 250; pt.g = 105; pt.b = 55;
+    cloud->push_back(pt);
+    pcl::IndicesPtr indices(new pcl::Indices(1,1)); // will transform only second point
+    result = util3d::transformPointCloud(cloud, indices, tf);
+    ASSERT_EQ(result->size(), 1);
+    EXPECT_EQ(result->at(0).r, 250);
+    EXPECT_EQ(result->at(0).g, 105);
+    EXPECT_EQ(result->at(0).b, 55);
+}
+
+TEST(Util3DTransforms, transformPointCloudPointNormal)
+{
+    pcl::PointCloud<pcl::PointNormal>::Ptr cloud(new pcl::PointCloud<pcl::PointNormal>);
+    pcl::PointNormal pt;
+    pt.x = 1; pt.y = 0; pt.z = 0;
+    pt.normal_x = 0; pt.normal_y = 1; pt.normal_z = 0;
+    cloud->push_back(pt);
+
+    Transform tf(0, -1, 0, 0,
+                 1,  0, 0, 0,
+                 0,  0, 1, 0); // 90 deg rotation around Z
+
+    auto result = util3d::transformPointCloud(cloud, tf);
+    ASSERT_EQ(result->size(), 1);
+    EXPECT_NEAR(result->at(0).x, 0.0f, 1e-6);
+    EXPECT_NEAR(result->at(0).y, 1.0f, 1e-6);
+    EXPECT_NEAR(result->at(0).normal_x, -1.0f, 1e-6);
+    EXPECT_NEAR(result->at(0).normal_y, 0.0f, 1e-6);
+
+    // test indices
+    pt.x = 0; pt.y = 1; pt.z = 0;
+    pt.normal_x = 1; pt.normal_y = 0; pt.normal_z = 0;
+    cloud->push_back(pt);
+    pcl::IndicesPtr indices(new pcl::Indices(1,1)); // will transform only second point
+    result = util3d::transformPointCloud(cloud, indices, tf);
+    ASSERT_EQ(result->size(), 1);
+    EXPECT_NEAR(result->at(0).x, -1.0f, 1e-6);
+    EXPECT_NEAR(result->at(0).y, 0.0f, 1e-6);
+    EXPECT_NEAR(result->at(0).normal_x, 0.0f, 1e-6);
+    EXPECT_NEAR(result->at(0).normal_y, 1.0f, 1e-6);
+}
+
+TEST(Util3DTransforms, transformPointCloudXYZINormal)
+{
+    pcl::PointCloud<pcl::PointXYZINormal>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZINormal>);
+    pcl::PointXYZINormal pt;
+    pt.x = 0; pt.y = 1; pt.z = 0; pt.intensity = 42.0f;
+    pt.normal_x = 1; pt.normal_y = 0; pt.normal_z = 0;
+    cloud->push_back(pt);
+
+    Transform tf(0, -1, 0, 0,
+                 1,  0, 0, 0,
+                 0,  0, 1, 0);
+
+    auto result = util3d::transformPointCloud(cloud, tf);
+    ASSERT_EQ(result->size(), 1);
+    EXPECT_FLOAT_EQ(result->at(0).intensity, 42.0f);
+    EXPECT_NEAR(result->at(0).normal_x, 0.0f, 1e-6);
+    EXPECT_NEAR(result->at(0).normal_y, 1.0f, 1e-6);
+
+    // test indices
+    pt.intensity = 52.0f;
+    cloud->push_back(pt);
+    pcl::IndicesPtr indices(new pcl::Indices(1,1)); // will transform only second point
+    result = util3d::transformPointCloud(cloud, indices, tf);
+    ASSERT_EQ(result->size(), 1);
+    EXPECT_FLOAT_EQ(result->at(0).intensity, 52.0f);
+}
+
+TEST(Util3DTransforms, transformPointCloudXYZRGBNormal)
+{
+    pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGBNormal>);
+    pcl::PointXYZRGBNormal pt;
+    pt.x = 0; pt.y = 1; pt.z = 0;
+    pt.r = 10; pt.g = 20; pt.b = 30;
+    pt.normal_x = 1; pt.normal_y = 0; pt.normal_z = 0;
+    cloud->push_back(pt);
+
+    Transform tf(0, -1, 0, 0,
+                 1,  0, 0, 0,
+                 0,  0, 1, 0);
+
+    auto result = util3d::transformPointCloud(cloud, tf);
+    ASSERT_EQ(result->size(), 1);
+    EXPECT_EQ(result->at(0).r, 10);
+    EXPECT_EQ(result->at(0).g, 20);
+    EXPECT_EQ(result->at(0).b, 30);
+    EXPECT_NEAR(result->at(0).normal_x, 0.0f, 1e-6);
+    EXPECT_NEAR(result->at(0).normal_y, 1.0f, 1e-6);
+
+    // test indices
+    pt.r = 250; pt.g = 105; pt.b = 55;
+    cloud->push_back(pt);
+    pcl::IndicesPtr indices(new pcl::Indices(1,1)); // will transform only second point
+    result = util3d::transformPointCloud(cloud, indices, tf);
+    ASSERT_EQ(result->size(), 1);
+    EXPECT_EQ(result->at(0).r, 250);
+    EXPECT_EQ(result->at(0).g, 105);
+    EXPECT_EQ(result->at(0).b, 55);
+}
+
+TEST(Util3DTransforms, transformCVPointF)
+{
+    cv::Point3f pt(1.0f, 2.0f, 3.0f);
+
+    Transform tf(1, 0, 0, 1,
+                 0, 1, 0, 2,
+                 0, 0, 1, 3); // Translate (1,2,3)
+
+    auto result = util3d::transformPoint(pt, tf);
+
+    EXPECT_FLOAT_EQ(result.x, 2.0f);
+    EXPECT_FLOAT_EQ(result.y, 4.0f);
+    EXPECT_FLOAT_EQ(result.z, 6.0f);
+}
+
+TEST(Util3DTransforms, transformCVPointD)
+{
+    cv::Point3d pt(1.0f, 2.0f, 3.0f);
+
+    Transform tf(1, 0, 0, 1,
+                 0, 1, 0, 2,
+                 0, 0, 1, 3); // Translate (1,2,3)
+
+    auto result = util3d::transformPoint(pt, tf);
+
+    EXPECT_FLOAT_EQ(result.x, 2.0);
+    EXPECT_FLOAT_EQ(result.y, 4.0);
+    EXPECT_FLOAT_EQ(result.z, 6.0);
+}
+
+TEST(Util3DTransforms, transformPointXYZ)
+{
+    pcl::PointXYZ pt(1.0f, 2.0f, 3.0f);
+
+    Transform tf(1, 0, 0, 1,
+                 0, 1, 0, 2,
+                 0, 0, 1, 3); // Translate (1,2,3)
+
+    auto result = util3d::transformPoint(pt, tf);
+
+    EXPECT_FLOAT_EQ(result.x, 2.0f);
+    EXPECT_FLOAT_EQ(result.y, 4.0f);
+    EXPECT_FLOAT_EQ(result.z, 6.0f);
+}
+
+TEST(Util3DTransforms, transformPointXYZI)
+{
+    pcl::PointXYZI pt;
+    pt.x = 1; pt.y = 1; pt.z = 1; pt.intensity = 42.0f;
+
+    Transform tf(1, 0, 0, 0,
+                 0, 1, 0, 0,
+                 0, 0, 1, 1); // Translate (0,0,1)
+
+    auto result = util3d::transformPoint(pt, tf);
+    EXPECT_FLOAT_EQ(result.z, 2.0f);
+    EXPECT_FLOAT_EQ(result.intensity, 42.0f);
+}
+
+TEST(Util3DTransforms, transformPointXYZRGB)
+{
+    pcl::PointXYZRGB pt;
+    pt.x = 0; pt.y = 0; pt.z = 0;
+    pt.r = 255; pt.g = 100; pt.b = 50;
+
+    Transform tf(1, 0, 0, 0,
+                 0, 1, 0, 0,
+                 0, 0, 1, 0);
+
+    auto result = util3d::transformPoint(pt, tf);
+    EXPECT_EQ(result.r, 255);
+    EXPECT_EQ(result.g, 100);
+    EXPECT_EQ(result.b, 50);
+}
+
+TEST(Util3DTransforms, transformPointPointNormal)
+{
+    pcl::PointNormal pt;
+    pt.x = 1; pt.y = 0; pt.z = 0;
+    pt.normal_x = 0; pt.normal_y = 1; pt.normal_z = 0;
+
+    Transform tf(0, -1, 0, 0,
+                 1,  0, 0, 0,
+                 0,  0, 1, 0); // 90 deg rotation around Z
+
+    auto result = util3d::transformPoint(pt, tf);
+    EXPECT_NEAR(result.x, 0.0f, 1e-6);
+    EXPECT_NEAR(result.y, 1.0f, 1e-6);
+    EXPECT_NEAR(result.normal_x, -1.0f, 1e-6);
+    EXPECT_NEAR(result.normal_y, 0.0f, 1e-6);
+}
+
+TEST(Util3DTransforms, transformPointXYZINormal)
+{
+    pcl::PointXYZINormal pt;
+    pt.x = 0; pt.y = 1; pt.z = 0; pt.intensity = 42.0f;
+    pt.normal_x = 1; pt.normal_y = 0; pt.normal_z = 0;
+
+    Transform tf(0, -1, 0, 0,
+                 1,  0, 0, 0,
+                 0,  0, 1, 0);
+
+    auto result = util3d::transformPoint(pt, tf);
+    EXPECT_FLOAT_EQ(result.intensity, 42.0f);
+    EXPECT_NEAR(result.normal_x, 0.0f, 1e-6);
+    EXPECT_NEAR(result.normal_y, 1.0f, 1e-6);
+}
+
+TEST(Util3DTransforms, transformPointXYZRGBNormal)
+{
+    pcl::PointXYZRGBNormal pt;
+    pt.x = 0; pt.y = 1; pt.z = 0;
+    pt.r = 10; pt.g = 20; pt.b = 30;
+    pt.normal_x = 1; pt.normal_y = 0; pt.normal_z = 0;
+
+    Transform tf(0, -1, 0, 0,
+                 1,  0, 0, 0,
+                 0,  0, 1, 0);
+
+    auto result = util3d::transformPoint(pt, tf);
+    EXPECT_EQ(result.r, 10);
+    EXPECT_EQ(result.g, 20);
+    EXPECT_EQ(result.b, 30);
+    EXPECT_NEAR(result.normal_x, 0.0f, 1e-6);
+    EXPECT_NEAR(result.normal_y, 1.0f, 1e-6);
+}