diff --git a/cpp/kiss_icp/core/Preprocessing.cpp b/cpp/kiss_icp/core/Preprocessing.cpp
index 5256275f..8dbd67b3 100644
--- a/cpp/kiss_icp/core/Preprocessing.cpp
+++ b/cpp/kiss_icp/core/Preprocessing.cpp
@@ -23,11 +23,10 @@
 #include "Preprocessing.hpp"
 
 #include <tbb/parallel_for.h>
-#include <tsl/robin_map.h>
+#include <tsl/robin_set.h>
 
 #include <Eigen/Core>
 #include <algorithm>
-#include <cmath>
 #include <sophus/se3.hpp>
 #include <vector>
 
@@ -43,20 +42,23 @@ struct VoxelHash {
 
 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 = Voxel((point / voxel_size).cast<int>());
-        if (grid.contains(voxel)) continue;
-        grid.insert({voxel, point});
-    }
+                                             const double voxel_size) {
+    tsl::robin_set<Voxel, VoxelHash> voxels;
     std::vector<Eigen::Vector3d> frame_dowsampled;
-    frame_dowsampled.reserve(grid.size());
-    for (const auto &[voxel, point] : grid) {
-        (void)voxel;
-        frame_dowsampled.emplace_back(point);
-    }
+
+    voxels.reserve(frame.size());
+    frame_dowsampled.reserve(frame.size());
+
+    std::for_each(frame.cbegin(), frame.cend(), [&](const Eigen::Vector3d &point) {
+        const auto voxel = Voxel((point / voxel_size).cast<int>());
+        if (!voxels.contains(voxel)) {
+            voxels.insert(voxel);
+            frame_dowsampled.emplace_back(point);
+        }
+    });
+
+    frame_dowsampled.shrink_to_fit();
+
     return frame_dowsampled;
 }
 
diff --git a/cpp/kiss_icp/core/Preprocessing.hpp b/cpp/kiss_icp/core/Preprocessing.hpp
index e919a422..69cc9935 100644
--- a/cpp/kiss_icp/core/Preprocessing.hpp
+++ b/cpp/kiss_icp/core/Preprocessing.hpp
@@ -41,5 +41,5 @@ std::vector<Eigen::Vector3d> CorrectKITTIScan(const std::vector<Eigen::Vector3d>
 
 /// 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);
 }  // namespace kiss_icp
diff --git a/python/kiss_icp/kiss_icp.py b/python/kiss_icp/kiss_icp.py
index e7087dc8..8cff5848 100644
--- a/python/kiss_icp/kiss_icp.py
+++ b/python/kiss_icp/kiss_icp.py
@@ -50,7 +50,7 @@ def register_frame(self, frame, timestamps):
         frame = self.preprocess(frame)
 
         # Voxelize
-        source, frame_downsample = self.voxelize(frame)
+        source = voxel_down_sample(frame, self.config.mapping.voxel_size * 1.5)
 
         # Get adaptive_threshold
         sigma = self.adaptive_threshold.get_threshold()
@@ -72,14 +72,9 @@ def register_frame(self, frame, timestamps):
 
         # Update step: threshold, local map, delta, and the last pose
         self.adaptive_threshold.update_model_deviation(model_deviation)
-        self.local_map.update(frame_downsample, new_pose)
+        self.local_map.update(source, new_pose)
         self.last_delta = np.linalg.inv(self.last_pose) @ new_pose
         self.last_pose = new_pose
 
         # Return the (deskew) input raw scan (frame) and the points used for registration (source)
         return frame, source
-
-    def voxelize(self, iframe):
-        frame_downsample = voxel_down_sample(iframe, self.config.mapping.voxel_size * 0.5)
-        source = voxel_down_sample(frame_downsample, self.config.mapping.voxel_size * 1.5)
-        return source, frame_downsample