Merge pull request #2 from PRBonn/main

Make voxel computation consistent across all source code (PRBonn#354)

.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/CMakeLists.txt

@@ -21,7 +21,7 @@
 # OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
 # SOFTWARE.
 cmake_minimum_required(VERSION 3.16...3.26)
-project(kiss_icp_cpp VERSION 0.4.0 LANGUAGES CXX)
+project(kiss_icp_cpp VERSION 1.0.0 LANGUAGES CXX)
 
 # Setup build options
 option(USE_CCACHE "Build using Ccache if found on the path" ON)

cpp/kiss_icp/core/CMakeLists.txt

@@ -21,6 +21,7 @@
 # OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
 # SOFTWARE.
 add_library(kiss_icp_core STATIC)
-target_sources(kiss_icp_core PRIVATE Registration.cpp Deskew.cpp VoxelHashMap.cpp Preprocessing.cpp Threshold.cpp)
+target_sources(kiss_icp_core PRIVATE Registration.cpp Deskew.cpp VoxelHashMap.cpp VoxelUtils.cpp Preprocessing.cpp
+                                     Threshold.cpp)
 target_link_libraries(kiss_icp_core PUBLIC Eigen3::Eigen tsl::robin_map TBB::tbb Sophus::Sophus)
 set_global_target_properties(kiss_icp_core)

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

@@ -22,44 +22,16 @@
 // SOFTWARE.
 #include "Preprocessing.hpp"
 
-#include <tbb/parallel_for.h>
 #include <tsl/robin_map.h>
 
 #include <Eigen/Core>
+#include <Eigen/Geometry>
 #include <algorithm>
-#include <cmath>
-#include <sophus/se3.hpp>
 #include <vector>
 
-namespace {
-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);
-    }
-};
-}  // 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;
-    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});
-    }
-    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;
-}
-
 std::vector<Eigen::Vector3d> Preprocess(const std::vector<Eigen::Vector3d> &frame,
                                         double max_range,
                                         double min_range) {
@@ -71,15 +43,4 @@ std::vector<Eigen::Vector3d> Preprocess(const std::vector<Eigen::Vector3d> &fram
     return inliers;
 }
 
-std::vector<Eigen::Vector3d> CorrectKITTIScan(const std::vector<Eigen::Vector3d> &frame) {
-    constexpr double VERTICAL_ANGLE_OFFSET = (0.205 * M_PI) / 180.0;
-    std::vector<Eigen::Vector3d> corrected_frame(frame.size());
-    tbb::parallel_for(size_t(0), frame.size(), [&](size_t i) {
-        const auto &pt = frame[i];
-        const Eigen::Vector3d rotationVector = pt.cross(Eigen::Vector3d(0., 0., 1.));
-        corrected_frame[i] =
-            Eigen::AngleAxisd(VERTICAL_ANGLE_OFFSET, rotationVector.normalized()) * pt;
-    });
-    return corrected_frame;
-}
 }  // namespace kiss_icp

cpp/kiss_icp/core/Preprocessing.hpp

@@ -23,8 +23,6 @@
 #pragma once
 
 #include <Eigen/Core>
-#include <sophus/se3.hpp>
-#include <utility>
 #include <vector>
 
 namespace kiss_icp {
@@ -33,13 +31,4 @@ namespace kiss_icp {
 std::vector<Eigen::Vector3d> Preprocess(const std::vector<Eigen::Vector3d> &frame,
                                         double max_range,
                                         double min_range);
-
-/// This function only applies for the KITTI dataset, and should NOT be used by any other dataset,
-/// the original idea and part of the implementation is taking from CT-ICP(Although IMLS-SLAM
-/// 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>
@@ -35,6 +36,7 @@
 #include <tuple>
 
 #include "VoxelHashMap.hpp"
+#include "VoxelUtils.hpp"
 
 namespace Eigen {
 using Matrix6d = Eigen::Matrix<double, 6, 6>;
@@ -53,7 +55,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 +71,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 +173,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/Threshold.cpp

@@ -22,8 +22,9 @@
 // SOFTWARE.
 #include "Threshold.hpp"
 
-#include <Eigen/Core>
+#include <Eigen/Geometry>
 #include <cmath>
+#include <sophus/se3.hpp>
 
 namespace kiss_icp {
 AdaptiveThreshold::AdaptiveThreshold(double initial_threshold,

cpp/kiss_icp/core/VoxelHashMap.cpp

@@ -24,10 +24,11 @@
 
 #include <Eigen/Core>
 #include <algorithm>
-#include <tuple>
-#include <utility>
+#include <sophus/se3.hpp>
 #include <vector>
 
+#include "VoxelUtils.hpp"
+
 namespace kiss_icp {
 
 std::vector<Eigen::Vector3d> VoxelHashMap::GetPoints(const std::vector<Voxel> &query_voxels) const {
@@ -36,9 +37,8 @@ std::vector<Eigen::Vector3d> VoxelHashMap::GetPoints(const std::vector<Voxel> &q
     std::for_each(query_voxels.cbegin(), query_voxels.cend(), [&](const auto &query) {
         auto search = map_.find(query);
         if (search != map_.end()) {
-            for (const auto &point : search->second.points) {
-                points.emplace_back(point);
-            }
+            const auto &voxel_points = search->second.points;
+            points.insert(points.end(), voxel_points.cbegin(), voxel_points.cend());
         }
     });
     points.shrink_to_fit();
@@ -49,11 +49,8 @@ std::vector<Eigen::Vector3d> VoxelHashMap::Pointcloud() const {
     std::vector<Eigen::Vector3d> points;
     points.reserve(map_.size() * static_cast<size_t>(max_points_per_voxel_));
     std::for_each(map_.cbegin(), map_.cend(), [&](const auto &map_element) {
-        const auto &[voxel, voxel_block] = map_element;
-        (void)voxel;
-        for (const auto &point : voxel_block.points) {
-            points.emplace_back(point);
-        }
+        const auto &voxel_points = map_element.second.points;
+        points.insert(points.end(), voxel_points.cbegin(), voxel_points.cend());
     });
     points.shrink_to_fit();
     return points;
@@ -75,7 +72,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 = Voxel((point / voxel_size_).template cast<int>());
+        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,12 +44,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 ((1 << 20) - 1) & (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),
@@ -57,11 +52,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>(point.x() / voxel_size_),
-                     static_cast<int>(point.y() / voxel_size_),
-                     static_cast<int>(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 +62,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.cpp

@@ -0,0 +1,23 @@
+#include "VoxelUtils.hpp"
+
+#include <tsl/robin_map.h>
+
+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;
+    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;
+    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;
+}
+
+}  // namespace kiss_icp

cpp/kiss_icp/core/VoxelUtils.hpp

@@ -0,0 +1,51 @@
+// 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 <vector>
+
+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)));
+}
+
+/// Voxelize a point cloud keeping the original coordinates
+std::vector<Eigen::Vector3d> VoxelDownsample(const std::vector<Eigen::Vector3d> &frame,
+                                             const double voxel_size);
+
+}  // namespace kiss_icp
+
+template <>
+struct std::hash<kiss_icp::Voxel> {
+    std::size_t operator()(const kiss_icp::Voxel &voxel) const {
+        const uint32_t *vec = reinterpret_cast<const uint32_t *>(voxel.data());
+        return (vec[0] * 73856093 ^ vec[1] * 19349669 ^ vec[2] * 83492791);
+    }
+};

cpp/kiss_icp/metrics/Metrics.cpp

@@ -23,6 +23,7 @@
 #include "Metrics.hpp"
 
 #include <Eigen/Core>
+#include <Eigen/Geometry>
 #include <algorithm>
 #include <cassert>
 #include <cmath>

python/CMakeLists.txt

@@ -21,7 +21,7 @@
 # OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
 # SOFTWARE.
 cmake_minimum_required(VERSION 3.16...3.26)
-project(kiss_icp_pybind VERSION 0.4.0 LANGUAGES CXX)
+project(kiss_icp_pybind VERSION 1.0.0 LANGUAGES CXX)
 
 # Set build type
 set(CMAKE_BUILD_TYPE Release)

python/kiss_icp/__init__.py

@@ -20,4 +20,4 @@
 # 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.
-__version__ = "0.4.0"
+__version__ = "1.0.0"