Merge pull request #8 from VladimirSvoboda/organized_pcd

Fix loading of binary_compressed organized PCD

src/pypcd4/pypcd4.py

@@ -191,10 +191,10 @@ def _parse_pc_data(fp: BinaryIO, metadata: MetaData) -> npt.NDArray:
                 )
 
             offset = 0
-            pc_data = np.zeros(metadata.width, dtype=dtype)
+            pc_data = np.zeros(metadata.points, dtype=dtype)
             for name in dtype.names:  # type: ignore
                 dt: np.dtype = dtype[name]
-                bytes = dt.itemsize * metadata.width
+                bytes = dt.itemsize * metadata.points
                 pc_data[name] = np.frombuffer(buffer[offset : (offset + bytes)], dtype=dt)
                 offset += bytes
     else:

tests/conftest.py

@@ -292,6 +292,10 @@ def xyzrgb_ascii_with_empty_points_path():
     return f"{Path(__file__).resolve().parent}/pcd/ascii_with_empty_points.pcd"
 
 
+@pytest.fixture
+def xyzintensity_ascii_organized_path():
+    return f"{Path(__file__).resolve().parent}/pcd/ascii_organized.pcd"
+
 @pytest.fixture
 def xyzrgb_binary_path():
     return f"{Path(__file__).resolve().parent}/pcd/binary.pcd"
@@ -300,3 +304,7 @@ def xyzrgb_binary_path():
 @pytest.fixture
 def xyzrgb_binary_compressed_path():
     return f"{Path(__file__).resolve().parent}/pcd/binary_compressed.pcd"
+
+@pytest.fixture
+def xyzintensity_binary_compressed_organized_path():
+    return f"{Path(__file__).resolve().parent}/pcd/binary_compressed_organized.pcd"

tests/pcd/ascii_organized.pcd

@@ -0,0 +1,19 @@
+# .PCD v.7 - Point Cloud Data file format
+VERSION .7
+FIELDS x y z intensity
+SIZE 4 4 4 4
+TYPE F F F F
+COUNT 1 1 1 1
+WIDTH 4
+HEIGHT 2
+VIEWPOINT 0 0 0 1 0 0 0
+POINTS 8
+DATA ascii
+0.0 0.0 0.0 0.1
+0.0 0.0 1.0 0.2
+0.0 1.0 0.0 0.3
+0.0 1.0 1.0 0.4
+1.0 0.0 0.0 0.5
+1.0 0.0 1.0 0.6
+1.0 1.0 0.0 0.7
+1.0 1.0 1.0 0.8

tests/test/test_pypcd4.py

@@ -242,6 +242,14 @@ def test_load_xyzrgb_ascii_with_empty_points_pcd(xyzrgb_ascii_with_empty_points_
     assert len(pc.pc_data) == pc.metadata.points
 
 
+def test_load_xyzintensity_ascii_organized_pcd(xyzintensity_ascii_organized_path):
+    pc = PointCloud.from_path(xyzintensity_ascii_organized_path)
+
+    assert pc.metadata.data == "ascii"
+    assert pc.pc_data.dtype.names == pc.metadata.fields
+    assert len(pc.pc_data) == pc.metadata.points
+
+
 def test_load_binary_pcd(xyzrgb_binary_path):
     pc = PointCloud.from_path(xyzrgb_binary_path)
 
@@ -258,6 +266,14 @@ def test_load_binary_compressed_pcd(xyzrgb_binary_compressed_path):
     assert len(pc.pc_data) == pc.metadata.points
 
 
+def test_load_binary_compressed_organized_pcd(xyzintensity_binary_compressed_organized_path):
+    pc = PointCloud.from_path(xyzintensity_binary_compressed_organized_path)
+
+    assert pc.metadata.data == "binary_compressed"
+    assert pc.pc_data.dtype.names == pc.metadata.fields
+    assert len(pc.pc_data) == pc.metadata.points
+
+
 def test_from_points():
     array = np.array([[1, 2, 3], [4, 5, 6]])
     fields = ("x", "y", "z")