From bdd84251ebde00c80eff7d8c7b59e9b80d389613 Mon Sep 17 00:00:00 2001 From: rawang Date: Fri, 26 Sep 2025 11:14:14 +0800 Subject: [PATCH 1/2] Transfer the point cloud to the target coordinate system --- src/pypcd4/pypcd4.py | 14 ++++++++++++++ 1 file changed, 14 insertions(+) diff --git a/src/pypcd4/pypcd4.py b/src/pypcd4/pypcd4.py index 9b9d509..16b6c1c 100644 --- a/src/pypcd4/pypcd4.py +++ b/src/pypcd4/pypcd4.py @@ -838,6 +838,20 @@ def numpy(self, fields: Optional[Sequence[str]] = None) -> npt.NDArray: return np.vstack(_stack).T + def transform(self, transform_matrix: npt.NDArray) -> None: + """Transfer the point cloud to the target coordinate system + + :param transform_matrix: 4*4 matrix + + """ + xyz_pcd = self.numpy(('x', 'y', 'z')) + points_quantic = np.column_stack( + (xyz_pcd, np.ones(xyz_pcd.shape[0]))) + transformed_points = np.dot(transform_matrix, points_quantic.T).T + self.pc_data['x'] = transformed_points[:, 0] + self.pc_data['y'] = transformed_points[:, 1] + self.pc_data['z'] = transformed_points[:, 2] + def _save_as_ascii(self, fp: BinaryIO) -> None: """Saves point cloud to a file as a ascii From 6c2c381b5e1e3b8889f171e8608451d94cb7bd68 Mon Sep 17 00:00:00 2001 From: rawang Date: Fri, 26 Sep 2025 11:28:39 +0800 Subject: [PATCH 2/2] Consider adding a shape check --- src/pypcd4/pypcd4.py | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/pypcd4/pypcd4.py b/src/pypcd4/pypcd4.py index 16b6c1c..b6f4d76 100644 --- a/src/pypcd4/pypcd4.py +++ b/src/pypcd4/pypcd4.py @@ -844,6 +844,8 @@ def transform(self, transform_matrix: npt.NDArray) -> None: :param transform_matrix: 4*4 matrix """ + if transform_matrix.shape != (4, 4): + raise ValueError(f"Expected 4x4 transformation matrix, got {transform_matrix.shape}") xyz_pcd = self.numpy(('x', 'y', 'z')) points_quantic = np.column_stack( (xyz_pcd, np.ones(xyz_pcd.shape[0])))