From c998bbb8b270c7eea6b95df39e27beff97ce3c5a Mon Sep 17 00:00:00 2001 From: "k.koide" Date: Sun, 31 Mar 2024 17:47:18 +0900 Subject: [PATCH] python binding (WIP) --- CMakeLists.txt | 26 ++- src/example/basic_registration.py | 88 ++++++++ src/python/python.cpp | 344 ++++++++++++++++++++++++++++++ 3 files changed, 452 insertions(+), 6 deletions(-) create mode 100644 src/example/basic_registration.py create mode 100644 src/python/python.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 79fe078..c901cc1 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -60,21 +60,35 @@ install(DIRECTORY include/ DESTINATION include) # Helper library if(BUILD_HELPER) - add_library(small_gicp SHARED + add_library(small_gicp_helper SHARED src/small_gicp/registration/registration.cpp src/small_gicp/registration/registration_helper.cpp ) - target_include_directories(small_gicp PUBLIC + target_include_directories(small_gicp_helper PUBLIC include ${EIGEN3_INCLUDE_DIR} ) - target_link_libraries(small_gicp + target_link_libraries(small_gicp_helper OpenMP::OpenMP_CXX ) - install(TARGETS small_gicp DESTINATION lib) + install(TARGETS small_gicp_helper DESTINATION lib) endif() +find_package(Python COMPONENTS Interpreter Development) +find_package(pybind11 CONFIG) + +# Python binding +pybind11_add_module(small_gicp src/python/python.cpp) +target_include_directories(small_gicp PUBLIC + include + ${EIGEN3_INCLUDE_DIR} +) +target_link_libraries(small_gicp PRIVATE + small_gicp_helper + OpenMP::OpenMP_CXX +) + ############### ## Benchmark ## ############### @@ -182,7 +196,7 @@ if(BUILD_EXAMPLES) ${EIGEN3_INCLUDE_DIR} ) target_link_libraries(${EXAMPLE_NAME} - small_gicp + small_gicp_helper fmt::fmt OpenMP::OpenMP_CXX ${PCL_LIBRARIES} @@ -213,7 +227,7 @@ if(BUILD_TESTS) ${EIGEN3_INCLUDE_DIR} ) target_link_libraries(${TEST_NAME} - small_gicp + small_gicp_helper fmt::fmt OpenMP::OpenMP_CXX GTest::gtest_main diff --git a/src/example/basic_registration.py b/src/example/basic_registration.py new file mode 100644 index 0000000..24927e6 --- /dev/null +++ b/src/example/basic_registration.py @@ -0,0 +1,88 @@ +#!/usr/bin/python3 +import numpy +from scipy.spatial.transform import Rotation + +import small_gicp +from pyridescence import * + +# Verity the estimated transformation matrix (for testing) +def verify_result(T_target_source, gt_T_target_source): + error = numpy.linalg.inv(T_target_source) @ gt_T_target_source + error_trans = numpy.linalg.norm(error[:3, 3]) + error_rot = Rotation.from_matrix(error[:3, :3]).magnitude() + + if error_trans > 0.1 or error_rot > 0.1: + print('error_trans={:.4f}, error_rot={:.4f}'.format(error_trans, error_rot)) + exit(1) + + +# Basic registation example with numpy arrays +def example1(target_raw_numpy : numpy.ndarray, source_raw_numpy : numpy.ndarray, gt_T_target_source : numpy.ndarray): + # Example A : Perform registration with numpy arrays + # Arguments + # - target_points : Nx4 or Nx3 numpy array of the target point cloud + # - source_points : Nx4 or Nx3 numpy array of the source point cloud + # Optional arguments + # - init_T_target_source : Initial guess of the transformation matrix (4x4 numpy array) + # - registration_type : Registration type ("ICP", "PLANE_ICP", "GICP", "VGICP") + # - voxel_resolution : Voxel resolution for VGICP + # - max_correspondence_distance : Maximum correspondence distance + # - max_iterations : Maximum number of iterations + result = small_gicp.align_points(target_raw_numpy, source_raw_numpy) + + # Verity the estimated transformation matrix + verify_result(result.T_target_source, gt_T_target_source) + + + # Example B : Perform preprocessing and registration separately + # Preprocess point clouds + # Arguments + # - points_numpy : Nx4 or Nx3 numpy array of the target point cloud + # Optional arguments + # - downsampling_resolution : Downsampling resolution + # - num_neighbors : Number of neighbors for normal and covariance estimation + # - num_threads : Number of threads + target, target_tree = small_gicp.preprocess_points(points_numpy=target_raw_numpy, downsampling_resolution=0.25) + source, source_tree = small_gicp.preprocess_points(points_numpy=source_raw_numpy, downsampling_resolution=0.25) + + # Align point clouds + # Arguments + # - target : Target point cloud (small_gicp.PointCloud) + # - source : Source point cloud (small_gicp.PointCloud) + # - target_tree : KD-tree of the target point cloud + # Optional arguments + # - init_T_target_source : Initial guess of the transformation matrix (4x4 numpy array) + # - max_correspondence_distance : Maximum correspondence distance + # - num_threads : Number of threads + result = small_gicp.align(target, source, target_tree) + + # Verity the estimated transformation matrix + verify_result(result.T_target_source, gt_T_target_source) + + +# Basic registation example with small_gicp.PointCloud +def example2(target_raw_numpy : numpy.ndarray, source_raw_numpy : numpy.ndarray, gt_T_target_source : numpy.ndarray): + # Convert numpy arrays to small_gicp.PointCloud + target_raw = small_gicp.PointCloud(target_raw_numpy) + source_raw = small_gicp.PointCloud(source_raw_numpy) + pass + + +def main(): + gt_T_target_source = numpy.loadtxt('../data/T_target_source.txt') # Load the ground truth transformation matrix + print('--- gt_T_target_source ---') + print(gt_T_target_source) + + target_raw = small_gicp.read_ply(('../data/target.ply')) # Read the target point cloud (small_gicp.PointCloud) + source_raw = small_gicp.read_ply(('../data/source.ply')) # Read the source point cloud (small_gicp.PointCloud) + + target_raw_numpy = target_raw.points() # Nx4 numpy array of the target point cloud + source_raw_numpy = source_raw.points() # Nx4 numpy array of the source point cloud + + example1(target_raw_numpy, source_raw_numpy, gt_T_target_source) + example2(target_raw_numpy, source_raw_numpy, gt_T_target_source) + return + + +if __name__ == "__main__": + main() diff --git a/src/python/python.cpp b/src/python/python.cpp new file mode 100644 index 0000000..0b52341 --- /dev/null +++ b/src/python/python.cpp @@ -0,0 +1,344 @@ +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace py = pybind11; +using namespace small_gicp; + +PYBIND11_MODULE(small_gicp, m) { + m.doc() = "Small GICP"; + + // PointCloud + py::class_>(m, "PointCloud") // + .def(py::init([](const Eigen::MatrixXd& points) { + if (points.cols() != 3 && points.cols() != 4) { + std::cerr << "points must be Nx3 or Nx4" << std::endl; + return std::make_shared(); + } + + auto pc = std::make_shared(); + pc->resize(points.rows()); + if (points.cols() == 3) { + for (size_t i = 0; i < points.rows(); i++) { + pc->point(i) << points.row(i).transpose(), 1.0; + } + } else { + for (size_t i = 0; i < points.rows(); i++) { + pc->point(i) << points.row(i).transpose(); + } + } + + return pc; + })) // + .def("__repr__", [](const PointCloud& points) { return "small_gicp.PointCloud (" + std::to_string(points.size()) + " points)"; }) + .def("size", &PointCloud::size) + .def( + "points", + [](const PointCloud& points) -> Eigen::MatrixXd { return Eigen::Map>(points.points[0].data(), points.size(), 4); }) + .def( + "normals", + [](const PointCloud& points) -> Eigen::MatrixXd { return Eigen::Map>(points.normals[0].data(), points.size(), 4); }) + .def("covs", [](const PointCloud& points) { return points.covs; }) + .def("point", [](const PointCloud& points, size_t i) -> Eigen::Vector4d { return points.point(i); }) + .def("normal", [](const PointCloud& points, size_t i) -> Eigen::Vector4d { return points.normal(i); }) + .def("cov", [](const PointCloud& points, size_t i) -> Eigen::Matrix4d { return points.cov(i); }); + + // KdTree + py::class_, std::shared_ptr>>(m, "KdTree") // + .def( + py::init([](const PointCloud::ConstPtr& points, int num_threads) { return std::make_shared>(points, num_threads); }), + py::arg("points"), + py::arg("num_threads") = 1) + .def( + "nearest_neighbor_search", + [](const KdTreeOMP& kdtree, const Eigen::Vector3d& pt) { + size_t k_index = -1; + double k_sq_dist = std::numeric_limits::max(); + const size_t found = traits::nearest_neighbor_search(kdtree, Eigen::Vector4d(pt.x(), pt.y(), pt.z(), 1.0), &k_index, &k_sq_dist); + return std::make_tuple(found, k_index, k_sq_dist); + }) + .def("knn_search", [](const KdTreeOMP& kdtree, const Eigen::Vector3d& pt, int k) { + std::vector k_indices(k, -1); + std::vector k_sq_dists(k, std::numeric_limits::max()); + const size_t found = traits::knn_search(kdtree, Eigen::Vector4d(pt.x(), pt.y(), pt.z(), 1.0), k, k_indices.data(), k_sq_dists.data()); + return std::make_pair(k_indices, k_sq_dists); + }); + + // GaussianVoxelMap + py::class_>(m, "GaussianVoxelMap") // + .def(py::init([](double voxel_resolution) { return std::make_shared(voxel_resolution); }), py::arg("voxel_resolution")) + .def( + "insert", + [](GaussianVoxelMap& voxelmap, const PointCloud& points, const Eigen::Matrix4d& T) { voxelmap.insert(points, Eigen::Isometry3d(T)); }, + py::arg("points"), + py::arg("T") = Eigen::Matrix4d::Identity()); + + // RegistrationResult + py::class_(m, "RegistrationResult") // + .def( + "__repr__", + [](const RegistrationResult& result) { + std::stringstream sst; + sst << "small_gicp.RegistrationResult\n"; + sst << "converted=" << result.converged << "\n"; + sst << "iterations=" << result.iterations << "\n"; + sst << "num_inliers=" << result.num_inliers << "\n"; + sst << "T_target_source=\n" << result.T_target_source.matrix() << "\n"; + sst << "H=\n" << result.H << "\n"; + sst << "b=\n" << result.b.transpose() << "\n"; + sst << "error= " << result.error << "\n"; + return sst.str(); + }) + .def_property_readonly("T_target_source", [](const RegistrationResult& result) -> Eigen::Matrix4d { return result.T_target_source.matrix(); }) + .def_readonly("converged", &RegistrationResult::converged) + .def_readonly("iterations", &RegistrationResult::iterations) + .def_readonly("num_inliers", &RegistrationResult::num_inliers) + .def_readonly("H", &RegistrationResult::H) + .def_readonly("b", &RegistrationResult::b) + .def_readonly("error", &RegistrationResult::error); + + // read_ply + m.def( + "read_ply", + [](const std::string& filename) { + const auto points = read_ply(filename); + return std::make_shared(points); + }, + "Read PLY file", + py::arg("filename")); + + // voxelgrid_sampling + m.def( + "voxelgrid_sampling", + [](const PointCloud& points, double resolution, int num_threads) { + if (num_threads == 1) { + return voxelgrid_sampling(points, resolution); + } + return voxelgrid_sampling_omp(points, resolution, num_threads); + }, + "Voxelgrid sampling", + py::arg("points (Nx3) or (Nx4)"), + py::arg("resolution"), + py::arg("num_threads") = 1); + + // estimate_normals + m.def( + "estimate_normals", + [](PointCloud::Ptr points, std::shared_ptr> tree, int num_neighbors, int num_threads) { + if (tree == nullptr) { + tree = std::make_shared>(points, num_threads); + } + + if (num_threads == 1) { + estimate_normals(*points, *tree, num_neighbors); + } else { + estimate_normals_omp(*points, *tree, num_neighbors, num_threads); + } + }, + py::arg("points"), + py::arg("tree") = nullptr, + py::arg("num_neighbors") = 20, + py::arg("num_threads") = 1); + + // estimate_covariances + m.def( + "estimate_covariances", + [](PointCloud::Ptr points, std::shared_ptr> tree, int num_neighbors, int num_threads) { + if (tree == nullptr) { + tree = std::make_shared>(points, num_threads); + } + + if (num_threads == 1) { + estimate_covariances(*points, *tree, num_neighbors); + } else { + estimate_covariances_omp(*points, *tree, num_neighbors, num_threads); + } + }, + py::arg("points"), + py::arg("tree") = nullptr, + py::arg("num_neighbors") = 20, + py::arg("num_threads") = 1); + + // estimate_normals_covariances + m.def( + "estimate_normals_covariances", + [](PointCloud::Ptr points, std::shared_ptr> tree, int num_neighbors, int num_threads) { + if (tree == nullptr) { + tree = std::make_shared>(points, num_threads); + } + + if (num_threads == 1) { + estimate_normals_covariances(*points, *tree, num_neighbors); + } else { + estimate_normals_covariances_omp(*points, *tree, num_neighbors, num_threads); + } + }, + py::arg("points"), + py::arg("tree") = nullptr, + py::arg("num_neighbors") = 20, + py::arg("num_threads") = 1); + + // preprocess_points + m.def( + "preprocess_points", + [](PointCloud::ConstPtr points, const Eigen::MatrixXd points_numpy, double downsampling_resolution, int num_neighbors, int num_threads) + -> std::pair::Ptr> { + if (points == nullptr && points_numpy.size() == 0) { + std::cerr << "points or points_numpy must be provided" << std::endl; + return {nullptr, nullptr}; + } + + if (!points) { + if (points_numpy.cols() != 3 && points_numpy.cols() != 4) { + std::cerr << "points_numpy must be Nx3 or Nx4" << std::endl; + return {nullptr, nullptr}; + } + + auto pts = std::make_shared(); + pts->resize(points_numpy.rows()); + for (size_t i = 0; i < points_numpy.rows(); i++) { + if (points_numpy.cols() == 3) { + pts->point(i) << points_numpy.row(i).transpose(), 1.0; + } else { + pts->point(i) << points_numpy.row(i).transpose(); + } + } + points = pts; + } + + auto downsampled = voxelgrid_sampling_omp(*points, downsampling_resolution, num_threads); + auto kdtree = std::make_shared>(downsampled, num_threads); + estimate_normals_covariances_omp(*downsampled, *kdtree, num_neighbors, num_threads); + return {downsampled, kdtree}; + }, + py::arg("points") = nullptr, + py::arg("points_numpy") = Eigen::MatrixXd(), + py::arg("downsampling_resolution") = 0.25, + py::arg("num_neighbors") = 10, + py::arg("num_threads") = 1); + + // align_points + m.def( + "align_points", + []( + const Eigen::MatrixXd& target_points, + const Eigen::MatrixXd& source_points, + const Eigen::Matrix4d& init_T_target_source, + const std::string& registration_type, + double voxel_resolution, + double downsampling_resolution, + double max_corresponding_distance, + int num_threads) { + if (target_points.cols() != 3 && target_points.cols() != 4) { + std::cerr << "target_points must be Nx3 or Nx4" << std::endl; + return RegistrationResult(Eigen::Isometry3d::Identity()); + } + if (source_points.cols() != 3 && source_points.cols() != 4) { + std::cerr << "source_points must be Nx3 or Nx4" << std::endl; + return RegistrationResult(Eigen::Isometry3d::Identity()); + } + + RegistrationSetting setting; + if (registration_type == "ICP") { + setting.type = RegistrationSetting::ICP; + } else if (registration_type == "PLANE_ICP") { + setting.type = RegistrationSetting::PLANE_ICP; + } else if (registration_type == "GICP") { + setting.type = RegistrationSetting::GICP; + } else if (registration_type == "VGICP") { + setting.type = RegistrationSetting::VGICP; + } else { + std::cerr << "invalid registration type" << std::endl; + return RegistrationResult(Eigen::Isometry3d::Identity()); + } + + setting.voxel_resolution = voxel_resolution; + setting.downsampling_resolution = downsampling_resolution; + setting.max_correspondence_distance = max_corresponding_distance; + setting.num_threads = num_threads; + + std::vector target(target_points.rows()); + if (target_points.cols() == 3) { + for (size_t i = 0; i < target_points.rows(); i++) { + target[i] << target_points.row(i).transpose(), 1.0; + } + } else { + for (size_t i = 0; i < target_points.rows(); i++) { + target[i] << target_points.row(i).transpose(); + } + } + + std::vector source(source_points.rows()); + if (source_points.cols() == 3) { + for (size_t i = 0; i < source_points.rows(); i++) { + source[i] << source_points.row(i).transpose(), 1.0; + } + } else { + for (size_t i = 0; i < source_points.rows(); i++) { + source[i] << source_points.row(i).transpose(); + } + } + + return align(target, source, Eigen::Isometry3d(init_T_target_source), setting); + }, + py::arg("target_points"), + py::arg("source_points"), + py::arg("init_T_target_source") = Eigen::Matrix4d::Identity(), + py::arg("registration_type") = "GICP", + py::arg("voxel_resolution") = 1.0, + py::arg("downsampling_resolution") = 0.25, + py::arg("max_corresponding_distance") = 1.0, + py::arg("num_threads") = 1); + + // align + m.def( + "align", + []( + PointCloud::ConstPtr target, + PointCloud::ConstPtr source, + KdTreeOMP::ConstPtr target_tree, + const Eigen::Matrix4d& init_T_target_source, + GaussianVoxelMap::ConstPtr target_voxelmap, + double max_correspondence_distance, + int num_threads) { + if (target == nullptr && target_voxelmap == nullptr) { + std::cerr << "target or target_voxelmap must be provided" << std::endl; + return RegistrationResult(Eigen::Isometry3d::Identity()); + } + + Registration registration; + registration.rejector.max_dist_sq = max_correspondence_distance * max_correspondence_distance; + registration.reduction.num_threads = num_threads; + + if (target) { + if (target_tree == nullptr) { + target_tree = std::make_shared>(target, num_threads); + } + auto result = registration.align(*target, *source, *target_tree, Eigen::Isometry3d(init_T_target_source)); + return result; + } else { + return registration.align(*target_voxelmap, *source, *target_voxelmap, Eigen::Isometry3d(init_T_target_source)); + } + }, + py::arg("target") = nullptr, + py::arg("source") = nullptr, + py::arg("target_tree") = nullptr, + py::arg("init_T_target_source") = Eigen::Matrix4d::Identity(), + py::arg("target_voxelmap") = nullptr, + py::arg("max_correspondence_distance") = 1.0, + py::arg("num_threads") = 1); +} \ No newline at end of file