From 609ebb5d068c99d8dbc50e5c8e2c4f475b8fc43f Mon Sep 17 00:00:00 2001
From: tizianoGuadagnino <frevo93@gmail.com>
Date: Tue, 9 Jul 2024 18:28:56 +0200
Subject: [PATCH 01/12] Unify Voxel functionalities, now everything comes from
 VoxelHashMap

---
 cpp/kiss_icp/core/Preprocessing.cpp | 11 +++--------
 1 file changed, 3 insertions(+), 8 deletions(-)

diff --git a/cpp/kiss_icp/core/Preprocessing.cpp b/cpp/kiss_icp/core/Preprocessing.cpp
index 0a6a6e30..56e4d12d 100644
--- a/cpp/kiss_icp/core/Preprocessing.cpp
+++ b/cpp/kiss_icp/core/Preprocessing.cpp
@@ -28,18 +28,13 @@
 #include <Eigen/Core>
 #include <algorithm>
 #include <cmath>
+#include <kiss_icp/core/VoxelHashMap.hpp>
 #include <sophus/se3.hpp>
 #include <vector>
 
 namespace {
-// TODO(all): Maybe try to merge these voxel uitls with VoxelHashMap implementation
-using Voxel = Eigen::Vector3i;
-struct VoxelHash {
-    size_t operator()(const Voxel &voxel) const {
-        const uint32_t *vec = reinterpret_cast<const uint32_t *>(voxel.data());
-        return ((1 << 20) - 1) & (vec[0] * 73856093 ^ vec[1] * 19349669 ^ vec[2] * 83492791);
-    }
-};
+using Voxel = kiss_icp::VoxelHashMap::Voxel;
+using VoxelHash = kiss_icp::VoxelHashMap::VoxelHash;
 
 Voxel PointToVoxel(const Eigen::Vector3d &point, double voxel_size) {
     return Voxel(static_cast<int>(std::floor(point.x() / voxel_size)),

From 8875319e0f5fe3bf45f7da81ff3653f3cbad1be8 Mon Sep 17 00:00:00 2001
From: tizianoGuadagnino <frevo93@gmail.com>
Date: Tue, 9 Jul 2024 18:36:57 +0200
Subject: [PATCH 02/12] This looks like an even better unification

---
 cpp/kiss_icp/core/Preprocessing.cpp | 28 +++-------------------------
 1 file changed, 3 insertions(+), 25 deletions(-)

diff --git a/cpp/kiss_icp/core/Preprocessing.cpp b/cpp/kiss_icp/core/Preprocessing.cpp
index 56e4d12d..ae452cf9 100644
--- a/cpp/kiss_icp/core/Preprocessing.cpp
+++ b/cpp/kiss_icp/core/Preprocessing.cpp
@@ -32,34 +32,12 @@
 #include <sophus/se3.hpp>
 #include <vector>
 
-namespace {
-using Voxel = kiss_icp::VoxelHashMap::Voxel;
-using VoxelHash = kiss_icp::VoxelHashMap::VoxelHash;
-
-Voxel PointToVoxel(const Eigen::Vector3d &point, double voxel_size) {
-    return Voxel(static_cast<int>(std::floor(point.x() / voxel_size)),
-                 static_cast<int>(std::floor(point.y() / voxel_size)),
-                 static_cast<int>(std::floor(point.z() / voxel_size)));
-}
-}  // namespace
-
 namespace kiss_icp {
 std::vector<Eigen::Vector3d> VoxelDownsample(const std::vector<Eigen::Vector3d> &frame,
                                              double voxel_size) {
-    tsl::robin_map<Voxel, Eigen::Vector3d, VoxelHash> grid;
-    grid.reserve(frame.size());
-    for (const auto &point : frame) {
-        const auto voxel = PointToVoxel(point, voxel_size);
-        if (grid.contains(voxel)) continue;
-        grid.insert({voxel, point});
-    }
-    std::vector<Eigen::Vector3d> frame_dowsampled;
-    frame_dowsampled.reserve(grid.size());
-    for (const auto &[voxel, point] : grid) {
-        (void)voxel;
-        frame_dowsampled.emplace_back(point);
-    }
-    return frame_dowsampled;
+    VoxelHashMap grid(voxel_size, 1.0, 1);
+    grid.AddPoints(frame);
+    return grid.Pointcloud();
 }
 
 std::vector<Eigen::Vector3d> Preprocess(const std::vector<Eigen::Vector3d> &frame,

From 409e2e851bcdf70b00cbc9dd68ffa8ff9a29d155 Mon Sep 17 00:00:00 2001
From: tizianoGuadagnino <frevo93@gmail.com>
Date: Wed, 10 Jul 2024 11:28:40 +0200
Subject: [PATCH 03/12] Modernize VoxelDownsample

---
 cpp/kiss_icp/core/Preprocessing.cpp | 24 +++++++++++++++++++++---
 1 file changed, 21 insertions(+), 3 deletions(-)

diff --git a/cpp/kiss_icp/core/Preprocessing.cpp b/cpp/kiss_icp/core/Preprocessing.cpp
index ae452cf9..ff741392 100644
--- a/cpp/kiss_icp/core/Preprocessing.cpp
+++ b/cpp/kiss_icp/core/Preprocessing.cpp
@@ -32,12 +32,30 @@
 #include <sophus/se3.hpp>
 #include <vector>
 
+namespace {
+using Voxel = kiss_icp::VoxelHashMap::Voxel;
+using VoxelHash = kiss_icp::VoxelHashMap::VoxelHash;
+
+Voxel PointToVoxel(const Eigen::Vector3d &point, double voxel_size) {
+    return Voxel(static_cast<int>(std::floor(point.x() / voxel_size)),
+                 static_cast<int>(std::floor(point.y() / voxel_size)),
+                 static_cast<int>(std::floor(point.z() / voxel_size)));
+}
+}  // namespace
+
 namespace kiss_icp {
 std::vector<Eigen::Vector3d> VoxelDownsample(const std::vector<Eigen::Vector3d> &frame,
                                              double voxel_size) {
-    VoxelHashMap grid(voxel_size, 1.0, 1);
-    grid.AddPoints(frame);
-    return grid.Pointcloud();
+    tsl::robin_map<Voxel, Eigen::Vector3d, VoxelHash> grid;
+    grid.reserve(frame.size());
+    std::for_each(frame.cbegin(), frame.cend(), [&](const auto &point) {
+        const auto voxel = PointToVoxel(point, voxel_size);
+        if (!grid.contains(voxel)) grid.insert({voxel, point});
+    });
+    std::vector<Eigen::Vector3d> frame_dowsampled(grid.size());
+    std::transform(grid.begin(), grid.end(), frame_dowsampled.begin(),
+                   [](const auto &voxel_and_point) { return voxel_and_point.value(); });
+    return frame_dowsampled;
 }
 
 std::vector<Eigen::Vector3d> Preprocess(const std::vector<Eigen::Vector3d> &frame,

From fa5c9b8fe1901c7b789fc81008a8f2caaa8650d1 Mon Sep 17 00:00:00 2001
From: tizianoGuadagnino <frevo93@gmail.com>
Date: Wed, 10 Jul 2024 11:40:08 +0200
Subject: [PATCH 04/12] VoxelUtils is born, now everthing is liteally in one
 place, and we have some side effect code reduction

---
 cpp/kiss_icp/core/Preprocessing.cpp | 17 ++---------
 cpp/kiss_icp/core/Preprocessing.hpp |  6 ++--
 cpp/kiss_icp/core/Registration.cpp  |  4 +--
 cpp/kiss_icp/core/VoxelHashMap.cpp  |  2 +-
 cpp/kiss_icp/core/VoxelHashMap.hpp  | 16 ++--------
 cpp/kiss_icp/core/VoxelUtils.hpp    | 45 +++++++++++++++++++++++++++++
 6 files changed, 56 insertions(+), 34 deletions(-)
 create mode 100644 cpp/kiss_icp/core/VoxelUtils.hpp

diff --git a/cpp/kiss_icp/core/Preprocessing.cpp b/cpp/kiss_icp/core/Preprocessing.cpp
index ff741392..13203af2 100644
--- a/cpp/kiss_icp/core/Preprocessing.cpp
+++ b/cpp/kiss_icp/core/Preprocessing.cpp
@@ -28,25 +28,14 @@
 #include <Eigen/Core>
 #include <algorithm>
 #include <cmath>
-#include <kiss_icp/core/VoxelHashMap.hpp>
+#include <kiss_icp/core/VoxelUtils.hpp>
 #include <sophus/se3.hpp>
 #include <vector>
 
-namespace {
-using Voxel = kiss_icp::VoxelHashMap::Voxel;
-using VoxelHash = kiss_icp::VoxelHashMap::VoxelHash;
-
-Voxel PointToVoxel(const Eigen::Vector3d &point, double voxel_size) {
-    return Voxel(static_cast<int>(std::floor(point.x() / voxel_size)),
-                 static_cast<int>(std::floor(point.y() / voxel_size)),
-                 static_cast<int>(std::floor(point.z() / voxel_size)));
-}
-}  // namespace
-
 namespace kiss_icp {
 std::vector<Eigen::Vector3d> VoxelDownsample(const std::vector<Eigen::Vector3d> &frame,
                                              double voxel_size) {
-    tsl::robin_map<Voxel, Eigen::Vector3d, VoxelHash> grid;
+    tsl::robin_map<Voxel, Eigen::Vector3d> grid;
     grid.reserve(frame.size());
     std::for_each(frame.cbegin(), frame.cend(), [&](const auto &point) {
         const auto voxel = PointToVoxel(point, voxel_size);
@@ -54,7 +43,7 @@ std::vector<Eigen::Vector3d> VoxelDownsample(const std::vector<Eigen::Vector3d>
     });
     std::vector<Eigen::Vector3d> frame_dowsampled(grid.size());
     std::transform(grid.begin(), grid.end(), frame_dowsampled.begin(),
-                   [](const auto &voxel_and_point) { return voxel_and_point.value(); });
+                   [](const auto &voxel_and_point) { return voxel_and_point.second; });
     return frame_dowsampled;
 }
 
diff --git a/cpp/kiss_icp/core/Preprocessing.hpp b/cpp/kiss_icp/core/Preprocessing.hpp
index e919a422..fd752fe7 100644
--- a/cpp/kiss_icp/core/Preprocessing.hpp
+++ b/cpp/kiss_icp/core/Preprocessing.hpp
@@ -28,6 +28,9 @@
 #include <vector>
 
 namespace kiss_icp {
+/// Voxelize point cloud keeping the original coordinates
+std::vector<Eigen::Vector3d> VoxelDownsample(const std::vector<Eigen::Vector3d> &frame,
+                                             double voxel_size);
 
 /// Crop the frame with max/min ranges
 std::vector<Eigen::Vector3d> Preprocess(const std::vector<Eigen::Vector3d> &frame,
@@ -39,7 +42,4 @@ std::vector<Eigen::Vector3d> Preprocess(const std::vector<Eigen::Vector3d> &fram
 /// Originally introduced the calibration factor)
 std::vector<Eigen::Vector3d> CorrectKITTIScan(const std::vector<Eigen::Vector3d> &frame);
 
-/// Voxelize point cloud keeping the original coordinates
-std::vector<Eigen::Vector3d> VoxelDownsample(const std::vector<Eigen::Vector3d> &frame,
-                                             double voxel_size);
 }  // namespace kiss_icp
diff --git a/cpp/kiss_icp/core/Registration.cpp b/cpp/kiss_icp/core/Registration.cpp
index d7adb4df..f03eaf2e 100644
--- a/cpp/kiss_icp/core/Registration.cpp
+++ b/cpp/kiss_icp/core/Registration.cpp
@@ -53,7 +53,7 @@ void TransformPoints(const Sophus::SE3d &T, std::vector<Eigen::Vector3d> &points
                    [&](const auto &point) { return T * point; });
 }
 
-using Voxel = kiss_icp::VoxelHashMap::Voxel;
+using Voxel = kiss_icp::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) {
@@ -69,7 +69,7 @@ std::vector<Voxel> GetAdjacentVoxels(const Voxel &voxel, int adjacent_voxels = 1
 std::tuple<Eigen::Vector3d, double> GetClosestNeighbor(const Eigen::Vector3d &point,
                                                        const kiss_icp::VoxelHashMap &voxel_map) {
     // Convert the point to voxel coordinates
-    const auto &voxel = voxel_map.PointToVoxel(point);
+    const auto &voxel = kiss_icp::PointToVoxel(point, voxel_map.voxel_size_);
     // Get nearby voxels on the map
     const auto &query_voxels = GetAdjacentVoxels(voxel);
     // Extract the points contained within the neighborhood voxels
diff --git a/cpp/kiss_icp/core/VoxelHashMap.cpp b/cpp/kiss_icp/core/VoxelHashMap.cpp
index 65790fba..8701d110 100644
--- a/cpp/kiss_icp/core/VoxelHashMap.cpp
+++ b/cpp/kiss_icp/core/VoxelHashMap.cpp
@@ -75,7 +75,7 @@ void VoxelHashMap::Update(const std::vector<Eigen::Vector3d> &points, const Soph
 
 void VoxelHashMap::AddPoints(const std::vector<Eigen::Vector3d> &points) {
     std::for_each(points.cbegin(), points.cend(), [&](const auto &point) {
-        auto voxel = PointToVoxel(point);
+        auto voxel = PointToVoxel(point, voxel_size_);
         auto search = map_.find(voxel);
         if (search != map_.end()) {
             auto &voxel_block = search.value();
diff --git a/cpp/kiss_icp/core/VoxelHashMap.hpp b/cpp/kiss_icp/core/VoxelHashMap.hpp
index 8ffe9ed9..071f22a4 100644
--- a/cpp/kiss_icp/core/VoxelHashMap.hpp
+++ b/cpp/kiss_icp/core/VoxelHashMap.hpp
@@ -29,12 +29,12 @@
 #include <tsl/robin_map.h>
 
 #include <Eigen/Core>
+#include <kiss_icp/core/VoxelUtils.hpp>
 #include <sophus/se3.hpp>
 #include <vector>
 
 namespace kiss_icp {
 struct VoxelHashMap {
-    using Voxel = Eigen::Vector3i;
     struct VoxelBlock {
         // buffer of points with a max limit of n_points
         std::vector<Eigen::Vector3d> points;
@@ -43,13 +43,6 @@ struct VoxelHashMap {
             if (points.size() < static_cast<size_t>(num_points_)) points.push_back(point);
         }
     };
-    struct VoxelHash {
-        size_t operator()(const Voxel &voxel) const {
-            const uint32_t *vec = reinterpret_cast<const uint32_t *>(voxel.data());
-            return (vec[0] * 73856093 ^ vec[1] * 19349669 ^ vec[2] * 83492791);
-        }
-    };
-
     explicit VoxelHashMap(double voxel_size, double max_distance, int max_points_per_voxel)
         : voxel_size_(voxel_size),
           max_distance_(max_distance),
@@ -57,11 +50,6 @@ 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>(std::floor(point.x() / voxel_size_)),
-                     static_cast<int>(std::floor(point.y() / voxel_size_)),
-                     static_cast<int>(std::floor(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);
@@ -72,6 +60,6 @@ struct VoxelHashMap {
     double voxel_size_;
     double max_distance_;
     int max_points_per_voxel_;
-    tsl::robin_map<Voxel, VoxelBlock, VoxelHash> map_;
+    tsl::robin_map<Voxel, VoxelBlock> map_;
 };
 }  // namespace kiss_icp
diff --git a/cpp/kiss_icp/core/VoxelUtils.hpp b/cpp/kiss_icp/core/VoxelUtils.hpp
new file mode 100644
index 00000000..8778e789
--- /dev/null
+++ b/cpp/kiss_icp/core/VoxelUtils.hpp
@@ -0,0 +1,45 @@
+// MIT License
+//
+// Copyright (c) 2024 Ignacio Vizzo, Tiziano Guadagnino, Benedikt Mersch, 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 <bits/functional_hash.h>
+
+#include <Eigen/Core>
+
+namespace kiss_icp {
+using Voxel = Eigen::Vector3i;
+inline Voxel PointToVoxel(const Eigen::Vector3d &point, const double voxel_size) {
+    return Voxel(static_cast<int>(std::floor(point.x() / voxel_size)),
+                 static_cast<int>(std::floor(point.y() / voxel_size)),
+                 static_cast<int>(std::floor(point.z() / voxel_size)));
+}
+}  // namespace kiss_icp
+
+template <>
+struct std::hash<kiss_icp::Voxel> {
+    std::size_t operator()(const kiss_icp::Voxel &voxel) const noexcept {
+        const uint32_t *vec = reinterpret_cast<const uint32_t *>(voxel.data());
+        return (vec[0] * 73856093 ^ vec[1] * 19349669 ^ vec[2] * 83492791);
+    }
+};

From 0cbc1d9bf1f2cca9942882f2bc2005012dadd3f0 Mon Sep 17 00:00:00 2001
From: tizianoGuadagnino <frevo93@gmail.com>
Date: Wed, 10 Jul 2024 11:43:22 +0200
Subject: [PATCH 05/12] Macos does not like my include

---
 cpp/kiss_icp/core/VoxelUtils.hpp | 3 +--
 1 file changed, 1 insertion(+), 2 deletions(-)

diff --git a/cpp/kiss_icp/core/VoxelUtils.hpp b/cpp/kiss_icp/core/VoxelUtils.hpp
index 8778e789..7d5888fb 100644
--- a/cpp/kiss_icp/core/VoxelUtils.hpp
+++ b/cpp/kiss_icp/core/VoxelUtils.hpp
@@ -23,9 +23,8 @@
 //
 #pragma once
 
-#include <bits/functional_hash.h>
-
 #include <Eigen/Core>
+#include <memory>
 
 namespace kiss_icp {
 using Voxel = Eigen::Vector3i;

From 3e7abb6f69070b9a378bda1e5f21cf7daccad2cb Mon Sep 17 00:00:00 2001
From: tizianoGuadagnino <frevo93@gmail.com>
Date: Thu, 11 Jul 2024 09:01:56 +0200
Subject: [PATCH 06/12] Local style include

---
 cpp/kiss_icp/core/Preprocessing.cpp | 3 ++-
 cpp/kiss_icp/core/VoxelHashMap.hpp  | 3 ++-
 cpp/kiss_icp/core/VoxelUtils.hpp    | 1 +
 3 files changed, 5 insertions(+), 2 deletions(-)

diff --git a/cpp/kiss_icp/core/Preprocessing.cpp b/cpp/kiss_icp/core/Preprocessing.cpp
index 13203af2..60c7ecc1 100644
--- a/cpp/kiss_icp/core/Preprocessing.cpp
+++ b/cpp/kiss_icp/core/Preprocessing.cpp
@@ -28,10 +28,11 @@
 #include <Eigen/Core>
 #include <algorithm>
 #include <cmath>
-#include <kiss_icp/core/VoxelUtils.hpp>
 #include <sophus/se3.hpp>
 #include <vector>
 
+#include "VoxelUtils.hpp"
+
 namespace kiss_icp {
 std::vector<Eigen::Vector3d> VoxelDownsample(const std::vector<Eigen::Vector3d> &frame,
                                              double voxel_size) {
diff --git a/cpp/kiss_icp/core/VoxelHashMap.hpp b/cpp/kiss_icp/core/VoxelHashMap.hpp
index 071f22a4..3c9c4c36 100644
--- a/cpp/kiss_icp/core/VoxelHashMap.hpp
+++ b/cpp/kiss_icp/core/VoxelHashMap.hpp
@@ -29,10 +29,11 @@
 #include <tsl/robin_map.h>
 
 #include <Eigen/Core>
-#include <kiss_icp/core/VoxelUtils.hpp>
 #include <sophus/se3.hpp>
 #include <vector>
 
+#include "VoxelUtils.hpp"
+
 namespace kiss_icp {
 struct VoxelHashMap {
     struct VoxelBlock {
diff --git a/cpp/kiss_icp/core/VoxelUtils.hpp b/cpp/kiss_icp/core/VoxelUtils.hpp
index 7d5888fb..710ade77 100644
--- a/cpp/kiss_icp/core/VoxelUtils.hpp
+++ b/cpp/kiss_icp/core/VoxelUtils.hpp
@@ -24,6 +24,7 @@
 #pragma once
 
 #include <Eigen/Core>
+#include <cmath>
 #include <memory>
 
 namespace kiss_icp {

From 07f97b19f1b2b5c0e548b38349a9a7aa16f73bde Mon Sep 17 00:00:00 2001
From: tizianoGuadagnino <frevo93@gmail.com>
Date: Thu, 11 Jul 2024 09:30:11 +0200
Subject: [PATCH 07/12] With move

---
 cpp/kiss_icp/core/Preprocessing.cpp | 7 +++++--
 1 file changed, 5 insertions(+), 2 deletions(-)

diff --git a/cpp/kiss_icp/core/Preprocessing.cpp b/cpp/kiss_icp/core/Preprocessing.cpp
index 60c7ecc1..89c70f2d 100644
--- a/cpp/kiss_icp/core/Preprocessing.cpp
+++ b/cpp/kiss_icp/core/Preprocessing.cpp
@@ -28,6 +28,7 @@
 #include <Eigen/Core>
 #include <algorithm>
 #include <cmath>
+#include <iterator>
 #include <sophus/se3.hpp>
 #include <vector>
 
@@ -43,8 +44,10 @@ std::vector<Eigen::Vector3d> VoxelDownsample(const std::vector<Eigen::Vector3d>
         if (!grid.contains(voxel)) grid.insert({voxel, point});
     });
     std::vector<Eigen::Vector3d> frame_dowsampled(grid.size());
-    std::transform(grid.begin(), grid.end(), frame_dowsampled.begin(),
-                   [](const auto &voxel_and_point) { return voxel_and_point.second; });
+    std::transform(std::make_move_iterator(grid.begin()),  //
+                   std::make_move_iterator(grid.end()),    //
+                   frame_dowsampled.begin(),
+                   [](auto &&voxel_and_point) { return voxel_and_point.second; });
     return frame_dowsampled;
 }
 

From 9f87b9b66ebb444b298c3795abed74b4de3870bc Mon Sep 17 00:00:00 2001
From: tizianoGuadagnino <frevo93@gmail.com>
Date: Thu, 11 Jul 2024 10:46:14 +0200
Subject: [PATCH 08/12] As of benchmark

---
 cpp/kiss_icp/core/Preprocessing.cpp | 6 ++----
 1 file changed, 2 insertions(+), 4 deletions(-)

diff --git a/cpp/kiss_icp/core/Preprocessing.cpp b/cpp/kiss_icp/core/Preprocessing.cpp
index 89c70f2d..fd1d9481 100644
--- a/cpp/kiss_icp/core/Preprocessing.cpp
+++ b/cpp/kiss_icp/core/Preprocessing.cpp
@@ -44,10 +44,8 @@ std::vector<Eigen::Vector3d> VoxelDownsample(const std::vector<Eigen::Vector3d>
         if (!grid.contains(voxel)) grid.insert({voxel, point});
     });
     std::vector<Eigen::Vector3d> frame_dowsampled(grid.size());
-    std::transform(std::make_move_iterator(grid.begin()),  //
-                   std::make_move_iterator(grid.end()),    //
-                   frame_dowsampled.begin(),
-                   [](auto &&voxel_and_point) { return voxel_and_point.second; });
+    std::transform(grid.cbegin(), grid.cend(), frame_dowsampled.begin(),
+                   [](const auto &voxel_and_point) { return voxel_and_point.second; });
     return frame_dowsampled;
 }
 

From 2fc877f466f351cdffee4f5fefdd2eaadab94917 Mon Sep 17 00:00:00 2001
From: tizianoGuadagnino <frevo93@gmail.com>
Date: Thu, 11 Jul 2024 10:52:05 +0200
Subject: [PATCH 09/12] For each for the win

---
 cpp/kiss_icp/core/Preprocessing.cpp | 8 +++++---
 1 file changed, 5 insertions(+), 3 deletions(-)

diff --git a/cpp/kiss_icp/core/Preprocessing.cpp b/cpp/kiss_icp/core/Preprocessing.cpp
index fd1d9481..c5fe4549 100644
--- a/cpp/kiss_icp/core/Preprocessing.cpp
+++ b/cpp/kiss_icp/core/Preprocessing.cpp
@@ -43,9 +43,11 @@ std::vector<Eigen::Vector3d> VoxelDownsample(const std::vector<Eigen::Vector3d>
         const auto voxel = PointToVoxel(point, voxel_size);
         if (!grid.contains(voxel)) grid.insert({voxel, point});
     });
-    std::vector<Eigen::Vector3d> frame_dowsampled(grid.size());
-    std::transform(grid.cbegin(), grid.cend(), frame_dowsampled.begin(),
-                   [](const auto &voxel_and_point) { return voxel_and_point.second; });
+    std::vector<Eigen::Vector3d> frame_dowsampled;
+    frame_dowsampled.reserve(grid.size());
+    std::for_each(grid.cbegin(), grid.cend(), [&](const auto &voxel_and_point) {
+        frame_dowsampled.emplace_back(voxel_and_point.second);
+    });
     return frame_dowsampled;
 }
 

From aeb7b2167da42fc6e77b78054130ef54da1a2f3a Mon Sep 17 00:00:00 2001
From: tizianoGuadagnino <frevo93@gmail.com>
Date: Thu, 11 Jul 2024 10:58:35 +0200
Subject: [PATCH 10/12] Remove unnecessary include

---
 cpp/kiss_icp/core/Preprocessing.cpp | 1 -
 1 file changed, 1 deletion(-)

diff --git a/cpp/kiss_icp/core/Preprocessing.cpp b/cpp/kiss_icp/core/Preprocessing.cpp
index c5fe4549..5486de2d 100644
--- a/cpp/kiss_icp/core/Preprocessing.cpp
+++ b/cpp/kiss_icp/core/Preprocessing.cpp
@@ -28,7 +28,6 @@
 #include <Eigen/Core>
 #include <algorithm>
 #include <cmath>
-#include <iterator>
 #include <sophus/se3.hpp>
 #include <vector>
 

From 37d520c22a31be4fb0b146f77f6577d296026a5f Mon Sep 17 00:00:00 2001
From: Luca Lobefaro <luca.lobefaro95@gmail.com>
Date: Thu, 11 Jul 2024 11:20:14 +0200
Subject: [PATCH 11/12] Const correcteness

---
 cpp/kiss_icp/core/Preprocessing.cpp | 2 +-
 cpp/kiss_icp/core/Preprocessing.hpp | 2 +-
 2 files changed, 2 insertions(+), 2 deletions(-)

diff --git a/cpp/kiss_icp/core/Preprocessing.cpp b/cpp/kiss_icp/core/Preprocessing.cpp
index 5486de2d..732a8720 100644
--- a/cpp/kiss_icp/core/Preprocessing.cpp
+++ b/cpp/kiss_icp/core/Preprocessing.cpp
@@ -35,7 +35,7 @@
 
 namespace kiss_icp {
 std::vector<Eigen::Vector3d> VoxelDownsample(const std::vector<Eigen::Vector3d> &frame,
-                                             double voxel_size) {
+                                             const double voxel_size) {
     tsl::robin_map<Voxel, Eigen::Vector3d> grid;
     grid.reserve(frame.size());
     std::for_each(frame.cbegin(), frame.cend(), [&](const auto &point) {
diff --git a/cpp/kiss_icp/core/Preprocessing.hpp b/cpp/kiss_icp/core/Preprocessing.hpp
index fd752fe7..0042837a 100644
--- a/cpp/kiss_icp/core/Preprocessing.hpp
+++ b/cpp/kiss_icp/core/Preprocessing.hpp
@@ -30,7 +30,7 @@
 namespace kiss_icp {
 /// Voxelize point cloud keeping the original coordinates
 std::vector<Eigen::Vector3d> VoxelDownsample(const std::vector<Eigen::Vector3d> &frame,
-                                             double voxel_size);
+                                             const double voxel_size);
 
 /// Crop the frame with max/min ranges
 std::vector<Eigen::Vector3d> Preprocess(const std::vector<Eigen::Vector3d> &frame,

From 9b2b2cd216b397fc511e81305b0e1916f7a8a9c1 Mon Sep 17 00:00:00 2001
From: Ignacio Vizzo <ignacio@dexory.com>
Date: Thu, 11 Jul 2024 13:13:21 +0200
Subject: [PATCH 12/12] Refresh changes

---
 cpp/kiss_icp/core/Preprocessing.cpp | 18 ++++++++++--------
 cpp/kiss_icp/core/Preprocessing.hpp |  6 +++---
 cpp/kiss_icp/core/Registration.cpp  |  1 +
 cpp/kiss_icp/core/VoxelHashMap.cpp  |  2 ++
 cpp/kiss_icp/core/VoxelHashMap.hpp  |  3 ++-
 cpp/kiss_icp/core/VoxelUtils.hpp    | 10 +++++-----
 6 files changed, 23 insertions(+), 17 deletions(-)

diff --git a/cpp/kiss_icp/core/Preprocessing.cpp b/cpp/kiss_icp/core/Preprocessing.cpp
index 732a8720..95b82643 100644
--- a/cpp/kiss_icp/core/Preprocessing.cpp
+++ b/cpp/kiss_icp/core/Preprocessing.cpp
@@ -35,18 +35,20 @@
 
 namespace kiss_icp {
 std::vector<Eigen::Vector3d> VoxelDownsample(const std::vector<Eigen::Vector3d> &frame,
-                                             const double voxel_size) {
-    tsl::robin_map<Voxel, Eigen::Vector3d> grid;
+                                             double voxel_size) {
+    tsl::robin_map<Voxel, Eigen::Vector3d, VoxelHash> grid;
     grid.reserve(frame.size());
-    std::for_each(frame.cbegin(), frame.cend(), [&](const auto &point) {
+    for (const auto &point : frame) {
         const auto voxel = PointToVoxel(point, voxel_size);
-        if (!grid.contains(voxel)) grid.insert({voxel, point});
-    });
+        if (grid.contains(voxel)) continue;
+        grid.insert({voxel, point});
+    }
     std::vector<Eigen::Vector3d> frame_dowsampled;
     frame_dowsampled.reserve(grid.size());
-    std::for_each(grid.cbegin(), grid.cend(), [&](const auto &voxel_and_point) {
-        frame_dowsampled.emplace_back(voxel_and_point.second);
-    });
+    for (const auto &[voxel, point] : grid) {
+        (void)voxel;
+        frame_dowsampled.emplace_back(point);
+    }
     return frame_dowsampled;
 }
 
diff --git a/cpp/kiss_icp/core/Preprocessing.hpp b/cpp/kiss_icp/core/Preprocessing.hpp
index 0042837a..e919a422 100644
--- a/cpp/kiss_icp/core/Preprocessing.hpp
+++ b/cpp/kiss_icp/core/Preprocessing.hpp
@@ -28,9 +28,6 @@
 #include <vector>
 
 namespace kiss_icp {
-/// Voxelize point cloud keeping the original coordinates
-std::vector<Eigen::Vector3d> VoxelDownsample(const std::vector<Eigen::Vector3d> &frame,
-                                             const double voxel_size);
 
 /// Crop the frame with max/min ranges
 std::vector<Eigen::Vector3d> Preprocess(const std::vector<Eigen::Vector3d> &frame,
@@ -42,4 +39,7 @@ std::vector<Eigen::Vector3d> Preprocess(const std::vector<Eigen::Vector3d> &fram
 /// Originally introduced the calibration factor)
 std::vector<Eigen::Vector3d> CorrectKITTIScan(const std::vector<Eigen::Vector3d> &frame);
 
+/// Voxelize point cloud keeping the original coordinates
+std::vector<Eigen::Vector3d> VoxelDownsample(const std::vector<Eigen::Vector3d> &frame,
+                                             double voxel_size);
 }  // namespace kiss_icp
diff --git a/cpp/kiss_icp/core/Registration.cpp b/cpp/kiss_icp/core/Registration.cpp
index 78168197..b8acea10 100644
--- a/cpp/kiss_icp/core/Registration.cpp
+++ b/cpp/kiss_icp/core/Registration.cpp
@@ -36,6 +36,7 @@
 #include <tuple>
 
 #include "VoxelHashMap.hpp"
+#include "VoxelUtils.hpp"
 
 namespace Eigen {
 using Matrix6d = Eigen::Matrix<double, 6, 6>;
diff --git a/cpp/kiss_icp/core/VoxelHashMap.cpp b/cpp/kiss_icp/core/VoxelHashMap.cpp
index 8701d110..b58457d2 100644
--- a/cpp/kiss_icp/core/VoxelHashMap.cpp
+++ b/cpp/kiss_icp/core/VoxelHashMap.cpp
@@ -28,6 +28,8 @@
 #include <utility>
 #include <vector>
 
+#include "VoxelUtils.hpp"
+
 namespace kiss_icp {
 
 std::vector<Eigen::Vector3d> VoxelHashMap::GetPoints(const std::vector<Voxel> &query_voxels) const {
diff --git a/cpp/kiss_icp/core/VoxelHashMap.hpp b/cpp/kiss_icp/core/VoxelHashMap.hpp
index 3c9c4c36..a49b7096 100644
--- a/cpp/kiss_icp/core/VoxelHashMap.hpp
+++ b/cpp/kiss_icp/core/VoxelHashMap.hpp
@@ -44,6 +44,7 @@ struct VoxelHashMap {
             if (points.size() < static_cast<size_t>(num_points_)) points.push_back(point);
         }
     };
+
     explicit VoxelHashMap(double voxel_size, double max_distance, int max_points_per_voxel)
         : voxel_size_(voxel_size),
           max_distance_(max_distance),
@@ -61,6 +62,6 @@ struct VoxelHashMap {
     double voxel_size_;
     double max_distance_;
     int max_points_per_voxel_;
-    tsl::robin_map<Voxel, VoxelBlock> map_;
+    tsl::robin_map<Voxel, VoxelBlock, VoxelHash> map_;
 };
 }  // namespace kiss_icp
diff --git a/cpp/kiss_icp/core/VoxelUtils.hpp b/cpp/kiss_icp/core/VoxelUtils.hpp
index 710ade77..331a2ce1 100644
--- a/cpp/kiss_icp/core/VoxelUtils.hpp
+++ b/cpp/kiss_icp/core/VoxelUtils.hpp
@@ -25,21 +25,21 @@
 
 #include <Eigen/Core>
 #include <cmath>
-#include <memory>
 
 namespace kiss_icp {
+
 using Voxel = Eigen::Vector3i;
+
 inline Voxel PointToVoxel(const Eigen::Vector3d &point, const double voxel_size) {
     return Voxel(static_cast<int>(std::floor(point.x() / voxel_size)),
                  static_cast<int>(std::floor(point.y() / voxel_size)),
                  static_cast<int>(std::floor(point.z() / voxel_size)));
 }
-}  // namespace kiss_icp
 
-template <>
-struct std::hash<kiss_icp::Voxel> {
-    std::size_t operator()(const kiss_icp::Voxel &voxel) const noexcept {
+struct VoxelHash {
+    size_t operator()(const Voxel &voxel) const {
         const uint32_t *vec = reinterpret_cast<const uint32_t *>(voxel.data());
         return (vec[0] * 73856093 ^ vec[1] * 19349669 ^ vec[2] * 83492791);
     }
 };
+}  // namespace kiss_icp