Add spires_cpp package with octomap utility for reconstruction filter…

…ing (#2)

.gitignore

@@ -161,4 +161,5 @@ cython_debug/
 #  option (not recommended) you can uncomment the following to ignore the entire idea folder.
 #.idea/
 
-.vscode
+.vscode
+*test.py

.pre-commit-config.yaml

@@ -9,4 +9,10 @@ repos:
         args: [ --fix ]
       # Run the formatter.
       - id: ruff-format
-        types_or: [python, pyi, jupyter]
+        types_or: [python, pyi, jupyter]
+
+  - repo: https://github.com/pre-commit/mirrors-clang-format
+    rev: v18.1.8  # Replace with the latest version
+    hooks:
+      - id: clang-format
+        args: ["--style={BasedOnStyle: LLVM, ColumnLimit: 120}"]  # This makes it use your .clang-format file, if present

README.md

@@ -1,11 +1,25 @@
 # Oxford Spires Dataset
 This repository contains scripts that are used to evaluate Lidar/Visual SLAM on the Oxford Spires Dataset.
 
+## Installation
+### oxford_spires_utils (Python)
+```bash
+pip install -e .
+```
+
+### spires_cpp (C++ Pybind)
+Install [octomap](https://github.com/OctoMap/octomap) and [PCL](https://github.com/PointCloudLibrary/pcl) to your system, then
+```bash
+cd spires_cpp
+pip install -e .
+```
+
+
 ## Contributing
 Please refer to Angular's guide for contributing(https://github.com/angular/angular/blob/22b96b96902e1a42ee8c5e807720424abad3082a/CONTRIBUTING.md).
 
 ### Formatting
-We use [Ruff](https://github.com/astral-sh/ruff) as the formatter and linter. Installing the `pre-commit` will format and lint your code before you commit:
+We use [Ruff](https://github.com/astral-sh/ruff) as the formatter and linter for Python, and Clang for C++. Installing the `pre-commit` will format and lint your code before you commit:
 
 ```bash
 $ pip install pre-commit

oxford_spires_utils/bash_command.py

@@ -0,0 +1,30 @@
+import subprocess
+from pathlib import Path
+
+
+class TerminalColors:
+    RED = "\033[91m"
+    GREEN = "\033[92m"
+    YELLOW = "\033[93m"
+    ORANGE = "\033[94m"
+    PURPLE = "\033[95m"
+    CYAN = "\033[96m"
+    ENDC = "\033[0m"
+    BOLD = "\033[1m"
+    UNDERLINE = "\033[4m"
+
+
+def print_with_colour(text, colour=TerminalColors.CYAN):
+    print(f"{colour}{text}{TerminalColors.ENDC}")
+
+
+def run_command(cmd, log_path=None, print_command=True):
+    if print_command:
+        print_with_colour(f"Running command: {cmd}")
+    process = subprocess.Popen(cmd, stdout=subprocess.PIPE, shell=True, universal_newlines=True)
+    for line in process.stdout:
+        print(line, end="")
+        if log_path is not None:
+            assert isinstance(log_path, (Path, str))
+            with open(log_path, "a") as f:
+                f.write(line)

oxford_spires_utils/io.py

@@ -1,10 +1,11 @@
 import numpy as np
 import open3d as o3d
+import pye57
+from pypcd4 import PointCloud
+from scipy.spatial.transform import Rotation
 
 from oxford_spires_utils.se3 import xyz_quat_xyzw_to_se3_matrix
 
-np.set_printoptions(suppress=True, precision=4)
-
 
 def read_pcd_with_viewpoint(file_path: str):
     assert file_path.endswith(".pcd")
@@ -21,3 +22,110 @@ def read_pcd_with_viewpoint(file_path: str):
     cloud = o3d.io.read_point_cloud(file_path)
     cloud.transform(se3_matrix)
     return cloud
+
+
+def modify_pcd_viewpoint(file_path: str, new_file_path: str, new_viewpoint_xyz_wxyz: np.ndarray):
+    """
+    open3d does not support writing viewpoint to pcd file, so we need to modify the pcd file manually.
+    """
+    assert file_path.endswith(".pcd")
+    assert new_viewpoint_xyz_wxyz.shape == (7,), f"{new_viewpoint_xyz_wxyz} should be t_xyz_quat_wxyz"
+    new_viewpoint = new_viewpoint_xyz_wxyz
+    header_lines = []
+    with open(file_path, mode="rb") as file:
+        line = file.readline().decode("utf-8").strip()
+        while line != "DATA binary":
+            header_lines.append(line)
+            line = file.readline().decode("utf-8").strip()
+        binary_data = file.read()
+    for i in range(len(header_lines)):
+        if header_lines[i].startswith("VIEWPOINT"):
+            header_lines[i] = (
+                f"VIEWPOINT {new_viewpoint[0]} {new_viewpoint[1]} {new_viewpoint[2]} {new_viewpoint[3]} {new_viewpoint[4]} {new_viewpoint[5]} {new_viewpoint[6]}"
+            )
+            break
+    with open(new_file_path, mode="wb") as file:
+        for line in header_lines:
+            file.write(f"{line}\n".encode("utf-8"))
+        file.write(b"DATA binary\n")
+        file.write(binary_data)
+
+
+def convert_e57_to_pcd(e57_file_path, pcd_file_path, check_output=True, pcd_lib="pypcd4"):
+    # Load E57 file
+    e57_file_path, pcd_file_path = str(e57_file_path), str(pcd_file_path)
+    e57_file = pye57.E57(e57_file_path)
+
+    header = e57_file.get_header(0)
+    t_xyz = header.translation
+    quat_wxyz = header.rotation
+    quat_xyzw = [quat_wxyz[1], quat_wxyz[2], quat_wxyz[3], quat_wxyz[0]]
+    rot_matrix = Rotation.from_quat(quat_xyzw).as_matrix()
+    assert np.allclose(rot_matrix, header.rotation_matrix)
+
+    viewpoint_matrix = xyz_quat_xyzw_to_se3_matrix(t_xyz, quat_xyzw)
+    viewpoint = np.concatenate((t_xyz, quat_wxyz))
+
+    has_colour = "colorRed" in header.point_fields
+    has_intensity = "intensity" in header.point_fields
+    # Get the first point cloud (assuming the E57 file contains at least one)
+    data = e57_file.read_scan(0, intensity=has_intensity, colors=has_colour)
+
+    # Extract Cartesian coordinates
+    x = data["cartesianX"]
+    y = data["cartesianY"]
+    z = data["cartesianZ"]
+
+    # Create a numpy array of points
+    points_np = np.vstack((x, y, z)).T
+    points_homogeneous = np.hstack((points_np, np.ones((points_np.shape[0], 1))))
+    points_sensor_frame = (np.linalg.inv(viewpoint_matrix) @ points_homogeneous.T).T[:, :3]
+
+    if has_colour:
+        colours = np.vstack((data["colorRed"], data["colorGreen"], data["colorBlue"])).T
+
+    if pcd_lib == "open3d":
+        # cannot save intensity to pcd file using open3d
+        pcd = o3d.geometry.PointCloud()
+        pcd.points = o3d.utility.Vector3dVector(points_sensor_frame)
+        # pcd.points = o3d.utility.Vector3dVector(points_np)
+        if has_colour:
+            pcd.colors = o3d.utility.Vector3dVector(colours / 255)
+        o3d.io.write_point_cloud(pcd_file_path, pcd)
+        print(f"PCD file saved to {pcd_file_path}")
+        modify_pcd_viewpoint(pcd_file_path, pcd_file_path, viewpoint)
+    elif pcd_lib == "pypcd4":
+        # supported fields: x, y, z, rgb, intensity
+        fields = ["x", "y", "z"]
+        types = [np.float32, np.float32, np.float32]
+
+        pcd_data = points_sensor_frame
+
+        if has_colour:
+            fields += ["rgb"]
+            types += [np.float32]
+            encoded_colour = PointCloud.encode_rgb(colours)
+            encoded_colour = encoded_colour.reshape(-1, 1)
+            pcd_data = np.hstack((pcd_data, encoded_colour))
+        #     pcd_data = np.hstack((pcd_data, colours / 255))
+
+        if has_intensity:
+            fields += ["intensity"]
+            types += [np.float32]
+            pcd_data = np.hstack((pcd_data, data["intensity"].reshape(-1, 1)))
+
+        fields = tuple(fields)
+        types = tuple(types)
+        pcd = PointCloud.from_points(pcd_data, fields, types)
+        pcd.metadata.viewpoint = tuple(viewpoint)
+        pcd.save(pcd_file_path)
+    else:
+        raise ValueError(f"Unsupported pcd library: {pcd_lib}")
+
+    if check_output:
+        saved_cloud = read_pcd_with_viewpoint(pcd_file_path)
+        saved_cloud_np = np.array(saved_cloud.points)
+        assert np.allclose(saved_cloud_np, points_np, rtol=1e-5, atol=1e-5)
+        if has_colour:
+            colours_np = np.array(saved_cloud.colors)
+            assert np.allclose(colours_np, colours / 255, rtol=1e-5, atol=1e-8)

oxford_spires_utils/point_cloud.py

@@ -1,9 +1,26 @@
 from pathlib import Path
 
+import numpy as np
 import open3d as o3d
 from tqdm import tqdm
 
 from oxford_spires_utils.io import read_pcd_with_viewpoint
+from oxford_spires_utils.se3 import is_se3_matrix
+
+
+def transform_3d_cloud(cloud_np, transform_matrix):
+    """Apply a transformation to the point cloud."""
+    # Convert points to homogeneous coordinates
+    assert isinstance(cloud_np, np.ndarray)
+    assert cloud_np.shape[1] == 3
+    assert is_se3_matrix(transform_matrix)[0], is_se3_matrix(transform_matrix)[1]
+
+    ones = np.ones((cloud_np.shape[0], 1))
+    homogenous_points = np.hstack((cloud_np, ones))
+
+    transformed_points = homogenous_points @ transform_matrix.T
+
+    return transformed_points[:, :3]
 
 
 def merge_downsample_clouds(cloud_path_list, output_cloud_path, downsample_voxel_size=0.05):

oxford_spires_utils/se3.py

@@ -2,14 +2,69 @@
 from scipy.spatial.transform import Rotation
 
 
+def quat_xyzw_to_quat_wxyz(quat_xyzw):
+    if isinstance(quat_xyzw, list):
+        quat_xyzw = np.array(quat_xyzw)
+    assert is_quaternion(quat_xyzw), f"{quat_xyzw} is not a valid quaternion"
+    return quat_xyzw[[3, 0, 1, 2]]
+
+
+def quat_wxyz_to_quat_xyzw(quat_wxyz):
+    if isinstance(quat_wxyz, list):
+        quat_wxyz = np.array(quat_wxyz)
+    assert is_quaternion(quat_wxyz), f"{quat_wxyz} is not a valid quaternion"
+    return quat_wxyz[[1, 2, 3, 0]]
+
+
 def se3_matrix_to_xyz_quat_xyzw(se3_matrix):
+    assert is_se3_matrix(se3_matrix)[0], f"{se3_matrix} not valid, {is_se3_matrix(se3_matrix)[1]}"
     xyz = se3_matrix[:3, 3]
     quat_xyzw = Rotation.from_matrix(se3_matrix[:3, :3]).as_quat()
     return xyz, quat_xyzw
 
 
+def se3_matrix_to_xyz_quat_wxyz(se3_matrix):
+    assert is_se3_matrix(se3_matrix)[0], f"{se3_matrix} not valid, {is_se3_matrix(se3_matrix)[1]}"
+    xyz, quat_xyzw = se3_matrix_to_xyz_quat_xyzw(se3_matrix)
+    quat_wxyz = quat_xyzw_to_quat_wxyz(quat_xyzw)
+    return xyz, quat_wxyz
+
+
 def xyz_quat_xyzw_to_se3_matrix(xyz, quat_xyzw):
+    if isinstance(quat_xyzw, list):
+        quat_xyzw = np.array(quat_xyzw)
+    assert is_quaternion(quat_xyzw), f"{quat_xyzw} is not a valid quaternion"
     se3_matrix = np.eye(4)
     se3_matrix[:3, 3] = xyz
     se3_matrix[:3, :3] = Rotation.from_quat(quat_xyzw).as_matrix()
+    assert is_se3_matrix(se3_matrix)[0], f"{se3_matrix} not valid, {is_se3_matrix(se3_matrix)[1]}"
     return se3_matrix
+
+
+def xyz_quat_wxyz_to_se3_matrix(xyz, quat_wxyz):
+    quat_xyzw = quat_wxyz_to_quat_xyzw(quat_wxyz)
+    return xyz_quat_xyzw_to_se3_matrix(xyz, quat_xyzw)
+
+
+def is_se3_matrix(se3_matrix):
+    valid_shape = se3_matrix.shape == (4, 4)
+    valid_last_row = np.allclose(se3_matrix[3], [0, 0, 0, 1])  # chec k the last row
+    R = se3_matrix[:3, :3]
+    valid_rot_det = np.isclose(np.linalg.det(R), 1.0, atol=1e-6)  # check the rotation matrix
+    valid_orthogonality = np.allclose(R @ R.T, np.eye(3), atol=1e-6)  # check the orthogonality
+    is_valid = valid_shape and valid_last_row and valid_rot_det and valid_orthogonality
+    debug_info = {
+        "valid_shape": valid_shape,
+        "valid_last_row": valid_last_row,
+        "valid_rot_det": valid_rot_det,
+        "valid_orthogonality": valid_orthogonality,
+    }
+    return is_valid, debug_info
+
+
+def is_quaternion(quaternion):
+    if isinstance(quaternion, list):
+        quaternion = np.array(quaternion)
+    assert isinstance(quaternion, np.ndarray), f"{quaternion} is not a numpy array or list"
+    assert quaternion.shape == (4,), f"{quaternion} is not a 4D quaternion"
+    return np.isclose(np.linalg.norm(quaternion), 1.0)

requirements.txt

@@ -1,4 +1,5 @@
-numpy >= 1.24.4
-open3d >= 0.17.0
-scipy >= 1.10.1
-pytest>=8.0.0
+numpy>=1.24.4
+open3d>=0.17.0
+scipy>=1.10.1
+pytest>=8.0.0
+pypcd4>=1.1.0

scripts/pcd_writer.py

@@ -0,0 +1,22 @@
+from pathlib import Path
+
+from oxford_spires_utils.io import convert_e57_to_pcd
+
+
+def convert_e57_folder_to_pcd_folder(e57_folder, pcd_folder):
+    Path(pcd_folder).mkdir(parents=True, exist_ok=True)
+    e57_files = list(Path(e57_folder).glob("*.e57"))
+    for e57_file in e57_files:
+        pcd_file = Path(pcd_folder) / (e57_file.stem + ".pcd")
+        print(f"Converting {e57_file} to {pcd_file}")
+        convert_e57_to_pcd(e57_file, pcd_file, pcd_lib="pypcd4")
+
+
+if __name__ == "__main__":
+    # e57_file_path = "/media/yifu/Samsung_T71/oxford_spires/2024-03-13-maths/gt/individual/Math Inst- 001.e57"
+    # output_pcd = "/home/yifu/workspace/oxford_spires_dataset/output.pcd"
+    # new_pcd = "/home/yifu/workspace/oxford_spires_dataset/output_new.pcd"
+    # convert_e57_to_pcd(e57_file_path, output_pcd)
+    e57_folder = "/media/yifu/Samsung_T71/oxford_spires/2024-03-13-maths/gt/individual"
+    pcd_folder = "/media/yifu/Samsung_T71/oxford_spires/2024-03-13-maths/gt/individual_pcd"
+    convert_e57_folder_to_pcd_folder(e57_folder, pcd_folder)

scripts/recon_benchmark.py

@@ -0,0 +1,49 @@
+import multiprocessing
+from pathlib import Path
+
+from spires_cpp import convertOctreeToPointCloud, processPCDFolder, removeUnknownPoints
+
+# input_cloud_folder_path = "/home/yifu/workspace/Spires_2025/2024-03-13-maths_1/input_cloud_test"
+# gt_cloud_folder_path = "/home/yifu/workspace/Spires_2025/2024-03-13-maths_1/gt_cloud_test"
+input_cloud_folder_path = "/home/yifu/data/nerf_data_pipeline/2024-03-13-maths_1/raw/individual_clouds_new"
+gt_cloud_folder_path = "/media/yifu/Samsung_T71/oxford_spires/2024-03-13-maths/gt/individual_pcd"
+project_folder = "/home/yifu/workspace/Spires_2025/2024-03-13-maths_1"
+Path(project_folder).mkdir(parents=True, exist_ok=True)
+
+input_cloud_bt_path = Path(project_folder) / "input_cloud.bt"
+gt_cloud_bt_path = Path(project_folder) / "gt_cloud.bt"
+
+
+processes = []
+octomap_resolution = 0.1
+for cloud_folder, cloud_bt_path in zip(
+    [input_cloud_folder_path, gt_cloud_folder_path], [input_cloud_bt_path, gt_cloud_bt_path]
+):
+    process = multiprocessing.Process(
+        target=processPCDFolder, args=(str(cloud_folder), octomap_resolution, str(cloud_bt_path))
+    )
+    processes.append(process)
+
+
+# for process in processes:
+#     process.start()
+
+# for process in processes:
+#     process.join()
+
+
+gt_cloud_free_path = str(Path(gt_cloud_bt_path).with_name(f"{Path(gt_cloud_bt_path).stem}_free.pcd"))
+gt_cloud_occ_path = str(Path(gt_cloud_bt_path).with_name(f"{Path(gt_cloud_bt_path).stem}_occ.pcd"))
+input_cloud_free_path = str(Path(input_cloud_bt_path).with_name(f"{Path(input_cloud_bt_path).stem}_free.pcd"))
+input_cloud_occ_path = str(Path(input_cloud_bt_path).with_name(f"{Path(input_cloud_bt_path).stem}_occ.pcd"))
+convertOctreeToPointCloud(str(gt_cloud_bt_path), str(gt_cloud_free_path), str(gt_cloud_occ_path))
+convertOctreeToPointCloud(str(input_cloud_bt_path), str(input_cloud_free_path), str(input_cloud_occ_path))
+
+input_occ_pcd_path = str(Path(input_cloud_bt_path).with_name(f"{Path(input_cloud_bt_path).stem}_occ.pcd"))
+input_occ_filtered_path = str(Path(input_cloud_bt_path).with_name(f"{Path(input_cloud_bt_path).stem}_occ_filtered.pcd"))
+gt_occ_pcd_path = str(Path(gt_cloud_bt_path).with_name(f"{Path(gt_cloud_bt_path).stem}_occ.pcd"))
+gt_occ_filtered_path = str(Path(gt_cloud_bt_path).with_name(f"{Path(gt_cloud_bt_path).stem}_occ_filtered.pcd"))
+
+
+removeUnknownPoints(input_occ_pcd_path, str(gt_cloud_bt_path), input_occ_filtered_path)
+removeUnknownPoints(gt_occ_pcd_path, str(input_cloud_bt_path), gt_occ_filtered_path)