added a couple of example of nn search and mad registration

mad_icp/.clang-format

@@ -38,7 +38,6 @@ BraceWrapping:
   SplitEmptyRecord: true
   SplitEmptyNamespace: true
 BreakBeforeBinaryOperators: None
-BreakBeforeBraces: Attach
 BreakBeforeInheritanceComma: false
 BreakBeforeTernaryOperators: true
 BreakConstructorInitializersBeforeComma: false

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()

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

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

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

mad_icp/src/pybind/CMakeLists.txt

@@ -1,8 +1,38 @@
+# 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
     odometry
     tools
 )
 
-install(TARGETS pypeline DESTINATION .)
+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 .)

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 <pybind11/chrono.h>
-#include <pybind11/complex.h>
-#include <pybind11/eigen.h>
-#include <pybind11/functional.h>
-#include <pybind11/numpy.h>
 #include <pybind11/pybind11.h>
-#include <pybind11/stl.h>
-#include <pybind11/stl_bind.h>
-
-// std stuff
-#include <Eigen/Core>
-#include <memory>
-#include <vector>
 
-#include "eigen_stl_bindings.h"
 #include "odometry/pipeline.h"
 
-PYBIND11_MAKE_OPAQUE(std::vector<Eigen::Vector3d>);
-
 namespace py11 = pybind11;
 using namespace py11::literals;
 
 PYBIND11_MODULE(pypeline, m) {
-  auto vector3dvector = pybind_eigen_vector_of_vector<Eigen::Vector3d>(
-      m, "VectorEigen3d", "std::vector<Eigen::Vector3d>",
-      py11::py_array_to_vectors_double<Eigen::Vector3d>);
-
-  auto pipeline =
-      py11::class_<Pipeline>(m, "Pipeline")
-          .def(py11::init<double, bool, double, double, double, double, double,
-                          int, int, bool>(),
-               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_<Pipeline>(m, "Pipeline")
+                    .def(py11::init<double, bool, double, double, double, double, double, int, int, bool>(),
+                         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);
 }