Added Python Binding support.

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.
 
-<img src="data/screenshot.png" height="400pix" /><br>
+<img src="data/screenshot.png" height="400px" /><br>
 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.
+
+### 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/ali-rehman-ML/py_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 will also not be able to use open3d voxel 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)

bindings/ndt_omp_binding.cpp

@@ -0,0 +1,80 @@
+#include <pybind11/pybind11.h>
+#include <pybind11/stl.h>
+#include <pybind11/eigen.h>
+#include <pcl/point_cloud.h>
+#include <pcl/point_types.h>
+#include <pclomp/ndt_omp.h>
+#include <flann/flann.hpp>
+
+namespace py = pybind11;
+
+#include <pcl/console/print.h>
+
+pcl::PointCloud<pcl::PointXYZ>::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<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
+    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_<pclomp::NormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ>>(m, "NDTOMP")
+        .def(py::init<>())
+        .def("set_num_threads", &pclomp::NormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ>::setNumThreads)
+        .def("set_neighborhood_search_method", &pclomp::NormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ>::setNeighborhoodSearchMethod)
+        .def("set_resolution", &pclomp::NormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ>::setResolution)
+        .def("set_transformation_epsilon", &pclomp::NormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ>::setTransformationEpsilon)
+        .def("set_maximum_iterations", &pclomp::NormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ>::setMaximumIterations)
+        .def("set_input_source", [](pclomp::NormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ>& 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<pcl::PointXYZ, pcl::PointXYZ>& ndt, 
+                                   const Eigen::MatrixXf& target) {
+            auto target_cloud = numpy_to_point_cloud(target);
+            ndt.setInputTarget(target_cloud);
+        }, py::arg("target"))
+        .def("align", [](pclomp::NormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ>& ndt, 
+                        const Eigen::Matrix4f& guess) {
+            try {
+                pcl::PointCloud<pcl::PointXYZ> 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<pcl::PointXYZ, pcl::PointXYZ>::getFinalTransformation)
+        .def("has_converged", &pclomp::NormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ>::hasConverged);
+
+    py::enum_<pclomp::NeighborSearchMethod>(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);
+}

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,
+)