From 95f6dfdf54cae8eaecdb7fa66c09e0b0fa0489b6 Mon Sep 17 00:00:00 2001
From: Tiziano Guadagnino <37455909+tizianoGuadagnino@users.noreply.github.com>
Date: Thu, 8 Aug 2024 20:57:41 +0200
Subject: [PATCH] Remove GetPoints (#389)

* Now we can search neighbors from the voxel hash

* Integrate Nacho's Comment

Co-authored-by: Ignacio Vizzo <ignacio@dexory.com>

* Remove useless function, we now can do everything from the voxel hash
map

* Renaming for consistency

* Integrate Nacho's and Ben comments

---------

Co-authored-by: Ignacio Vizzo <ignacio@dexory.com>
---
 cpp/kiss_icp/core/VoxelHashMap.cpp | 40 +++++++++++-------------------
 cpp/kiss_icp/core/VoxelHashMap.hpp |  2 +-
 2 files changed, 16 insertions(+), 26 deletions(-)

diff --git a/cpp/kiss_icp/core/VoxelHashMap.cpp b/cpp/kiss_icp/core/VoxelHashMap.cpp
index 78f1b5a3..bd2a5403 100644
--- a/cpp/kiss_icp/core/VoxelHashMap.cpp
+++ b/cpp/kiss_icp/core/VoxelHashMap.cpp
@@ -43,42 +43,32 @@ std::vector<Voxel> GetAdjacentVoxels(const Voxel &voxel, int adjacent_voxels = 1
     }
     return voxel_neighborhood;
 }
-std::vector<Eigen::Vector3d> GetPoints(const std::vector<Voxel> &query_voxels,
-                                       const kiss_icp::VoxelHashMap &voxel_map) {
-    std::vector<Eigen::Vector3d> points;
-    points.reserve(query_voxels.size() * static_cast<size_t>(voxel_map.max_points_per_voxel_));
-    std::for_each(query_voxels.cbegin(), query_voxels.cend(), [&](const auto &query) {
-        auto search = voxel_map.map_.find(query);
-        if (search != voxel_map.map_.end()) {
-            const auto &voxel_points = search.value();
-            points.insert(points.end(), voxel_points.cbegin(), voxel_points.cend());
-        }
-    });
-    points.shrink_to_fit();
-    return points;
-}
-
 }  // namespace
 
 namespace kiss_icp {
 
 std::tuple<Eigen::Vector3d, double> VoxelHashMap::GetClosestNeighbor(
-    const Eigen::Vector3d &point) const {
+    const Eigen::Vector3d &query) const {
     // Convert the point to voxel coordinates
-    const auto &voxel = PointToVoxel(point, voxel_size_);
+    const auto &voxel = PointToVoxel(query, voxel_size_);
     // Get nearby voxels on the map
     const auto &query_voxels = GetAdjacentVoxels(voxel);
-    // Extract the points contained within the neighborhood voxels
-    const auto &neighbors = GetPoints(query_voxels, *this);
-
     // Find the nearest neighbor
     Eigen::Vector3d closest_neighbor = Eigen::Vector3d::Zero();
     double closest_distance = std::numeric_limits<double>::max();
-    std::for_each(neighbors.cbegin(), neighbors.cend(), [&](const auto &neighbor) {
-        double distance = (neighbor - point).norm();
-        if (distance < closest_distance) {
-            closest_neighbor = neighbor;
-            closest_distance = distance;
+    std::for_each(query_voxels.cbegin(), query_voxels.cend(), [&](const auto &query_voxel) {
+        auto search = map_.find(query_voxel);
+        if (search != map_.end()) {
+            const auto &points = search.value();
+            const Eigen::Vector3d &neighbor = *std::min_element(
+                points.cbegin(), 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);
diff --git a/cpp/kiss_icp/core/VoxelHashMap.hpp b/cpp/kiss_icp/core/VoxelHashMap.hpp
index 0c677885..48a7c9de 100644
--- a/cpp/kiss_icp/core/VoxelHashMap.hpp
+++ b/cpp/kiss_icp/core/VoxelHashMap.hpp
@@ -48,7 +48,7 @@ struct VoxelHashMap {
     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 &point) const;
+    std::tuple<Eigen::Vector3d, double> GetClosestNeighbor(const Eigen::Vector3d &query) const;
 
     double voxel_size_;
     double max_distance_;