From a71c0e7593a25144db30469b961c49c0d3275e37 Mon Sep 17 00:00:00 2001
From: Ignacio Vizzo <ignaciovizzo@gmail.com>
Date: Mon, 18 Mar 2024 10:52:14 +0100
Subject: [PATCH] Nacho/get rid of get adjacent voxels (#291)

* Remove function from voxelhashmap

* Add getter and comments

* no typo no nacho

* I like this version better

now stop
---
 cpp/kiss_icp/core/Registration.cpp | 21 ++++++++++++++++++++-
 cpp/kiss_icp/core/VoxelHashMap.cpp | 16 ----------------
 cpp/kiss_icp/core/VoxelHashMap.hpp |  7 +++++--
 3 files changed, 25 insertions(+), 19 deletions(-)

diff --git a/cpp/kiss_icp/core/Registration.cpp b/cpp/kiss_icp/core/Registration.cpp
index 2634b0a8..b3d8d1cf 100644
--- a/cpp/kiss_icp/core/Registration.cpp
+++ b/cpp/kiss_icp/core/Registration.cpp
@@ -52,10 +52,29 @@ void TransformPoints(const Sophus::SE3d &T, std::vector<Eigen::Vector3d> &points
                    [&](const auto &point) { return T * point; });
 }
 
+using Voxel = kiss_icp::VoxelHashMap::Voxel;
+std::vector<Voxel> GetAdjacentVoxels(const Voxel &voxel, int adjacent_voxels = 1) {
+    std::vector<Voxel> voxel_neighborhood;
+    for (int i = voxel.x() - adjacent_voxels; i < voxel.x() + adjacent_voxels + 1; ++i) {
+        for (int j = voxel.y() - adjacent_voxels; j < voxel.y() + adjacent_voxels + 1; ++j) {
+            for (int k = voxel.z() - adjacent_voxels; k < voxel.z() + adjacent_voxels + 1; ++k) {
+                voxel_neighborhood.emplace_back(i, j, k);
+            }
+        }
+    }
+    return voxel_neighborhood;
+}
+
 Eigen::Vector3d GetClosestNeighbor(const Eigen::Vector3d &point,
                                    const kiss_icp::VoxelHashMap &voxel_map) {
-    const auto &query_voxels = voxel_map.GetAdjacentVoxels(point);
+    // Convert the point to voxel coordinates
+    const auto &voxel = voxel_map.PointToVoxel(point);
+    // Get nearby voxels on the map
+    const auto &query_voxels = GetAdjacentVoxels(voxel);
+    // Extract the points contained within the neighborhood voxels
     const auto &neighbors = voxel_map.GetPoints(query_voxels);
+
+    // Find the nearest neighbor
     Eigen::Vector3d closest_neighbor;
     double closest_distance2 = std::numeric_limits<double>::max();
     std::for_each(neighbors.cbegin(), neighbors.cend(), [&](const auto &neighbor) {
diff --git a/cpp/kiss_icp/core/VoxelHashMap.cpp b/cpp/kiss_icp/core/VoxelHashMap.cpp
index 39a0e703..3f710d26 100644
--- a/cpp/kiss_icp/core/VoxelHashMap.cpp
+++ b/cpp/kiss_icp/core/VoxelHashMap.cpp
@@ -31,22 +31,6 @@
 
 namespace kiss_icp {
 
-std::vector<VoxelHashMap::Voxel> VoxelHashMap::GetAdjacentVoxels(const Eigen::Vector3d &point,
-                                                                 int adjacent_voxels) const {
-    auto kx = static_cast<int>(point[0] / voxel_size_);
-    auto ky = static_cast<int>(point[1] / voxel_size_);
-    auto kz = static_cast<int>(point[2] / voxel_size_);
-    std::vector<Voxel> voxel_neighborhood;
-    for (int i = kx - adjacent_voxels; i < kx + adjacent_voxels + 1; ++i) {
-        for (int j = ky - adjacent_voxels; j < ky + adjacent_voxels + 1; ++j) {
-            for (int k = kz - adjacent_voxels; k < kz + adjacent_voxels + 1; ++k) {
-                voxel_neighborhood.emplace_back(i, j, k);
-            }
-        }
-    }
-    return voxel_neighborhood;
-}
-
 std::vector<Eigen::Vector3d> VoxelHashMap::GetPoints(const std::vector<Voxel> &query_voxels) const {
     std::vector<Eigen::Vector3d> points;
     points.reserve(query_voxels.size() * static_cast<size_t>(max_points_per_voxel_));
diff --git a/cpp/kiss_icp/core/VoxelHashMap.hpp b/cpp/kiss_icp/core/VoxelHashMap.hpp
index 65a1f94f..7eff4347 100644
--- a/cpp/kiss_icp/core/VoxelHashMap.hpp
+++ b/cpp/kiss_icp/core/VoxelHashMap.hpp
@@ -57,14 +57,17 @@ struct VoxelHashMap {
 
     inline void Clear() { map_.clear(); }
     inline bool Empty() const { return map_.empty(); }
+    inline Voxel PointToVoxel(const Eigen::Vector3d &point) const {
+        return Voxel(static_cast<int>(point.x() / voxel_size_),
+                     static_cast<int>(point.y() / voxel_size_),
+                     static_cast<int>(point.z() / voxel_size_));
+    }
     void Update(const std::vector<Eigen::Vector3d> &points, const Eigen::Vector3d &origin);
     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::vector<Eigen::Vector3d> GetPoints(const std::vector<Voxel> &query_voxels) const;
-    std::vector<Voxel> GetAdjacentVoxels(const Eigen::Vector3d &point,
-                                         int adjacent_voxels = 1) const;
 
     double voxel_size_;
     double max_distance_;