Introduce Bonxai as Map representation

cpp/kinematic_icp/3rdparty/bonxai/bonxai.cmake

@@ -0,0 +1,32 @@
+# MIT License
+#
+# Copyright (c) 2024 Tiziano Guadagnino, Benedikt Mersch, Ignacio Vizzo, Cyrill
+# Stachniss.
+#
+# Permission is hereby granted, free of charge, to any person obtaining a copy
+# of this software and associated documentation files (the "Software"), to deal
+# in the Software without restriction, including without limitation the rights
+# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+# copies of the Software, and to permit persons to whom the Software is
+# furnished to do so, subject to the following conditions:
+#
+# The above copyright notice and this permission notice shall be included in all
+# copies or substantial portions of the Software.
+#
+# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+# AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+# SOFTWARE.
+# Silence timestamp warning
+if(CMAKE_VERSION VERSION_GREATER 3.24)
+  cmake_policy(SET CMP0135 OLD)
+endif()
+
+include(FetchContent)
+FetchContent_Declare(
+  bonxai URL https://github.com/facontidavide/Bonxai/archive/refs/tags/v0.6.0.tar.gz SOURCE_SUBDIR bonxai_core
+  PATCH_COMMAND patch -p1 < ${CMAKE_CURRENT_LIST_DIR}/bonxai.patch UPDATE_DISCONNECTED 1)
+FetchContent_MakeAvailable(bonxai)

cpp/kinematic_icp/3rdparty/bonxai/bonxai.patch

@@ -0,0 +1,52 @@
+diff --git a/bonxai_core/include/bonxai/bonxai.hpp b/bonxai_core/include/bonxai/bonxai.hpp
+index 7d360cf..e43771e 100644
+--- a/bonxai_core/include/bonxai/bonxai.hpp
++++ b/bonxai_core/include/bonxai/bonxai.hpp
+@@ -367,7 +367,7 @@ inline size_t Grid<DataT>::memUsage() const {
+ template <typename DataT>
+ inline void VoxelGrid<DataT>::releaseUnusedMemory() {
+   std::vector<CoordT> keys_to_delete;
+-  for (const auto& [key, inner_grid] : root_map) {
++  for (auto& [key, inner_grid] : root_map) {
+     for (auto inner_it = inner_grid.mask().beginOn(); inner_it; ++inner_it) {
+       const int32_t inner_index = *inner_it;
+       auto& leaf_grid = inner_grid.cell(inner_index);
+@@ -404,9 +404,9 @@ inline VoxelGrid<DataT>::VoxelGrid(double voxel_size, uint8_t inner_bits, uint8_
+ template <typename DataT>
+ inline CoordT VoxelGrid<DataT>::posToCoord(double x, double y, double z) const {
+   return {
+-      static_cast<int32_t>(std::nearbyint(x * inv_resolution)),
+-      static_cast<int32_t>(std::nearbyint(y * inv_resolution)),
+-      static_cast<int32_t>(std::nearbyint(z * inv_resolution))};
++      static_cast<int32_t>(std::floor(x * inv_resolution)),
++      static_cast<int32_t>(std::floor(y * inv_resolution)),
++      static_cast<int32_t>(std::floor(z * inv_resolution))};
+ }
+
+ template <typename DataT>
+diff --git a/bonxai_core/include/bonxai/grid_coord.hpp b/bonxai_core/include/bonxai/grid_coord.hpp
+index da8fb7d..1f9727b 100644
+--- a/bonxai_core/include/bonxai/grid_coord.hpp
++++ b/bonxai_core/include/bonxai/grid_coord.hpp
+@@ -79,9 +79,9 @@ struct CoordT {
+
+ [[nodiscard]] inline CoordT PosToCoord(const Point3D& point, double inv_resolution) {
+   return {
+-      static_cast<int32_t>(std::nearbyint(point.x * inv_resolution)),
+-      static_cast<int32_t>(std::nearbyint(point.y * inv_resolution)),
+-      static_cast<int32_t>(std::nearbyint(point.z * inv_resolution))};
++      static_cast<int32_t>(std::floor(point.x * inv_resolution)),
++      static_cast<int32_t>(std::floor(point.y * inv_resolution)),
++      static_cast<int32_t>(std::floor(point.z * inv_resolution))};
+ }
+
+ [[nodiscard]] inline Point3D CoordToPos(const CoordT& coord, double resolution) {
+@@ -212,7 +212,7 @@ struct hash<Bonxai::CoordT> {
+   std::size_t operator()(const Bonxai::CoordT& p) const {
+     // same as OpenVDB
+     return ((1 << 20) - 1) & (static_cast<int64_t>(p.x) * 73856093 ^  //
+-                              static_cast<int64_t>(p.y) * 19349663 ^  //
++                              static_cast<int64_t>(p.y) * 19349669 ^  //
+                               static_cast<int64_t>(p.z) * 83492791);
+   }
+ };

cpp/kinematic_icp/3rdparty/include_dependencies.cmake

@@ -0,0 +1,2 @@
+include(${CMAKE_CURRENT_LIST_DIR}/kiss_icp/kiss-icp.cmake)
+include(${CMAKE_CURRENT_LIST_DIR}/bonxai/bonxai.cmake)

cpp/kinematic_icp/CMakeLists.txt

@@ -29,7 +29,8 @@ option(USE_SYSTEM_EIGEN3 "Use system pre-installed Eigen" ON)
 option(USE_SYSTEM_SOPHUS "Use system pre-installed Sophus" ON)
 option(USE_SYSTEM_TSL-ROBIN-MAP "Use system pre-installed tsl_robin" ON)
 option(USE_SYSTEM_TBB "Use system pre-installed oneAPI/tbb" ON)
-include(kiss_icp/kiss-icp.cmake)
+
+include(3rdparty/include_dependencies.cmake)
 
 # KISS dependecies that are also used in Kinematic.
 # If the internal FetchContent from kiss is in use (through the above options)
@@ -54,6 +55,7 @@ set(CMAKE_POSITION_INDEPENDENT_CODE ON)
 
 include(cmake/CompilerOptions.cmake)
 
+add_subdirectory(local_map)
 add_subdirectory(correspondence_threshold)
 add_subdirectory(registration)
 add_subdirectory(pipeline)

cpp/kinematic_icp/local_map/CMakeLists.txt

@@ -0,0 +1,26 @@
+# MIT License
+#
+# Copyright (c) 2024 Tiziano Guadagnino, Benedikt Mersch, Ignacio Vizzo, Cyrill
+# Stachniss.
+#
+# Permission is hereby granted, free of charge, to any person obtaining a copy
+# of this software and associated documentation files (the "Software"), to deal
+# in the Software without restriction, including without limitation the rights
+# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+# copies of the Software, and to permit persons to whom the Software is
+# furnished to do so, subject to the following conditions:
+#
+# The above copyright notice and this permission notice shall be included in all
+# copies or substantial portions of the Software.
+#
+# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+# AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+# SOFTWARE.
+add_library(kinematic_icp_local_map STATIC)
+target_sources(kinematic_icp_local_map PRIVATE SparseVoxelGrid.cpp)
+target_link_libraries(kinematic_icp_local_map PUBLIC Eigen3::Eigen Sophus::Sophus bonxai_core)
+set_global_target_properties(kinematic_icp_local_map)

cpp/kinematic_icp/local_map/SparseVoxelGrid.cpp

@@ -0,0 +1,148 @@
+// MIT License
+
+// Copyright (c) 2024 Tiziano Guadagnino, Benedikt Mersch, Ignacio Vizzo, Cyrill
+// Stachniss.
+
+// Permission is hereby granted, free of charge, to any person obtaining a copy
+// of this software and associated documentation files (the "Software"), to deal
+// in the Software without restriction, including without limitation the rights
+// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+// copies of the Software, and to permit persons to whom the Software is
+// furnished to do so, subject to the following conditions:
+
+// The above copyright notice and this permission notice shall be included in all
+// copies or substantial portions of the Software.
+
+// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+// SOFTWARE.
+
+#include "SparseVoxelGrid.hpp"
+
+#include <Eigen/Core>
+#include <array>
+#include <bonxai/bonxai.hpp>
+#include <cstdint>
+#include <sophus/se3.hpp>
+
+#include "bonxai/grid_coord.hpp"
+
+namespace {
+static constexpr std::array<Bonxai::CoordT, 27> shifts{
+    Bonxai::CoordT{-1, -1, -1}, Bonxai::CoordT{-1, -1, 0}, Bonxai::CoordT{-1, -1, 1},
+    Bonxai::CoordT{-1, 0, -1},  Bonxai::CoordT{-1, 0, 0},  Bonxai::CoordT{-1, 0, 1},
+    Bonxai::CoordT{-1, 1, -1},  Bonxai::CoordT{-1, 1, 0},  Bonxai::CoordT{-1, 1, 1},
+
+    Bonxai::CoordT{0, -1, -1},  Bonxai::CoordT{0, -1, 0},  Bonxai::CoordT{0, -1, 1},
+    Bonxai::CoordT{0, 0, -1},   Bonxai::CoordT{0, 0, 0},   Bonxai::CoordT{0, 0, 1},
+    Bonxai::CoordT{0, 1, -1},   Bonxai::CoordT{0, 1, 0},   Bonxai::CoordT{0, 1, 1},
+
+    Bonxai::CoordT{1, -1, -1},  Bonxai::CoordT{1, -1, 0},  Bonxai::CoordT{1, -1, 1},
+    Bonxai::CoordT{1, 0, -1},   Bonxai::CoordT{1, 0, 0},   Bonxai::CoordT{1, 0, 1},
+    Bonxai::CoordT{1, 1, -1},   Bonxai::CoordT{1, 1, 0},   Bonxai::CoordT{1, 1, 1}};
+
+static constexpr uint8_t inner_grid_log2_dim = 2;
+static constexpr uint8_t leaf_grid_log2_dim = 3;
+}  // namespace
+
+namespace kinematic_icp {
+
+SparseVoxelGrid::SparseVoxelGrid(const double voxel_size,
+                                 const double clipping_distance,
+                                 const unsigned int max_points_per_voxel)
+    : voxel_size_(voxel_size),
+      clipping_distance_(clipping_distance),
+      max_points_per_voxel_(max_points_per_voxel),
+      map_(voxel_size, inner_grid_log2_dim, leaf_grid_log2_dim),
+      accessor_(map_.createAccessor()) {}
+
+std::tuple<Eigen::Vector3d, double> SparseVoxelGrid::GetClosestNeighbor(
+    const Eigen::Vector3d &query) const {
+    Eigen::Vector3d closest_neighbor = Eigen::Vector3d::Zero();
+    double closest_distance = std::numeric_limits<double>::max();
+    const auto const_accessor = map_.createConstAccessor();
+    const Bonxai::CoordT voxel = map_.posToCoord(query);
+    std::for_each(shifts.cbegin(), shifts.cend(), [&](const Bonxai::CoordT &voxel_shift) {
+        const Bonxai::CoordT query_voxel = voxel + voxel_shift;
+        const VoxelBlock *voxel_points = const_accessor.value(query_voxel);
+        if (voxel_points != nullptr) {
+            const Eigen::Vector3d &neighbor =
+                *std::min_element(voxel_points->cbegin(), voxel_points->cend(),
+                                  [&](const auto &lhs, const auto &rhs) {
+                                      return (lhs - query).norm() < (rhs - query).norm();
+                                  });
+            double distance = (neighbor - query).norm();
+            if (distance < closest_distance) {
+                closest_neighbor = neighbor;
+                closest_distance = distance;
+            }
+        }
+    });
+    return std::make_tuple(closest_neighbor, closest_distance);
+}
+
+void SparseVoxelGrid::AddPoints(const std::vector<Eigen::Vector3d> &points) {
+    const double map_resolution = std::sqrt(voxel_size_ * voxel_size_ / max_points_per_voxel_);
+    std::for_each(points.cbegin(), points.cend(), [&](const Eigen::Vector3d &p) {
+        const auto voxel_coordinates = map_.posToCoord(p);
+        VoxelBlock *voxel_points = accessor_.value(voxel_coordinates, /*create_if_missing=*/true);
+        if (voxel_points->size() == max_points_per_voxel_ ||
+            std::any_of(voxel_points->cbegin(), voxel_points->cend(), [&](const auto &voxel_point) {
+                return (voxel_point - p).norm() < map_resolution;
+            })) {
+            return;
+        }
+        voxel_points->reserve(max_points_per_voxel_);
+        voxel_points->emplace_back(p);
+    });
+}
+
+void SparseVoxelGrid::RemovePointsFarFromLocation(const Eigen::Vector3d &origin) {
+    auto is_too_far_away = [&](const VoxelBlock &block) {
+        return (block.front() - origin).norm() > clipping_distance_;
+    };
+
+    std::vector<Bonxai::CoordT> keys_to_delete;
+    auto &root_map = map_.rootMap();
+    for (auto &[key, inner_grid] : root_map) {
+        for (auto inner_it = inner_grid.mask().beginOn(); inner_it; ++inner_it) {
+            const int32_t inner_index = *inner_it;
+            auto &leaf_grid = inner_grid.cell(inner_index);
+            const auto &voxel_block = leaf_grid->cell(leaf_grid->mask().findFirstOn());
+            if (is_too_far_away(voxel_block)) {
+                inner_grid.mask().setOff(inner_index);
+                leaf_grid.reset();
+            }
+        }
+        if (inner_grid.mask().isOff()) {
+            keys_to_delete.push_back(key);
+        }
+    }
+    for (const auto &key : keys_to_delete) {
+        root_map.erase(key);
+    }
+}
+
+void SparseVoxelGrid::Update(const std::vector<Eigen::Vector3d> &points, const Sophus::SE3d &pose) {
+    std::vector<Eigen::Vector3d> points_transformed(points.size());
+    std::transform(points.cbegin(), points.cend(), points_transformed.begin(),
+                   [&](const auto &point) { return pose * point; });
+    const Eigen::Vector3d &origin = pose.translation();
+    AddPoints(points_transformed);
+    RemovePointsFarFromLocation(origin);
+}
+
+std::vector<Eigen::Vector3d> SparseVoxelGrid::Pointcloud() const {
+    std::vector<Eigen::Vector3d> point_cloud;
+    point_cloud.reserve(map_.activeCellsCount() * max_points_per_voxel_);
+    map_.forEachCell([&point_cloud, this](VoxelBlock &block, const auto &) {
+        point_cloud.insert(point_cloud.end(), block.cbegin(), block.cend());
+    });
+    return point_cloud;
+}
+
+}  // namespace kinematic_icp

cpp/kinematic_icp/local_map/SparseVoxelGrid.hpp

@@ -0,0 +1,56 @@
+// MIT License
+
+// Copyright (c) 2024 Tiziano Guadagnino, Benedikt Mersch, Ignacio Vizzo, Cyrill
+// Stachniss.
+
+// Permission is hereby granted, free of charge, to any person obtaining a copy
+// of this software and associated documentation files (the "Software"), to deal
+// in the Software without restriction, including without limitation the rights
+// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+// copies of the Software, and to permit persons to whom the Software is
+// furnished to do so, subject to the following conditions:
+
+// The above copyright notice and this permission notice shall be included in all
+// copies or substantial portions of the Software.
+
+// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+// SOFTWARE.
+#pragma once
+#include <Eigen/Core>
+#include <array>
+#include <bonxai/bonxai.hpp>
+#include <sophus/se3.hpp>
+
+namespace kinematic_icp {
+
+using VoxelBlock = std::vector<Eigen::Vector3d>;
+
+struct SparseVoxelGrid {
+    explicit SparseVoxelGrid(const double voxel_size,
+                             const double clipping_distance,
+                             const unsigned int max_points_per_voxel);
+
+    inline void Clear() { map_.clear(Bonxai::ClearOption::CLEAR_MEMORY); }
+    inline bool Empty() const { return map_.activeCellsCount() == 0; }
+    void Update(const std::vector<Eigen::Vector3d> &points, const Sophus::SE3d &pose);
+    void AddPoints(const std::vector<Eigen::Vector3d> &points);
+    void RemovePointsFarFromLocation(const Eigen::Vector3d &origin);
+    std::vector<Eigen::Vector3d> Pointcloud() const;
+    std::tuple<Eigen::Vector3d, double> GetClosestNeighbor(const Eigen::Vector3d &query) const;
+
+    double voxel_size_;
+    double clipping_distance_;
+    unsigned int max_points_per_voxel_;
+    Bonxai::VoxelGrid<VoxelBlock> map_;
+
+private:
+    using AccessorType = typename Bonxai::VoxelGrid<VoxelBlock>::Accessor;
+    AccessorType accessor_;
+};
+
+}  // namespace kinematic_icp

cpp/kinematic_icp/pipeline/CMakeLists.txt

@@ -23,5 +23,5 @@
 add_library(kinematic_icp_pipeline STATIC)
 target_sources(kinematic_icp_pipeline PRIVATE KinematicICP.cpp)
 target_link_libraries(kinematic_icp_pipeline PUBLIC kinematic_icp_registration kinematic_icp_threshold
-                                                    kiss_icp_pipeline)
+                                                    kinematic_icp_local_map kiss_icp_core)
 set_global_target_properties(kinematic_icp_pipeline)

cpp/kinematic_icp/pipeline/KinematicICP.cpp

@@ -25,8 +25,7 @@
 #include <Eigen/Core>
 #include <kiss_icp/core/Deskew.hpp>
 #include <kiss_icp/core/Preprocessing.hpp>
-#include <kiss_icp/core/Registration.hpp>
-#include <kiss_icp/core/VoxelHashMap.hpp>
+#include <kiss_icp/core/VoxelUtils.hpp>
 #include <vector>
 
 namespace {

cpp/kinematic_icp/pipeline/KinematicICP.hpp

@@ -24,14 +24,12 @@
 
 #include <Eigen/Core>
 #include <cmath>
-#include <kiss_icp/core/Threshold.hpp>
-#include <kiss_icp/core/VoxelHashMap.hpp>
-#include <kiss_icp/pipeline/KissICP.hpp>
 #include <sophus/se3.hpp>
 #include <tuple>
 #include <vector>
 
 #include "kinematic_icp/correspondence_threshold/CorrespondenceThreshold.hpp"
+#include "kinematic_icp/local_map/SparseVoxelGrid.hpp"
 #include "kinematic_icp/registration/Registration.hpp"
 
 namespace kinematic_icp::pipeline {
@@ -91,8 +89,8 @@ class KinematicICP {
 
     std::vector<Eigen::Vector3d> LocalMap() const { return local_map_.Pointcloud(); };
 
-    const kiss_icp::VoxelHashMap &VoxelMap() const { return local_map_; };
-    kiss_icp::VoxelHashMap &VoxelMap() { return local_map_; };
+    const SparseVoxelGrid &VoxelMap() const { return local_map_; };
+    SparseVoxelGrid &VoxelMap() { return local_map_; };
 
     const Sophus::SE3d &pose() const { return last_pose_; }
     Sophus::SE3d &pose() { return last_pose_; }
@@ -103,8 +101,7 @@ class KinematicICP {
     KinematicRegistration registration_;
     CorrespondenceThreshold correspondence_threshold_;
     Config config_;
-    // KISS-ICP pipeline modules
-    kiss_icp::VoxelHashMap local_map_;
+    SparseVoxelGrid local_map_;
 };
 
 }  // namespace kinematic_icp::pipeline