From 1702b94f57e3fc5d03508b1139c5c1ab24b4c39a Mon Sep 17 00:00:00 2001
From: matlabbe <matlabbe@gmail.com>
Date: Sat, 26 Sep 2026 13:40:40 -0700
Subject: [PATCH] Fixing roundtrip laserScan <-> Pointcloud2 on all formats.

---
 corelib/src/util3d.cpp       | 51 +++++++++++++++++++++++++++++++++++-
 corelib/test/test_util3d.cpp | 41 +++++++++++++++++++++++++++++
 2 files changed, 91 insertions(+), 1 deletion(-)

diff --git a/corelib/src/util3d.cpp b/corelib/src/util3d.cpp
index 757fb3e8..895e158a 100644
--- a/corelib/src/util3d.cpp
+++ b/corelib/src/util3d.cpp
@@ -2328,10 +2328,59 @@ pcl::PCLPointCloud2::Ptr laserScanToPointCloud2(const LaserScan & laserScan, con
 	{
 		pcl::toPCLPointCloud2(*laserScanToPointCloud(laserScan, transform), *cloud);
 	}
-	else if(laserScan.format() == LaserScan::kXYI || laserScan.format() == LaserScan::kXYZI || laserScan.format() == LaserScan::kXYZIT || laserScan.format() == LaserScan::kXYZIRT)
+	else if(laserScan.format() == LaserScan::kXYI || laserScan.format() == LaserScan::kXYZI)
 	{
 		pcl::toPCLPointCloud2(*laserScanToPointCloudI(laserScan, transform), *cloud);
 	}
+	else if(laserScan.format() == LaserScan::kXYZIT || laserScan.format() == LaserScan::kXYZIRT)
+	{
+		// PCL has no point type with time (and ring): append them to the XYZI fields, with
+		// the types laserScanFromPointCloud() reads back (time FLOAT32, ring UINT16).
+		pcl::PCLPointCloud2 xyzi;
+		pcl::toPCLPointCloud2(*laserScanToPointCloudI(laserScan, transform), xyzi);
+		const bool hasRing = laserScan.format() == LaserScan::kXYZIRT;
+
+		cloud->header = xyzi.header;
+		cloud->height = xyzi.height;
+		cloud->width = xyzi.width;
+		cloud->is_bigendian = xyzi.is_bigendian;
+		cloud->is_dense = xyzi.is_dense;
+		cloud->fields = xyzi.fields;
+		pcl::PCLPointField time;
+		time.name = "time";
+		time.offset = xyzi.point_step;
+		time.datatype = pcl::PCLPointField::FLOAT32;
+		time.count = 1;
+		cloud->fields.push_back(time);
+		cloud->point_step = xyzi.point_step + 4;
+		pcl::PCLPointField ring;
+		if(hasRing)
+		{
+			ring.name = "ring";
+			ring.offset = cloud->point_step;
+			ring.datatype = pcl::PCLPointField::UINT16;
+			ring.count = 1;
+			cloud->fields.push_back(ring);
+			cloud->point_step += 4; // keep points 4-byte aligned
+		}
+		cloud->row_step = cloud->point_step * cloud->width;
+		cloud->data.resize(size_t(cloud->row_step) * cloud->height, 0);
+
+		const int cols = laserScan.data().cols;
+		const size_t points = size_t(cloud->width) * cloud->height;
+		for(size_t i=0; i<points; ++i)
+		{
+			unsigned char * dst = &cloud->data[i * cloud->point_step];
+			memcpy(dst, &xyzi.data[i * xyzi.point_step], xyzi.point_step);
+			const float * src = laserScan.data().ptr<float>(int(i) / cols, int(i) % cols);
+			memcpy(dst + time.offset, src + laserScan.getTimeOffset(), sizeof(float));
+			if(hasRing)
+			{
+				const std::uint16_t r = (std::uint16_t)src[laserScan.getRingOffset()];
+				memcpy(dst + ring.offset, &r, sizeof(r));
+			}
+		}
+	}
 	else if(laserScan.format() == LaserScan::kXYNormal || laserScan.format() == LaserScan::kXYZNormal)
 	{
 		pcl::toPCLPointCloud2(*laserScanToPointCloudNormal(laserScan, transform), *cloud);
diff --git a/corelib/test/test_util3d.cpp b/corelib/test/test_util3d.cpp
index 9c3aeb78..de36a4e7 100644
--- a/corelib/test/test_util3d.cpp
+++ b/corelib/test/test_util3d.cpp
@@ -1024,6 +1024,47 @@ TEST(Util3dTest, LaserScanFromPointCloudXYZINormal) {
     EXPECT_FLOAT_EQ(pt.normal_z, 1.0f);
 }
 
+// laserScanToPointCloud2() and laserScanFromPointCloud() are each other's inverse, in
+// every format: the per-point time and ring included, which no PCL point type holds, and
+// 2D scans, which come back 2D when is2D is set.
+TEST(Util3dTest, LaserScanPointCloud2RoundTripEveryFormat) {
+    for(int f = LaserScan::kXY; f <= LaserScan::kXYZIRT; ++f)
+    {
+        const LaserScan::Format format = (LaserScan::Format)f;
+        SCOPED_TRACE(LaserScan::formatName(format));
+        const int channels = LaserScan::channels(format);
+
+        // Small integers: exact through float and through the ring's UINT16 field.
+        cv::Mat points(1, 3, CV_32FC(channels));
+        for(int i = 0; i < points.cols; ++i)
+        {
+            float * p = points.ptr<float>(0, i);
+            for(int c = 0; c < channels; ++c)
+            {
+                p[c] = float(1 + i + c);
+            }
+        }
+        const LaserScan scan(points, 360, 10.0f, format);
+        if(scan.hasRGB())
+        {
+            for(int i = 0; i < points.cols; ++i)
+            {
+                const uint32_t rgb = 0x00102030u + i; // packed 0x00RRGGBB, as PCL stores it
+                memcpy(points.ptr<float>(0, i) + scan.getRGBOffset(), &rgb, sizeof(float));
+            }
+        }
+
+        pcl::PCLPointCloud2::Ptr cloud = util3d::laserScanToPointCloud2(scan);
+        ASSERT_EQ(cloud->width * cloud->height, (unsigned int)points.cols);
+        const LaserScan out = util3d::laserScanFromPointCloud(*cloud, true, scan.is2d());
+
+        EXPECT_EQ(out.format(), format);
+        ASSERT_EQ(out.data().size(), scan.data().size());
+        ASSERT_EQ(out.data().type(), scan.data().type());
+        EXPECT_EQ(0, memcmp(out.data().data, scan.data().data, scan.data().total() * scan.data().elemSize()));
+    }
+}
+
 TEST(Util3dTest, LaserScan2dFromPointCloudXYZ) {
     pcl::PointCloud<pcl::PointXYZ> cloud;
     cloud.push_back(pcl::PointXYZ(1.0f, 2.0f, 3.0f));