Nacho/voxel utils style change

.github/workflows/cpp.yml

@@ -25,3 +25,32 @@ jobs:
         run: cmake -B ${{github.workspace}}/build -DCMAKE_BUILD_TYPE=${{env.BUILD_TYPE}} ${{github.workspace}}/cpp/kiss_icp
       - name: Build
         run: cmake --build ${{github.workspace}}/build --config ${{env.BUILD_TYPE}}
+
+  # As the previous job will always install the dependencies from cmake, and this is guaranteed to
+  # work, we also want to support dev sandboxes where the main dependencies are already
+  # pre-installed in the system. For now, we only support dev machines under a GNU/Linux
+  # environmnets. If you are reading this and need the same functionallity in Windows/macOS please
+  # open a ticket.
+  cpp_api_dev:
+    runs-on: ${{ matrix.os }}
+    strategy:
+      matrix:
+        os: [ubuntu-22.04, ubuntu-20.04]
+
+    steps:
+      - uses: actions/checkout@v3
+      - name: Cache dependencies
+        uses: actions/cache@v2
+        with:
+          path: ~/.apt/cache
+          key: ${{ runner.os }}-apt-${{ hashFiles('**/ubuntu_dependencies.yml') }}
+          restore-keys: |
+            ${{ runner.os }}-apt-
+      - name: Install dependencies
+        run: |
+          sudo apt-get update
+          sudo apt-get install -y build-essential cmake git libeigen3-dev libtbb-dev
+      - name: Configure CMake
+        run: cmake -B ${{github.workspace}}/build -DCMAKE_BUILD_TYPE=${{env.BUILD_TYPE}} ${{github.workspace}}/cpp/kiss_icp
+      - name: Build
+        run: cmake --build ${{github.workspace}}/build --config ${{env.BUILD_TYPE}}

cpp/kiss_icp/core/Deskew.cpp

@@ -39,6 +39,7 @@ std::vector<Eigen::Vector3d> DeSkewScan(const std::vector<Eigen::Vector3d> &fram
                                         const Sophus::SE3d &delta) {
     const auto delta_pose = delta.log();
     std::vector<Eigen::Vector3d> corrected_frame(frame.size());
+    // TODO(All): This tbb execution is ignoring the max_n_threads config value
     tbb::parallel_for(size_t(0), frame.size(), [&](size_t i) {
         const auto motion = Sophus::SE3d::exp((timestamps[i] - mid_pose_timestamp) * delta_pose);
         corrected_frame[i] = motion * frame[i];

cpp/kiss_icp/core/Preprocessing.cpp

@@ -31,39 +31,22 @@
 #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
+#include "VoxelUtils.hpp"
 
 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;
+                                             const double voxel_size) {
+    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});
-    }
+        if (!grid.contains(voxel)) 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);
-    }
+    std::for_each(grid.cbegin(), grid.cend(), [&](const auto &voxel_and_point) {
+        frame_dowsampled.emplace_back(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,
+                                             const 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

@@ -26,6 +26,7 @@
 #include <tbb/global_control.h>
 #include <tbb/info.h>
 #include <tbb/parallel_reduce.h>
+#include <tbb/task_arena.h>
 
 #include <algorithm>
 #include <cmath>
@@ -53,7 +54,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 +70,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
@@ -171,7 +172,8 @@ Registration::Registration(int max_num_iteration, double convergence_criterion,
     : max_num_iterations_(max_num_iteration),
       convergence_criterion_(convergence_criterion),
       // Only manipulate the number of threads if the user specifies something greater than 0
-      max_num_threads_(max_num_threads > 0 ? max_num_threads : tbb::info::default_concurrency()) {
+      max_num_threads_(max_num_threads > 0 ? max_num_threads
+                                           : tbb::this_task_arena::max_concurrency()) {
     // This global variable requires static duration storage to be able to manipulate the max
     // concurrency from TBB across the entire class
     static const auto tbb_control_settings = tbb::global_control(

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

@@ -32,9 +32,10 @@
 #include <sophus/se3.hpp>
 #include <vector>
 
+#include "VoxelUtils.hpp"
+
 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 +44,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 +61,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,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 <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 {
+        const uint32_t *vec = reinterpret_cast<const uint32_t *>(voxel.data());
+        return (vec[0] * 73856093 ^ vec[1] * 19349669 ^ vec[2] * 83492791);
+    }
+};

python/kiss_icp/pipeline.py

@@ -179,12 +179,14 @@ def _run_evaluation(self):
         # Run timing metrics evaluation, always
         def _get_fps():
             total_time_s = sum(self.times) * 1e-9
-            return float(len(self.times) / total_time_s)
-
-        avg_fps = int(np.ceil(_get_fps()))
-        avg_ms = int(np.ceil(1e3 * (1 / _get_fps())))
-        self.results.append(desc="Average Frequency", units="Hz", value=avg_fps, trunc=True)
-        self.results.append(desc="Average Runtime", units="ms", value=avg_ms, trunc=True)
+            return float(len(self.times) / total_time_s) if total_time_s > 0 else 0
+
+        fps = _get_fps()
+        avg_fps = int(np.floor(fps))
+        avg_ms = int(np.ceil(1e3 / fps)) if fps > 0 else 0
+        if avg_fps > 0:
+            self.results.append(desc="Average Frequency", units="Hz", value=avg_fps, trunc=True)
+            self.results.append(desc="Average Runtime", units="ms", value=avg_ms, trunc=True)
 
     def _write_log(self):
         if not self.results.empty():