From 3b0528378da09fe0dbb9b56937adbb2866416c31 Mon Sep 17 00:00:00 2001 From: Yifu Tao Date: Fri, 6 Sep 2024 16:40:10 +0100 Subject: [PATCH 01/54] feat: add code to build binary octomap from pcd --- octomap_utils/pcd2bt.cpp | 105 +++++++++++++++++++++++++++++++++++++++ 1 file changed, 105 insertions(+) create mode 100644 octomap_utils/pcd2bt.cpp diff --git a/octomap_utils/pcd2bt.cpp b/octomap_utils/pcd2bt.cpp new file mode 100644 index 0000000..89077cb --- /dev/null +++ b/octomap_utils/pcd2bt.cpp @@ -0,0 +1,105 @@ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +// Function to list files in a directory +std::vector listFiles(const std::string& folderPath) { + std::vector files; + DIR* dir; + struct dirent* ent; + if ((dir = opendir(folderPath.c_str())) != NULL) { + while ((ent = readdir(dir)) != NULL) { + std::string filename = ent->d_name; + if (filename.length() > 4 && filename.substr(filename.length() - 4) == ".pcd") { + files.push_back(folderPath + "/" + filename); + } + } + closedir(dir); + } + return files; +} + +void processPCDFolder(const std::string& folderPath, double resolution = 0.1) { + octomap::OcTree tree(resolution); + + std::vector pcdFiles = listFiles(folderPath); + + // Sort the PCD files + std::sort(pcdFiles.begin(), pcdFiles.end()); + + for (const auto& filePath : pcdFiles) { + std::string filename = filePath.substr(filePath.find_last_of("/") + 1); + std::cout << "Processing file: " << filename << std::endl; + + pcl::PointCloud::Ptr cloud(new pcl::PointCloud); + + if (pcl::io::loadPCDFile(filePath, *cloud) == -1) { + std::cerr << "Couldn't read file " << filename << std::endl; + continue; + } + + std::cout << "Point cloud size: " << cloud->size() << std::endl; + + // Extract viewpoint directly from the PCD file + Eigen::Vector4f origin = cloud->sensor_origin_; + Eigen::Quaternionf orientation = cloud->sensor_orientation_; + + std::cout << "Viewpoint: " + << origin.x() << ", " + << origin.y() << ", " + << origin.z() << ", " + << orientation.x() << ", " + << orientation.y() << ", " + << orientation.z() << ", " + << orientation.w() << std::endl; + + // Convert origin to octomap point3d + octomap::point3d sensor_origin(origin.x(), origin.y(), origin.z()); + + // Transform the point cloud using the origin and orientation + Eigen::Affine3f transform = Eigen::Affine3f::Identity(); + transform.translation() << origin.x(), origin.y(), origin.z(); + transform.rotate(orientation); + + // Apply the transformation to the point cloud + pcl::PointCloud::Ptr transformed_cloud(new pcl::PointCloud()); + pcl::transformPointCloud(*cloud, *transformed_cloud, transform); + + // Convert transformed PCL cloud to OctoMap point cloud + octomap::Pointcloud octomap_cloud; + for (const auto& point : transformed_cloud->points) { + octomap_cloud.push_back(point.x, point.y, point.z); + } + + // Insert the transformed cloud into the OctoMap + tree.insertPointCloud(octomap_cloud, sensor_origin, -1, true, true); + } + + // Optionally, you can save the resulting octomap + tree.writeBinary("result_octomap.bt"); + std::cout << "Octomap saved to result_octomap.bt" << std::endl; +} + +int main(int argc, char** argv) { + if (argc < 2) { + std::cerr << "Usage: " << argv[0] << " [resolution]" << std::endl; + return 1; + } + + double resolution = 0.1; + if (argc > 2) { + resolution = std::stod(argv[2]); + } + + processPCDFolder(argv[1], resolution); + + return 0; +} From ae1730bc2c3a4387218bd03fc1c454c9f96ce951 Mon Sep 17 00:00:00 2001 From: Yifu Tao Date: Fri, 6 Sep 2024 16:40:35 +0100 Subject: [PATCH 02/54] feat: add codeto go through the leaves in the binary octree --- octomap_utils/traverse_bt.cpp | 111 ++++++++++++++++++++++++++++++++++ 1 file changed, 111 insertions(+) create mode 100644 octomap_utils/traverse_bt.cpp diff --git a/octomap_utils/traverse_bt.cpp b/octomap_utils/traverse_bt.cpp new file mode 100644 index 0000000..f8c90fa --- /dev/null +++ b/octomap_utils/traverse_bt.cpp @@ -0,0 +1,111 @@ +#include +#include +#include +#include +#include +#include +#include + +using namespace std; +using namespace octomap; +using PointCloud = pcl::PointCloud; + +// Function to display a simple progress bar +void displayProgressBar(size_t current, size_t total) { + const int barWidth = 50; // Width of the progress bar + float progress = static_cast(current) / total; + int pos = static_cast(barWidth * progress); + + cout << "["; + for (int i = 0; i < barWidth; ++i) { + if (i < pos) cout << "="; + else if (i == pos) cout << ">"; + else cout << " "; + } + cout << "] " << int(progress * 100.0) << "%\r"; + cout.flush(); +} + +int main(int argc, char** argv) { + // Check if a file path is provided + if (argc < 2) { + cerr << "Usage: " << argv[0] << " " << endl; + return 1; + } + + // Load the .bt file into an OcTree + string bt_file = argv[1]; + OcTree tree(bt_file); + + if (!tree.size()) { + cerr << "Failed to load the .bt file or the tree is empty." << endl; + return 1; + } + + // Retrieve and print the thresholds + double occupancyThreshold = tree.getOccupancyThres(); + double freeThreshold = tree.getProbMiss(); // ProbMiss is related to how free space is handled + + cout << "Occupancy threshold: " << occupancyThreshold << endl; // Default is 0.5 + cout << "Free threshold (probMiss): " << freeThreshold << endl; // Default is 0.12 + + + // Point clouds for free, occupied, and unknown voxels + PointCloud::Ptr free_cloud(new PointCloud); + PointCloud::Ptr occupied_cloud(new PointCloud); + // PointCloud::Ptr unknown_cloud(new PointCloud); + + // Get the total number of leaf nodes for the progress bar + size_t total_leafs = tree.getNumLeafNodes(); + size_t processed_leafs = 0; + + // Iterate through all leaf nodes + for (OcTree::leaf_iterator it = tree.begin_leafs(), end = tree.end_leafs(); it != end; ++it) { + point3d coord = it.getCoordinate(); + + // Classify voxel as free, occupied, or unknown and add to corresponding cloud + if (tree.isNodeOccupied(*it)) { + occupied_cloud->points.emplace_back(coord.x(), coord.y(), coord.z()); + } + else { + free_cloud->points.emplace_back(coord.x(), coord.y(), coord.z()); + } + + // Update the progress bar + processed_leafs++; + if (processed_leafs % 100 == 0 || processed_leafs == total_leafs) { // Update every 100 iterations + displayProgressBar(processed_leafs, total_leafs); + } + } + + // Final progress bar completion + displayProgressBar(total_leafs, total_leafs); + cout << endl; + + cout << "size of free_cloud: " << free_cloud->points.size() << endl; + cout << "size of occupied_cloud: " << occupied_cloud->points.size() << endl; + + + // Set cloud properties + free_cloud->width = free_cloud->points.size(); + free_cloud->height = 1; + free_cloud->is_dense = true; + + occupied_cloud->width = occupied_cloud->points.size(); + occupied_cloud->height = 1; + occupied_cloud->is_dense = true; + + + // Save the point clouds as PCD files + cout << "Saving free cloud with " << free_cloud->points.size() << " points..." << endl; + pcl::io::savePCDFileASCII("free_voxels.pcd", *free_cloud); + cout << "Saving occupied cloud with " << occupied_cloud->points.size() << " points..." << endl; + pcl::io::savePCDFileASCII("occupied_voxels.pcd", *occupied_cloud); + + + cout << "\nPoint clouds saved as:" << endl; + cout << "Free voxels: free_voxels.pcd (" << free_cloud->points.size() << " points)" << endl; + cout << "Occupied voxels: occupied_voxels.pcd (" << occupied_cloud->points.size() << " points)" << endl; + + return 0; +} From c4f3170a3d4b4dc4196c62de58b141bd8acae22a Mon Sep 17 00:00:00 2001 From: Yifu Tao Date: Fri, 6 Sep 2024 16:58:47 +0100 Subject: [PATCH 03/54] feat: add CMakeList --- octomap_utils/CMakeLists.txt | 22 ++++++++++++++++++++++ 1 file changed, 22 insertions(+) create mode 100644 octomap_utils/CMakeLists.txt diff --git a/octomap_utils/CMakeLists.txt b/octomap_utils/CMakeLists.txt new file mode 100644 index 0000000..852f9d5 --- /dev/null +++ b/octomap_utils/CMakeLists.txt @@ -0,0 +1,22 @@ +cmake_minimum_required(VERSION 3.10) + +project(spires_octomap_utils) + +# Set C++ standard +set(CMAKE_CXX_STANDARD 11) + +# Find OctoMap package +find_package(Octomap REQUIRED) +find_package(PCL REQUIRED) + + +# Include the OctoMap headers +include_directories(${OCTOMAP_INCLUDE_DIRS}) +include_directories(${PCL_INCLUDE_DIRS}) + + +add_executable(pcd2bt pcd2bt.cpp) +target_link_libraries(pcd2bt ${OCTOMAP_LIBRARIES} ${PCL_LIBRARIES}) + +add_executable(traverse_bt traverse_bt.cpp) +target_link_libraries(traverse_bt ${OCTOMAP_LIBRARIES} ${PCL_LIBRARIES}) \ No newline at end of file From d48d4177004cd474bed4ead4fb0b52c7204f53a4 Mon Sep 17 00:00:00 2001 From: Yifu Tao Date: Fri, 6 Sep 2024 17:06:06 +0100 Subject: [PATCH 04/54] docs: add installation instruction --- README.md | 23 +++++++++++++++++++++++ 1 file changed, 23 insertions(+) diff --git a/README.md b/README.md index 31d2973..410b450 100644 --- a/README.md +++ b/README.md @@ -1,6 +1,29 @@ # 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 . +``` + +### octomap_utils (C++) +Install octomap to your system +```bash +git clone https://github.com/OctoMap/octomap.git && cd octomap +mkdir build && cd build +cmake .. +make +sudo make install +``` +Then install the sripts in Spires. +```bash +cd /octomap_utils +mkdir build && cd build +cmake .. +make +``` + ## Contributing Please refer to Angular's guide for contributing(https://github.com/angular/angular/blob/22b96b96902e1a42ee8c5e807720424abad3082a/CONTRIBUTING.md). From 43f3a5e50c47bd1f999eb344215385ea4684d109 Mon Sep 17 00:00:00 2001 From: Yifu Tao Date: Sat, 7 Sep 2024 13:14:34 +0100 Subject: [PATCH 05/54] feat: add function to modify pcd viewpoints open3d doesn't support custom viewpoint --- oxford_spires_utils/io.py | 27 +++++++++++++++++++++++++++ 1 file changed, 27 insertions(+) diff --git a/oxford_spires_utils/io.py b/oxford_spires_utils/io.py index 82a4862..edcf82c 100644 --- a/oxford_spires_utils/io.py +++ b/oxford_spires_utils/io.py @@ -21,3 +21,30 @@ 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) From fc9f34eeed18497d8bc752fb50724bf06bc6a1a2 Mon Sep 17 00:00:00 2001 From: Yifu Tao Date: Sat, 7 Sep 2024 13:14:48 +0100 Subject: [PATCH 06/54] feat: add scirpt to convert e57 to pcd --- scripts/pcd_writer.py | 55 +++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 55 insertions(+) create mode 100644 scripts/pcd_writer.py diff --git a/scripts/pcd_writer.py b/scripts/pcd_writer.py new file mode 100644 index 0000000..822f3a1 --- /dev/null +++ b/scripts/pcd_writer.py @@ -0,0 +1,55 @@ +import numpy as np +import open3d as o3d +import pye57 +from scipy.spatial.transform import Rotation + +from oxford_spires_utils.io import modify_pcd_viewpoint +from oxford_spires_utils.se3 import xyz_quat_xyzw_to_se3_matrix + + +def convert_e57_to_pcd(e57_file_path, pcd_file_path): + # Load E57 file + e57_file = pye57.E57(e57_file_path) + + # Get the first point cloud (assuming the E57 file contains at least one) + data = e57_file.read_scan(0, intensity=True, colors=True) + + 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)) + + # 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] + + colours = np.vstack((data["colorRed"], data["colorGreen"], data["colorBlue"])).T + + # Create an Open3D PointCloud object + pcd = o3d.geometry.PointCloud() + pcd.points = o3d.utility.Vector3dVector(points_sensor_frame) + # pcd.points = o3d.utility.Vector3dVector(points_np) + pcd.colors = o3d.utility.Vector3dVector(colours / 255) + + # Save the point cloud as a PCD file + 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) + + +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) From 38e0b284394632b450758cc317687340a1152ca7 Mon Sep 17 00:00:00 2001 From: Yifu Tao Date: Sat, 7 Sep 2024 13:55:50 +0100 Subject: [PATCH 07/54] refactor: move e57 to pcd into python module --- oxford_spires_utils/io.py | 43 +++++++++++++++++++++++++++++++++ scripts/pcd_writer.py | 50 +-------------------------------------- 2 files changed, 44 insertions(+), 49 deletions(-) diff --git a/oxford_spires_utils/io.py b/oxford_spires_utils/io.py index edcf82c..a50528d 100644 --- a/oxford_spires_utils/io.py +++ b/oxford_spires_utils/io.py @@ -1,5 +1,7 @@ import numpy as np import open3d as o3d +import pye57 +from scipy.spatial.transform import Rotation from oxford_spires_utils.se3 import xyz_quat_xyzw_to_se3_matrix @@ -48,3 +50,44 @@ def modify_pcd_viewpoint(file_path: str, new_file_path: str, new_viewpoint_xyz_w 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): + # Load E57 file + e57_file = pye57.E57(e57_file_path) + + # Get the first point cloud (assuming the E57 file contains at least one) + data = e57_file.read_scan(0, intensity=True, colors=True) + + 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)) + + # 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] + + colours = np.vstack((data["colorRed"], data["colorGreen"], data["colorBlue"])).T + + # Create an Open3D PointCloud object + pcd = o3d.geometry.PointCloud() + pcd.points = o3d.utility.Vector3dVector(points_sensor_frame) + # pcd.points = o3d.utility.Vector3dVector(points_np) + pcd.colors = o3d.utility.Vector3dVector(colours / 255) + + # Save the point cloud as a PCD file + 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) diff --git a/scripts/pcd_writer.py b/scripts/pcd_writer.py index 822f3a1..2330b1d 100644 --- a/scripts/pcd_writer.py +++ b/scripts/pcd_writer.py @@ -1,52 +1,4 @@ -import numpy as np -import open3d as o3d -import pye57 -from scipy.spatial.transform import Rotation - -from oxford_spires_utils.io import modify_pcd_viewpoint -from oxford_spires_utils.se3 import xyz_quat_xyzw_to_se3_matrix - - -def convert_e57_to_pcd(e57_file_path, pcd_file_path): - # Load E57 file - e57_file = pye57.E57(e57_file_path) - - # Get the first point cloud (assuming the E57 file contains at least one) - data = e57_file.read_scan(0, intensity=True, colors=True) - - 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)) - - # 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] - - colours = np.vstack((data["colorRed"], data["colorGreen"], data["colorBlue"])).T - - # Create an Open3D PointCloud object - pcd = o3d.geometry.PointCloud() - pcd.points = o3d.utility.Vector3dVector(points_sensor_frame) - # pcd.points = o3d.utility.Vector3dVector(points_np) - pcd.colors = o3d.utility.Vector3dVector(colours / 255) - - # Save the point cloud as a PCD file - 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) - +from oxford_spires_utils.io import convert_e57_to_pcd if __name__ == "__main__": e57_file_path = "/media/yifu/Samsung_T71/oxford_spires/2024-03-13-maths/gt/individual/Math Inst- 001.e57" From 1770f725ec9170cfa1b02f96b54a0dc8dd6391fc Mon Sep 17 00:00:00 2001 From: Yifu Tao Date: Sat, 7 Sep 2024 13:56:24 +0100 Subject: [PATCH 08/54] refactor: remove np print option --- oxford_spires_utils/io.py | 2 -- 1 file changed, 2 deletions(-) diff --git a/oxford_spires_utils/io.py b/oxford_spires_utils/io.py index a50528d..cc695c9 100644 --- a/oxford_spires_utils/io.py +++ b/oxford_spires_utils/io.py @@ -5,8 +5,6 @@ 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") From 40498b1a363ef5bf12278d639f0afb297be0318d Mon Sep 17 00:00:00 2001 From: Yifu Tao Date: Sat, 7 Sep 2024 14:04:34 +0100 Subject: [PATCH 09/54] fix: convert file path to str in case it's from pathlib --- oxford_spires_utils/io.py | 1 + 1 file changed, 1 insertion(+) diff --git a/oxford_spires_utils/io.py b/oxford_spires_utils/io.py index cc695c9..d3e185d 100644 --- a/oxford_spires_utils/io.py +++ b/oxford_spires_utils/io.py @@ -52,6 +52,7 @@ def modify_pcd_viewpoint(file_path: str, new_file_path: str, new_viewpoint_xyz_w def convert_e57_to_pcd(e57_file_path, pcd_file_path): # Load E57 file + e57_file_path, pcd_file_path = str(e57_file_path), str(pcd_file_path) e57_file = pye57.E57(e57_file_path) # Get the first point cloud (assuming the E57 file contains at least one) From 34db19fd4052d72fde7b16db46d271bef4c15271 Mon Sep 17 00:00:00 2001 From: Yifu Tao Date: Sat, 7 Sep 2024 14:06:17 +0100 Subject: [PATCH 10/54] feat: add output check for e57 converter --- oxford_spires_utils/io.py | 9 ++++++++- 1 file changed, 8 insertions(+), 1 deletion(-) diff --git a/oxford_spires_utils/io.py b/oxford_spires_utils/io.py index d3e185d..294404f 100644 --- a/oxford_spires_utils/io.py +++ b/oxford_spires_utils/io.py @@ -50,7 +50,7 @@ def modify_pcd_viewpoint(file_path: str, new_file_path: str, new_viewpoint_xyz_w file.write(binary_data) -def convert_e57_to_pcd(e57_file_path, pcd_file_path): +def convert_e57_to_pcd(e57_file_path, pcd_file_path, check_output=True): # Load E57 file e57_file_path, pcd_file_path = str(e57_file_path), str(pcd_file_path) e57_file = pye57.E57(e57_file_path) @@ -90,3 +90,10 @@ def convert_e57_to_pcd(e57_file_path, pcd_file_path): 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) + + 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-6) + colours_np = np.array(saved_cloud.colors) + assert np.allclose(colours_np, colours / 255, rtol=1e-5, atol=1e-8) From f1d3927e648031c3735e8dc7c529c13ed22988fe Mon Sep 17 00:00:00 2001 From: Yifu Tao Date: Sat, 7 Sep 2024 14:06:50 +0100 Subject: [PATCH 11/54] feat: add script to convert e57 folder to pcds --- scripts/pcd_writer.py | 23 +++++++++++++++++++---- 1 file changed, 19 insertions(+), 4 deletions(-) diff --git a/scripts/pcd_writer.py b/scripts/pcd_writer.py index 2330b1d..3d3025e 100644 --- a/scripts/pcd_writer.py +++ b/scripts/pcd_writer.py @@ -1,7 +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) + + 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_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) From 200548694d4b63e2f591abc1dde9dd4435f311e5 Mon Sep 17 00:00:00 2001 From: Yifu Tao Date: Sat, 7 Sep 2024 15:20:25 +0100 Subject: [PATCH 12/54] fix: support no colour scenario --- oxford_spires_utils/io.py | 18 +++++++++++------- 1 file changed, 11 insertions(+), 7 deletions(-) diff --git a/oxford_spires_utils/io.py b/oxford_spires_utils/io.py index 294404f..c029256 100644 --- a/oxford_spires_utils/io.py +++ b/oxford_spires_utils/io.py @@ -55,9 +55,6 @@ def convert_e57_to_pcd(e57_file_path, pcd_file_path, check_output=True): e57_file_path, pcd_file_path = str(e57_file_path), str(pcd_file_path) e57_file = pye57.E57(e57_file_path) - # Get the first point cloud (assuming the E57 file contains at least one) - data = e57_file.read_scan(0, intensity=True, colors=True) - header = e57_file.get_header(0) t_xyz = header.translation quat_wxyz = header.rotation @@ -68,6 +65,10 @@ def convert_e57_to_pcd(e57_file_path, pcd_file_path, check_output=True): 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 + # Get the first point cloud (assuming the E57 file contains at least one) + data = e57_file.read_scan(0, intensity=False, colors=has_colour) + # Extract Cartesian coordinates x = data["cartesianX"] y = data["cartesianY"] @@ -78,13 +79,15 @@ def convert_e57_to_pcd(e57_file_path, pcd_file_path, check_output=True): 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] - colours = np.vstack((data["colorRed"], data["colorGreen"], data["colorBlue"])).T + if has_colour: + colours = np.vstack((data["colorRed"], data["colorGreen"], data["colorBlue"])).T # Create an Open3D PointCloud object pcd = o3d.geometry.PointCloud() pcd.points = o3d.utility.Vector3dVector(points_sensor_frame) # pcd.points = o3d.utility.Vector3dVector(points_np) - pcd.colors = o3d.utility.Vector3dVector(colours / 255) + if has_colour: + pcd.colors = o3d.utility.Vector3dVector(colours / 255) # Save the point cloud as a PCD file o3d.io.write_point_cloud(pcd_file_path, pcd) @@ -95,5 +98,6 @@ def convert_e57_to_pcd(e57_file_path, pcd_file_path, check_output=True): 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-6) - colours_np = np.array(saved_cloud.colors) - assert np.allclose(colours_np, colours / 255, rtol=1e-5, atol=1e-8) + if has_colour: + colours_np = np.array(saved_cloud.colors) + assert np.allclose(colours_np, colours / 255, rtol=1e-5, atol=1e-8) From 1d97950a72044ffcdf927aed61eb846953c7ab88 Mon Sep 17 00:00:00 2001 From: Yifu Tao Date: Sat, 7 Sep 2024 15:20:51 +0100 Subject: [PATCH 13/54] refactor: reduce cloud check threshold --- oxford_spires_utils/io.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/oxford_spires_utils/io.py b/oxford_spires_utils/io.py index c029256..2d1196d 100644 --- a/oxford_spires_utils/io.py +++ b/oxford_spires_utils/io.py @@ -97,7 +97,7 @@ def convert_e57_to_pcd(e57_file_path, pcd_file_path, check_output=True): 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-6) + 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) From b6f3fffb0422fdc0704ef47d8793f530d941b48a Mon Sep 17 00:00:00 2001 From: Yifu Tao Date: Sun, 8 Sep 2024 22:18:50 +0100 Subject: [PATCH 14/54] feat: add intensity loading for e57 --- oxford_spires_utils/io.py | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/oxford_spires_utils/io.py b/oxford_spires_utils/io.py index 2d1196d..156f708 100644 --- a/oxford_spires_utils/io.py +++ b/oxford_spires_utils/io.py @@ -66,8 +66,9 @@ def convert_e57_to_pcd(e57_file_path, pcd_file_path, check_output=True): 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=False, colors=has_colour) + data = e57_file.read_scan(0, intensity=has_intensity, colors=has_colour) # Extract Cartesian coordinates x = data["cartesianX"] From 3eb4a991d9308695e741a9de6e5505b390cb977d Mon Sep 17 00:00:00 2001 From: Yifu Tao Date: Sun, 8 Sep 2024 23:12:05 +0100 Subject: [PATCH 15/54] feat: add pypcd4 lib for saving pcd --- oxford_spires_utils/io.py | 49 ++++++++++++++++++++++++++++++--------- requirements.txt | 3 ++- scripts/pcd_writer.py | 2 +- 3 files changed, 41 insertions(+), 13 deletions(-) diff --git a/oxford_spires_utils/io.py b/oxford_spires_utils/io.py index 156f708..0c8abea 100644 --- a/oxford_spires_utils/io.py +++ b/oxford_spires_utils/io.py @@ -1,6 +1,7 @@ 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 @@ -50,7 +51,7 @@ def modify_pcd_viewpoint(file_path: str, new_file_path: str, new_viewpoint_xyz_w file.write(binary_data) -def convert_e57_to_pcd(e57_file_path, pcd_file_path, check_output=True): +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) @@ -83,17 +84,43 @@ def convert_e57_to_pcd(e57_file_path, pcd_file_path, check_output=True): if has_colour: colours = np.vstack((data["colorRed"], data["colorGreen"], data["colorBlue"])).T - # Create an Open3D PointCloud object - 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) + 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] - # Save the point cloud as a PCD file - 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) + pcd_data = points_np + + 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.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) diff --git a/requirements.txt b/requirements.txt index e743267..cfa87ab 100644 --- a/requirements.txt +++ b/requirements.txt @@ -1,4 +1,5 @@ numpy >= 1.24.4 open3d >= 0.17.0 scipy >= 1.10.1 -pytest>=8.0.0 \ No newline at end of file +pytest>=8.0.0 +pypcd4>=1.1.0 \ No newline at end of file diff --git a/scripts/pcd_writer.py b/scripts/pcd_writer.py index 3d3025e..e206c79 100644 --- a/scripts/pcd_writer.py +++ b/scripts/pcd_writer.py @@ -9,7 +9,7 @@ def convert_e57_folder_to_pcd_folder(e57_folder, pcd_folder): 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) + convert_e57_to_pcd(e57_file, pcd_file, pcd_lib="pypcd4") if __name__ == "__main__": From 4082d06a0ffcfb613d7755e2e92f97cb6fb0423c Mon Sep 17 00:00:00 2001 From: Yifu Tao Date: Sun, 8 Sep 2024 23:27:32 +0100 Subject: [PATCH 16/54] fix: pcd2bt usage string --- octomap_utils/pcd2bt.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/octomap_utils/pcd2bt.cpp b/octomap_utils/pcd2bt.cpp index 89077cb..9e6e1f5 100644 --- a/octomap_utils/pcd2bt.cpp +++ b/octomap_utils/pcd2bt.cpp @@ -90,7 +90,7 @@ void processPCDFolder(const std::string& folderPath, double resolution = 0.1) { int main(int argc, char** argv) { if (argc < 2) { - std::cerr << "Usage: " << argv[0] << " [resolution]" << std::endl; + std::cerr << "Usage: " << argv[0] << " [resolution]" << std::endl; return 1; } From 3888c6073cc2dee4610b130fb7c5d662c38ead1b Mon Sep 17 00:00:00 2001 From: Yifu Tao Date: Mon, 9 Sep 2024 00:06:39 +0100 Subject: [PATCH 17/54] refactor: add save_path --- octomap_utils/pcd2bt.cpp | 21 +++++++++++++-------- 1 file changed, 13 insertions(+), 8 deletions(-) diff --git a/octomap_utils/pcd2bt.cpp b/octomap_utils/pcd2bt.cpp index 9e6e1f5..39128b8 100644 --- a/octomap_utils/pcd2bt.cpp +++ b/octomap_utils/pcd2bt.cpp @@ -27,7 +27,7 @@ std::vector listFiles(const std::string& folderPath) { return files; } -void processPCDFolder(const std::string& folderPath, double resolution = 0.1) { +void processPCDFolder(const std::string& folderPath, double resolution = 0.1, std::string save_path = "result_octomap.bt") { octomap::OcTree tree(resolution); std::vector pcdFiles = listFiles(folderPath); @@ -84,22 +84,27 @@ void processPCDFolder(const std::string& folderPath, double resolution = 0.1) { } // Optionally, you can save the resulting octomap - tree.writeBinary("result_octomap.bt"); - std::cout << "Octomap saved to result_octomap.bt" << std::endl; + tree.writeBinary(save_path); + std::cout << std::endl; + std::cout << "Octomap saved to " << save_path << std::endl; } int main(int argc, char** argv) { if (argc < 2) { - std::cerr << "Usage: " << argv[0] << " [resolution]" << std::endl; + std::cerr << "Usage: " << argv[0] << " [resolution] [saved_path]" << std::endl; return 1; } - + std::cout << "pcd_folder: " << argv[1] << std::endl; double resolution = 0.1; - if (argc > 2) { + std::cout << "resolution: " << argv[2] << std::endl; + if (argc == 3) { resolution = std::stod(argv[2]); } - - processPCDFolder(argv[1], resolution); + std::string save_path = "result_octomap.bt"; + if (argc == 4) { + save_path = argv[3]; + } + processPCDFolder(argv[1], resolution, save_path); return 0; } From 900c5cffdd01ee5acb7f6812ff6394611a91b75f Mon Sep 17 00:00:00 2001 From: Yifu Tao Date: Mon, 9 Sep 2024 00:07:20 +0100 Subject: [PATCH 18/54] fix: viewpoint and pcd in sensor frame --- oxford_spires_utils/io.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/oxford_spires_utils/io.py b/oxford_spires_utils/io.py index 0c8abea..a65c490 100644 --- a/oxford_spires_utils/io.py +++ b/oxford_spires_utils/io.py @@ -99,7 +99,7 @@ def convert_e57_to_pcd(e57_file_path, pcd_file_path, check_output=True, pcd_lib= fields = ["x", "y", "z"] types = [np.float32, np.float32, np.float32] - pcd_data = points_np + pcd_data = points_sensor_frame if has_colour: fields += ["rgb"] @@ -117,7 +117,7 @@ def convert_e57_to_pcd(e57_file_path, pcd_file_path, check_output=True, pcd_lib= fields = tuple(fields) types = tuple(types) pcd = PointCloud.from_points(pcd_data, fields, types) - pcd.viewpoint = tuple(viewpoint) + pcd.metadata.viewpoint = tuple(viewpoint) pcd.save(pcd_file_path) else: raise ValueError(f"Unsupported pcd library: {pcd_lib}") From 1ba551758fcba9afd37542db8c6fefcd741f5b61 Mon Sep 17 00:00:00 2001 From: Yifu Tao Date: Mon, 9 Sep 2024 11:31:07 +0100 Subject: [PATCH 19/54] refactor: ignore test.py --- .gitignore | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/.gitignore b/.gitignore index ecb2b2b..99bf764 100644 --- a/.gitignore +++ b/.gitignore @@ -161,4 +161,5 @@ cython_debug/ # option (not recommended) you can uncomment the following to ignore the entire idea folder. #.idea/ -.vscode \ No newline at end of file +.vscode +*test.py \ No newline at end of file From 0e921a705ec72ef16f48ec5ecf735ce66ea4e5b5 Mon Sep 17 00:00:00 2001 From: Yifu Tao Date: Tue, 10 Sep 2024 17:27:01 +0100 Subject: [PATCH 20/54] refactor: wrap octree2cloud into a function --- octomap_utils/traverse_bt.cpp | 56 +++++++++++++++++++---------------- 1 file changed, 30 insertions(+), 26 deletions(-) diff --git a/octomap_utils/traverse_bt.cpp b/octomap_utils/traverse_bt.cpp index f8c90fa..36ec39f 100644 --- a/octomap_utils/traverse_bt.cpp +++ b/octomap_utils/traverse_bt.cpp @@ -26,6 +26,35 @@ void displayProgressBar(size_t current, size_t total) { cout.flush(); } +void convertOctreeToPointCloud(const OcTree& tree, PointCloud::Ptr& free_cloud, PointCloud::Ptr& occupied_cloud) { + // Get the total number of leaf nodes for the progress bar + size_t total_leafs = tree.getNumLeafNodes(); + size_t processed_leafs = 0; + + // Iterate through all leaf nodes + for (OcTree::leaf_iterator it = tree.begin_leafs(), end = tree.end_leafs(); it != end; ++it) { + point3d coord = it.getCoordinate(); + + // Classify voxel as free, occupied, or unknown and add to corresponding cloud + if (tree.isNodeOccupied(*it)) { + occupied_cloud->points.emplace_back(coord.x(), coord.y(), coord.z()); + } + else { + free_cloud->points.emplace_back(coord.x(), coord.y(), coord.z()); + } + + // Update the progress bar + processed_leafs++; + if (processed_leafs % 100 == 0 || processed_leafs == total_leafs) { // Update every 100 iterations + displayProgressBar(processed_leafs, total_leafs); + } + } + + // Final progress bar completion + displayProgressBar(total_leafs, total_leafs); + cout << endl; +} + int main(int argc, char** argv) { // Check if a file path is provided if (argc < 2) { @@ -55,32 +84,7 @@ int main(int argc, char** argv) { PointCloud::Ptr occupied_cloud(new PointCloud); // PointCloud::Ptr unknown_cloud(new PointCloud); - // Get the total number of leaf nodes for the progress bar - size_t total_leafs = tree.getNumLeafNodes(); - size_t processed_leafs = 0; - - // Iterate through all leaf nodes - for (OcTree::leaf_iterator it = tree.begin_leafs(), end = tree.end_leafs(); it != end; ++it) { - point3d coord = it.getCoordinate(); - - // Classify voxel as free, occupied, or unknown and add to corresponding cloud - if (tree.isNodeOccupied(*it)) { - occupied_cloud->points.emplace_back(coord.x(), coord.y(), coord.z()); - } - else { - free_cloud->points.emplace_back(coord.x(), coord.y(), coord.z()); - } - - // Update the progress bar - processed_leafs++; - if (processed_leafs % 100 == 0 || processed_leafs == total_leafs) { // Update every 100 iterations - displayProgressBar(processed_leafs, total_leafs); - } - } - - // Final progress bar completion - displayProgressBar(total_leafs, total_leafs); - cout << endl; + convertOctreeToPointCloud(tree, free_cloud, occupied_cloud); cout << "size of free_cloud: " << free_cloud->points.size() << endl; cout << "size of occupied_cloud: " << occupied_cloud->points.size() << endl; From c480d6fbe91e88eb03f9e348b19aca03962e470a Mon Sep 17 00:00:00 2001 From: Yifu Tao Date: Tue, 10 Sep 2024 17:27:53 +0100 Subject: [PATCH 21/54] docs: add usage instructions --- README.md | 12 ++++++++++++ 1 file changed, 12 insertions(+) diff --git a/README.md b/README.md index 410b450..f521487 100644 --- a/README.md +++ b/README.md @@ -24,6 +24,18 @@ cmake .. make ``` +## Usage +### octomap_utils +```bash +cd /octomap_utils/build +./pcd2bt -r [resolution] -s [saved_path] -tf [x,y,z, quat_w, quat_x, quat_y, quat_z] +``` + +```bash +cd /octomap_utils/build +./traverse_bt +``` + ## Contributing Please refer to Angular's guide for contributing(https://github.com/angular/angular/blob/22b96b96902e1a42ee8c5e807720424abad3082a/CONTRIBUTING.md). From b484338547fae998087308a8aeb1e17a935b35f5 Mon Sep 17 00:00:00 2001 From: Yifu Tao Date: Wed, 11 Sep 2024 10:32:30 +0100 Subject: [PATCH 22/54] feat: add se3 matrix check --- oxford_spires_utils/se3.py | 18 ++++++++++++++++++ 1 file changed, 18 insertions(+) diff --git a/oxford_spires_utils/se3.py b/oxford_spires_utils/se3.py index b66735e..4cdc17b 100644 --- a/oxford_spires_utils/se3.py +++ b/oxford_spires_utils/se3.py @@ -3,6 +3,7 @@ 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 @@ -12,4 +13,21 @@ def xyz_quat_xyzw_to_se3_matrix(xyz, quat_xyzw): 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 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 From cef0ceaceafca626afe0b5c876603306dc68b03d Mon Sep 17 00:00:00 2001 From: Yifu Tao Date: Wed, 11 Sep 2024 10:32:52 +0100 Subject: [PATCH 23/54] feat: add se3 to xyz quat wxyz --- oxford_spires_utils/se3.py | 22 ++++++++++++++++++++++ 1 file changed, 22 insertions(+) diff --git a/oxford_spires_utils/se3.py b/oxford_spires_utils/se3.py index 4cdc17b..878d5ec 100644 --- a/oxford_spires_utils/se3.py +++ b/oxford_spires_utils/se3.py @@ -2,6 +2,13 @@ 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 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] @@ -9,6 +16,13 @@ def se3_matrix_to_xyz_quat_xyzw(se3_matrix): 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): se3_matrix = np.eye(4) se3_matrix[:3, 3] = xyz @@ -31,3 +45,11 @@ def is_se3_matrix(se3_matrix): "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) From cd2106358b4ce66b46d9f54fc16745274b962f12 Mon Sep 17 00:00:00 2001 From: Yifu Tao Date: Wed, 11 Sep 2024 10:35:31 +0100 Subject: [PATCH 24/54] feat: add quaternion check --- oxford_spires_utils/se3.py | 3 +++ 1 file changed, 3 insertions(+) diff --git a/oxford_spires_utils/se3.py b/oxford_spires_utils/se3.py index 878d5ec..85e4dc1 100644 --- a/oxford_spires_utils/se3.py +++ b/oxford_spires_utils/se3.py @@ -24,6 +24,9 @@ def se3_matrix_to_xyz_quat_wxyz(se3_matrix): 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() From b1730c017da8b44a7505dfd77c2b55de2838f81d Mon Sep 17 00:00:00 2001 From: Yifu Tao Date: Wed, 11 Sep 2024 11:01:27 +0100 Subject: [PATCH 25/54] feat: add initial recon benchmark script with ocotmap building --- oxford_spires_utils/bash_command.py | 30 +++++++++++++++++++++++++ scripts/recon_benchmark.py | 35 +++++++++++++++++++++++++++++ 2 files changed, 65 insertions(+) create mode 100644 oxford_spires_utils/bash_command.py create mode 100644 scripts/recon_benchmark.py diff --git a/oxford_spires_utils/bash_command.py b/oxford_spires_utils/bash_command.py new file mode 100644 index 0000000..257b84b --- /dev/null +++ b/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) diff --git a/scripts/recon_benchmark.py b/scripts/recon_benchmark.py new file mode 100644 index 0000000..d64b507 --- /dev/null +++ b/scripts/recon_benchmark.py @@ -0,0 +1,35 @@ +import multiprocessing +from pathlib import Path + +from oxford_spires_utils.bash_command import run_command + + +def run_pcd2bt(cloud_folder, cloud_bt_path): + pcd2bt_command = [str(pcd2bt_path), cloud_folder, "-s", str(cloud_bt_path)] + pcd2bt_command = " ".join(pcd2bt_command) + run_command(pcd2bt_command) + + +input_cloud_folder_path = "/home/yifu/data/nerf_data_pipeline/2024-03-13-maths_1/raw/individual_clouds" +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" + +octomap_utils_path = Path(__file__).parent.parent / "octomap_utils" +pcd2bt_path = octomap_utils_path / "build" / "pcd2bt" + +processes = [] +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=run_pcd2bt, args=(cloud_folder, cloud_bt_path)) + processes.append(process) + +for process in processes: + process.start() + +for process in processes: + process.join() From c17a1a10afccd28f2687c6cc14f2924a3cb8f91f Mon Sep 17 00:00:00 2001 From: Yifu Tao Date: Wed, 11 Sep 2024 11:27:03 +0100 Subject: [PATCH 26/54] feat: add args for custom save path --- octomap_utils/traverse_bt.cpp | 51 +++++++++++++++++++++-------------- 1 file changed, 31 insertions(+), 20 deletions(-) diff --git a/octomap_utils/traverse_bt.cpp b/octomap_utils/traverse_bt.cpp index 36ec39f..9256760 100644 --- a/octomap_utils/traverse_bt.cpp +++ b/octomap_utils/traverse_bt.cpp @@ -55,15 +55,29 @@ void convertOctreeToPointCloud(const OcTree& tree, PointCloud::Ptr& free_cloud, cout << endl; } -int main(int argc, char** argv) { - // Check if a file path is provided +int main(int argc, char **argv) { + // Default paths for saving the point clouds + string free_pcd_path = "free_voxels.pcd"; + string occupied_pcd_path = "occupied_voxels.pcd"; + + // Check if the correct number of arguments is provided if (argc < 2) { - cerr << "Usage: " << argv[0] << " " << endl; + cerr << "Usage: " << argv[0] << " -sf -so " << endl; return 1; } - // Load the .bt file into an OcTree + // Parse arguments string bt_file = argv[1]; + for (int i = 2; i < argc; ++i) { + string arg = argv[i]; + if (arg == "-sf" && i + 1 < argc) { + free_pcd_path = argv[++i]; + } else if (arg == "-so" && i + 1 < argc) { + occupied_pcd_path = argv[++i]; + } + } + + // Load the .bt file into an OcTree OcTree tree(bt_file); if (!tree.size()) { @@ -73,22 +87,20 @@ int main(int argc, char** argv) { // Retrieve and print the thresholds double occupancyThreshold = tree.getOccupancyThres(); - double freeThreshold = tree.getProbMiss(); // ProbMiss is related to how free space is handled + double freeThreshold = tree.getProbMiss(); // ProbMiss is related to how free space is handled cout << "Occupancy threshold: " << occupancyThreshold << endl; // Default is 0.5 cout << "Free threshold (probMiss): " << freeThreshold << endl; // Default is 0.12 - - // Point clouds for free, occupied, and unknown voxels + // Point clouds for free and occupied voxels PointCloud::Ptr free_cloud(new PointCloud); PointCloud::Ptr occupied_cloud(new PointCloud); - // PointCloud::Ptr unknown_cloud(new PointCloud); + // Convert octree to point clouds convertOctreeToPointCloud(tree, free_cloud, occupied_cloud); - cout << "size of free_cloud: " << free_cloud->points.size() << endl; - cout << "size of occupied_cloud: " << occupied_cloud->points.size() << endl; - + cout << "Size of free_cloud: " << free_cloud->points.size() << endl; + cout << "Size of occupied_cloud: " << occupied_cloud->points.size() << endl; // Set cloud properties free_cloud->width = free_cloud->points.size(); @@ -99,17 +111,16 @@ int main(int argc, char** argv) { occupied_cloud->height = 1; occupied_cloud->is_dense = true; + // Save the point clouds as PCD files with specified file paths + cout << "Saving free cloud with " << free_cloud->points.size() << " points to " << free_pcd_path << "..." << endl; + pcl::io::savePCDFileASCII(free_pcd_path, *free_cloud); - // Save the point clouds as PCD files - cout << "Saving free cloud with " << free_cloud->points.size() << " points..." << endl; - pcl::io::savePCDFileASCII("free_voxels.pcd", *free_cloud); - cout << "Saving occupied cloud with " << occupied_cloud->points.size() << " points..." << endl; - pcl::io::savePCDFileASCII("occupied_voxels.pcd", *occupied_cloud); - + cout << "Saving occupied cloud with " << occupied_cloud->points.size() << " points to " << occupied_pcd_path << "..." << endl; + pcl::io::savePCDFileASCII(occupied_pcd_path, *occupied_cloud); cout << "\nPoint clouds saved as:" << endl; - cout << "Free voxels: free_voxels.pcd (" << free_cloud->points.size() << " points)" << endl; - cout << "Occupied voxels: occupied_voxels.pcd (" << occupied_cloud->points.size() << " points)" << endl; + cout << "Free voxels: " << free_pcd_path << " (" << free_cloud->points.size() << " points)" << endl; + cout << "Occupied voxels: " << occupied_pcd_path << " (" << occupied_cloud->points.size() << " points)" << endl; return 0; -} +} \ No newline at end of file From 8765b4a1f238325f7f8224ed6fbdd3a90cf11eda Mon Sep 17 00:00:00 2001 From: Yifu Tao Date: Wed, 11 Sep 2024 11:28:18 +0100 Subject: [PATCH 27/54] feat: add custom transform to each pcd --- octomap_utils/pcd2bt.cpp | 59 ++++++++++++++++++++++++++++++++-------- 1 file changed, 47 insertions(+), 12 deletions(-) diff --git a/octomap_utils/pcd2bt.cpp b/octomap_utils/pcd2bt.cpp index 39128b8..67fef05 100644 --- a/octomap_utils/pcd2bt.cpp +++ b/octomap_utils/pcd2bt.cpp @@ -27,9 +27,8 @@ std::vector listFiles(const std::string& folderPath) { return files; } -void processPCDFolder(const std::string& folderPath, double resolution = 0.1, std::string save_path = "result_octomap.bt") { +void processPCDFolder(const std::string& folderPath, double resolution, const std::string& save_path, const Eigen::Affine3f& custom_transform) { octomap::OcTree tree(resolution); - std::vector pcdFiles = listFiles(folderPath); // Sort the PCD files @@ -72,6 +71,7 @@ void processPCDFolder(const std::string& folderPath, double resolution = 0.1, st // Apply the transformation to the point cloud pcl::PointCloud::Ptr transformed_cloud(new pcl::PointCloud()); pcl::transformPointCloud(*cloud, *transformed_cloud, transform); + pcl::transformPointCloud(*transformed_cloud, *transformed_cloud, custom_transform); // Convert transformed PCL cloud to OctoMap point cloud octomap::Pointcloud octomap_cloud; @@ -83,7 +83,7 @@ void processPCDFolder(const std::string& folderPath, double resolution = 0.1, st tree.insertPointCloud(octomap_cloud, sensor_origin, -1, true, true); } - // Optionally, you can save the resulting octomap + // Save the resulting octomap tree.writeBinary(save_path); std::cout << std::endl; std::cout << "Octomap saved to " << save_path << std::endl; @@ -91,20 +91,55 @@ void processPCDFolder(const std::string& folderPath, double resolution = 0.1, st int main(int argc, char** argv) { if (argc < 2) { - std::cerr << "Usage: " << argv[0] << " [resolution] [saved_path]" << std::endl; + std::cerr << "Usage: " << argv[0] << " -r [resolution] -s [saved_path] -tf [transform_path]" << std::endl; return 1; } - std::cout << "pcd_folder: " << argv[1] << std::endl; + std::string folderPath = argv[1]; double resolution = 0.1; - std::cout << "resolution: " << argv[2] << std::endl; - if (argc == 3) { - resolution = std::stod(argv[2]); - } std::string save_path = "result_octomap.bt"; - if (argc == 4) { - save_path = argv[3]; + Eigen::Affine3f custom_transform = Eigen::Affine3f::Identity(); + bool transform_set = false; + + // Parse command line arguments + for (int i = 2; i < argc; ++i) { + std::string arg = argv[i]; + if (arg == "-r" && i + 1 < argc) { + resolution = std::stod(argv[++i]); + } else if (arg == "-s" && i + 1 < argc) { + save_path = argv[++i]; + } else if (arg == "-tf" && i + 7 < argc) { + float tx = std::stof(argv[++i]); + float ty = std::stof(argv[++i]); + float tz = std::stof(argv[++i]); + float qw = std::stof(argv[++i]); + float qx = std::stof(argv[++i]); + float qy = std::stof(argv[++i]); + float qz = std::stof(argv[++i]); + + custom_transform = Eigen::Affine3f::Identity(); + custom_transform.translation() << tx, ty, tz; + Eigen::Quaternionf quaternion(qw, qx, qy, qz); + custom_transform.rotate(quaternion); + + std::cout << "Applied custom transform: " + << "t(" << tx << ", " << ty << ", " << tz << ") " + << "quat(" << qw << ", " << qx << ", " << qy << ", " << qz << ")" << std::endl; + + transform_set = true; + } else { + std::cerr << "Unknown argument or missing value: " << arg << std::endl; + std::cerr << "Usage: " << argv[0] << " -r [resolution] -s [saved_path] -tf x y z quat_wxyz" << std::endl; + return 1; + } } - processPCDFolder(argv[1], resolution, save_path); + + // If no transformation is provided, use the identity matrix + if (!transform_set) { + std::cout << "No custom transform provided. Using identity transformation." << std::endl; + } + std::cout << " transformation:\n" << custom_transform.matrix() << std::endl; + + processPCDFolder(folderPath, resolution, save_path, custom_transform); return 0; } From 7a954beeea1237da14fa2d93f91bba2e7f1593d8 Mon Sep 17 00:00:00 2001 From: Yifu Tao Date: Wed, 11 Sep 2024 11:45:16 +0100 Subject: [PATCH 28/54] feat: add custom tf to pcd2bt --- scripts/recon_benchmark.py | 16 +++++++++++----- 1 file changed, 11 insertions(+), 5 deletions(-) diff --git a/scripts/recon_benchmark.py b/scripts/recon_benchmark.py index d64b507..26ebcf2 100644 --- a/scripts/recon_benchmark.py +++ b/scripts/recon_benchmark.py @@ -1,11 +1,12 @@ import multiprocessing from pathlib import Path +from typing import List from oxford_spires_utils.bash_command import run_command -def run_pcd2bt(cloud_folder, cloud_bt_path): - pcd2bt_command = [str(pcd2bt_path), cloud_folder, "-s", str(cloud_bt_path)] +def run_pcd2bt(cloud_folder: str, cloud_bt_path: str, transform: List[float]): + pcd2bt_command = [str(pcd2bt_path), cloud_folder, "-s", str(cloud_bt_path), "-tf", *map(str, transform)] pcd2bt_command = " ".join(pcd2bt_command) run_command(pcd2bt_command) @@ -14,6 +15,9 @@ def run_pcd2bt(cloud_folder, cloud_bt_path): 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) +# tf: tx ty tz qw, qx, qy, qz +input_cloud_tf = [-85.867073, 21.640722, 0.720384, 0.766620, -0.0019915, 0.0196701, -0.641796] +gt_cloud_tf = [0, 0, 0, 1, 0, 0, 0] input_cloud_bt_path = Path(project_folder) / "input_cloud.bt" gt_cloud_bt_path = Path(project_folder) / "gt_cloud.bt" @@ -22,10 +26,12 @@ def run_pcd2bt(cloud_folder, cloud_bt_path): pcd2bt_path = octomap_utils_path / "build" / "pcd2bt" processes = [] -for cloud_folder, cloud_bt_path in zip( - [input_cloud_folder_path, gt_cloud_folder_path], [input_cloud_bt_path, gt_cloud_bt_path] +for cloud_folder, cloud_bt_path, transform in zip( + [input_cloud_folder_path, gt_cloud_folder_path], + [input_cloud_bt_path, gt_cloud_bt_path, gt_cloud_bt_path], + [input_cloud_tf, gt_cloud_tf], ): - process = multiprocessing.Process(target=run_pcd2bt, args=(cloud_folder, cloud_bt_path)) + process = multiprocessing.Process(target=run_pcd2bt, args=(cloud_folder, cloud_bt_path, transform)) processes.append(process) for process in processes: From 3f4b2ad81e3de4fe46bb43b27d6ec2dcca6cdd0a Mon Sep 17 00:00:00 2001 From: Yifu Tao Date: Wed, 11 Sep 2024 14:03:03 +0100 Subject: [PATCH 29/54] feat: add traverse_bt to the benchmakr script --- scripts/recon_benchmark.py | 19 ++++++++++++++++--- 1 file changed, 16 insertions(+), 3 deletions(-) diff --git a/scripts/recon_benchmark.py b/scripts/recon_benchmark.py index 26ebcf2..88de3d6 100644 --- a/scripts/recon_benchmark.py +++ b/scripts/recon_benchmark.py @@ -5,11 +5,21 @@ from oxford_spires_utils.bash_command import run_command -def run_pcd2bt(cloud_folder: str, cloud_bt_path: str, transform: List[float]): - pcd2bt_command = [str(pcd2bt_path), cloud_folder, "-s", str(cloud_bt_path), "-tf", *map(str, transform)] +def run_pcd2bt(cloud_folder: str, cloud_bt_path: str, pcd2bt_path: str, traverse_bt_path: str, transform: List[float]): + cloud_folder, cloud_bt_path = str(cloud_folder), str(cloud_bt_path) + pcd2bt_path, traverse_bt_path = str(pcd2bt_path), str(traverse_bt_path) + + pcd2bt_command = [pcd2bt_path, cloud_folder, "-s", str(cloud_bt_path), "-tf", *map(str, transform)] pcd2bt_command = " ".join(pcd2bt_command) run_command(pcd2bt_command) + saved_occ_pcd_path = str(Path(cloud_bt_path).with_name(f"{Path(cloud_bt_path).stem}_occ.pcd")) + saved_free_pcd_path = str(Path(cloud_bt_path).with_name(f"{Path(cloud_bt_path).stem}_free.pcd")) + traverse_bt_command = [traverse_bt_path, cloud_bt_path, "-so", saved_occ_pcd_path, "-sf", saved_free_pcd_path] + print(traverse_bt_command) + traverse_bt_command = " ".join(traverse_bt_command) + run_command(traverse_bt_command) + input_cloud_folder_path = "/home/yifu/data/nerf_data_pipeline/2024-03-13-maths_1/raw/individual_clouds" gt_cloud_folder_path = "/media/yifu/Samsung_T71/oxford_spires/2024-03-13-maths/gt/individual_pcd" @@ -24,6 +34,7 @@ def run_pcd2bt(cloud_folder: str, cloud_bt_path: str, transform: List[float]): octomap_utils_path = Path(__file__).parent.parent / "octomap_utils" pcd2bt_path = octomap_utils_path / "build" / "pcd2bt" +traverse_bt_path = octomap_utils_path / "build" / "traverse_bt" processes = [] for cloud_folder, cloud_bt_path, transform in zip( @@ -31,7 +42,9 @@ def run_pcd2bt(cloud_folder: str, cloud_bt_path: str, transform: List[float]): [input_cloud_bt_path, gt_cloud_bt_path, gt_cloud_bt_path], [input_cloud_tf, gt_cloud_tf], ): - process = multiprocessing.Process(target=run_pcd2bt, args=(cloud_folder, cloud_bt_path, transform)) + process = multiprocessing.Process( + target=run_pcd2bt, args=(cloud_folder, cloud_bt_path, pcd2bt_path, traverse_bt_path, transform) + ) processes.append(process) for process in processes: From 3392b1776ffdd7c4c599b5eabbb9e985cb689372 Mon Sep 17 00:00:00 2001 From: Yifu Tao Date: Wed, 11 Sep 2024 16:04:44 +0100 Subject: [PATCH 30/54] refactor: remove the custom transform It will be moved to a pre-processing script --- octomap_utils/pcd2bt.cpp | 36 ++++-------------------------------- scripts/recon_benchmark.py | 16 +++++----------- 2 files changed, 9 insertions(+), 43 deletions(-) diff --git a/octomap_utils/pcd2bt.cpp b/octomap_utils/pcd2bt.cpp index 67fef05..c2fbfc6 100644 --- a/octomap_utils/pcd2bt.cpp +++ b/octomap_utils/pcd2bt.cpp @@ -27,7 +27,7 @@ std::vector listFiles(const std::string& folderPath) { return files; } -void processPCDFolder(const std::string& folderPath, double resolution, const std::string& save_path, const Eigen::Affine3f& custom_transform) { +void processPCDFolder(const std::string& folderPath, double resolution, const std::string& save_path) { octomap::OcTree tree(resolution); std::vector pcdFiles = listFiles(folderPath); @@ -71,7 +71,6 @@ void processPCDFolder(const std::string& folderPath, double resolution, const st // Apply the transformation to the point cloud pcl::PointCloud::Ptr transformed_cloud(new pcl::PointCloud()); pcl::transformPointCloud(*cloud, *transformed_cloud, transform); - pcl::transformPointCloud(*transformed_cloud, *transformed_cloud, custom_transform); // Convert transformed PCL cloud to OctoMap point cloud octomap::Pointcloud octomap_cloud; @@ -91,14 +90,12 @@ void processPCDFolder(const std::string& folderPath, double resolution, const st int main(int argc, char** argv) { if (argc < 2) { - std::cerr << "Usage: " << argv[0] << " -r [resolution] -s [saved_path] -tf [transform_path]" << std::endl; + std::cerr << "Usage: " << argv[0] << " -r [resolution] -s [saved_path]" << std::endl; return 1; } std::string folderPath = argv[1]; double resolution = 0.1; std::string save_path = "result_octomap.bt"; - Eigen::Affine3f custom_transform = Eigen::Affine3f::Identity(); - bool transform_set = false; // Parse command line arguments for (int i = 2; i < argc; ++i) { @@ -107,39 +104,14 @@ int main(int argc, char** argv) { resolution = std::stod(argv[++i]); } else if (arg == "-s" && i + 1 < argc) { save_path = argv[++i]; - } else if (arg == "-tf" && i + 7 < argc) { - float tx = std::stof(argv[++i]); - float ty = std::stof(argv[++i]); - float tz = std::stof(argv[++i]); - float qw = std::stof(argv[++i]); - float qx = std::stof(argv[++i]); - float qy = std::stof(argv[++i]); - float qz = std::stof(argv[++i]); - - custom_transform = Eigen::Affine3f::Identity(); - custom_transform.translation() << tx, ty, tz; - Eigen::Quaternionf quaternion(qw, qx, qy, qz); - custom_transform.rotate(quaternion); - - std::cout << "Applied custom transform: " - << "t(" << tx << ", " << ty << ", " << tz << ") " - << "quat(" << qw << ", " << qx << ", " << qy << ", " << qz << ")" << std::endl; - - transform_set = true; } else { std::cerr << "Unknown argument or missing value: " << arg << std::endl; - std::cerr << "Usage: " << argv[0] << " -r [resolution] -s [saved_path] -tf x y z quat_wxyz" << std::endl; + std::cerr << "Usage: " << argv[0] << " -r [resolution] -s [saved_path]" << std::endl; return 1; } } - // If no transformation is provided, use the identity matrix - if (!transform_set) { - std::cout << "No custom transform provided. Using identity transformation." << std::endl; - } - std::cout << " transformation:\n" << custom_transform.matrix() << std::endl; - - processPCDFolder(folderPath, resolution, save_path, custom_transform); + processPCDFolder(folderPath, resolution, save_path); return 0; } diff --git a/scripts/recon_benchmark.py b/scripts/recon_benchmark.py index 88de3d6..883b931 100644 --- a/scripts/recon_benchmark.py +++ b/scripts/recon_benchmark.py @@ -1,15 +1,14 @@ import multiprocessing from pathlib import Path -from typing import List from oxford_spires_utils.bash_command import run_command -def run_pcd2bt(cloud_folder: str, cloud_bt_path: str, pcd2bt_path: str, traverse_bt_path: str, transform: List[float]): +def run_pcd2bt(cloud_folder: str, cloud_bt_path: str, pcd2bt_path: str, traverse_bt_path: str): cloud_folder, cloud_bt_path = str(cloud_folder), str(cloud_bt_path) pcd2bt_path, traverse_bt_path = str(pcd2bt_path), str(traverse_bt_path) - pcd2bt_command = [pcd2bt_path, cloud_folder, "-s", str(cloud_bt_path), "-tf", *map(str, transform)] + pcd2bt_command = [pcd2bt_path, cloud_folder, "-s", str(cloud_bt_path)] pcd2bt_command = " ".join(pcd2bt_command) run_command(pcd2bt_command) @@ -25,9 +24,6 @@ def run_pcd2bt(cloud_folder: str, cloud_bt_path: str, pcd2bt_path: str, traverse 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) -# tf: tx ty tz qw, qx, qy, qz -input_cloud_tf = [-85.867073, 21.640722, 0.720384, 0.766620, -0.0019915, 0.0196701, -0.641796] -gt_cloud_tf = [0, 0, 0, 1, 0, 0, 0] input_cloud_bt_path = Path(project_folder) / "input_cloud.bt" gt_cloud_bt_path = Path(project_folder) / "gt_cloud.bt" @@ -37,13 +33,11 @@ def run_pcd2bt(cloud_folder: str, cloud_bt_path: str, pcd2bt_path: str, traverse traverse_bt_path = octomap_utils_path / "build" / "traverse_bt" processes = [] -for cloud_folder, cloud_bt_path, transform in zip( - [input_cloud_folder_path, gt_cloud_folder_path], - [input_cloud_bt_path, gt_cloud_bt_path, gt_cloud_bt_path], - [input_cloud_tf, gt_cloud_tf], +for cloud_folder, cloud_bt_path in zip( + [input_cloud_folder_path, gt_cloud_folder_path], [input_cloud_bt_path, gt_cloud_bt_path, gt_cloud_bt_path] ): process = multiprocessing.Process( - target=run_pcd2bt, args=(cloud_folder, cloud_bt_path, pcd2bt_path, traverse_bt_path, transform) + target=run_pcd2bt, args=(cloud_folder, cloud_bt_path, pcd2bt_path, traverse_bt_path) ) processes.append(process) From 6f3162c472246bb6eec657b029a5208c79a959c7 Mon Sep 17 00:00:00 2001 From: Yifu Tao Date: Wed, 11 Sep 2024 16:06:19 +0100 Subject: [PATCH 31/54] feat: support quat wxyz input in se3 lib --- oxford_spires_utils/se3.py | 12 ++++++++++++ tests/test_se3.py | 29 ++++++++++++++++++++++++++++- 2 files changed, 40 insertions(+), 1 deletion(-) diff --git a/oxford_spires_utils/se3.py b/oxford_spires_utils/se3.py index 85e4dc1..8f0d4bc 100644 --- a/oxford_spires_utils/se3.py +++ b/oxford_spires_utils/se3.py @@ -9,6 +9,13 @@ def quat_xyzw_to_quat_wxyz(quat_xyzw): 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] @@ -34,6 +41,11 @@ def xyz_quat_xyzw_to_se3_matrix(xyz, quat_xyzw): 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 diff --git a/tests/test_se3.py b/tests/test_se3.py index 86a8e3b..e72cbe9 100644 --- a/tests/test_se3.py +++ b/tests/test_se3.py @@ -2,7 +2,13 @@ import pytest from scipy.spatial.transform import Rotation -from oxford_spires_utils.se3 import se3_matrix_to_xyz_quat_xyzw, xyz_quat_xyzw_to_se3_matrix +from oxford_spires_utils.se3 import ( + quat_xyzw_to_quat_wxyz, + se3_matrix_to_xyz_quat_wxyz, + se3_matrix_to_xyz_quat_xyzw, + xyz_quat_wxyz_to_se3_matrix, + xyz_quat_xyzw_to_se3_matrix, +) def test_se3_matrix_to_xyz_quat_xyzw(): @@ -69,6 +75,27 @@ def test_xyz_quat_xyzw_to_se3_matrix(): assert np.allclose(se3, expected) +def test_se3_matrix_to_xyz_quat_wxyz(): + # Test conversion from SE(3) matrix to XYZ + quaternion (WXYZ) + se3 = np.eye(4) + se3[:3, 3] = [1, 2, 3] + se3[:3, :3] = Rotation.from_euler("xyz", [45, 45, 45], degrees=True).as_matrix() + xyz, quat = se3_matrix_to_xyz_quat_wxyz(se3) + expected_quat = quat_xyzw_to_quat_wxyz(Rotation.from_euler("xyz", [45, 45, 45], degrees=True).as_quat()) + assert np.allclose(xyz, [1, 2, 3], atol=1e-6) + assert np.allclose(quat, expected_quat, atol=1e-6) + + +def test_xyz_quat_wxyz_to_se3_matrix(): + # Test conversion from XYZ + quaternion (WXYZ) to SE(3) matrix + xyz = [1, 2, 3] + quat = [1, 0, 0, 0] # Identity quaternion WXYZ + se3 = xyz_quat_wxyz_to_se3_matrix(xyz, quat) + expected = np.eye(4) + expected[:3, 3] = xyz + assert np.allclose(se3, expected, atol=1e-6) + + def test_roundtrip_conversion(): # Test that converting from SE3 to xyz-quat and back gives the original matrix original_se3 = np.eye(4) From b919c6a941bd3994f760fab819685f1d7122fa8a Mon Sep 17 00:00:00 2001 From: Yifu Tao Date: Wed, 11 Sep 2024 16:33:30 +0100 Subject: [PATCH 32/54] feat: add transform_3d_cloud function --- oxford_spires_utils/point_cloud.py | 17 +++++++++++++++++ 1 file changed, 17 insertions(+) diff --git a/oxford_spires_utils/point_cloud.py b/oxford_spires_utils/point_cloud.py index cc2eaa6..f6df406 100644 --- a/oxford_spires_utils/point_cloud.py +++ b/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): From f01a3b8d1c4f234b3fc2b420b45e383f9b976faf Mon Sep 17 00:00:00 2001 From: Yifu Tao Date: Wed, 11 Sep 2024 16:35:19 +0100 Subject: [PATCH 33/54] feat: add script to transform a folder of pcds --- scripts/transform_pcd_folder.py | 50 +++++++++++++++++++++++++++++++++ 1 file changed, 50 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..339e7b2 --- /dev/null +++ b/scripts/transform_pcd_folder.py @@ -0,0 +1,50 @@ +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]]) + se3_matrix_txt_path = "/home/yifu/workspace/Spires_2025/2024-03-13-maths_1/T_rtc_lidar" + transform_matrix = np.loadtxt(se3_matrix_txt_path) + + transform_pcd_folder(pcd_folder_path, new_pcd_folder_path, transform_matrix) From 4cd101c7bd8c92f4c4b908ad2ae4fe4423b878bf Mon Sep 17 00:00:00 2001 From: Yifu Tao Date: Wed, 11 Sep 2024 17:12:08 +0100 Subject: [PATCH 34/54] refactor: rename traverse_bt to get_occ_free_from_bt --- README.md | 2 +- octomap_utils/CMakeLists.txt | 4 ++-- ...averse_bt.cpp => get_occ_free_from_bt.cpp} | 0 scripts/recon_benchmark.py | 23 ++++++++++++------- 4 files changed, 18 insertions(+), 11 deletions(-) rename octomap_utils/{traverse_bt.cpp => get_occ_free_from_bt.cpp} (100%) diff --git a/README.md b/README.md index f521487..8bcfa04 100644 --- a/README.md +++ b/README.md @@ -33,7 +33,7 @@ cd /octomap_utils/build ```bash cd /octomap_utils/build -./traverse_bt +./octomap_utils/get_occ_free_from_bt.cpp -sf -so ``` ## Contributing diff --git a/octomap_utils/CMakeLists.txt b/octomap_utils/CMakeLists.txt index 852f9d5..605e4a7 100644 --- a/octomap_utils/CMakeLists.txt +++ b/octomap_utils/CMakeLists.txt @@ -18,5 +18,5 @@ include_directories(${PCL_INCLUDE_DIRS}) add_executable(pcd2bt pcd2bt.cpp) target_link_libraries(pcd2bt ${OCTOMAP_LIBRARIES} ${PCL_LIBRARIES}) -add_executable(traverse_bt traverse_bt.cpp) -target_link_libraries(traverse_bt ${OCTOMAP_LIBRARIES} ${PCL_LIBRARIES}) \ No newline at end of file +add_executable(get_occ_free_from_bt get_occ_free_from_bt.cpp) +target_link_libraries(get_occ_free_from_bt ${OCTOMAP_LIBRARIES} ${PCL_LIBRARIES}) \ No newline at end of file diff --git a/octomap_utils/traverse_bt.cpp b/octomap_utils/get_occ_free_from_bt.cpp similarity index 100% rename from octomap_utils/traverse_bt.cpp rename to octomap_utils/get_occ_free_from_bt.cpp diff --git a/scripts/recon_benchmark.py b/scripts/recon_benchmark.py index 883b931..87c1bd8 100644 --- a/scripts/recon_benchmark.py +++ b/scripts/recon_benchmark.py @@ -4,9 +4,9 @@ from oxford_spires_utils.bash_command import run_command -def run_pcd2bt(cloud_folder: str, cloud_bt_path: str, pcd2bt_path: str, traverse_bt_path: str): +def run_pcd2bt(cloud_folder: str, cloud_bt_path: str, pcd2bt_path: str, get_occ_free_from_bt_path: str): cloud_folder, cloud_bt_path = str(cloud_folder), str(cloud_bt_path) - pcd2bt_path, traverse_bt_path = str(pcd2bt_path), str(traverse_bt_path) + pcd2bt_path, get_occ_free_from_bt_path = str(pcd2bt_path), str(get_occ_free_from_bt_path) pcd2bt_command = [pcd2bt_path, cloud_folder, "-s", str(cloud_bt_path)] pcd2bt_command = " ".join(pcd2bt_command) @@ -14,10 +14,17 @@ def run_pcd2bt(cloud_folder: str, cloud_bt_path: str, pcd2bt_path: str, traverse saved_occ_pcd_path = str(Path(cloud_bt_path).with_name(f"{Path(cloud_bt_path).stem}_occ.pcd")) saved_free_pcd_path = str(Path(cloud_bt_path).with_name(f"{Path(cloud_bt_path).stem}_free.pcd")) - traverse_bt_command = [traverse_bt_path, cloud_bt_path, "-so", saved_occ_pcd_path, "-sf", saved_free_pcd_path] - print(traverse_bt_command) - traverse_bt_command = " ".join(traverse_bt_command) - run_command(traverse_bt_command) + get_occ_free_from_bt_command = [ + get_occ_free_from_bt_path, + cloud_bt_path, + "-so", + saved_occ_pcd_path, + "-sf", + saved_free_pcd_path, + ] + print(get_occ_free_from_bt_command) + get_occ_free_from_bt_command = " ".join(get_occ_free_from_bt_command) + run_command(get_occ_free_from_bt_command) input_cloud_folder_path = "/home/yifu/data/nerf_data_pipeline/2024-03-13-maths_1/raw/individual_clouds" @@ -30,14 +37,14 @@ def run_pcd2bt(cloud_folder: str, cloud_bt_path: str, pcd2bt_path: str, traverse octomap_utils_path = Path(__file__).parent.parent / "octomap_utils" pcd2bt_path = octomap_utils_path / "build" / "pcd2bt" -traverse_bt_path = octomap_utils_path / "build" / "traverse_bt" +get_occ_free_from_bt_path = octomap_utils_path / "build" / "get_occ_free_from_bt" processes = [] for cloud_folder, cloud_bt_path in zip( [input_cloud_folder_path, gt_cloud_folder_path], [input_cloud_bt_path, gt_cloud_bt_path, gt_cloud_bt_path] ): process = multiprocessing.Process( - target=run_pcd2bt, args=(cloud_folder, cloud_bt_path, pcd2bt_path, traverse_bt_path) + target=run_pcd2bt, args=(cloud_folder, cloud_bt_path, pcd2bt_path, get_occ_free_from_bt_path) ) processes.append(process) From b2135260c3abfb7f9411a5b89866a38f46cab054 Mon Sep 17 00:00:00 2001 From: Yifu Tao Date: Wed, 11 Sep 2024 18:20:36 +0100 Subject: [PATCH 35/54] refactor: move progressbar to shared lib --- octomap_utils/CMakeLists.txt | 19 +++++++++++++------ octomap_utils/get_occ_free_from_bt.cpp | 18 ++---------------- octomap_utils/include/progress_bar.h | 11 +++++++++++ octomap_utils/progress_bar.cpp | 19 +++++++++++++++++++ 4 files changed, 45 insertions(+), 22 deletions(-) create mode 100644 octomap_utils/include/progress_bar.h create mode 100644 octomap_utils/progress_bar.cpp diff --git a/octomap_utils/CMakeLists.txt b/octomap_utils/CMakeLists.txt index 605e4a7..6e40349 100644 --- a/octomap_utils/CMakeLists.txt +++ b/octomap_utils/CMakeLists.txt @@ -10,13 +10,20 @@ find_package(Octomap REQUIRED) find_package(PCL REQUIRED) -# Include the OctoMap headers -include_directories(${OCTOMAP_INCLUDE_DIRS}) -include_directories(${PCL_INCLUDE_DIRS}) - +include_directories( + include + ${OCTOMAP_INCLUDE_DIRS} + ${PCL_INCLUDE_DIRS} +) + +add_library(${PROJECT_NAME} SHARED + progress_bar.cpp +) +target_link_libraries(${PROJECT_NAME} ${OCTOMAP_LIBRARIES}) +target_link_libraries(${PROJECT_NAME} ${PCL_LIBRARIES}) add_executable(pcd2bt pcd2bt.cpp) -target_link_libraries(pcd2bt ${OCTOMAP_LIBRARIES} ${PCL_LIBRARIES}) +target_link_libraries(pcd2bt ${PROJECT_NAME}) add_executable(get_occ_free_from_bt get_occ_free_from_bt.cpp) -target_link_libraries(get_occ_free_from_bt ${OCTOMAP_LIBRARIES} ${PCL_LIBRARIES}) \ No newline at end of file +target_link_libraries(get_occ_free_from_bt ${PROJECT_NAME}) \ No newline at end of file diff --git a/octomap_utils/get_occ_free_from_bt.cpp b/octomap_utils/get_occ_free_from_bt.cpp index 9256760..e107f32 100644 --- a/octomap_utils/get_occ_free_from_bt.cpp +++ b/octomap_utils/get_occ_free_from_bt.cpp @@ -6,26 +6,12 @@ #include #include +#include "progress_bar.h" + using namespace std; using namespace octomap; using PointCloud = pcl::PointCloud; -// Function to display a simple progress bar -void displayProgressBar(size_t current, size_t total) { - const int barWidth = 50; // Width of the progress bar - float progress = static_cast(current) / total; - int pos = static_cast(barWidth * progress); - - cout << "["; - for (int i = 0; i < barWidth; ++i) { - if (i < pos) cout << "="; - else if (i == pos) cout << ">"; - else cout << " "; - } - cout << "] " << int(progress * 100.0) << "%\r"; - cout.flush(); -} - void convertOctreeToPointCloud(const OcTree& tree, PointCloud::Ptr& free_cloud, PointCloud::Ptr& occupied_cloud) { // Get the total number of leaf nodes for the progress bar size_t total_leafs = tree.getNumLeafNodes(); diff --git a/octomap_utils/include/progress_bar.h b/octomap_utils/include/progress_bar.h new file mode 100644 index 0000000..1154813 --- /dev/null +++ b/octomap_utils/include/progress_bar.h @@ -0,0 +1,11 @@ +// progress_bar.h + +#ifndef PROGRESS_BAR_H +#define PROGRESS_BAR_H + +#include + +// Function to display a simple progress bar +void displayProgressBar(size_t current, size_t total); + +#endif // PROGRESS_BAR_H diff --git a/octomap_utils/progress_bar.cpp b/octomap_utils/progress_bar.cpp new file mode 100644 index 0000000..bf5a3bc --- /dev/null +++ b/octomap_utils/progress_bar.cpp @@ -0,0 +1,19 @@ +#include "progress_bar.h" + +void displayProgressBar(size_t current, size_t total) { + const int barWidth = 50; // Width of the progress bar + float progress = static_cast(current) / total; + int pos = static_cast(barWidth * progress); + + std::cout << "["; + for (int i = 0; i < barWidth; ++i) { + if (i < pos) + std::cout << "="; + else if (i == pos) + std::cout << ">"; + else + std::cout << " "; + } + std::cout << "] " << int(progress * 100.0) << "%\r"; + std::cout.flush(); +} From 3f87e8092b5e997a99bc282da7a2967f2eb19440 Mon Sep 17 00:00:00 2001 From: Yifu Tao Date: Wed, 11 Sep 2024 22:19:15 +0100 Subject: [PATCH 36/54] feat: add script filtering points unknown in an octomap --- octomap_utils/CMakeLists.txt | 5 ++- octomap_utils/filter_pcd_with_bt.cpp | 62 ++++++++++++++++++++++++++++ 2 files changed, 66 insertions(+), 1 deletion(-) create mode 100644 octomap_utils/filter_pcd_with_bt.cpp diff --git a/octomap_utils/CMakeLists.txt b/octomap_utils/CMakeLists.txt index 6e40349..c8a7698 100644 --- a/octomap_utils/CMakeLists.txt +++ b/octomap_utils/CMakeLists.txt @@ -26,4 +26,7 @@ add_executable(pcd2bt pcd2bt.cpp) target_link_libraries(pcd2bt ${PROJECT_NAME}) add_executable(get_occ_free_from_bt get_occ_free_from_bt.cpp) -target_link_libraries(get_occ_free_from_bt ${PROJECT_NAME}) \ No newline at end of file +target_link_libraries(get_occ_free_from_bt ${PROJECT_NAME}) + +add_executable(filter_pcd_with_bt filter_pcd_with_bt.cpp ) +target_link_libraries(filter_pcd_with_bt ${PROJECT_NAME}) \ No newline at end of file diff --git a/octomap_utils/filter_pcd_with_bt.cpp b/octomap_utils/filter_pcd_with_bt.cpp new file mode 100644 index 0000000..1e33470 --- /dev/null +++ b/octomap_utils/filter_pcd_with_bt.cpp @@ -0,0 +1,62 @@ +#include +#include +#include +#include + +// Function to check if a point is unknown in the octree +bool isUnknownInOctree(const octomap::OcTree& tree, const octomap::point3d& point) { + auto node = tree.search(point); + // if the node does not exist, then it is unknown + return !node; +} + +// Function to remove unknown points from the point cloud +void removeUnknownPoints( + pcl::PointCloud& cloud, + const octomap::OcTree& tree, + const std::string& output_file) { + + pcl::PointCloud filtered_cloud; + + for (const auto& pcl_point : cloud) { + // Convert the PCL point to an octomap point + octomap::point3d point(pcl_point.x, pcl_point.y, pcl_point.z); + + // Check if the point is unknown in the octree + if (!isUnknownInOctree(tree, point)) { + filtered_cloud.push_back(pcl_point); + } + } + + // Save the filtered point cloud to a file + pcl::io::savePCDFileASCII(output_file, filtered_cloud); + std::cout << "Filtered point cloud saved to " << output_file << std::endl; +} + +int main(int argc, char** argv) { + if (argc < 4) { + std::cerr << "Usage: " << argv[0] << " " << std::endl; + return 1; + } + + // Load the octree from the BT file + octomap::OcTree tree(argv[1]); + if (tree.size() == 0) { + std::cerr << "Failed to load octomap from " << argv[1] << std::endl; + return 1; + } + + // Load the input point cloud + pcl::PointCloud cloud; + if (pcl::io::loadPCDFile(argv[2], cloud) == -1) { + std::cerr << "Failed to load point cloud from " << argv[2] << std::endl; + return 1; + } + + std::cout << "Loaded point cloud with " << cloud.size() << " points." << std::endl; + + // Remove unknown points from the point cloud + removeUnknownPoints(cloud, tree, argv[3]); + + return 0; +} From 0fe76c3f67b7a98b0a00ce249a267c64843db7b1 Mon Sep 17 00:00:00 2001 From: Yifu Tao Date: Wed, 11 Sep 2024 22:45:52 +0100 Subject: [PATCH 37/54] feat: add cloud filter script --- scripts/recon_benchmark.py | 12 ++++++++++++ 1 file changed, 12 insertions(+) diff --git a/scripts/recon_benchmark.py b/scripts/recon_benchmark.py index 87c1bd8..1addc39 100644 --- a/scripts/recon_benchmark.py +++ b/scripts/recon_benchmark.py @@ -38,6 +38,7 @@ def run_pcd2bt(cloud_folder: str, cloud_bt_path: str, pcd2bt_path: str, get_occ_ octomap_utils_path = Path(__file__).parent.parent / "octomap_utils" pcd2bt_path = octomap_utils_path / "build" / "pcd2bt" get_occ_free_from_bt_path = octomap_utils_path / "build" / "get_occ_free_from_bt" +filter_pcd_with_bt_path = octomap_utils_path / "build" / "filter_pcd_with_bt" processes = [] for cloud_folder, cloud_bt_path in zip( @@ -53,3 +54,14 @@ def run_pcd2bt(cloud_folder: str, cloud_bt_path: str, pcd2bt_path: str, get_occ_ for process in processes: process.join() + +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")) +filter_pcd_command = [str(filter_pcd_with_bt_path), str(gt_cloud_bt_path), input_occ_pcd_path, input_occ_filtered_path] +filter_pcd_command = " ".join(filter_pcd_command) +run_command(filter_pcd_command) +filter_pcd_command = [str(filter_pcd_with_bt_path), str(input_cloud_bt_path), gt_occ_pcd_path, gt_occ_filtered_path] +filter_pcd_command = " ".join(filter_pcd_command) +run_command(filter_pcd_command) From 38fd7095a20ed0cc2ce545fc7195afbcde3d53d8 Mon Sep 17 00:00:00 2001 From: Yifu Tao Date: Fri, 13 Sep 2024 02:48:17 +0100 Subject: [PATCH 38/54] feat: add pybind11 in new spires_cpp folder this is to store all cpp files --- spires_cpp/CMakeLists.txt | 28 +++++++ spires_cpp/README.md | 0 spires_cpp/pyproject.toml | 92 +++++++++++++++++++++++ spires_cpp/src/spires_cpp/__init__.py | 5 ++ spires_cpp/src/spires_cpp/my_lib/main.cpp | 45 +++++++++++ 5 files changed, 170 insertions(+) create mode 100644 spires_cpp/CMakeLists.txt create mode 100644 spires_cpp/README.md create mode 100644 spires_cpp/pyproject.toml create mode 100644 spires_cpp/src/spires_cpp/__init__.py create mode 100644 spires_cpp/src/spires_cpp/my_lib/main.cpp diff --git a/spires_cpp/CMakeLists.txt b/spires_cpp/CMakeLists.txt new file mode 100644 index 0000000..baa4993 --- /dev/null +++ b/spires_cpp/CMakeLists.txt @@ -0,0 +1,28 @@ +# Require CMake 3.15+ (matching scikit-build-core) Use new versions of all +# policies up to CMake 3.27 +cmake_minimum_required(VERSION 3.15...3.27) + +# Scikit-build-core sets these values for you, or you can just hard-code the +# name and version. +project( + ${SKBUILD_PROJECT_NAME} + VERSION ${SKBUILD_PROJECT_VERSION} + LANGUAGES CXX) + +# Find the module development requirements (requires FindPython from 3.17 or +# scikit-build-core's built-in backport) +find_package(Python REQUIRED COMPONENTS Interpreter Development.Module) +find_package(pybind11 CONFIG REQUIRED) +find_package(PCL REQUIRED) + +# Add a library using FindPython's tooling (pybind11 also provides a helper like +# this) +include_directories(${PCL_INCLUDE_DIRS}) +python_add_library(_core MODULE src/spires_cpp/my_lib/main.cpp WITH_SOABI) +target_link_libraries(_core PRIVATE pybind11::headers ${PCL_LIBRARIES}) + +# This is passing in the version as a define just as an example +target_compile_definitions(_core PRIVATE VERSION_INFO=${PROJECT_VERSION}) + +# The install directory is the output (wheel) directory +install(TARGETS _core DESTINATION spires_cpp) \ No newline at end of file diff --git a/spires_cpp/README.md b/spires_cpp/README.md new file mode 100644 index 0000000..e69de29 diff --git a/spires_cpp/pyproject.toml b/spires_cpp/pyproject.toml new file mode 100644 index 0000000..0234551 --- /dev/null +++ b/spires_cpp/pyproject.toml @@ -0,0 +1,92 @@ +[build-system] +requires = ["scikit-build-core>=0.3.3", "pybind11"] +build-backend = "scikit_build_core.build" + + +[project] +name = "spires_cpp" +version = "0.0.1" +description="A minimal example package (with pybind11)" +readme = "README.md" +authors = [ + { name = "My Name", email = "me@email.com" }, +] +requires-python = ">=3.7" +classifiers = [ + "Development Status :: 4 - Beta", + "License :: OSI Approved :: MIT License", + "Programming Language :: Python :: 3 :: Only", + "Programming Language :: Python :: 3.7", + "Programming Language :: Python :: 3.8", + "Programming Language :: Python :: 3.9", + "Programming Language :: Python :: 3.10", + "Programming Language :: Python :: 3.11", + "Programming Language :: Python :: 3.12", +] + +[project.optional-dependencies] +test = ["pytest"] + + +[tool.scikit-build] +wheel.expand-macos-universal-tags = true +wheel.packages = ["src/spires_cpp"] + + +[tool.pytest.ini_options] +minversion = "6.0" +addopts = ["-ra", "--showlocals", "--strict-markers", "--strict-config"] +xfail_strict = true +log_cli_level = "INFO" +filterwarnings = [ + "error", + "ignore::pytest.PytestCacheWarning", +] +testpaths = ["tests"] + + +[tool.cibuildwheel] +build-frontend = "build[uv]" +test-command = "pytest {project}/tests" +test-extras = ["test"] + +[tool.cibuildwheel.pyodide] +environment.CFLAGS = "-fexceptions" +environment.LDFLAGS = "-fexceptions" +build-frontend = {name = "build", args = ["--exports", "whole_archive"]} + +[tool.ruff] +src = ["src"] + +[tool.ruff.lint] +extend-select = [ + "B", # flake8-bugbear + "I", # isort + "ARG", # flake8-unused-arguments + "C4", # flake8-comprehensions + "EM", # flake8-errmsg + "ICN", # flake8-import-conventions + "G", # flake8-logging-format + "PGH", # pygrep-hooks + "PIE", # flake8-pie + "PL", # pylint + "PT", # flake8-pytest-style + "PTH", # flake8-use-pathlib + "RET", # flake8-return + "RUF", # Ruff-specific + "SIM", # flake8-simplify + "T20", # flake8-print + "UP", # pyupgrade + "YTT", # flake8-2020 + "EXE", # flake8-executable + "NPY", # NumPy specific rules + "PD", # pandas-vet +] +ignore = [ + "PLR09", # Too many X + "PLR2004", # Magic comparison +] +isort.required-imports = ["from __future__ import annotations"] + +[tool.ruff.lint.per-file-ignores] +"tests/**" = ["T20"] \ No newline at end of file diff --git a/spires_cpp/src/spires_cpp/__init__.py b/spires_cpp/src/spires_cpp/__init__.py new file mode 100644 index 0000000..15188e5 --- /dev/null +++ b/spires_cpp/src/spires_cpp/__init__.py @@ -0,0 +1,5 @@ +from __future__ import annotations + +from ._core import __doc__, __version__, add, subtract + +__all__ = ["__doc__", "__version__", "add", "subtract"] diff --git a/spires_cpp/src/spires_cpp/my_lib/main.cpp b/spires_cpp/src/spires_cpp/my_lib/main.cpp new file mode 100644 index 0000000..884398c --- /dev/null +++ b/spires_cpp/src/spires_cpp/my_lib/main.cpp @@ -0,0 +1,45 @@ +#include +#include +#include + +#define STRINGIFY(x) #x +#define MACRO_STRINGIFY(x) STRINGIFY(x) + +int add(int i, int j) { + return i + j; +} + +namespace py = pybind11; + +PYBIND11_MODULE(_core, m) { + m.doc() = R"pbdoc( + Pybind11 example plugin + ----------------------- + + .. currentmodule:: scikit_build_example + + .. autosummary:: + :toctree: _generate + + add + subtract + )pbdoc"; + + m.def("add", &add, R"pbdoc( + Add two numbers + + Some other explanation about the add function. + )pbdoc"); + + m.def("subtract", [](int i, int j) { return i - j; }, R"pbdoc( + Subtract two numbers + + Some other explanation about the subtract function. + )pbdoc"); + +#ifdef VERSION_INFO + m.attr("__version__") = MACRO_STRINGIFY(VERSION_INFO); +#else + m.attr("__version__") = "dev"; +#endif +} \ No newline at end of file From e2bf36d1cd044cbda2e962bfcb33553985e6b440 Mon Sep 17 00:00:00 2001 From: Yifu Tao Date: Fri, 13 Sep 2024 02:51:44 +0100 Subject: [PATCH 39/54] refactor: move octomap code to spires_cpp --- .../src/spires_cpp}/include/progress_bar.h | 0 .../src/spires_cpp/octomap}/filter_pcd_with_bt.cpp | 0 .../src/spires_cpp/octomap}/get_occ_free_from_bt.cpp | 0 {octomap_utils => spires_cpp/src/spires_cpp/octomap}/pcd2bt.cpp | 0 .../src/spires_cpp/octomap}/progress_bar.cpp | 0 5 files changed, 0 insertions(+), 0 deletions(-) rename {octomap_utils => spires_cpp/src/spires_cpp}/include/progress_bar.h (100%) rename {octomap_utils => spires_cpp/src/spires_cpp/octomap}/filter_pcd_with_bt.cpp (100%) rename {octomap_utils => spires_cpp/src/spires_cpp/octomap}/get_occ_free_from_bt.cpp (100%) rename {octomap_utils => spires_cpp/src/spires_cpp/octomap}/pcd2bt.cpp (100%) rename {octomap_utils => spires_cpp/src/spires_cpp/octomap}/progress_bar.cpp (100%) diff --git a/octomap_utils/include/progress_bar.h b/spires_cpp/src/spires_cpp/include/progress_bar.h similarity index 100% rename from octomap_utils/include/progress_bar.h rename to spires_cpp/src/spires_cpp/include/progress_bar.h diff --git a/octomap_utils/filter_pcd_with_bt.cpp b/spires_cpp/src/spires_cpp/octomap/filter_pcd_with_bt.cpp similarity index 100% rename from octomap_utils/filter_pcd_with_bt.cpp rename to spires_cpp/src/spires_cpp/octomap/filter_pcd_with_bt.cpp diff --git a/octomap_utils/get_occ_free_from_bt.cpp b/spires_cpp/src/spires_cpp/octomap/get_occ_free_from_bt.cpp similarity index 100% rename from octomap_utils/get_occ_free_from_bt.cpp rename to spires_cpp/src/spires_cpp/octomap/get_occ_free_from_bt.cpp diff --git a/octomap_utils/pcd2bt.cpp b/spires_cpp/src/spires_cpp/octomap/pcd2bt.cpp similarity index 100% rename from octomap_utils/pcd2bt.cpp rename to spires_cpp/src/spires_cpp/octomap/pcd2bt.cpp diff --git a/octomap_utils/progress_bar.cpp b/spires_cpp/src/spires_cpp/octomap/progress_bar.cpp similarity index 100% rename from octomap_utils/progress_bar.cpp rename to spires_cpp/src/spires_cpp/octomap/progress_bar.cpp From c90df625b1558eb6a96fe09a7b1ef08a17ceff70 Mon Sep 17 00:00:00 2001 From: Yifu Tao Date: Fri, 13 Sep 2024 15:37:10 +0100 Subject: [PATCH 40/54] refactor: remove the dummy folder --- spires_cpp/CMakeLists.txt | 2 +- spires_cpp/src/spires_cpp/{my_lib => }/main.cpp | 0 2 files changed, 1 insertion(+), 1 deletion(-) rename spires_cpp/src/spires_cpp/{my_lib => }/main.cpp (100%) diff --git a/spires_cpp/CMakeLists.txt b/spires_cpp/CMakeLists.txt index baa4993..d499a5e 100644 --- a/spires_cpp/CMakeLists.txt +++ b/spires_cpp/CMakeLists.txt @@ -18,7 +18,7 @@ find_package(PCL REQUIRED) # Add a library using FindPython's tooling (pybind11 also provides a helper like # this) include_directories(${PCL_INCLUDE_DIRS}) -python_add_library(_core MODULE src/spires_cpp/my_lib/main.cpp WITH_SOABI) +python_add_library(_core MODULE src/spires_cpp/main.cpp WITH_SOABI) target_link_libraries(_core PRIVATE pybind11::headers ${PCL_LIBRARIES}) # This is passing in the version as a define just as an example diff --git a/spires_cpp/src/spires_cpp/my_lib/main.cpp b/spires_cpp/src/spires_cpp/main.cpp similarity index 100% rename from spires_cpp/src/spires_cpp/my_lib/main.cpp rename to spires_cpp/src/spires_cpp/main.cpp From 0a6c87e1def883edecea1df41d86d9da58ffc2bb Mon Sep 17 00:00:00 2001 From: Yifu Tao Date: Fri, 13 Sep 2024 16:38:17 +0100 Subject: [PATCH 41/54] feat: add pcd2bt to pybind lib --- spires_cpp/CMakeLists.txt | 19 ++++++++-- spires_cpp/include/pcd2bt.h | 20 ++++++++++ spires_cpp/src/spires_cpp/__init__.py | 4 +- spires_cpp/src/spires_cpp/main.cpp | 19 +++------- spires_cpp/src/spires_cpp/octomap/pcd2bt.cpp | 39 +------------------- 5 files changed, 45 insertions(+), 56 deletions(-) create mode 100644 spires_cpp/include/pcd2bt.h diff --git a/spires_cpp/CMakeLists.txt b/spires_cpp/CMakeLists.txt index d499a5e..4b013cf 100644 --- a/spires_cpp/CMakeLists.txt +++ b/spires_cpp/CMakeLists.txt @@ -14,12 +14,25 @@ project( find_package(Python REQUIRED COMPONENTS Interpreter Development.Module) find_package(pybind11 CONFIG REQUIRED) find_package(PCL REQUIRED) +find_package(Eigen3 REQUIRED) +find_package(octomap REQUIRED) # Add a library using FindPython's tooling (pybind11 also provides a helper like # this) -include_directories(${PCL_INCLUDE_DIRS}) -python_add_library(_core MODULE src/spires_cpp/main.cpp WITH_SOABI) -target_link_libraries(_core PRIVATE pybind11::headers ${PCL_LIBRARIES}) +include_directories(${PCL_INCLUDE_DIRS} ${Eigen3_INCLUDE_DIRS} ${OCTOMAP_INCLUDE_DIRS}) +include_directories(${CMAKE_SOURCE_DIR}/include) + +set(SOURCES + src/spires_cpp/main.cpp + src/spires_cpp/octomap/pcd2bt.cpp +) + +python_add_library(_core MODULE ${SOURCES} WITH_SOABI) +target_link_libraries(_core PRIVATE + pybind11::headers + ${PCL_LIBRARIES} + ${OCTOMAP_LIBRARIES} +) # This is passing in the version as a define just as an example target_compile_definitions(_core PRIVATE VERSION_INFO=${PROJECT_VERSION}) diff --git a/spires_cpp/include/pcd2bt.h b/spires_cpp/include/pcd2bt.h new file mode 100644 index 0000000..8f05ed8 --- /dev/null +++ b/spires_cpp/include/pcd2bt.h @@ -0,0 +1,20 @@ +#ifndef PROCESS_PCD_FOLDER_H +#define PROCESS_PCD_FOLDER_H + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + + +// Function to process PCD files in a folder and generate an OctoMap +void processPCDFolder(const std::string& folderPath, double resolution, const std::string& save_path); + +#endif // PROCESS_PCD_FOLDER_H diff --git a/spires_cpp/src/spires_cpp/__init__.py b/spires_cpp/src/spires_cpp/__init__.py index 15188e5..1d9f8e5 100644 --- a/spires_cpp/src/spires_cpp/__init__.py +++ b/spires_cpp/src/spires_cpp/__init__.py @@ -1,5 +1,5 @@ from __future__ import annotations -from ._core import __doc__, __version__, add, subtract +from ._core import __doc__, __version__, processPCDFolder -__all__ = ["__doc__", "__version__", "add", "subtract"] +__all__ = ["__doc__", "__version__", "processPCDFolder"] diff --git a/spires_cpp/src/spires_cpp/main.cpp b/spires_cpp/src/spires_cpp/main.cpp index 884398c..ba92c1b 100644 --- a/spires_cpp/src/spires_cpp/main.cpp +++ b/spires_cpp/src/spires_cpp/main.cpp @@ -1,6 +1,8 @@ #include #include #include +#include "progress_bar.h" +#include "pcd2bt.h" #define STRINGIFY(x) #x #define MACRO_STRINGIFY(x) STRINGIFY(x) @@ -24,19 +26,10 @@ PYBIND11_MODULE(_core, m) { add subtract )pbdoc"; - - m.def("add", &add, R"pbdoc( - Add two numbers - - Some other explanation about the add function. - )pbdoc"); - - m.def("subtract", [](int i, int j) { return i - j; }, R"pbdoc( - Subtract two numbers - - Some other explanation about the subtract function. - )pbdoc"); - + m.def("processPCDFolder", &processPCDFolder, "Process PCD files and save to OctoMap", + py::arg("folderPath"), py::arg("resolution"), py::arg("save_path")); + // m.def("processPCDFolder", &processPCDFolder, "Process PCD files and save to OctoMap", + // py::arg("folderPath"), py::arg("resolution"), py::arg("save_path")); #ifdef VERSION_INFO m.attr("__version__") = MACRO_STRINGIFY(VERSION_INFO); #else diff --git a/spires_cpp/src/spires_cpp/octomap/pcd2bt.cpp b/spires_cpp/src/spires_cpp/octomap/pcd2bt.cpp index c2fbfc6..44b313c 100644 --- a/spires_cpp/src/spires_cpp/octomap/pcd2bt.cpp +++ b/spires_cpp/src/spires_cpp/octomap/pcd2bt.cpp @@ -1,14 +1,4 @@ -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include +#include "pcd2bt.h" // Function to list files in a directory std::vector listFiles(const std::string& folderPath) { @@ -88,30 +78,3 @@ void processPCDFolder(const std::string& folderPath, double resolution, const st std::cout << "Octomap saved to " << save_path << std::endl; } -int main(int argc, char** argv) { - if (argc < 2) { - std::cerr << "Usage: " << argv[0] << " -r [resolution] -s [saved_path]" << std::endl; - return 1; - } - std::string folderPath = argv[1]; - double resolution = 0.1; - std::string save_path = "result_octomap.bt"; - - // Parse command line arguments - for (int i = 2; i < argc; ++i) { - std::string arg = argv[i]; - if (arg == "-r" && i + 1 < argc) { - resolution = std::stod(argv[++i]); - } else if (arg == "-s" && i + 1 < argc) { - save_path = argv[++i]; - } else { - std::cerr << "Unknown argument or missing value: " << arg << std::endl; - std::cerr << "Usage: " << argv[0] << " -r [resolution] -s [saved_path]" << std::endl; - return 1; - } - } - - processPCDFolder(folderPath, resolution, save_path); - - return 0; -} From 048ee0551129f9ae339d880e11e690c030275733 Mon Sep 17 00:00:00 2001 From: Yifu Tao Date: Fri, 13 Sep 2024 17:22:07 +0100 Subject: [PATCH 42/54] feat: add remove_unkown_points to the pybind --- spires_cpp/CMakeLists.txt | 3 +-- spires_cpp/include/filter_pcd_with_bt.h | 12 ++++++++++++ spires_cpp/src/spires_cpp/main.cpp | 7 +++++-- .../src/spires_cpp/octomap/filter_pcd_with_bt.cpp | 5 +---- 4 files changed, 19 insertions(+), 8 deletions(-) create mode 100644 spires_cpp/include/filter_pcd_with_bt.h diff --git a/spires_cpp/CMakeLists.txt b/spires_cpp/CMakeLists.txt index 4b013cf..99d23e0 100644 --- a/spires_cpp/CMakeLists.txt +++ b/spires_cpp/CMakeLists.txt @@ -17,14 +17,13 @@ find_package(PCL REQUIRED) find_package(Eigen3 REQUIRED) find_package(octomap REQUIRED) -# Add a library using FindPython's tooling (pybind11 also provides a helper like -# this) include_directories(${PCL_INCLUDE_DIRS} ${Eigen3_INCLUDE_DIRS} ${OCTOMAP_INCLUDE_DIRS}) include_directories(${CMAKE_SOURCE_DIR}/include) set(SOURCES src/spires_cpp/main.cpp src/spires_cpp/octomap/pcd2bt.cpp + src/spires_cpp/octomap/filter_pcd_with_bt.cpp ) python_add_library(_core MODULE ${SOURCES} WITH_SOABI) diff --git a/spires_cpp/include/filter_pcd_with_bt.h b/spires_cpp/include/filter_pcd_with_bt.h new file mode 100644 index 0000000..453565a --- /dev/null +++ b/spires_cpp/include/filter_pcd_with_bt.h @@ -0,0 +1,12 @@ +#include +#include +#include +#include + + +void removeUnknownPoints( + pcl::PointCloud& cloud, + const octomap::OcTree& tree, + const std::string& output_file); + + diff --git a/spires_cpp/src/spires_cpp/main.cpp b/spires_cpp/src/spires_cpp/main.cpp index ba92c1b..5c3ed0e 100644 --- a/spires_cpp/src/spires_cpp/main.cpp +++ b/spires_cpp/src/spires_cpp/main.cpp @@ -3,6 +3,7 @@ #include #include "progress_bar.h" #include "pcd2bt.h" +#include "filter_pcd_with_bt.h" #define STRINGIFY(x) #x #define MACRO_STRINGIFY(x) STRINGIFY(x) @@ -28,8 +29,10 @@ PYBIND11_MODULE(_core, m) { )pbdoc"; m.def("processPCDFolder", &processPCDFolder, "Process PCD files and save to OctoMap", py::arg("folderPath"), py::arg("resolution"), py::arg("save_path")); - // m.def("processPCDFolder", &processPCDFolder, "Process PCD files and save to OctoMap", - // py::arg("folderPath"), py::arg("resolution"), py::arg("save_path")); + m.def("removeUnknownPoints", &removeUnknownPoints, "Remove unknown points from PCD file", + py::arg("cloud"), py::arg("tree"), py::arg("output_file")); + + #ifdef VERSION_INFO m.attr("__version__") = MACRO_STRINGIFY(VERSION_INFO); #else diff --git a/spires_cpp/src/spires_cpp/octomap/filter_pcd_with_bt.cpp b/spires_cpp/src/spires_cpp/octomap/filter_pcd_with_bt.cpp index 1e33470..47ed13f 100644 --- a/spires_cpp/src/spires_cpp/octomap/filter_pcd_with_bt.cpp +++ b/spires_cpp/src/spires_cpp/octomap/filter_pcd_with_bt.cpp @@ -1,7 +1,4 @@ -#include -#include -#include -#include +#include "filter_pcd_with_bt.h" // Function to check if a point is unknown in the octree bool isUnknownInOctree(const octomap::OcTree& tree, const octomap::point3d& point) { From 2e6a90f427836627e2e3af15dee857a369d18988 Mon Sep 17 00:00:00 2001 From: Yifu Tao Date: Fri, 13 Sep 2024 21:01:23 +0100 Subject: [PATCH 43/54] feat: add octomap pybind class --- spires_cpp/src/spires_cpp/__init__.py | 10 ++++++++-- spires_cpp/src/spires_cpp/main.cpp | 9 ++++++++- 2 files changed, 16 insertions(+), 3 deletions(-) diff --git a/spires_cpp/src/spires_cpp/__init__.py b/spires_cpp/src/spires_cpp/__init__.py index 1d9f8e5..0a52a85 100644 --- a/spires_cpp/src/spires_cpp/__init__.py +++ b/spires_cpp/src/spires_cpp/__init__.py @@ -1,5 +1,11 @@ from __future__ import annotations -from ._core import __doc__, __version__, processPCDFolder +from ._core import OcTree, __doc__, __version__, processPCDFolder, removeUnknownPoints -__all__ = ["__doc__", "__version__", "processPCDFolder"] +__all__ = [ + "__doc__", + "__version__", + "processPCDFolder", + "removeUnknownPoints", + "OcTree", +] diff --git a/spires_cpp/src/spires_cpp/main.cpp b/spires_cpp/src/spires_cpp/main.cpp index 5c3ed0e..957eb31 100644 --- a/spires_cpp/src/spires_cpp/main.cpp +++ b/spires_cpp/src/spires_cpp/main.cpp @@ -31,7 +31,14 @@ PYBIND11_MODULE(_core, m) { py::arg("folderPath"), py::arg("resolution"), py::arg("save_path")); m.def("removeUnknownPoints", &removeUnknownPoints, "Remove unknown points from PCD file", py::arg("cloud"), py::arg("tree"), py::arg("output_file")); - + py::class_(m, "OcTree") + .def(py::init(), py::arg("resolution")) + .def(py::init(), py::arg("filename")) + .def("readBinary", static_cast(&octomap::OcTree::readBinary)) + .def("writeBinary", static_cast(&octomap::OcTree::writeBinary)) + .def("getResolution", &octomap::OcTree::getResolution) + .def("size", &octomap::OcTree::size) + .def("getTreeDepth", &octomap::OcTree::getTreeDepth); #ifdef VERSION_INFO m.attr("__version__") = MACRO_STRINGIFY(VERSION_INFO); From 825e3d2a733953e7fa7cc6c5142d3481ec539fed Mon Sep 17 00:00:00 2001 From: Yifu Tao Date: Fri, 13 Sep 2024 21:29:53 +0100 Subject: [PATCH 44/54] feat: add clang to precommit --- .pre-commit-config.yaml | 8 +++++++- 1 file changed, 7 insertions(+), 1 deletion(-) diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index bda8c04..1231f36 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -9,4 +9,10 @@ repos: args: [ --fix ] # Run the formatter. - id: ruff-format - types_or: [python, pyi, jupyter] \ No newline at end of file + 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 From 112f1b2253b2e7466e124128f6b679fc203d03ed Mon Sep 17 00:00:00 2001 From: Yifu Tao Date: Fri, 13 Sep 2024 21:34:44 +0100 Subject: [PATCH 45/54] refactor: format with clang --- spires_cpp/src/spires_cpp/main.cpp | 44 +++-- .../spires_cpp/octomap/filter_pcd_with_bt.cpp | 78 ++++----- .../octomap/get_occ_free_from_bt.cpp | 164 +++++++++--------- spires_cpp/src/spires_cpp/octomap/pcd2bt.cpp | 113 ++++++------ .../src/spires_cpp/octomap/progress_bar.cpp | 28 +-- 5 files changed, 204 insertions(+), 223 deletions(-) diff --git a/spires_cpp/src/spires_cpp/main.cpp b/spires_cpp/src/spires_cpp/main.cpp index 957eb31..59b344b 100644 --- a/spires_cpp/src/spires_cpp/main.cpp +++ b/spires_cpp/src/spires_cpp/main.cpp @@ -1,21 +1,19 @@ -#include +#include "filter_pcd_with_bt.h" +#include "pcd2bt.h" +#include "progress_bar.h" #include #include -#include "progress_bar.h" -#include "pcd2bt.h" -#include "filter_pcd_with_bt.h" +#include #define STRINGIFY(x) #x #define MACRO_STRINGIFY(x) STRINGIFY(x) -int add(int i, int j) { - return i + j; -} +int add(int i, int j) { return i + j; } namespace py = pybind11; PYBIND11_MODULE(_core, m) { - m.doc() = R"pbdoc( + m.doc() = R"pbdoc( Pybind11 example plugin ----------------------- @@ -27,22 +25,22 @@ PYBIND11_MODULE(_core, m) { add subtract )pbdoc"; - m.def("processPCDFolder", &processPCDFolder, "Process PCD files and save to OctoMap", - py::arg("folderPath"), py::arg("resolution"), py::arg("save_path")); - m.def("removeUnknownPoints", &removeUnknownPoints, "Remove unknown points from PCD file", - py::arg("cloud"), py::arg("tree"), py::arg("output_file")); - py::class_(m, "OcTree") - .def(py::init(), py::arg("resolution")) - .def(py::init(), py::arg("filename")) - .def("readBinary", static_cast(&octomap::OcTree::readBinary)) - .def("writeBinary", static_cast(&octomap::OcTree::writeBinary)) - .def("getResolution", &octomap::OcTree::getResolution) - .def("size", &octomap::OcTree::size) - .def("getTreeDepth", &octomap::OcTree::getTreeDepth); + m.def("processPCDFolder", &processPCDFolder, "Process PCD files and save to OctoMap", py::arg("folderPath"), + py::arg("resolution"), py::arg("save_path")); + m.def("removeUnknownPoints", &removeUnknownPoints, "Remove unknown points from PCD file", py::arg("cloud"), + py::arg("tree"), py::arg("output_file")); + py::class_(m, "OcTree") + .def(py::init(), py::arg("resolution")) + .def(py::init(), py::arg("filename")) + .def("readBinary", static_cast(&octomap::OcTree::readBinary)) + .def("writeBinary", static_cast(&octomap::OcTree::writeBinary)) + .def("getResolution", &octomap::OcTree::getResolution) + .def("size", &octomap::OcTree::size) + .def("getTreeDepth", &octomap::OcTree::getTreeDepth); #ifdef VERSION_INFO - m.attr("__version__") = MACRO_STRINGIFY(VERSION_INFO); + m.attr("__version__") = MACRO_STRINGIFY(VERSION_INFO); #else - m.attr("__version__") = "dev"; + m.attr("__version__") = "dev"; #endif -} \ No newline at end of file +} diff --git a/spires_cpp/src/spires_cpp/octomap/filter_pcd_with_bt.cpp b/spires_cpp/src/spires_cpp/octomap/filter_pcd_with_bt.cpp index 47ed13f..fc2cccd 100644 --- a/spires_cpp/src/spires_cpp/octomap/filter_pcd_with_bt.cpp +++ b/spires_cpp/src/spires_cpp/octomap/filter_pcd_with_bt.cpp @@ -1,59 +1,57 @@ #include "filter_pcd_with_bt.h" // Function to check if a point is unknown in the octree -bool isUnknownInOctree(const octomap::OcTree& tree, const octomap::point3d& point) { - auto node = tree.search(point); - // if the node does not exist, then it is unknown - return !node; +bool isUnknownInOctree(const octomap::OcTree &tree, const octomap::point3d &point) { + auto node = tree.search(point); + // if the node does not exist, then it is unknown + return !node; } // Function to remove unknown points from the point cloud -void removeUnknownPoints( - pcl::PointCloud& cloud, - const octomap::OcTree& tree, - const std::string& output_file) { +void removeUnknownPoints(pcl::PointCloud &cloud, const octomap::OcTree &tree, + const std::string &output_file) { - pcl::PointCloud filtered_cloud; + pcl::PointCloud filtered_cloud; - for (const auto& pcl_point : cloud) { - // Convert the PCL point to an octomap point - octomap::point3d point(pcl_point.x, pcl_point.y, pcl_point.z); + for (const auto &pcl_point : cloud) { + // Convert the PCL point to an octomap point + octomap::point3d point(pcl_point.x, pcl_point.y, pcl_point.z); - // Check if the point is unknown in the octree - if (!isUnknownInOctree(tree, point)) { - filtered_cloud.push_back(pcl_point); - } + // Check if the point is unknown in the octree + if (!isUnknownInOctree(tree, point)) { + filtered_cloud.push_back(pcl_point); } + } - // Save the filtered point cloud to a file - pcl::io::savePCDFileASCII(output_file, filtered_cloud); - std::cout << "Filtered point cloud saved to " << output_file << std::endl; + // Save the filtered point cloud to a file + pcl::io::savePCDFileASCII(output_file, filtered_cloud); + std::cout << "Filtered point cloud saved to " << output_file << std::endl; } -int main(int argc, char** argv) { - if (argc < 4) { - std::cerr << "Usage: " << argv[0] << " " << std::endl; - return 1; - } +int main(int argc, char **argv) { + if (argc < 4) { + std::cerr << "Usage: " << argv[0] << " " << std::endl; + return 1; + } - // Load the octree from the BT file - octomap::OcTree tree(argv[1]); - if (tree.size() == 0) { - std::cerr << "Failed to load octomap from " << argv[1] << std::endl; - return 1; - } + // Load the octree from the BT file + octomap::OcTree tree(argv[1]); + if (tree.size() == 0) { + std::cerr << "Failed to load octomap from " << argv[1] << std::endl; + return 1; + } - // Load the input point cloud - pcl::PointCloud cloud; - if (pcl::io::loadPCDFile(argv[2], cloud) == -1) { - std::cerr << "Failed to load point cloud from " << argv[2] << std::endl; - return 1; - } + // Load the input point cloud + pcl::PointCloud cloud; + if (pcl::io::loadPCDFile(argv[2], cloud) == -1) { + std::cerr << "Failed to load point cloud from " << argv[2] << std::endl; + return 1; + } - std::cout << "Loaded point cloud with " << cloud.size() << " points." << std::endl; + std::cout << "Loaded point cloud with " << cloud.size() << " points." << std::endl; - // Remove unknown points from the point cloud - removeUnknownPoints(cloud, tree, argv[3]); + // Remove unknown points from the point cloud + removeUnknownPoints(cloud, tree, argv[3]); - return 0; + return 0; } diff --git a/spires_cpp/src/spires_cpp/octomap/get_occ_free_from_bt.cpp b/spires_cpp/src/spires_cpp/octomap/get_occ_free_from_bt.cpp index e107f32..bdcd31f 100644 --- a/spires_cpp/src/spires_cpp/octomap/get_occ_free_from_bt.cpp +++ b/spires_cpp/src/spires_cpp/octomap/get_occ_free_from_bt.cpp @@ -1,112 +1,104 @@ -#include -#include -#include -#include -#include -#include -#include -#include "progress_bar.h" using namespace std; using namespace octomap; using PointCloud = pcl::PointCloud; -void convertOctreeToPointCloud(const OcTree& tree, PointCloud::Ptr& free_cloud, PointCloud::Ptr& occupied_cloud) { - // Get the total number of leaf nodes for the progress bar - size_t total_leafs = tree.getNumLeafNodes(); - size_t processed_leafs = 0; - - // Iterate through all leaf nodes - for (OcTree::leaf_iterator it = tree.begin_leafs(), end = tree.end_leafs(); it != end; ++it) { - point3d coord = it.getCoordinate(); - - // Classify voxel as free, occupied, or unknown and add to corresponding cloud - if (tree.isNodeOccupied(*it)) { - occupied_cloud->points.emplace_back(coord.x(), coord.y(), coord.z()); - } - else { - free_cloud->points.emplace_back(coord.x(), coord.y(), coord.z()); - } - - // Update the progress bar - processed_leafs++; - if (processed_leafs % 100 == 0 || processed_leafs == total_leafs) { // Update every 100 iterations - displayProgressBar(processed_leafs, total_leafs); - } - } +void convertOctreeToPointCloud(const OcTree &tree, PointCloud::Ptr &free_cloud, PointCloud::Ptr &occupied_cloud) { + // Get the total number of leaf nodes for the progress bar + size_t total_leafs = tree.getNumLeafNodes(); + size_t processed_leafs = 0; - // Final progress bar completion - displayProgressBar(total_leafs, total_leafs); - cout << endl; -} + // Iterate through all leaf nodes + for (OcTree::leaf_iterator it = tree.begin_leafs(), end = tree.end_leafs(); it != end; ++it) { + point3d coord = it.getCoordinate(); -int main(int argc, char **argv) { - // Default paths for saving the point clouds - string free_pcd_path = "free_voxels.pcd"; - string occupied_pcd_path = "occupied_voxels.pcd"; - - // Check if the correct number of arguments is provided - if (argc < 2) { - cerr << "Usage: " << argv[0] << " -sf -so " << endl; - return 1; + // Classify voxel as free, occupied, or unknown and add to corresponding cloud + if (tree.isNodeOccupied(*it)) { + occupied_cloud->points.emplace_back(coord.x(), coord.y(), coord.z()); + } else { + free_cloud->points.emplace_back(coord.x(), coord.y(), coord.z()); } - // Parse arguments - string bt_file = argv[1]; - for (int i = 2; i < argc; ++i) { - string arg = argv[i]; - if (arg == "-sf" && i + 1 < argc) { - free_pcd_path = argv[++i]; - } else if (arg == "-so" && i + 1 < argc) { - occupied_pcd_path = argv[++i]; - } + // Update the progress bar + processed_leafs++; + if (processed_leafs % 100 == 0 || processed_leafs == total_leafs) { // Update every 100 iterations + displayProgressBar(processed_leafs, total_leafs); } + } - // Load the .bt file into an OcTree - OcTree tree(bt_file); + // Final progress bar completion + displayProgressBar(total_leafs, total_leafs); + cout << endl; +} - if (!tree.size()) { - cerr << "Failed to load the .bt file or the tree is empty." << endl; - return 1; +int main(int argc, char **argv) { + // Default paths for saving the point clouds + string free_pcd_path = "free_voxels.pcd"; + string occupied_pcd_path = "occupied_voxels.pcd"; + + // Check if the correct number of arguments is provided + if (argc < 2) { + cerr << "Usage: " << argv[0] << " -sf -so " << endl; + return 1; + } + + // Parse arguments + string bt_file = argv[1]; + for (int i = 2; i < argc; ++i) { + string arg = argv[i]; + if (arg == "-sf" && i + 1 < argc) { + free_pcd_path = argv[++i]; + } else if (arg == "-so" && i + 1 < argc) { + occupied_pcd_path = argv[++i]; } + } + + // Load the .bt file into an OcTree + OcTree tree(bt_file); + + if (!tree.size()) { + cerr << "Failed to load the .bt file or the tree is empty." << endl; + return 1; + } - // Retrieve and print the thresholds - double occupancyThreshold = tree.getOccupancyThres(); - double freeThreshold = tree.getProbMiss(); // ProbMiss is related to how free space is handled + // Retrieve and print the thresholds + double occupancyThreshold = tree.getOccupancyThres(); + double freeThreshold = tree.getProbMiss(); // ProbMiss is related to how free space is handled - cout << "Occupancy threshold: " << occupancyThreshold << endl; // Default is 0.5 - cout << "Free threshold (probMiss): " << freeThreshold << endl; // Default is 0.12 + cout << "Occupancy threshold: " << occupancyThreshold << endl; // Default is 0.5 + cout << "Free threshold (probMiss): " << freeThreshold << endl; // Default is 0.12 - // Point clouds for free and occupied voxels - PointCloud::Ptr free_cloud(new PointCloud); - PointCloud::Ptr occupied_cloud(new PointCloud); + // Point clouds for free and occupied voxels + PointCloud::Ptr free_cloud(new PointCloud); + PointCloud::Ptr occupied_cloud(new PointCloud); - // Convert octree to point clouds - convertOctreeToPointCloud(tree, free_cloud, occupied_cloud); + // Convert octree to point clouds + convertOctreeToPointCloud(tree, free_cloud, occupied_cloud); - cout << "Size of free_cloud: " << free_cloud->points.size() << endl; - cout << "Size of occupied_cloud: " << occupied_cloud->points.size() << endl; + cout << "Size of free_cloud: " << free_cloud->points.size() << endl; + cout << "Size of occupied_cloud: " << occupied_cloud->points.size() << endl; - // Set cloud properties - free_cloud->width = free_cloud->points.size(); - free_cloud->height = 1; - free_cloud->is_dense = true; + // Set cloud properties + free_cloud->width = free_cloud->points.size(); + free_cloud->height = 1; + free_cloud->is_dense = true; - occupied_cloud->width = occupied_cloud->points.size(); - occupied_cloud->height = 1; - occupied_cloud->is_dense = true; + occupied_cloud->width = occupied_cloud->points.size(); + occupied_cloud->height = 1; + occupied_cloud->is_dense = true; - // Save the point clouds as PCD files with specified file paths - cout << "Saving free cloud with " << free_cloud->points.size() << " points to " << free_pcd_path << "..." << endl; - pcl::io::savePCDFileASCII(free_pcd_path, *free_cloud); + // Save the point clouds as PCD files with specified file paths + cout << "Saving free cloud with " << free_cloud->points.size() << " points to " << free_pcd_path << "..." << endl; + pcl::io::savePCDFileASCII(free_pcd_path, *free_cloud); - cout << "Saving occupied cloud with " << occupied_cloud->points.size() << " points to " << occupied_pcd_path << "..." << endl; - pcl::io::savePCDFileASCII(occupied_pcd_path, *occupied_cloud); + cout << "Saving occupied cloud with " << occupied_cloud->points.size() << " points to " << occupied_pcd_path << "..." + << endl; + pcl::io::savePCDFileASCII(occupied_pcd_path, *occupied_cloud); - cout << "\nPoint clouds saved as:" << endl; - cout << "Free voxels: " << free_pcd_path << " (" << free_cloud->points.size() << " points)" << endl; - cout << "Occupied voxels: " << occupied_pcd_path << " (" << occupied_cloud->points.size() << " points)" << endl; + cout << "\nPoint clouds saved as:" << endl; + cout << "Free voxels: " << free_pcd_path << " (" << free_cloud->points.size() << " points)" << endl; + cout << "Occupied voxels: " << occupied_pcd_path << " (" << occupied_cloud->points.size() << " points)" << endl; - return 0; + return 0; } \ No newline at end of file diff --git a/spires_cpp/src/spires_cpp/octomap/pcd2bt.cpp b/spires_cpp/src/spires_cpp/octomap/pcd2bt.cpp index 44b313c..2a11820 100644 --- a/spires_cpp/src/spires_cpp/octomap/pcd2bt.cpp +++ b/spires_cpp/src/spires_cpp/octomap/pcd2bt.cpp @@ -1,80 +1,73 @@ #include "pcd2bt.h" // Function to list files in a directory -std::vector listFiles(const std::string& folderPath) { - std::vector files; - DIR* dir; - struct dirent* ent; - if ((dir = opendir(folderPath.c_str())) != NULL) { - while ((ent = readdir(dir)) != NULL) { - std::string filename = ent->d_name; - if (filename.length() > 4 && filename.substr(filename.length() - 4) == ".pcd") { - files.push_back(folderPath + "/" + filename); - } - } - closedir(dir); +std::vector listFiles(const std::string &folderPath) { + std::vector files; + DIR *dir; + struct dirent *ent; + if ((dir = opendir(folderPath.c_str())) != NULL) { + while ((ent = readdir(dir)) != NULL) { + std::string filename = ent->d_name; + if (filename.length() > 4 && filename.substr(filename.length() - 4) == ".pcd") { + files.push_back(folderPath + "/" + filename); + } } - return files; + closedir(dir); + } + return files; } -void processPCDFolder(const std::string& folderPath, double resolution, const std::string& save_path) { - octomap::OcTree tree(resolution); - std::vector pcdFiles = listFiles(folderPath); +void processPCDFolder(const std::string &folderPath, double resolution, const std::string &save_path) { + octomap::OcTree tree(resolution); + std::vector pcdFiles = listFiles(folderPath); - // Sort the PCD files - std::sort(pcdFiles.begin(), pcdFiles.end()); + // Sort the PCD files + std::sort(pcdFiles.begin(), pcdFiles.end()); - for (const auto& filePath : pcdFiles) { - std::string filename = filePath.substr(filePath.find_last_of("/") + 1); - std::cout << "Processing file: " << filename << std::endl; + for (const auto &filePath : pcdFiles) { + std::string filename = filePath.substr(filePath.find_last_of("/") + 1); + std::cout << "Processing file: " << filename << std::endl; - pcl::PointCloud::Ptr cloud(new pcl::PointCloud); + pcl::PointCloud::Ptr cloud(new pcl::PointCloud); - if (pcl::io::loadPCDFile(filePath, *cloud) == -1) { - std::cerr << "Couldn't read file " << filename << std::endl; - continue; - } - - std::cout << "Point cloud size: " << cloud->size() << std::endl; + if (pcl::io::loadPCDFile(filePath, *cloud) == -1) { + std::cerr << "Couldn't read file " << filename << std::endl; + continue; + } - // Extract viewpoint directly from the PCD file - Eigen::Vector4f origin = cloud->sensor_origin_; - Eigen::Quaternionf orientation = cloud->sensor_orientation_; + std::cout << "Point cloud size: " << cloud->size() << std::endl; - std::cout << "Viewpoint: " - << origin.x() << ", " - << origin.y() << ", " - << origin.z() << ", " - << orientation.x() << ", " - << orientation.y() << ", " - << orientation.z() << ", " - << orientation.w() << std::endl; + // Extract viewpoint directly from the PCD file + Eigen::Vector4f origin = cloud->sensor_origin_; + Eigen::Quaternionf orientation = cloud->sensor_orientation_; - // Convert origin to octomap point3d - octomap::point3d sensor_origin(origin.x(), origin.y(), origin.z()); + std::cout << "Viewpoint: " << origin.x() << ", " << origin.y() << ", " << origin.z() << ", " << orientation.x() + << ", " << orientation.y() << ", " << orientation.z() << ", " << orientation.w() << std::endl; - // Transform the point cloud using the origin and orientation - Eigen::Affine3f transform = Eigen::Affine3f::Identity(); - transform.translation() << origin.x(), origin.y(), origin.z(); - transform.rotate(orientation); + // Convert origin to octomap point3d + octomap::point3d sensor_origin(origin.x(), origin.y(), origin.z()); - // Apply the transformation to the point cloud - pcl::PointCloud::Ptr transformed_cloud(new pcl::PointCloud()); - pcl::transformPointCloud(*cloud, *transformed_cloud, transform); + // Transform the point cloud using the origin and orientation + Eigen::Affine3f transform = Eigen::Affine3f::Identity(); + transform.translation() << origin.x(), origin.y(), origin.z(); + transform.rotate(orientation); - // Convert transformed PCL cloud to OctoMap point cloud - octomap::Pointcloud octomap_cloud; - for (const auto& point : transformed_cloud->points) { - octomap_cloud.push_back(point.x, point.y, point.z); - } + // Apply the transformation to the point cloud + pcl::PointCloud::Ptr transformed_cloud(new pcl::PointCloud()); + pcl::transformPointCloud(*cloud, *transformed_cloud, transform); - // Insert the transformed cloud into the OctoMap - tree.insertPointCloud(octomap_cloud, sensor_origin, -1, true, true); + // Convert transformed PCL cloud to OctoMap point cloud + octomap::Pointcloud octomap_cloud; + for (const auto &point : transformed_cloud->points) { + octomap_cloud.push_back(point.x, point.y, point.z); } - // Save the resulting octomap - tree.writeBinary(save_path); - std::cout << std::endl; - std::cout << "Octomap saved to " << save_path << std::endl; -} + // Insert the transformed cloud into the OctoMap + tree.insertPointCloud(octomap_cloud, sensor_origin, -1, true, true); + } + // Save the resulting octomap + tree.writeBinary(save_path); + std::cout << std::endl; + std::cout << "Octomap saved to " << save_path << std::endl; +} diff --git a/spires_cpp/src/spires_cpp/octomap/progress_bar.cpp b/spires_cpp/src/spires_cpp/octomap/progress_bar.cpp index bf5a3bc..8e1ed97 100644 --- a/spires_cpp/src/spires_cpp/octomap/progress_bar.cpp +++ b/spires_cpp/src/spires_cpp/octomap/progress_bar.cpp @@ -1,19 +1,19 @@ #include "progress_bar.h" void displayProgressBar(size_t current, size_t total) { - const int barWidth = 50; // Width of the progress bar - float progress = static_cast(current) / total; - int pos = static_cast(barWidth * progress); + const int barWidth = 50; // Width of the progress bar + float progress = static_cast(current) / total; + int pos = static_cast(barWidth * progress); - std::cout << "["; - for (int i = 0; i < barWidth; ++i) { - if (i < pos) - std::cout << "="; - else if (i == pos) - std::cout << ">"; - else - std::cout << " "; - } - std::cout << "] " << int(progress * 100.0) << "%\r"; - std::cout.flush(); + std::cout << "["; + for (int i = 0; i < barWidth; ++i) { + if (i < pos) + std::cout << "="; + else if (i == pos) + std::cout << ">"; + else + std::cout << " "; + } + std::cout << "] " << int(progress * 100.0) << "%\r"; + std::cout.flush(); } From a12113515aff1394599a3648427ef2968e4dae12 Mon Sep 17 00:00:00 2001 From: Yifu Tao Date: Fri, 13 Sep 2024 22:02:57 +0100 Subject: [PATCH 46/54] refactor: add package name folder to include path --- spires_cpp/include/filter_pcd_with_bt.h | 12 ------------ .../spires_cpp/octomap/filter_pcd_with_bt.h | 7 +++++++ .../include/{ => spires_cpp/octomap}/pcd2bt.h | 17 ++++++++--------- .../spires_cpp/octomap}/progress_bar.h | 2 +- spires_cpp/src/spires_cpp/main.cpp | 6 +++--- .../spires_cpp/octomap/filter_pcd_with_bt.cpp | 2 +- spires_cpp/src/spires_cpp/octomap/pcd2bt.cpp | 2 +- .../src/spires_cpp/octomap/progress_bar.cpp | 2 +- 8 files changed, 22 insertions(+), 28 deletions(-) delete mode 100644 spires_cpp/include/filter_pcd_with_bt.h create mode 100644 spires_cpp/include/spires_cpp/octomap/filter_pcd_with_bt.h rename spires_cpp/include/{ => spires_cpp/octomap}/pcd2bt.h (80%) rename spires_cpp/{src/spires_cpp/include => include/spires_cpp/octomap}/progress_bar.h (88%) diff --git a/spires_cpp/include/filter_pcd_with_bt.h b/spires_cpp/include/filter_pcd_with_bt.h deleted file mode 100644 index 453565a..0000000 --- a/spires_cpp/include/filter_pcd_with_bt.h +++ /dev/null @@ -1,12 +0,0 @@ -#include -#include -#include -#include - - -void removeUnknownPoints( - pcl::PointCloud& cloud, - const octomap::OcTree& tree, - const std::string& output_file); - - diff --git a/spires_cpp/include/spires_cpp/octomap/filter_pcd_with_bt.h b/spires_cpp/include/spires_cpp/octomap/filter_pcd_with_bt.h new file mode 100644 index 0000000..6b77a9e --- /dev/null +++ b/spires_cpp/include/spires_cpp/octomap/filter_pcd_with_bt.h @@ -0,0 +1,7 @@ +#include +#include +#include +#include + +void removeUnknownPoints(pcl::PointCloud &cloud, const octomap::OcTree &tree, + const std::string &output_file); diff --git a/spires_cpp/include/pcd2bt.h b/spires_cpp/include/spires_cpp/octomap/pcd2bt.h similarity index 80% rename from spires_cpp/include/pcd2bt.h rename to spires_cpp/include/spires_cpp/octomap/pcd2bt.h index 8f05ed8..1624133 100644 --- a/spires_cpp/include/pcd2bt.h +++ b/spires_cpp/include/spires_cpp/octomap/pcd2bt.h @@ -1,20 +1,19 @@ #ifndef PROCESS_PCD_FOLDER_H #define PROCESS_PCD_FOLDER_H -#include -#include -#include +#include #include #include +#include +#include +#include +#include #include #include -#include -#include -#include -#include - +#include +#include // Function to process PCD files in a folder and generate an OctoMap -void processPCDFolder(const std::string& folderPath, double resolution, const std::string& save_path); +void processPCDFolder(const std::string &folderPath, double resolution, const std::string &save_path); #endif // PROCESS_PCD_FOLDER_H diff --git a/spires_cpp/src/spires_cpp/include/progress_bar.h b/spires_cpp/include/spires_cpp/octomap/progress_bar.h similarity index 88% rename from spires_cpp/src/spires_cpp/include/progress_bar.h rename to spires_cpp/include/spires_cpp/octomap/progress_bar.h index 1154813..a3b1969 100644 --- a/spires_cpp/src/spires_cpp/include/progress_bar.h +++ b/spires_cpp/include/spires_cpp/octomap/progress_bar.h @@ -7,5 +7,5 @@ // Function to display a simple progress bar void displayProgressBar(size_t current, size_t total); - +int test_add(int i, int j); #endif // PROGRESS_BAR_H diff --git a/spires_cpp/src/spires_cpp/main.cpp b/spires_cpp/src/spires_cpp/main.cpp index 59b344b..9485b26 100644 --- a/spires_cpp/src/spires_cpp/main.cpp +++ b/spires_cpp/src/spires_cpp/main.cpp @@ -1,6 +1,6 @@ -#include "filter_pcd_with_bt.h" -#include "pcd2bt.h" -#include "progress_bar.h" +#include "spires_cpp/octomap/filter_pcd_with_bt.h" +#include "spires_cpp/octomap/pcd2bt.h" +#include "spires_cpp/octomap/progress_bar.h" #include #include #include diff --git a/spires_cpp/src/spires_cpp/octomap/filter_pcd_with_bt.cpp b/spires_cpp/src/spires_cpp/octomap/filter_pcd_with_bt.cpp index fc2cccd..85396ae 100644 --- a/spires_cpp/src/spires_cpp/octomap/filter_pcd_with_bt.cpp +++ b/spires_cpp/src/spires_cpp/octomap/filter_pcd_with_bt.cpp @@ -1,4 +1,4 @@ -#include "filter_pcd_with_bt.h" +#include "spires_cpp/octomap/filter_pcd_with_bt.h" // Function to check if a point is unknown in the octree bool isUnknownInOctree(const octomap::OcTree &tree, const octomap::point3d &point) { diff --git a/spires_cpp/src/spires_cpp/octomap/pcd2bt.cpp b/spires_cpp/src/spires_cpp/octomap/pcd2bt.cpp index 2a11820..1e68954 100644 --- a/spires_cpp/src/spires_cpp/octomap/pcd2bt.cpp +++ b/spires_cpp/src/spires_cpp/octomap/pcd2bt.cpp @@ -1,4 +1,4 @@ -#include "pcd2bt.h" +#include "spires_cpp/octomap/pcd2bt.h" // Function to list files in a directory std::vector listFiles(const std::string &folderPath) { diff --git a/spires_cpp/src/spires_cpp/octomap/progress_bar.cpp b/spires_cpp/src/spires_cpp/octomap/progress_bar.cpp index 8e1ed97..e613682 100644 --- a/spires_cpp/src/spires_cpp/octomap/progress_bar.cpp +++ b/spires_cpp/src/spires_cpp/octomap/progress_bar.cpp @@ -1,4 +1,4 @@ -#include "progress_bar.h" +#include "spires_cpp/octomap/progress_bar.h" void displayProgressBar(size_t current, size_t total) { const int barWidth = 50; // Width of the progress bar From 44adf333fc1f211b2d0ee39890a9b4e4fc6cbe1c Mon Sep 17 00:00:00 2001 From: Yifu Tao Date: Sat, 14 Sep 2024 00:32:47 +0100 Subject: [PATCH 47/54] refactor: change removeUnknownPoints to all strings --- .../spires_cpp/octomap/filter_pcd_with_bt.h | 4 +- spires_cpp/src/spires_cpp/main.cpp | 4 +- .../spires_cpp/octomap/filter_pcd_with_bt.cpp | 58 ++++++------------- 3 files changed, 22 insertions(+), 44 deletions(-) diff --git a/spires_cpp/include/spires_cpp/octomap/filter_pcd_with_bt.h b/spires_cpp/include/spires_cpp/octomap/filter_pcd_with_bt.h index 6b77a9e..24336e0 100644 --- a/spires_cpp/include/spires_cpp/octomap/filter_pcd_with_bt.h +++ b/spires_cpp/include/spires_cpp/octomap/filter_pcd_with_bt.h @@ -3,5 +3,5 @@ #include #include -void removeUnknownPoints(pcl::PointCloud &cloud, const octomap::OcTree &tree, - const std::string &output_file); +void removeUnknownPoints(const std::string input_pcd_path, const std::string input_bt_path, + const std::string output_file_path); \ No newline at end of file diff --git a/spires_cpp/src/spires_cpp/main.cpp b/spires_cpp/src/spires_cpp/main.cpp index 9485b26..40aeaf0 100644 --- a/spires_cpp/src/spires_cpp/main.cpp +++ b/spires_cpp/src/spires_cpp/main.cpp @@ -27,8 +27,8 @@ PYBIND11_MODULE(_core, m) { )pbdoc"; m.def("processPCDFolder", &processPCDFolder, "Process PCD files and save to OctoMap", py::arg("folderPath"), py::arg("resolution"), py::arg("save_path")); - m.def("removeUnknownPoints", &removeUnknownPoints, "Remove unknown points from PCD file", py::arg("cloud"), - py::arg("tree"), py::arg("output_file")); + m.def("removeUnknownPoints", &removeUnknownPoints, "Remove unknown points from PCD file", py::arg("input_pcd_path"), + py::arg("input_bt_path"), py::arg("output_file_path")); py::class_(m, "OcTree") .def(py::init(), py::arg("resolution")) .def(py::init(), py::arg("filename")) diff --git a/spires_cpp/src/spires_cpp/octomap/filter_pcd_with_bt.cpp b/spires_cpp/src/spires_cpp/octomap/filter_pcd_with_bt.cpp index 85396ae..79e60df 100644 --- a/spires_cpp/src/spires_cpp/octomap/filter_pcd_with_bt.cpp +++ b/spires_cpp/src/spires_cpp/octomap/filter_pcd_with_bt.cpp @@ -1,57 +1,35 @@ #include "spires_cpp/octomap/filter_pcd_with_bt.h" -// Function to check if a point is unknown in the octree -bool isUnknownInOctree(const octomap::OcTree &tree, const octomap::point3d &point) { - auto node = tree.search(point); - // if the node does not exist, then it is unknown - return !node; -} - // Function to remove unknown points from the point cloud -void removeUnknownPoints(pcl::PointCloud &cloud, const octomap::OcTree &tree, - const std::string &output_file) { +void removeUnknownPoints(const std::string input_pcd_path, const std::string input_bt_path, + const std::string output_file_path) { + octomap::OcTree tree(input_bt_path); + if (tree.size() == 0) { + std::cerr << "Failed to load octomap from " << input_bt_path << std::endl; + return; + } + + pcl::PointCloud cloud; + if (pcl::io::loadPCDFile(input_pcd_path, cloud) == -1) { + std::cout << "Couldn't read file " << input_pcd_path << std::endl; + return; + } pcl::PointCloud filtered_cloud; for (const auto &pcl_point : cloud) { // Convert the PCL point to an octomap point octomap::point3d point(pcl_point.x, pcl_point.y, pcl_point.z); + auto node = tree.search(point); + bool is_unknown = !node; // Check if the point is unknown in the octree - if (!isUnknownInOctree(tree, point)) { + if (!is_unknown) { filtered_cloud.push_back(pcl_point); } } // Save the filtered point cloud to a file - pcl::io::savePCDFileASCII(output_file, filtered_cloud); - std::cout << "Filtered point cloud saved to " << output_file << std::endl; -} - -int main(int argc, char **argv) { - if (argc < 4) { - std::cerr << "Usage: " << argv[0] << " " << std::endl; - return 1; - } - - // Load the octree from the BT file - octomap::OcTree tree(argv[1]); - if (tree.size() == 0) { - std::cerr << "Failed to load octomap from " << argv[1] << std::endl; - return 1; - } - - // Load the input point cloud - pcl::PointCloud cloud; - if (pcl::io::loadPCDFile(argv[2], cloud) == -1) { - std::cerr << "Failed to load point cloud from " << argv[2] << std::endl; - return 1; - } - - std::cout << "Loaded point cloud with " << cloud.size() << " points." << std::endl; - - // Remove unknown points from the point cloud - removeUnknownPoints(cloud, tree, argv[3]); - - return 0; + pcl::io::savePCDFileASCII(output_file_path, filtered_cloud); + std::cout << "Filtered point cloud saved to " << output_file_path << std::endl; } From 04024509d51913cb5e8277b6440dde00b9810ac9 Mon Sep 17 00:00:00 2001 From: Yifu Tao Date: Sat, 14 Sep 2024 00:49:50 +0100 Subject: [PATCH 48/54] feat: add convertOctreeToPointCloud --- spires_cpp/CMakeLists.txt | 4 +- .../spires_cpp/octomap/get_occ_free_from_bt.h | 12 +++ spires_cpp/src/spires_cpp/__init__.py | 10 ++- spires_cpp/src/spires_cpp/main.cpp | 3 + .../octomap/get_occ_free_from_bt.cpp | 73 ++++++------------- 5 files changed, 50 insertions(+), 52 deletions(-) create mode 100644 spires_cpp/include/spires_cpp/octomap/get_occ_free_from_bt.h diff --git a/spires_cpp/CMakeLists.txt b/spires_cpp/CMakeLists.txt index 99d23e0..901f245 100644 --- a/spires_cpp/CMakeLists.txt +++ b/spires_cpp/CMakeLists.txt @@ -22,8 +22,10 @@ include_directories(${CMAKE_SOURCE_DIR}/include) set(SOURCES src/spires_cpp/main.cpp - src/spires_cpp/octomap/pcd2bt.cpp src/spires_cpp/octomap/filter_pcd_with_bt.cpp + src/spires_cpp/octomap/get_occ_free_from_bt.cpp + src/spires_cpp/octomap/pcd2bt.cpp + src/spires_cpp/octomap/progress_bar.cpp ) python_add_library(_core MODULE ${SOURCES} WITH_SOABI) diff --git a/spires_cpp/include/spires_cpp/octomap/get_occ_free_from_bt.h b/spires_cpp/include/spires_cpp/octomap/get_occ_free_from_bt.h new file mode 100644 index 0000000..8681d5c --- /dev/null +++ b/spires_cpp/include/spires_cpp/octomap/get_occ_free_from_bt.h @@ -0,0 +1,12 @@ +#include +#include +#include +#include +#include +#include +#include + +#include "progress_bar.h" + +void convertOctreeToPointCloud(const std::string bt_file_path, const std::string free_pcd_path, + const std::string occupied_pcd_path); diff --git a/spires_cpp/src/spires_cpp/__init__.py b/spires_cpp/src/spires_cpp/__init__.py index 0a52a85..6a38d58 100644 --- a/spires_cpp/src/spires_cpp/__init__.py +++ b/spires_cpp/src/spires_cpp/__init__.py @@ -1,6 +1,13 @@ from __future__ import annotations -from ._core import OcTree, __doc__, __version__, processPCDFolder, removeUnknownPoints +from ._core import ( + OcTree, + __doc__, + __version__, + convertOctreeToPointCloud, + processPCDFolder, + removeUnknownPoints, +) __all__ = [ "__doc__", @@ -8,4 +15,5 @@ "processPCDFolder", "removeUnknownPoints", "OcTree", + "convertOctreeToPointCloud", ] diff --git a/spires_cpp/src/spires_cpp/main.cpp b/spires_cpp/src/spires_cpp/main.cpp index 40aeaf0..573007e 100644 --- a/spires_cpp/src/spires_cpp/main.cpp +++ b/spires_cpp/src/spires_cpp/main.cpp @@ -1,4 +1,5 @@ #include "spires_cpp/octomap/filter_pcd_with_bt.h" +#include "spires_cpp/octomap/get_occ_free_from_bt.h" #include "spires_cpp/octomap/pcd2bt.h" #include "spires_cpp/octomap/progress_bar.h" #include @@ -29,6 +30,8 @@ PYBIND11_MODULE(_core, m) { py::arg("resolution"), py::arg("save_path")); m.def("removeUnknownPoints", &removeUnknownPoints, "Remove unknown points from PCD file", py::arg("input_pcd_path"), py::arg("input_bt_path"), py::arg("output_file_path")); + m.def("convertOctreeToPointCloud", &convertOctreeToPointCloud, "Convert Octree to PointCloud", + py::arg("bt_file_path"), py::arg("free_pcd_path"), py::arg("occupied_pcd_path")); py::class_(m, "OcTree") .def(py::init(), py::arg("resolution")) .def(py::init(), py::arg("filename")) diff --git a/spires_cpp/src/spires_cpp/octomap/get_occ_free_from_bt.cpp b/spires_cpp/src/spires_cpp/octomap/get_occ_free_from_bt.cpp index bdcd31f..e15848e 100644 --- a/spires_cpp/src/spires_cpp/octomap/get_occ_free_from_bt.cpp +++ b/spires_cpp/src/spires_cpp/octomap/get_occ_free_from_bt.cpp @@ -1,10 +1,30 @@ - +#include "spires_cpp/octomap/get_occ_free_from_bt.h" using namespace std; using namespace octomap; using PointCloud = pcl::PointCloud; -void convertOctreeToPointCloud(const OcTree &tree, PointCloud::Ptr &free_cloud, PointCloud::Ptr &occupied_cloud) { +void convertOctreeToPointCloud(const std::string bt_file_path, const std::string free_pcd_path, + const std::string occupied_pcd_path) { + // Load the .bt file into an OcTree + OcTree tree(bt_file_path); + + if (!tree.size()) { + cerr << "Failed to load the .bt file or the tree is empty." << endl; + return; + } + + // Retrieve and print the thresholds + double occupancyThreshold = tree.getOccupancyThres(); + double freeThreshold = tree.getProbMiss(); // ProbMiss is related to how free space is handled + + cout << "Occupancy threshold: " << occupancyThreshold << endl; // Default is 0.5 + cout << "Free threshold (probMiss): " << freeThreshold << endl; // Default is 0.12 + + // Point clouds for free and occupied voxels + PointCloud::Ptr free_cloud(new PointCloud); + PointCloud::Ptr occupied_cloud(new PointCloud); + // Get the total number of leaf nodes for the progress bar size_t total_leafs = tree.getNumLeafNodes(); size_t processed_leafs = 0; @@ -30,51 +50,6 @@ void convertOctreeToPointCloud(const OcTree &tree, PointCloud::Ptr &free_cloud, // Final progress bar completion displayProgressBar(total_leafs, total_leafs); cout << endl; -} - -int main(int argc, char **argv) { - // Default paths for saving the point clouds - string free_pcd_path = "free_voxels.pcd"; - string occupied_pcd_path = "occupied_voxels.pcd"; - - // Check if the correct number of arguments is provided - if (argc < 2) { - cerr << "Usage: " << argv[0] << " -sf -so " << endl; - return 1; - } - - // Parse arguments - string bt_file = argv[1]; - for (int i = 2; i < argc; ++i) { - string arg = argv[i]; - if (arg == "-sf" && i + 1 < argc) { - free_pcd_path = argv[++i]; - } else if (arg == "-so" && i + 1 < argc) { - occupied_pcd_path = argv[++i]; - } - } - - // Load the .bt file into an OcTree - OcTree tree(bt_file); - - if (!tree.size()) { - cerr << "Failed to load the .bt file or the tree is empty." << endl; - return 1; - } - - // Retrieve and print the thresholds - double occupancyThreshold = tree.getOccupancyThres(); - double freeThreshold = tree.getProbMiss(); // ProbMiss is related to how free space is handled - - cout << "Occupancy threshold: " << occupancyThreshold << endl; // Default is 0.5 - cout << "Free threshold (probMiss): " << freeThreshold << endl; // Default is 0.12 - - // Point clouds for free and occupied voxels - PointCloud::Ptr free_cloud(new PointCloud); - PointCloud::Ptr occupied_cloud(new PointCloud); - - // Convert octree to point clouds - convertOctreeToPointCloud(tree, free_cloud, occupied_cloud); cout << "Size of free_cloud: " << free_cloud->points.size() << endl; cout << "Size of occupied_cloud: " << occupied_cloud->points.size() << endl; @@ -99,6 +74,4 @@ int main(int argc, char **argv) { cout << "\nPoint clouds saved as:" << endl; cout << "Free voxels: " << free_pcd_path << " (" << free_cloud->points.size() << " points)" << endl; cout << "Occupied voxels: " << occupied_pcd_path << " (" << occupied_cloud->points.size() << " points)" << endl; - - return 0; -} \ No newline at end of file +} From 619fc1e1873bfb208a552b18f5103753edb005c3 Mon Sep 17 00:00:00 2001 From: Yifu Tao Date: Sun, 15 Sep 2024 00:13:12 +0100 Subject: [PATCH 49/54] refactor: replace terminal (cpp) files with pybind call --- scripts/recon_benchmark.py | 66 ++++++++++++++------------------------ 1 file changed, 24 insertions(+), 42 deletions(-) diff --git a/scripts/recon_benchmark.py b/scripts/recon_benchmark.py index 1addc39..03ca728 100644 --- a/scripts/recon_benchmark.py +++ b/scripts/recon_benchmark.py @@ -1,33 +1,11 @@ import multiprocessing from pathlib import Path -from oxford_spires_utils.bash_command import run_command +from spires_cpp import convertOctreeToPointCloud, processPCDFolder, removeUnknownPoints - -def run_pcd2bt(cloud_folder: str, cloud_bt_path: str, pcd2bt_path: str, get_occ_free_from_bt_path: str): - cloud_folder, cloud_bt_path = str(cloud_folder), str(cloud_bt_path) - pcd2bt_path, get_occ_free_from_bt_path = str(pcd2bt_path), str(get_occ_free_from_bt_path) - - pcd2bt_command = [pcd2bt_path, cloud_folder, "-s", str(cloud_bt_path)] - pcd2bt_command = " ".join(pcd2bt_command) - run_command(pcd2bt_command) - - saved_occ_pcd_path = str(Path(cloud_bt_path).with_name(f"{Path(cloud_bt_path).stem}_occ.pcd")) - saved_free_pcd_path = str(Path(cloud_bt_path).with_name(f"{Path(cloud_bt_path).stem}_free.pcd")) - get_occ_free_from_bt_command = [ - get_occ_free_from_bt_path, - cloud_bt_path, - "-so", - saved_occ_pcd_path, - "-sf", - saved_free_pcd_path, - ] - print(get_occ_free_from_bt_command) - get_occ_free_from_bt_command = " ".join(get_occ_free_from_bt_command) - run_command(get_occ_free_from_bt_command) - - -input_cloud_folder_path = "/home/yifu/data/nerf_data_pipeline/2024-03-13-maths_1/raw/individual_clouds" +# 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) @@ -35,33 +13,37 @@ def run_pcd2bt(cloud_folder: str, cloud_bt_path: str, pcd2bt_path: str, get_occ_ input_cloud_bt_path = Path(project_folder) / "input_cloud.bt" gt_cloud_bt_path = Path(project_folder) / "gt_cloud.bt" -octomap_utils_path = Path(__file__).parent.parent / "octomap_utils" -pcd2bt_path = octomap_utils_path / "build" / "pcd2bt" -get_occ_free_from_bt_path = octomap_utils_path / "build" / "get_occ_free_from_bt" -filter_pcd_with_bt_path = octomap_utils_path / "build" / "filter_pcd_with_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, gt_cloud_bt_path] + [input_cloud_folder_path, gt_cloud_folder_path], [input_cloud_bt_path, gt_cloud_bt_path] ): process = multiprocessing.Process( - target=run_pcd2bt, args=(cloud_folder, cloud_bt_path, pcd2bt_path, get_occ_free_from_bt_path) + 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() +# 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")) -filter_pcd_command = [str(filter_pcd_with_bt_path), str(gt_cloud_bt_path), input_occ_pcd_path, input_occ_filtered_path] -filter_pcd_command = " ".join(filter_pcd_command) -run_command(filter_pcd_command) -filter_pcd_command = [str(filter_pcd_with_bt_path), str(input_cloud_bt_path), gt_occ_pcd_path, gt_occ_filtered_path] -filter_pcd_command = " ".join(filter_pcd_command) -run_command(filter_pcd_command) + + +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) From 71140e0e83434f9d90f92a6c8f8792fb999a9b8d Mon Sep 17 00:00:00 2001 From: Yifu Tao Date: Tue, 17 Sep 2024 10:37:08 +0100 Subject: [PATCH 50/54] docs: update README --- README.md | 29 ++++------------------------- 1 file changed, 4 insertions(+), 25 deletions(-) diff --git a/README.md b/README.md index 8bcfa04..9531d73 100644 --- a/README.md +++ b/README.md @@ -7,34 +7,13 @@ This repository contains scripts that are used to evaluate Lidar/Visual SLAM on pip install -e . ``` -### octomap_utils (C++) -Install octomap to your system +### spires_cpp +Install [octomap](https://github.com/OctoMap/octomap) and [PCL](https://github.com/PointCloudLibrary/pcl) to your system, then ```bash -git clone https://github.com/OctoMap/octomap.git && cd octomap -mkdir build && cd build -cmake .. -make -sudo make install -``` -Then install the sripts in Spires. -```bash -cd /octomap_utils -mkdir build && cd build -cmake .. -make -``` - -## Usage -### octomap_utils -```bash -cd /octomap_utils/build -./pcd2bt -r [resolution] -s [saved_path] -tf [x,y,z, quat_w, quat_x, quat_y, quat_z] +cd spires_cpp +pip install -e . ``` -```bash -cd /octomap_utils/build -./octomap_utils/get_occ_free_from_bt.cpp -sf -so -``` ## Contributing Please refer to Angular's guide for contributing(https://github.com/angular/angular/blob/22b96b96902e1a42ee8c5e807720424abad3082a/CONTRIBUTING.md). From 43dfe287ee50424f492ebb072cab8cf75e1db64f Mon Sep 17 00:00:00 2001 From: Yifu Tao Date: Tue, 17 Sep 2024 10:39:14 +0100 Subject: [PATCH 51/54] refactor: requirements --- requirements.txt | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/requirements.txt b/requirements.txt index cfa87ab..61f893d 100644 --- a/requirements.txt +++ b/requirements.txt @@ -1,5 +1,5 @@ -numpy >= 1.24.4 -open3d >= 0.17.0 -scipy >= 1.10.1 +numpy>=1.24.4 +open3d>=0.17.0 +scipy>=1.10.1 pytest>=8.0.0 pypcd4>=1.1.0 \ No newline at end of file From 0e1912eb7e12e9eb9620e314373e733666ccc951 Mon Sep 17 00:00:00 2001 From: Yifu Tao Date: Tue, 17 Sep 2024 10:48:18 +0100 Subject: [PATCH 52/54] docs: update README --- README.md | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/README.md b/README.md index 9531d73..6a8777b 100644 --- a/README.md +++ b/README.md @@ -7,7 +7,7 @@ This repository contains scripts that are used to evaluate Lidar/Visual SLAM on pip install -e . ``` -### spires_cpp +### 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 @@ -19,7 +19,7 @@ pip install -e . 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 From c1dd07bab4ccd82954850925be523dc0d981bfc1 Mon Sep 17 00:00:00 2001 From: Yifu Tao Date: Tue, 17 Sep 2024 10:49:28 +0100 Subject: [PATCH 53/54] refactor: empty octomap utils --- octomap_utils/CMakeLists.txt | 32 -------------------------------- 1 file changed, 32 deletions(-) delete mode 100644 octomap_utils/CMakeLists.txt diff --git a/octomap_utils/CMakeLists.txt b/octomap_utils/CMakeLists.txt deleted file mode 100644 index c8a7698..0000000 --- a/octomap_utils/CMakeLists.txt +++ /dev/null @@ -1,32 +0,0 @@ -cmake_minimum_required(VERSION 3.10) - -project(spires_octomap_utils) - -# Set C++ standard -set(CMAKE_CXX_STANDARD 11) - -# Find OctoMap package -find_package(Octomap REQUIRED) -find_package(PCL REQUIRED) - - -include_directories( - include - ${OCTOMAP_INCLUDE_DIRS} - ${PCL_INCLUDE_DIRS} -) - -add_library(${PROJECT_NAME} SHARED - progress_bar.cpp -) -target_link_libraries(${PROJECT_NAME} ${OCTOMAP_LIBRARIES}) -target_link_libraries(${PROJECT_NAME} ${PCL_LIBRARIES}) - -add_executable(pcd2bt pcd2bt.cpp) -target_link_libraries(pcd2bt ${PROJECT_NAME}) - -add_executable(get_occ_free_from_bt get_occ_free_from_bt.cpp) -target_link_libraries(get_occ_free_from_bt ${PROJECT_NAME}) - -add_executable(filter_pcd_with_bt filter_pcd_with_bt.cpp ) -target_link_libraries(filter_pcd_with_bt ${PROJECT_NAME}) \ No newline at end of file From a3e4f31e62062473b977d0da641725a051918178 Mon Sep 17 00:00:00 2001 From: Yifu Tao Date: Tue, 17 Sep 2024 10:58:23 +0100 Subject: [PATCH 54/54] feat: add example script for spires_cpp --- spires_cpp/example.py | 32 ++++++++++++++++++++++++++++++++ 1 file changed, 32 insertions(+) create mode 100644 spires_cpp/example.py diff --git a/spires_cpp/example.py b/spires_cpp/example.py new file mode 100644 index 0000000..d59ecc6 --- /dev/null +++ b/spires_cpp/example.py @@ -0,0 +1,32 @@ +from __future__ import annotations + +from spires_cpp import ( + OcTree, + convertOctreeToPointCloud, + processPCDFolder, + removeUnknownPoints, +) + +pcd_folder = "/home/yifu/data/nerf_data_pipeline/2024-03-13-maths_1/raw/individual_clouds_new" +resolution = 0.1 +save_path = "/home/yifu/data/nerf_data_pipeline/2024-03-13-maths_1/raw/individual_clouds_new.bt" +processPCDFolder(pcd_folder, resolution, save_path) + + +octomap_bt_path = "/home/yifu/data/nerf_data_pipeline/2024-03-13-maths_1/raw/individual_clouds_new.bt" +octree = OcTree(octomap_bt_path) +octree.getResolution() +octree.size() +octree.getTreeDepth() + + +octomap_bt_path = "/home/yifu/workspace/Spires_2025/2024-03-13-maths_1/gt_cloud.bt" +input_cloud_path = "/home/yifu/workspace/Spires_2025/2024-03-13-maths_1/input_cloud_occ.pcd" +output_cloud_path = "/home/yifu/workspace/Spires_2025/2024-03-13-maths_1/input_cloud_occ_filtered.pcd" +removeUnknownPoints(input_cloud_path, octomap_bt_path, output_cloud_path) + + +octomap_bt_path = "/home/yifu/workspace/Spires_2025/2024-03-13-maths_1/gt_cloud.bt" +occ_cloud_path = "/home/yifu/workspace/Spires_2025/2024-03-13-maths_1/gt_cloud_occ.pcd" +free_cloud_path = "/home/yifu/workspace/Spires_2025/2024-03-13-maths_1/gt_cloud_free.pcd" +convertOctreeToPointCloud(octomap_bt_path, free_cloud_path, occ_cloud_path)