Registration with other ICP variants from Open3D using KISS-ICP

[espackman]

Hi team!

Congratulations on the release :) its been a long time coming.

I am curious if it was intended or even possible to modify the registration method using Open3D functions (either Python or C++), while keeping all other variables constant.

In my mind, I would like to run and analyze the results for a dataset using point to point, point to plane, and other distance metrics.

Thanks again!

[nachovizzo]

Hello,

I had a pure Open3D python implementation of the full system using only open3D, but the problem is that you can't really integrate the local map representation from kiss and the open3d API. Therefore you should use a point cloud as the target map and this is going to slow down the system by big big factor.

I guess doing this experiment with the Kiss pipileline is not trivial, but you can look into open3D on how to do the normal estimation and the Jacobians for the point to plane metric. I usually follow a same style so shouldn't be a problem to "copy paste"

We don't plan to do this since it's going to bloat the source code a lot and we don't intend to use any other metric so far for the optimization.

[espackman]

Great to hear, @nachovizzo!

I originally planned to use the different Open3D classes (Transformation Estimation, EstimateNormals) in the Registration.cpp file in place of the existing jacobian and residual calculation. Do you think this is the right direction? I apologize; I am attempting this on my own, so I appreciate any feedback or advice I can get.

(http://www.open3d.org/docs/latest/cpp_api/classopen3d_1_1pipelines_1_1registration_1_1_transformation_estimation.html http://www.open3d.org/html/cpp_api/classopen3d_1_1geometry_1_1_point_cloud.html)

[nachovizzo]

How I would do it?

Option 1: Implement everything in Open3D-python

Option 2: Probable the best one, merge KISS-ICP C++ API with Open3D, and still use the pipeline from kiss-icp

I will give you an example on how to implement point-to-plane ICP into the KISS-ICP system

Some pointers:

Add open3d to build (first you need to install it from source, go on their documentations on how to do it)

The patch on how to do this from the tip of the main branch is

diff --git a/src/cpp/kiss_icp/3rdparty/find_dependencies.cmake b/src/cpp/kiss_icp/3rdparty/find_dependencies.cmake
index ec11c88..92f6651 100644
--- a/src/cpp/kiss_icp/3rdparty/find_dependencies.cmake
+++ b/src/cpp/kiss_icp/3rdparty/find_dependencies.cmake
@@ -50,3 +50,6 @@ if(NOT USE_SYSTEM_TBB OR NOT TBB_FOUND)
   set(USE_SYSTEM_TBB OFF)
   include(${CMAKE_CURRENT_LIST_DIR}/tbb/tbb.cmake)
 endif()
+
+# Find installed Open3D, which exports Open3D::Open3D
+find_package(Open3D REQUIRED)
diff --git a/src/cpp/kiss_icp/core/CMakeLists.txt b/src/cpp/kiss_icp/core/CMakeLists.txt
index aa5a3a3..607afd7 100644
--- a/src/cpp/kiss_icp/core/CMakeLists.txt
+++ b/src/cpp/kiss_icp/core/CMakeLists.txt
@@ -23,5 +23,5 @@
 add_library(core STATIC)
 add_library(kiss_icp::core ALIAS core)
 target_sources(core PRIVATE Registration.cpp Deskew.cpp VoxelHashMap.cpp Preprocessing.cpp Threshold.cpp)
-target_link_libraries(core PUBLIC Eigen3::Eigen tsl::robin_map TBB::tbb)
+target_link_libraries(core PUBLIC Eigen3::Eigen tsl::robin_map TBB::tbb Open3D::Open3D)
 set_global_target_properties(core)

And also the pybind side

diff --git a/src/cpp/kiss_icp_pybind/CMakeLists.txt b/src/cpp/kiss_icp_pybind/CMakeLists.txt
index 944246b..93d275e 100644
--- a/src/cpp/kiss_icp_pybind/CMakeLists.txt
+++ b/src/cpp/kiss_icp_pybind/CMakeLists.txt
@@ -47,6 +47,8 @@ if(NOT USE_SYSTEM_PYBIND11 OR NOT pybind11_FOUND)
   include(${CMAKE_CURRENT_LIST_DIR}/pybind11/pybind11.cmake)
 endif()

+find_package(Open3D QUIET)
+
 pybind11_add_module(kiss_icp_pybind MODULE kiss_icp_pybind.cpp)
-target_link_libraries(kiss_icp_pybind PRIVATE kiss_icp::core kiss_icp::metrics)
+target_link_libraries(kiss_icp_pybind PRIVATE Open3D::Open3D kiss_icp::core kiss_icp::metrics)
 install(TARGETS kiss_icp_pybind DESTINATION .)

Use Open3D Registration pipeline from KISS-ICP

This is very hacky and needs more work, but at least took me 5 minutes to implement ;) You can also get rid of much stuff of the original pipeline

commit 285a708174dd6620c9d8a7f9fbbe5064f138b39f
Author: Ignacio Vizzo <ignaciovizzo@gmail.com>
Date:   Fri Jan 27 13:15:34 2023 +0100

    Add normal computation for target map

diff --git a/src/cpp/kiss_icp/core/VoxelHashMap.cpp b/src/cpp/kiss_icp/core/VoxelHashMap.cpp
index abc8c18..9bd5814 100644
--- a/src/cpp/kiss_icp/core/VoxelHashMap.cpp
+++ b/src/cpp/kiss_icp/core/VoxelHashMap.cpp
@@ -22,6 +22,8 @@
 // SOFTWARE.
 #include "VoxelHashMap.hpp"

+#include <open3d/geometry/PointCloud.h>
+#include <open3d/pipelines/registration/Registration.h>
 #include <tbb/blocked_range.h>
 #include <tbb/parallel_reduce.h>

@@ -65,26 +67,23 @@ Eigen::Matrix4d VoxelHashMap::RegisterPoinCloud(const Vector3dVector &points,
                                                 double kernel) {
     if (Empty()) return initial_guess;

-    // Equation (9)
-    Vector3dVector source = points;
-    TransformPoints(initial_guess, source);
-
-    // ICP-loop
-    Eigen::Matrix4d T_icp = Eigen::Matrix4d::Identity();
-    for (int j = 0; j < MAX_NUM_ITERATIONS_; ++j) {
-        // Equation (10)
-        const auto &[src, tgt] = GetCorrespondences(source, max_correspondence_distance);
-        // Equation (11)
-        auto [x, estimation] = AlignClouds(src, tgt, kernel);
-        // Equation (12)
-        TransformPoints(estimation, source);
-        // Update iterations
-        T_icp = estimation * T_icp;
-        // Termination criteria
-        if (x.norm() < ESTIMATION_THRESHOLD_) break;
-    }
-    // Spit the final transformation
-    return T_icp * initial_guess;
+    // Namespacealiases
+    namespace o3d = open3d;
+    using o3d::pipelines::registration::ICPConvergenceCriteria;
+    using o3d::pipelines::registration::RegistrationICP;
+    using o3d::pipelines::registration::TransformationEstimationPointToPlane;
+    using o3d::pipelines::registration::TransformationEstimationPointToPoint;
+
+    o3d::geometry::PointCloud source(points);
+    o3d::geometry::PointCloud target(Pointcloud());
+    target.EstimateNormals();
+
+    const auto estimation = TransformationEstimationPointToPlane();
+    const auto criteria = ICPConvergenceCriteria();
+    auto res = RegistrationICP(source, target, max_correspondence_distance, initial_guess,
+                               estimation, criteria);
+
+    return res.transformation_;
 }

Try it! It's actually working

I just hit make at the root directory and then tried it with KITTI, note that it's much slower due the normal computation

$ make
$ kiss_icp_pipeline --dataloader kitti $DATASETS/kitti-odometry/dataset --sequence 0 --visualize

Conclusion

The option2 is the easiest way to fix the "KISS-ICP" pipeline, adaptive threhsold + voxelization + map representaion + keypoints selection, and use the ICP loop from open3D (which they are indeed quite similar)

The drawback is that this is too generic and therefore, normal computation, NN searches (open3D uses a slow KDTree) are not optimnzed, therefore I obtain an avarge of 4 frames per second on a multicore machine, instead to 60 FPS with the original implementation

Results

On seq 04 of kitti I obtain

 ─────────────────────────────────────────────────
                           Metric   Value   Units
 ─────────────────────────────────────────────────
        Average Translation Error   0.451   %
         Average Rotational Error   0.085   deg/m
  Absoulte Trajectory Error (ATE)   0.934   m
  Absoulte Rotational Error (ARE)   0.259   rad
                Average Frequency     4     Hz
                  Average Runtime    250    ms
 ─────────────────────────────────────────────────

In contrast to the original kiss-icp implementation:

───────────────────────────────────────────────── 
                           Metric   Value   Units  
 ───────────────────────────────────────────────── 
        Average Translation Error   0.355   %      
         Average Rotational Error   0.142   deg/m  
  Absoulte Trajectory Error (ATE)   1.075   m      
  Absoulte Rotational Error (ARE)   0.665   rad    
                Average Frequency    49     Hz     
                  Average Runtime    20     ms     
 ─────────────────────────────────────────────────

[espackman]

Thank you, @nachovizzo! This was helpful. I have been playing around and was able to get some interesting results. I'm curious, would this method also work with the Point Cloud Library and other libraries? I am unfamiliar with CMake, so my attempts at replicating this process with other sources haven't succeeded.

Once I get this wrapped up, I'd be more than happy to share my results! Thank you so much for the help so far.

[nachovizzo]

@espackman, my pleasure.

This method should work within PCL or Open3D. But we decided to make it more library-independent just to be able to use it virtually everywhere. If you inject PCL as a dependency, now we have a problem :rofl:

And yes, please share whatever you find out!