From b3353da70bffbc57a3adc5a7f5a24fe513d99d0b Mon Sep 17 00:00:00 2001 From: digiamm Date: Thu, 14 Nov 2024 16:43:07 +0100 Subject: [PATCH] added a couple of example of nn search and mad registration --- mad_icp/.clang-format | 1 - mad_icp/apps/mad_icp.py | 5 +- mad_icp/apps/utils/tools/README.md | 0 mad_icp/apps/utils/tools/mad_registration.py | 119 +++++++++++++++++++ mad_icp/apps/utils/tools/nn_search.py | 62 ++++++++++ mad_icp/apps/utils/tools/tools_utils.py | 21 ++++ mad_icp/src/pybind/CMakeLists.txt | 32 ++++- mad_icp/src/pybind/pypeline.cpp | 87 ++++++++------ mad_icp/src/pybind/pyvector.cpp | 54 +++++++++ mad_icp/src/pybind/tools/mad_icp_wrapper.h | 112 +++++++++++++++++ mad_icp/src/pybind/tools/mad_tree_wrapper.h | 60 ++++++++++ mad_icp/src/pybind/tools/pymadicp.cpp | 52 ++++++++ mad_icp/src/pybind/tools/pymadtree.cpp | 47 ++++++++ 13 files changed, 611 insertions(+), 41 deletions(-) create mode 100644 mad_icp/apps/utils/tools/README.md create mode 100644 mad_icp/apps/utils/tools/mad_registration.py create mode 100644 mad_icp/apps/utils/tools/nn_search.py create mode 100644 mad_icp/apps/utils/tools/tools_utils.py create mode 100644 mad_icp/src/pybind/pyvector.cpp create mode 100644 mad_icp/src/pybind/tools/mad_icp_wrapper.h create mode 100644 mad_icp/src/pybind/tools/mad_tree_wrapper.h create mode 100644 mad_icp/src/pybind/tools/pymadicp.cpp create mode 100644 mad_icp/src/pybind/tools/pymadtree.cpp diff --git a/mad_icp/.clang-format b/mad_icp/.clang-format index 727bca2..20be094 100755 --- a/mad_icp/.clang-format +++ b/mad_icp/.clang-format @@ -38,7 +38,6 @@ BraceWrapping: SplitEmptyRecord: true SplitEmptyNamespace: true BreakBeforeBinaryOperators: None -BreakBeforeBraces: Attach BreakBeforeInheritanceComma: false BreakBeforeTernaryOperators: true BreakConstructorInitializersBeforeComma: false diff --git a/mad_icp/apps/mad_icp.py b/mad_icp/apps/mad_icp.py index d11f473..2b3233b 100755 --- a/mad_icp/apps/mad_icp.py +++ b/mad_icp/apps/mad_icp.py @@ -45,8 +45,9 @@ from mad_icp.apps.utils.visualizer import Visualizer from mad_icp.configurations.datasets.dataset_configurations import DatasetConfiguration_lut from mad_icp.configurations.mad_params import MADConfiguration_lut -# binded Odometry -from mad_icp.src.pybind.pypeline import Pipeline, VectorEigen3d +# binded vectors and odometry +from mad_icp.src.pybind.pyvector import VectorEigen3d +from mad_icp.src.pybind.pypeline import Pipeline console = Console() diff --git a/mad_icp/apps/utils/tools/README.md b/mad_icp/apps/utils/tools/README.md new file mode 100644 index 0000000..e69de29 diff --git a/mad_icp/apps/utils/tools/mad_registration.py b/mad_icp/apps/utils/tools/mad_registration.py new file mode 100644 index 0000000..2137ccb --- /dev/null +++ b/mad_icp/apps/utils/tools/mad_registration.py @@ -0,0 +1,119 @@ +# Copyright 2024 R(obots) V(ision) and P(erception) group +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions are met: +# +# 1. Redistributions of source code must retain the above copyright notice, +# this list of conditions and the following disclaimer. +# +# 2. Redistributions in binary form must reproduce the above copyright notice, +# this list of conditions and the following disclaimer in the documentation +# and/or other materials provided with the distribution. +# +# 3. Neither the name of the copyright holder nor the names of its contributors +# may be used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. + +import os +import numpy as np +import matplotlib.pyplot as plt +from matplotlib.animation import FuncAnimation +from scipy.spatial.transform import Rotation as R +import typer +from typing_extensions import Annotated + +# binded vectors and madtree +from mad_icp.src.pybind.pyvector import VectorEigen3d +from mad_icp.src.pybind.pymadicp import MADicp +from mad_icp.src.pybind.pymadtree import MADtree + +from mad_icp.apps.utils.tools.tools_utils import generate_four_walls_pointcloud + + +MAX_ITERATIONS = 15 +app = typer.Typer() +T_guess = np.eye(4) + +def main(viz: Annotated[bool, typer.Option(help="if true visualizer on (very slow)", show_default=True)] = False) -> None: + + # initialize reference and query clouds + np.random.seed(42) + ref_cloud = generate_four_walls_pointcloud(points_per_wall=1000) + query_cloud = ref_cloud.copy() + + # initial transformation guess + global T_guess + T_guess[:3, :3] = R.from_euler('xyz', [0.1, 0.1, 0.1]).as_matrix() + T_guess[:3, 3] = np.random.rand(3) + print("init guess T\n", T_guess) + print("gt T\n", np.eye(4)) + + # initialize mad icp object and set reference and query clouds + madicp = MADicp(num_threads=os.cpu_count()) + madicp.setReferenceCloud(VectorEigen3d(ref_cloud)) + madicp.setQueryCloud(VectorEigen3d(query_cloud)) + if not viz: + T_est = madicp.compute(T_guess, icp_iterations=MAX_ITERATIONS) + print("estimate \n", T_est) + exit(0) + + # create figure and 3d axis for visualization + fig = plt.figure() + ax = fig.add_subplot(111, projection='3d') + ref_scatter = ax.scatter(ref_cloud[:, 0], ref_cloud[:, 1], ref_cloud[:, 2], color='blue', s=1, label='reference cloud') + query_cloud_transformed = np.array([T_guess[:3, :3] @ point + T_guess[:3, 3] for point in query_cloud]) + query_scatter = ax.scatter(query_cloud_transformed[:, 0], query_cloud_transformed[:, 1], query_cloud_transformed[:, 2], color='red', s=1, label='query cloud') + + # set axis limits + ax.legend() + + # this is just to visualize convergence of ICP + # and matches during iterations + tree = MADtree() + tree.build(VectorEigen3d(ref_cloud)) + + # function to update the animation for each iteration + def update(iteration): + global T_guess + # perform one iteration of icp + T_guess = madicp.compute(T_guess, icp_iterations=1) + # transform the query cloud with the current estimate of the transformation + query_cloud_transformed = np.array([T_guess[:3, :3] @ point + T_guess[:3, 3] for point in query_cloud]) + ref_cloud_matched = tree.searchCloud(VectorEigen3d(query_cloud_transformed)) + ref_cloud_points = np.array([ref_point_matched for ref_point_matched, _ in ref_cloud_matched]) + + # remove existing lines from plot + [line.remove() for line in ax.lines] + + # draw lines between each matched point pair + [ax.plot([qp[0], rp[0]], [qp[1], rp[1]], [qp[2], rp[2]], color='green', linestyle='-', linewidth=0.2) + for qp, rp in zip(query_cloud_transformed, ref_cloud_points)] + + # update the query cloud scatter plot data + query_scatter._offsets3d = (query_cloud_transformed[:, 0], query_cloud_transformed[:, 1], query_cloud_transformed[:, 2]) + ax.set_title(f"MAD registration: iteration {iteration+1}") + + # create animation object, updating for each iteration + ani = FuncAnimation(fig, update, frames=MAX_ITERATIONS, interval=0, repeat=False) + + # display the animation + plt.show() + +def run(): + typer.run(main) + +if __name__ == '__main__': + run() + diff --git a/mad_icp/apps/utils/tools/nn_search.py b/mad_icp/apps/utils/tools/nn_search.py new file mode 100644 index 0000000..21173b1 --- /dev/null +++ b/mad_icp/apps/utils/tools/nn_search.py @@ -0,0 +1,62 @@ +# Copyright 2024 R(obots) V(ision) and P(erception) group +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions are met: +# +# 1. Redistributions of source code must retain the above copyright notice, +# this list of conditions and the following disclaimer. +# +# 2. Redistributions in binary form must reproduce the above copyright notice, +# this list of conditions and the following disclaimer in the documentation +# and/or other materials provided with the distribution. +# +# 3. Neither the name of the copyright holder nor the names of its contributors +# may be used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. + +import numpy as np + +# binded vectors and madtree +from mad_icp.src.pybind.pyvector import VectorEigen3d +from mad_icp.src.pybind.pymadtree import MADtree +from mad_icp.apps.utils.tools.tools_utils import generate_four_walls_pointcloud + +np.random.seed(42) + +if __name__ == '__main__': + # generate a random 4 walls + floor point cloud 3d + cloud = generate_four_walls_pointcloud() + print("single point nn") + # single point search with mad tree + qp = cloud[0, :] # query point + tree = MADtree() + tree.build(VectorEigen3d(cloud)) + # ref point and normal calculated by mad tree + ref_point, ref_normal = tree.search(qp) + print("query point {}".format(qp)) + print("ref point {} | ref normal {}".format(ref_point, ref_normal)) + print("error in matching {}".format(np.linalg.norm(ref_point-qp))) + + print(10*"=") + # full cloud search with mad tree + print("full cloud nn") + ref_cloud = tree.searchCloud(VectorEigen3d(cloud)) + + tot_matching_err = 0.0 + for (ref_point, ref_normal), query_point in zip(ref_cloud, cloud): + tot_matching_err += np.linalg.norm(ref_point-query_point) + + print("error in matching {}".format(tot_matching_err)) + diff --git a/mad_icp/apps/utils/tools/tools_utils.py b/mad_icp/apps/utils/tools/tools_utils.py new file mode 100644 index 0000000..098933c --- /dev/null +++ b/mad_icp/apps/utils/tools/tools_utils.py @@ -0,0 +1,21 @@ +import numpy as np + +def generate_four_walls_pointcloud(wall_height=2.0, wall_width=4.0, points_per_wall=10000): + # helper function to generate points for each plane + def generate_plane(x_range, y_range, z_range, points): + x = np.random.uniform(x_range[0], x_range[1], points) + y = np.random.uniform(y_range[0], y_range[1], points) + z = np.random.uniform(z_range[0], z_range[1], points) + return np.column_stack((x, y, z)) + + # generate points for each wall + wall1 = generate_plane([0, wall_width], [0, 0], [0, wall_height], points_per_wall) # wall along Y=0 + wall2 = generate_plane([0, wall_width], [wall_width, wall_width], [0, wall_height], points_per_wall) # wall along Y=wall_width + wall3 = generate_plane([0, 0], [0, wall_width], [0, wall_height], points_per_wall) # wall along X=0 + wall4 = generate_plane([wall_width, wall_width], [0, wall_width], [0, wall_height], points_per_wall) # wall along X=wall_width + + # generate points for the floor + floor = generate_plane([0, wall_width], [0, wall_width], [0, 0], points_per_wall) # floor at Z=0 + + # combine all walls and the floor into a single point cloud + return np.vstack((wall1, wall2, wall3, wall4, floor)) \ No newline at end of file diff --git a/mad_icp/src/pybind/CMakeLists.txt b/mad_icp/src/pybind/CMakeLists.txt index 8a7ac6f..6097dfe 100644 --- a/mad_icp/src/pybind/CMakeLists.txt +++ b/mad_icp/src/pybind/CMakeLists.txt @@ -1,3 +1,12 @@ +# simple std vector of eigen vectors +pybind11_add_module(pyvector MODULE pyvector.cpp) +target_link_libraries(pyvector PRIVATE + Eigen3::Eigen +) + +install(TARGETS pyvector DESTINATION .) + +# lidar odometry pybind11_add_module(pypeline MODULE pypeline.cpp) target_link_libraries(pypeline PRIVATE Eigen3::Eigen @@ -5,4 +14,25 @@ target_link_libraries(pypeline PRIVATE tools ) -install(TARGETS pypeline DESTINATION .) \ No newline at end of file +install(TARGETS pypeline DESTINATION .) + +########## others utils ########## + +# nn search with madtree +pybind11_add_module(pymadtree MODULE tools/pymadtree.cpp) +target_link_libraries(pymadtree PRIVATE + Eigen3::Eigen + tools +) + +install(TARGETS pymadtree DESTINATION .) + +# align two clouds with madicp +pybind11_add_module(pymadicp MODULE tools/pymadicp.cpp) +target_link_libraries(pymadicp PRIVATE + Eigen3::Eigen + odometry + tools +) + +install(TARGETS pymadicp DESTINATION .) \ No newline at end of file diff --git a/mad_icp/src/pybind/pypeline.cpp b/mad_icp/src/pybind/pypeline.cpp index bdbac5f..ad5f3e3 100755 --- a/mad_icp/src/pybind/pypeline.cpp +++ b/mad_icp/src/pybind/pypeline.cpp @@ -1,47 +1,60 @@ +// Copyright 2024 R(obots) V(ision) and P(erception) group +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// 1. Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// +// 2. Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// +// 3. Neither the name of the copyright holder nor the names of its contributors +// may be used to endorse or promote products derived from this software +// without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. + // pybind11 -#include -#include -#include -#include -#include #include -#include -#include - -// std stuff -#include -#include -#include -#include "eigen_stl_bindings.h" #include "odometry/pipeline.h" -PYBIND11_MAKE_OPAQUE(std::vector); - namespace py11 = pybind11; using namespace py11::literals; PYBIND11_MODULE(pypeline, m) { - auto vector3dvector = pybind_eigen_vector_of_vector( - m, "VectorEigen3d", "std::vector", - py11::py_array_to_vectors_double); - - auto pipeline = - py11::class_(m, "Pipeline") - .def(py11::init(), - py11::arg("sensor_hz"), py11::arg("deskew"), py11::arg("b_max"), - py11::arg("rho_ker"), py11::arg("p_th"), py11::arg("b_min"), - py11::arg("b_ratio"), py11::arg("num_keyframes"), - py11::arg("num_threads"), py11::arg("realtime")) - .def("currentPose", &Pipeline::currentPose) - .def("trajectory", &Pipeline::trajectory) - .def("keyframePose", &Pipeline::keyframePose) - .def("isInitialized", &Pipeline::isInitialized) - .def("isMapUpdated", &Pipeline::isMapUpdated) - .def("currentID", &Pipeline::currentID) - .def("keyframeID", &Pipeline::keyframeID) - .def("modelLeaves", &Pipeline::modelLeaves) - .def("currentLeaves", &Pipeline::currentLeaves) - .def("compute", &Pipeline::compute); + auto pipeline = py11::class_(m, "Pipeline") + .def(py11::init(), + py11::arg("sensor_hz"), + py11::arg("deskew"), + py11::arg("b_max"), + py11::arg("rho_ker"), + py11::arg("p_th"), + py11::arg("b_min"), + py11::arg("b_ratio"), + py11::arg("num_keyframes"), + py11::arg("num_threads"), + py11::arg("realtime")) + .def("currentPose", &Pipeline::currentPose) + .def("trajectory", &Pipeline::trajectory) + .def("keyframePose", &Pipeline::keyframePose) + .def("isInitialized", &Pipeline::isInitialized) + .def("isMapUpdated", &Pipeline::isMapUpdated) + .def("currentID", &Pipeline::currentID) + .def("keyframeID", &Pipeline::keyframeID) + .def("modelLeaves", &Pipeline::modelLeaves) + .def("currentLeaves", &Pipeline::currentLeaves) + .def("compute", &Pipeline::compute); } diff --git a/mad_icp/src/pybind/pyvector.cpp b/mad_icp/src/pybind/pyvector.cpp new file mode 100644 index 0000000..683c694 --- /dev/null +++ b/mad_icp/src/pybind/pyvector.cpp @@ -0,0 +1,54 @@ +// Copyright 2024 R(obots) V(ision) and P(erception) group +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// 1. Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// +// 2. Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// +// 3. Neither the name of the copyright holder nor the names of its contributors +// may be used to endorse or promote products derived from this software +// without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. + +// pybind11 +#include +#include +#include +#include +#include +#include +#include +#include + +// std stuff +#include +#include +#include + +#include "eigen_stl_bindings.h" + +PYBIND11_MAKE_OPAQUE(std::vector); + +namespace py11 = pybind11; +using namespace py11::literals; + +PYBIND11_MODULE(pyvector, m) { + auto vector3dvector = pybind_eigen_vector_of_vector( + m, "VectorEigen3d", "std::vector", py11::py_array_to_vectors_double); +} \ No newline at end of file diff --git a/mad_icp/src/pybind/tools/mad_icp_wrapper.h b/mad_icp/src/pybind/tools/mad_icp_wrapper.h new file mode 100644 index 0000000..53b8cbc --- /dev/null +++ b/mad_icp/src/pybind/tools/mad_icp_wrapper.h @@ -0,0 +1,112 @@ +// Copyright 2024 R(obots) V(ision) and P(erception) group +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// 1. Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// +// 2. Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// +// 3. Neither the name of the copyright holder nor the names of its contributors +// may be used to endorse or promote products derived from this software +// without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. + +#pragma once +#include +#include + +class MADicpWrapper { +public: + MADicpWrapper(const int num_threads) : num_threads_(num_threads_) { + max_parallel_levels_ = static_cast(std::log2(num_threads_)); + omp_set_num_threads(num_threads_); + } + + void setQueryCloud(ContainerType query, const double b_max, const double b_min) { + ContainerType* query_ptr = &query; + query_tree_.reset( + new MADtree(query_ptr, query_ptr->begin(), query_ptr->end(), b_max, b_min, 0, max_parallel_levels_, nullptr, nullptr)); + query_tree_->getLeafs(std::back_insert_iterator(query_leaves_)); + } + + void setReferenceCloud(ContainerType reference, const double b_max, const double b_min) { + ContainerType* reference_ptr = &reference; + ref_b_max_ = b_max; + ref_tree_.reset(new MADtree( + reference_ptr, reference_ptr->begin(), reference_ptr->end(), ref_b_max_, b_min, 0, max_parallel_levels_, nullptr, nullptr)); + } + + inline Eigen::Matrix4d compute(const Eigen::Matrix4d& T, + const size_t max_icp_iterations, + const double rho_ker, + double b_ratio, + const bool print_stats) { + mad_icp_.reset(new MADicp(ref_b_max_, rho_ker, b_ratio, 1)); + mad_icp_->setMoving(query_leaves_); + // make initial guess in right format + Eigen::Isometry3d dX = Eigen::Isometry3d::Identity(); + dX.linear() = T.block<3, 3>(0, 0); + dX.translation() = T.block<3, 1>(0, 3); + mad_icp_->init(dX); + + float icp_time = 0; + struct timeval icp_start, icp_end, icp_delta; + gettimeofday(&icp_start, nullptr); + + // icp loop + for (size_t icp_iteration = 0; icp_iteration < max_icp_iterations; ++icp_iteration) { + if (icp_iteration == max_icp_iterations - 1) { + for (MADtree* l : query_leaves_) { + l->matched_ = false; + } + } + mad_icp_->resetAdders(); + mad_icp_->update(ref_tree_.get()); + mad_icp_->updateState(); + } + + gettimeofday(&icp_end, nullptr); + timersub(&icp_end, &icp_start, &icp_delta); + icp_time = float(icp_delta.tv_sec) * 1000. + 1e-3 * icp_delta.tv_usec; + + if (print_stats) { + int matched_leaves = 0; + for (MADtree* l : query_leaves_) { + if (l->matched_) { + matched_leaves++; + } + } + const double inliers_ratio = double(matched_leaves) / double(query_leaves_.size()); + std::cout << "MADicp|compute time " << icp_time << " [ms] " << std::endl; + std::cout << "MADicp|inliers ratio " << inliers_ratio << std::endl; + std::cout << "--MADicp|matched leaves " << matched_leaves << std::endl; + std::cout << "--MADicp|total num leaves " << query_leaves_.size() << std::endl; + } + + return mad_icp_->X_.matrix(); + } + +protected: + std::unique_ptr mad_icp_ = nullptr; + std::unique_ptr ref_tree_ = nullptr; + std::unique_ptr query_tree_ = nullptr; + LeafList query_leaves_; + double ref_b_max_; + int max_parallel_levels_; + int num_threads_; +}; diff --git a/mad_icp/src/pybind/tools/mad_tree_wrapper.h b/mad_icp/src/pybind/tools/mad_tree_wrapper.h new file mode 100644 index 0000000..28e5169 --- /dev/null +++ b/mad_icp/src/pybind/tools/mad_tree_wrapper.h @@ -0,0 +1,60 @@ +// Copyright 2024 R(obots) V(ision) and P(erception) group +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// 1. Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// +// 2. Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// +// 3. Neither the name of the copyright holder nor the names of its contributors +// may be used to endorse or promote products derived from this software +// without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. + +#pragma once + +#include "../eigen_stl_bindings.h" +#include + +class MADtreeWrapper { +public: + void build(ContainerType vec, const double b_max, const double b_min, const int max_parallel_level) { + ContainerType* vec_add = &vec; + mad_tree_.reset( + new MADtree(vec_add, vec_add->begin(), vec_add->end(), b_max, b_min, 0, max_parallel_level, nullptr, nullptr)); + } + + std::pair search(const Eigen::Vector3d& query) { + const MADtree* match_leaf = mad_tree_->bestMatchingLeafFast(query); + // this is the median point and normal calculated with PCA + return std::make_pair(match_leaf->mean_, match_leaf->eigenvectors_.col(0)); + } + + std::vector> searchCloud(const ContainerType& query_cloud) { + std::vector> matches(query_cloud.size()); + int idx = 0; + for (const auto& query : query_cloud) { + const MADtree* match_leaf = mad_tree_->bestMatchingLeafFast(query); + matches[idx++] = std::make_pair(match_leaf->mean_, match_leaf->eigenvectors_.col(0)); + } + return matches; + } + +protected: + std::unique_ptr mad_tree_ = nullptr; +}; diff --git a/mad_icp/src/pybind/tools/pymadicp.cpp b/mad_icp/src/pybind/tools/pymadicp.cpp new file mode 100644 index 0000000..603ffc7 --- /dev/null +++ b/mad_icp/src/pybind/tools/pymadicp.cpp @@ -0,0 +1,52 @@ +// Copyright 2024 R(obots) V(ision) and P(erception) group +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// 1. Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// +// 2. Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// +// 3. Neither the name of the copyright holder nor the names of its contributors +// may be used to endorse or promote products derived from this software +// without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. + +#include "mad_icp_wrapper.h" +#include +#include +#include + +namespace py = pybind11; + +PYBIND11_MODULE(pymadicp, m) { + py::class_(m, "MADicp") + .def(py::init(), py::arg("num_threads")) + .def("setQueryCloud", &MADicpWrapper::setQueryCloud, py::arg("query"), py::arg("b_max") = 0.2, py::arg("b_min") = 0.1) + .def("setReferenceCloud", + &MADicpWrapper::setReferenceCloud, + py::arg("reference"), + py::arg("b_max") = 0.2, + py::arg("b_min") = 0.1) + .def("compute", + &MADicpWrapper::compute, + py::arg("T"), + py::arg("icp_iterations") = MAX_ICP_ITS, + py::arg("rho_ker") = 0.1, + py::arg("b_ratio") = 0.02, + py::arg("print_stats") = false); +} diff --git a/mad_icp/src/pybind/tools/pymadtree.cpp b/mad_icp/src/pybind/tools/pymadtree.cpp new file mode 100644 index 0000000..133e434 --- /dev/null +++ b/mad_icp/src/pybind/tools/pymadtree.cpp @@ -0,0 +1,47 @@ +// Copyright 2024 R(obots) V(ision) and P(erception) group +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// 1. Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// +// 2. Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// +// 3. Neither the name of the copyright holder nor the names of its contributors +// may be used to endorse or promote products derived from this software +// without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. + +#include "mad_tree_wrapper.h" +#include +#include + +namespace py11 = pybind11; +using namespace py11::literals; + +PYBIND11_MODULE(pymadtree, m) { + auto madtree = py11::class_(m, "MADtree") + .def(py11::init<>()) + .def("build", + &MADtreeWrapper::build, + py11::arg("vec"), + py11::arg("b_max") = 1e-5, + py11::arg("b_min") = 0.1, + py11::arg("max_parallel_level") = 2) + .def("search", &MADtreeWrapper::search, py11::arg("query")) + .def("searchCloud", &MADtreeWrapper::searchCloud, py11::arg("query_cloud")); +}