From 838539fd4a75e24d625331189cc6e7b38222a756 Mon Sep 17 00:00:00 2001 From: Yifu Tao Date: Wed, 11 Sep 2024 16:35:19 +0100 Subject: [PATCH] feat: add script to transform a folder of pcds --- scripts/transform_pcd_folder.py | 48 +++++++++++++++++++++++++++++++++ 1 file changed, 48 insertions(+) create mode 100644 scripts/transform_pcd_folder.py diff --git a/scripts/transform_pcd_folder.py b/scripts/transform_pcd_folder.py new file mode 100644 index 0000000..68d0e67 --- /dev/null +++ b/scripts/transform_pcd_folder.py @@ -0,0 +1,48 @@ +from pathlib import Path + +import numpy as np +import open3d as o3d +from pypcd4 import PointCloud + +from oxford_spires_utils.point_cloud import transform_3d_cloud +from oxford_spires_utils.se3 import se3_matrix_to_xyz_quat_wxyz, xyz_quat_wxyz_to_se3_matrix + + +def transform_pcd_folder(folder_path, new_folder_path, transform_matrix): + """Process all PCD files in the given folder with the predefined transform.""" + pcd_files = sorted(list(Path(folder_path).rglob("*.pcd"))) + new_folder_path.mkdir(exist_ok=True) + for filename in pcd_files: + pc = PointCloud.from_path(filename) + viewpoint = list(pc.metadata.viewpoint) + viewpoint_se3 = xyz_quat_wxyz_to_se3_matrix(viewpoint[:3], viewpoint[3:]) + new_viewpoint_se3 = transform_matrix @ viewpoint_se3 + new_viewpoint = se3_matrix_to_xyz_quat_wxyz(new_viewpoint_se3) + new_viewpoint = np.concatenate((new_viewpoint[0], new_viewpoint[1])) + new_viewpoint = new_viewpoint.tolist() + + points = pc.numpy(("x", "y", "z")) + transformed_points = transform_3d_cloud(points, transform_matrix) + + pc = o3d.io.read_point_cloud(str(filename)) + assert np.allclose(np.asarray(pc.points), points) + assert np.allclose(np.asarray(pc.transform(transform_matrix).points), transformed_points) + # colour = np.asarray(pc.colors) + # colour_encoded = PointCloud.encode_rgb(colour) + # colour_encoded = colour_encoded.reshape(-1, 1) + # points = np.hstack((points, colour_encoded)) + + saved_pcd4 = PointCloud.from_xyz_points(points) + saved_pcd4.metadata.viewpoint = new_viewpoint + + new_filename = new_folder_path / filename.name + saved_pcd4.save(new_filename) + + +if __name__ == "__main__": + pcd_folder_path = "/home/yifu/workspace/Spires_2025/2024-03-13-maths_1/input_cloud_test" + new_pcd_folder_path = Path(pcd_folder_path).parent / (Path(pcd_folder_path).name + "_new") + + transform_matrix = np.array([[1, 0, 0, 0], [0, 1, 0, 0], [0, 0, 1, 0], [0, 0, 0, 1]]) + + transform_pcd_folder(pcd_folder_path, new_pcd_folder_path, transform_matrix)