From a0e2b0ca407d5517124312f51a939217d64819f4 Mon Sep 17 00:00:00 2001 From: "ali.rehman@sw-motion.cn" Date: Sat, 29 Mar 2025 16:39:59 +0500 Subject: [PATCH 1/3] Added Python binding Support --- README.md | 71 +++++++++++++++++++++++++++++--- bindings/ndt_omp_binding.cpp | 80 ++++++++++++++++++++++++++++++++++++ setup.py | 71 ++++++++++++++++++++++++++++++++ 3 files changed, 216 insertions(+), 6 deletions(-) create mode 100644 bindings/ndt_omp_binding.cpp create mode 100644 setup.py diff --git a/README.md b/README.md index d4d585fa..0761f26e 100644 --- a/README.md +++ b/README.md @@ -1,12 +1,12 @@ # ndt_omp -This package provides an OpenMP-boosted Normal Distributions Transform (and GICP) algorithm derived from pcl. The NDT algorithm is modified to be SSE-friendly and multi-threaded. It can run up to 10 times faster than its original version in pcl. +This package provides an OpenMP-boosted Normal Distributions Transform (and GICP) algorithm derived from PCL. The NDT algorithm is modified to be SSE-friendly and multi-threaded, achieving up to 10 times faster performance than the original PCL version. -For using this package in non-ROS1 projects (ROS2 or without ROS), see forked repositories: [dfki-ric/pclomp](https://github.com/dfki-ric/pclomp) [tier4/ndt_omp](https://github.com/tier4/ndt_omp). +For using this package in non-ROS1 projects (ROS2 or without ROS), see forked repositories: [dfki-ric/pclomp](https://github.com/dfki-ric/pclomp), [tier4/ndt_omp](https://github.com/tier4/ndt_omp). [![Build](https://github.com/koide3/ndt_omp/actions/workflows/build.yml/badge.svg)](https://github.com/koide3/ndt_omp/actions/workflows/build.yml) -### Benchmark (on Core i7-6700K) +## Benchmark (on Core i7-6700K) ``` $ roscd ndt_omp/data $ rosrun ndt_omp align 251370668.pcd 251371071.pcd @@ -46,11 +46,70 @@ single : 17.2353[msec] fitness: 0.208511 ``` -Several methods for neighbor voxel search are implemented. If you select pclomp::KDTREE, results will be completely the same as that of the original pcl::NDT. We recommend using pclomp::DIRECT7 which is faster and stable. If you need extremely fast registration, choose pclomp::DIRECT1, but it might be a bit unstable. +Several methods for neighbor voxel search are implemented. If you select `pclomp::KDTREE`, results will be identical to the original `pcl::NDT`. We recommend using `pclomp::DIRECT7` for a balance of speed and stability. For extremely fast registration, choose `pclomp::DIRECT1`, though it may be less stable. -
+
Red: target, Green: source, Blue: aligned -## Related packages +## Python Bindings + +This repository now includes Python bindings for `ndt_omp`, enabling its use in Python projects. The bindings are designed to be compatible with Open3D, addressing previous FLANN-related segmentation faults by defaulting to the `DIRECT1` search method, which bypasses FLANN conflicts. + +### Installation +To install the Python bindings, ensure you have the following dependencies: +- PCL (any version 1.8+) +- Eigen3 +- pybind11 (`pip3 install pybind11`) +- Python 3.8+ (other versions may work but are untested) + +Clone the repository and install: +```bash +git clone https://github.com//ndt_omp.git +cd ndt_omp +sudo python3 setup.py install +``` + +The `setup.py` script dynamically detects your PCL installation, making it compatible with various PCL versions (e.g., 1.8, 1.9, 1.10+). + +### Usage +Here’s an example of using the Python bindings with Open3D: +```python +import ndt_omp +import numpy as np +import open3d as o3d #always import open3d after ndt_omp to avoid segmentation fault + + +# Load point clouds +pcd1 = o3d.io.read_point_cloud("map1.pcd") +pcd2 = o3d.io.read_point_cloud("map2.pcd") +source = np.asarray(pcd1.points).astype(np.float32) +target = np.asarray(pcd2.points).astype(np.float32) + +# Initialize NDT-OMP +ndt = ndt_omp.NDTOMP() +ndt.set_num_threads(4) +ndt.set_transformation_epsilon(0.001) +ndt.set_maximum_iterations(100) +ndt.set_neighborhood_search_method(ndt_omp.NeighborSearchMethod.DIRECT7) +ndt.set_resolution(1.0) + +# Set input clouds and align +ndt.set_input_source(source) +ndt.set_input_target(target) +init_guess = np.eye(4).astype(np.float32) +transform, fitness = ndt.align(init_guess) + +print("Transformation Matrix:\n", transform) +print("Fitness Score:", fitness) +``` + +#### Notes +- **Open3D Compatibility**: Importing `open3d` before `ndt_omp` result in segmentaion fault if your open3d and pcl has different flann versions. You alos will not be able to use open3d vozel downsampling and SOR filtering if flann version is mis-matched. + + +### Contributing +The Python bindings were added to enhance accessibility and compatibility. Contributions to improve the bindings, such as adding more PCL features or optimizing performance, are welcome! + +## Related Packages - [ndt_omp](https://github.com/koide3/ndt_omp) - [fast_gicp](https://github.com/SMRT-AIST/fast_gicp) diff --git a/bindings/ndt_omp_binding.cpp b/bindings/ndt_omp_binding.cpp new file mode 100644 index 00000000..b30511f0 --- /dev/null +++ b/bindings/ndt_omp_binding.cpp @@ -0,0 +1,80 @@ +#include +#include +#include +#include +#include +#include +#include + +namespace py = pybind11; + +#include + +pcl::PointCloud::Ptr numpy_to_point_cloud(const Eigen::MatrixXf& points) { + pcl::console::setVerbosityLevel(pcl::console::L_DEBUG); + if (points.cols() != 3) { + throw std::invalid_argument("Input array must have shape (N, 3)"); + } + pcl::PointCloud::Ptr cloud(new pcl::PointCloud); + try { + cloud->points.resize(points.rows()); + for (int i = 0; i < points.rows(); ++i) { + cloud->points[i].x = points(i, 0); + cloud->points[i].y = points(i, 1); + cloud->points[i].z = points(i, 2); + if (i < 5 || i >= points.rows() - 5) { // Log first and last 5 points + } + } + cloud->width = points.rows(); + cloud->height = 1; + cloud->is_dense = true; + + } catch (const std::exception& e) { + throw std::runtime_error("Failed to convert NumPy array to PCL PointCloud: " + std::string(e.what())); + } + return cloud; +} + +void bind_ndt_omp(py::module& m) { + PCL_DEBUG("ndt_omp binding initialized with FLANN version: %s\n", FLANN_VERSION_); + py::class_>(m, "NDTOMP") + .def(py::init<>()) + .def("set_num_threads", &pclomp::NormalDistributionsTransform::setNumThreads) + .def("set_neighborhood_search_method", &pclomp::NormalDistributionsTransform::setNeighborhoodSearchMethod) + .def("set_resolution", &pclomp::NormalDistributionsTransform::setResolution) + .def("set_transformation_epsilon", &pclomp::NormalDistributionsTransform::setTransformationEpsilon) + .def("set_maximum_iterations", &pclomp::NormalDistributionsTransform::setMaximumIterations) + .def("set_input_source", [](pclomp::NormalDistributionsTransform& ndt, + const Eigen::MatrixXf& source) { + auto source_cloud = numpy_to_point_cloud(source); + ndt.setInputSource(source_cloud); + }, py::arg("source")) + .def("set_input_target", [](pclomp::NormalDistributionsTransform& ndt, + const Eigen::MatrixXf& target) { + auto target_cloud = numpy_to_point_cloud(target); + ndt.setInputTarget(target_cloud); + }, py::arg("target")) + .def("align", [](pclomp::NormalDistributionsTransform& ndt, + const Eigen::Matrix4f& guess) { + try { + pcl::PointCloud output; + ndt.align(output, guess); + return std::make_tuple(ndt.getFinalTransformation(), ndt.getFitnessScore()); + } catch (const std::exception& e) { + throw std::runtime_error("NDT alignment failed: " + std::string(e.what())); + } + }, py::arg("guess") = Eigen::Matrix4f::Identity()) + .def("get_final_transformation", &pclomp::NormalDistributionsTransform::getFinalTransformation) + .def("has_converged", &pclomp::NormalDistributionsTransform::hasConverged); + + py::enum_(m, "NeighborSearchMethod") + .value("KDTREE", pclomp::NeighborSearchMethod::KDTREE) + .value("DIRECT7", pclomp::NeighborSearchMethod::DIRECT7) + .value("DIRECT1", pclomp::NeighborSearchMethod::DIRECT1) + .export_values(); +} + +PYBIND11_MODULE(ndt_omp, m) { + m.doc() = "Python bindings for ndt_omp package with NumPy support"; + bind_ndt_omp(m); +} \ No newline at end of file diff --git a/setup.py b/setup.py new file mode 100644 index 00000000..c71b68c2 --- /dev/null +++ b/setup.py @@ -0,0 +1,71 @@ +from setuptools import setup, Extension +import pybind11 +import os +import subprocess + +# Function to get PCL include directory dynamically +def get_pcl_include_dir(): + try: + # Use pkg-config to find PCL include directory + result = subprocess.check_output(['pkg-config', '--cflags-only-I', 'pcl_common'], text=True).strip() + # Extract include dirs (e.g., "-I/usr/include/pcl-1.10") + include_dirs = [d[2:] for d in result.split() if d.startswith('-I')] + if include_dirs: + return include_dirs[0] # Return the first PCL include dir + except subprocess.CalledProcessError: + pass # pkg-config failed, fall back to search + + # Fallback: Search common PCL installation paths + common_paths = [ + '/usr/include/pcl-1.12', + '/usr/include/pcl-1.11', + '/usr/include/pcl-1.10', + '/usr/include/pcl-1.9', + '/usr/include/pcl-1.8', + '/usr/local/include/pcl-1.12', + '/usr/local/include/pcl-1.11', + '/usr/local/include/pcl-1.10', + '/usr/local/include/pcl-1.9', + '/usr/local/include/pcl-1.8', + ] + for path in common_paths: + if os.path.exists(os.path.join(path, 'pcl/point_cloud.h')): + return path + + raise RuntimeError("Could not find PCL include directory. Please install PCL or specify its path.") + +# Get PCL include directory +pcl_include_dir = get_pcl_include_dir() + +# Define the extension module +ext_modules = [ + Extension( + 'ndt_omp', + [ + 'bindings/ndt_omp_binding.cpp', + 'src/pclomp/ndt_omp.cpp', + 'src/pclomp/voxel_grid_covariance_omp.cpp' + ], + include_dirs=[ + pybind11.get_include(), + 'include', + pcl_include_dir, + '/usr/include/eigen3', + ], + library_dirs=[ + '/usr/lib/x86_64-linux-gnu', + '/usr/local/lib', + ], + libraries=['pcl_common', 'pcl_registration', 'pcl_kdtree', 'pcl_filters', 'gomp'], + extra_compile_args=['-std=c++17','-O3', '-fopenmp'], + extra_link_args=['-fopenmp'], + language='c++' + ), +] + +setup( + name='ndt_omp', + version='0.1', + description='Python bindings for ndt_omp', + ext_modules=ext_modules, +) \ No newline at end of file From 5297de32db9add10d0170e1a35bc547d085b6c33 Mon Sep 17 00:00:00 2001 From: "ali.rehman@sw-motion.cn" Date: Sat, 29 Mar 2025 16:51:37 +0500 Subject: [PATCH 2/3] Added Python binding Support --- README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/README.md b/README.md index 0761f26e..528a50e8 100644 --- a/README.md +++ b/README.md @@ -53,7 +53,7 @@ Red: target, Green: source, Blue: aligned ## Python Bindings -This repository now includes Python bindings for `ndt_omp`, enabling its use in Python projects. The bindings are designed to be compatible with Open3D, addressing previous FLANN-related segmentation faults by defaulting to the `DIRECT1` search method, which bypasses FLANN conflicts. +This repository now includes Python bindings for `ndt_omp`, enabling its use in Python projects. ### Installation To install the Python bindings, ensure you have the following dependencies: From 007c04a085d1601b7e1b6c593fc3dabf921513b8 Mon Sep 17 00:00:00 2001 From: "ali.rehman@sw-motion.cn" Date: Sun, 30 Mar 2025 22:25:25 +0500 Subject: [PATCH 3/3] Merged into main --- README.md | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/README.md b/README.md index 528a50e8..64dc1bf4 100644 --- a/README.md +++ b/README.md @@ -64,7 +64,7 @@ To install the Python bindings, ensure you have the following dependencies: Clone the repository and install: ```bash -git clone https://github.com//ndt_omp.git +git clone https://github.com/ali-rehman-ML/py_ndt_omp.git cd ndt_omp sudo python3 setup.py install ``` @@ -104,7 +104,7 @@ print("Fitness Score:", fitness) ``` #### Notes -- **Open3D Compatibility**: Importing `open3d` before `ndt_omp` result in segmentaion fault if your open3d and pcl has different flann versions. You alos will not be able to use open3d vozel downsampling and SOR filtering if flann version is mis-matched. +- **Open3D Compatibility**: Importing `open3d` before `ndt_omp` result in segmentaion fault if your open3d and pcl has different flann versions. You will also not be able to use open3d voxel downsampling and SOR filtering if flann version is mis-matched. ### Contributing