Registration with other ICP variants from Open3D using KISS-ICP about kiss-icp HOT 5 CLOSED

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)

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

commit 285a708174dd6620c9d8a7f9fbbe5064f138b39f
Author: Ignacio Vizzo <[email protected]>
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_;
 }