Revise Voxel functionalities in VoxelHashMap and Preprocessing

cpp/kiss_icp/core/Preprocessing.cpp

@@ -28,42 +28,22 @@
 #include <Eigen/Core>
 #include <algorithm>
 #include <cmath>
+#include <kiss_icp/core/VoxelUtils.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);
-    }
-};
-
-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());
-    for (const auto &point : frame) {
+    std::for_each(frame.cbegin(), frame.cend(), [&](const auto &point) {
         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);
-    }
+        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; });
     return frame_dowsampled;
 }
 

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

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

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();

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,25 +43,13 @@ 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),
           max_points_per_voxel_(max_points_per_voxel) {}
 
     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

cpp/kiss_icp/core/VoxelUtils.hpp

@@ -0,0 +1,44 @@
+// 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 <Eigen/Core>
+#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 {
+        const uint32_t *vec = reinterpret_cast<const uint32_t *>(voxel.data());
+        return (vec[0] * 73856093 ^ vec[1] * 19349669 ^ vec[2] * 83492791);
+    }
+};