Make voxel computation consistent across all source code (#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/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

@@ -32,13 +32,20 @@
 #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 {
@@ -47,7 +54,7 @@ std::vector<Eigen::Vector3d> VoxelDownsample(const std::vector<Eigen::Vector3d>
     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>());
+        const auto voxel = PointToVoxel(point, voxel_size);
         if (grid.contains(voxel)) continue;
         grid.insert({voxel, point});
     }

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>
@@ -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 = Voxel((point / voxel_size_).template cast<int>());
+        auto voxel = PointToVoxel(point);
         auto search = map_.find(voxel);
         if (search != map_.end()) {
             auto &voxel_block = search.value();

cpp/kiss_icp/core/VoxelHashMap.hpp

@@ -46,7 +46,7 @@ struct VoxelHashMap {
     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);
+            return (vec[0] * 73856093 ^ vec[1] * 19349669 ^ vec[2] * 83492791);
         }
     };
 
@@ -58,9 +58,9 @@ 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_));
+        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);