From 609ebb5d068c99d8dbc50e5c8e2c4f475b8fc43f Mon Sep 17 00:00:00 2001 From: tizianoGuadagnino Date: Tue, 9 Jul 2024 18:28:56 +0200 Subject: [PATCH 01/13] Unify Voxel functionalities, now everything comes from VoxelHashMap --- cpp/kiss_icp/core/Preprocessing.cpp | 11 +++-------- 1 file changed, 3 insertions(+), 8 deletions(-) diff --git a/cpp/kiss_icp/core/Preprocessing.cpp b/cpp/kiss_icp/core/Preprocessing.cpp index 0a6a6e30..56e4d12d 100644 --- a/cpp/kiss_icp/core/Preprocessing.cpp +++ b/cpp/kiss_icp/core/Preprocessing.cpp @@ -28,18 +28,13 @@ #include #include #include +#include #include #include 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(voxel.data()); - return ((1 << 20) - 1) & (vec[0] * 73856093 ^ vec[1] * 19349669 ^ vec[2] * 83492791); - } -}; +using Voxel = kiss_icp::VoxelHashMap::Voxel; +using VoxelHash = kiss_icp::VoxelHashMap::VoxelHash; Voxel PointToVoxel(const Eigen::Vector3d &point, double voxel_size) { return Voxel(static_cast(std::floor(point.x() / voxel_size)), From 8875319e0f5fe3bf45f7da81ff3653f3cbad1be8 Mon Sep 17 00:00:00 2001 From: tizianoGuadagnino Date: Tue, 9 Jul 2024 18:36:57 +0200 Subject: [PATCH 02/13] This looks like an even better unification --- cpp/kiss_icp/core/Preprocessing.cpp | 28 +++------------------------- 1 file changed, 3 insertions(+), 25 deletions(-) diff --git a/cpp/kiss_icp/core/Preprocessing.cpp b/cpp/kiss_icp/core/Preprocessing.cpp index 56e4d12d..ae452cf9 100644 --- a/cpp/kiss_icp/core/Preprocessing.cpp +++ b/cpp/kiss_icp/core/Preprocessing.cpp @@ -32,34 +32,12 @@ #include #include -namespace { -using Voxel = kiss_icp::VoxelHashMap::Voxel; -using VoxelHash = kiss_icp::VoxelHashMap::VoxelHash; - -Voxel PointToVoxel(const Eigen::Vector3d &point, double voxel_size) { - return Voxel(static_cast(std::floor(point.x() / voxel_size)), - static_cast(std::floor(point.y() / voxel_size)), - static_cast(std::floor(point.z() / voxel_size))); -} -} // namespace - namespace kiss_icp { std::vector VoxelDownsample(const std::vector &frame, double voxel_size) { - tsl::robin_map grid; - grid.reserve(frame.size()); - for (const auto &point : frame) { - const auto voxel = PointToVoxel(point, voxel_size); - if (grid.contains(voxel)) continue; - grid.insert({voxel, point}); - } - std::vector frame_dowsampled; - frame_dowsampled.reserve(grid.size()); - for (const auto &[voxel, point] : grid) { - (void)voxel; - frame_dowsampled.emplace_back(point); - } - return frame_dowsampled; + VoxelHashMap grid(voxel_size, 1.0, 1); + grid.AddPoints(frame); + return grid.Pointcloud(); } std::vector Preprocess(const std::vector &frame, From 8596de861751b6646115ea549b56eea982d80eb0 Mon Sep 17 00:00:00 2001 From: Ignacio Vizzo Date: Wed, 10 Jul 2024 09:12:08 +0200 Subject: [PATCH 03/13] Fix Ubuntu 20.04 dev build (#361) * Add a new job just to proof the build is now broken * force 22.04 to make sure it's only a 20.04 problem * Revert "force 22.04 to make sure it's only a 20.04 problem" This reverts commit e1b5e0276273f1176b3bb2ab2b9ccbc56fcccce0. * Add cache to ci build * Attempt to fix build in Ubuntu 20.04 * add this comment just to annoy ourselves --- .github/workflows/cpp.yml | 29 +++++++++++++++++++++++++++++ cpp/kiss_icp/core/Deskew.cpp | 1 + cpp/kiss_icp/core/Registration.cpp | 4 +++- 3 files changed, 33 insertions(+), 1 deletion(-) diff --git a/.github/workflows/cpp.yml b/.github/workflows/cpp.yml index 131dda9e..bbb8ef8a 100644 --- a/.github/workflows/cpp.yml +++ b/.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}} diff --git a/cpp/kiss_icp/core/Deskew.cpp b/cpp/kiss_icp/core/Deskew.cpp index 9b721fe2..fe20b77e 100644 --- a/cpp/kiss_icp/core/Deskew.cpp +++ b/cpp/kiss_icp/core/Deskew.cpp @@ -39,6 +39,7 @@ std::vector DeSkewScan(const std::vector &fram const Sophus::SE3d &delta) { const auto delta_pose = delta.log(); std::vector 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]; diff --git a/cpp/kiss_icp/core/Registration.cpp b/cpp/kiss_icp/core/Registration.cpp index d7adb4df..137fa50c 100644 --- a/cpp/kiss_icp/core/Registration.cpp +++ b/cpp/kiss_icp/core/Registration.cpp @@ -26,6 +26,7 @@ #include #include #include +#include #include #include @@ -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( From 409e2e851bcdf70b00cbc9dd68ffa8ff9a29d155 Mon Sep 17 00:00:00 2001 From: tizianoGuadagnino Date: Wed, 10 Jul 2024 11:28:40 +0200 Subject: [PATCH 04/13] Modernize VoxelDownsample --- cpp/kiss_icp/core/Preprocessing.cpp | 24 +++++++++++++++++++++--- 1 file changed, 21 insertions(+), 3 deletions(-) diff --git a/cpp/kiss_icp/core/Preprocessing.cpp b/cpp/kiss_icp/core/Preprocessing.cpp index ae452cf9..ff741392 100644 --- a/cpp/kiss_icp/core/Preprocessing.cpp +++ b/cpp/kiss_icp/core/Preprocessing.cpp @@ -32,12 +32,30 @@ #include #include +namespace { +using Voxel = kiss_icp::VoxelHashMap::Voxel; +using VoxelHash = kiss_icp::VoxelHashMap::VoxelHash; + +Voxel PointToVoxel(const Eigen::Vector3d &point, double voxel_size) { + return Voxel(static_cast(std::floor(point.x() / voxel_size)), + static_cast(std::floor(point.y() / voxel_size)), + static_cast(std::floor(point.z() / voxel_size))); +} +} // namespace + namespace kiss_icp { std::vector VoxelDownsample(const std::vector &frame, double voxel_size) { - VoxelHashMap grid(voxel_size, 1.0, 1); - grid.AddPoints(frame); - return grid.Pointcloud(); + tsl::robin_map 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 frame_dowsampled(grid.size()); + std::transform(grid.begin(), grid.end(), frame_dowsampled.begin(), + [](const auto &voxel_and_point) { return voxel_and_point.value(); }); + return frame_dowsampled; } std::vector Preprocess(const std::vector &frame, From fa5c9b8fe1901c7b789fc81008a8f2caaa8650d1 Mon Sep 17 00:00:00 2001 From: tizianoGuadagnino Date: Wed, 10 Jul 2024 11:40:08 +0200 Subject: [PATCH 05/13] VoxelUtils is born, now everthing is liteally in one place, and we have some side effect code reduction --- cpp/kiss_icp/core/Preprocessing.cpp | 17 ++--------- cpp/kiss_icp/core/Preprocessing.hpp | 6 ++-- cpp/kiss_icp/core/Registration.cpp | 4 +-- cpp/kiss_icp/core/VoxelHashMap.cpp | 2 +- cpp/kiss_icp/core/VoxelHashMap.hpp | 16 ++-------- cpp/kiss_icp/core/VoxelUtils.hpp | 45 +++++++++++++++++++++++++++++ 6 files changed, 56 insertions(+), 34 deletions(-) create mode 100644 cpp/kiss_icp/core/VoxelUtils.hpp diff --git a/cpp/kiss_icp/core/Preprocessing.cpp b/cpp/kiss_icp/core/Preprocessing.cpp index ff741392..13203af2 100644 --- a/cpp/kiss_icp/core/Preprocessing.cpp +++ b/cpp/kiss_icp/core/Preprocessing.cpp @@ -28,25 +28,14 @@ #include #include #include -#include +#include #include #include -namespace { -using Voxel = kiss_icp::VoxelHashMap::Voxel; -using VoxelHash = kiss_icp::VoxelHashMap::VoxelHash; - -Voxel PointToVoxel(const Eigen::Vector3d &point, double voxel_size) { - return Voxel(static_cast(std::floor(point.x() / voxel_size)), - static_cast(std::floor(point.y() / voxel_size)), - static_cast(std::floor(point.z() / voxel_size))); -} -} // namespace - namespace kiss_icp { std::vector VoxelDownsample(const std::vector &frame, double voxel_size) { - tsl::robin_map grid; + tsl::robin_map grid; grid.reserve(frame.size()); std::for_each(frame.cbegin(), frame.cend(), [&](const auto &point) { const auto voxel = PointToVoxel(point, voxel_size); @@ -54,7 +43,7 @@ std::vector VoxelDownsample(const std::vector }); std::vector frame_dowsampled(grid.size()); std::transform(grid.begin(), grid.end(), frame_dowsampled.begin(), - [](const auto &voxel_and_point) { return voxel_and_point.value(); }); + [](const auto &voxel_and_point) { return voxel_and_point.second; }); return frame_dowsampled; } diff --git a/cpp/kiss_icp/core/Preprocessing.hpp b/cpp/kiss_icp/core/Preprocessing.hpp index e919a422..fd752fe7 100644 --- a/cpp/kiss_icp/core/Preprocessing.hpp +++ b/cpp/kiss_icp/core/Preprocessing.hpp @@ -28,6 +28,9 @@ #include namespace kiss_icp { +/// Voxelize point cloud keeping the original coordinates +std::vector VoxelDownsample(const std::vector &frame, + double voxel_size); /// Crop the frame with max/min ranges std::vector Preprocess(const std::vector &frame, @@ -39,7 +42,4 @@ std::vector Preprocess(const std::vector &fram /// Originally introduced the calibration factor) std::vector CorrectKITTIScan(const std::vector &frame); -/// Voxelize point cloud keeping the original coordinates -std::vector VoxelDownsample(const std::vector &frame, - double voxel_size); } // namespace kiss_icp diff --git a/cpp/kiss_icp/core/Registration.cpp b/cpp/kiss_icp/core/Registration.cpp index d7adb4df..f03eaf2e 100644 --- a/cpp/kiss_icp/core/Registration.cpp +++ b/cpp/kiss_icp/core/Registration.cpp @@ -53,7 +53,7 @@ void TransformPoints(const Sophus::SE3d &T, std::vector &points [&](const auto &point) { return T * point; }); } -using Voxel = kiss_icp::VoxelHashMap::Voxel; +using Voxel = kiss_icp::Voxel; std::vector GetAdjacentVoxels(const Voxel &voxel, int adjacent_voxels = 1) { std::vector voxel_neighborhood; for (int i = voxel.x() - adjacent_voxels; i < voxel.x() + adjacent_voxels + 1; ++i) { @@ -69,7 +69,7 @@ std::vector GetAdjacentVoxels(const Voxel &voxel, int adjacent_voxels = 1 std::tuple 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 diff --git a/cpp/kiss_icp/core/VoxelHashMap.cpp b/cpp/kiss_icp/core/VoxelHashMap.cpp index 65790fba..8701d110 100644 --- a/cpp/kiss_icp/core/VoxelHashMap.cpp +++ b/cpp/kiss_icp/core/VoxelHashMap.cpp @@ -75,7 +75,7 @@ void VoxelHashMap::Update(const std::vector &points, const Soph void VoxelHashMap::AddPoints(const std::vector &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(); diff --git a/cpp/kiss_icp/core/VoxelHashMap.hpp b/cpp/kiss_icp/core/VoxelHashMap.hpp index 8ffe9ed9..071f22a4 100644 --- a/cpp/kiss_icp/core/VoxelHashMap.hpp +++ b/cpp/kiss_icp/core/VoxelHashMap.hpp @@ -29,12 +29,12 @@ #include #include +#include #include #include namespace kiss_icp { struct VoxelHashMap { - using Voxel = Eigen::Vector3i; struct VoxelBlock { // buffer of points with a max limit of n_points std::vector points; @@ -43,13 +43,6 @@ struct VoxelHashMap { if (points.size() < static_cast(num_points_)) points.push_back(point); } }; - struct VoxelHash { - size_t operator()(const Voxel &voxel) const { - const uint32_t *vec = reinterpret_cast(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), @@ -57,11 +50,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(std::floor(point.x() / voxel_size_)), - static_cast(std::floor(point.y() / voxel_size_)), - static_cast(std::floor(point.z() / voxel_size_))); - } void Update(const std::vector &points, const Eigen::Vector3d &origin); void Update(const std::vector &points, const Sophus::SE3d &pose); void AddPoints(const std::vector &points); @@ -72,6 +60,6 @@ struct VoxelHashMap { double voxel_size_; double max_distance_; int max_points_per_voxel_; - tsl::robin_map map_; + tsl::robin_map map_; }; } // namespace kiss_icp diff --git a/cpp/kiss_icp/core/VoxelUtils.hpp b/cpp/kiss_icp/core/VoxelUtils.hpp new file mode 100644 index 00000000..8778e789 --- /dev/null +++ b/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 + +#include + +namespace kiss_icp { +using Voxel = Eigen::Vector3i; +inline Voxel PointToVoxel(const Eigen::Vector3d &point, const double voxel_size) { + return Voxel(static_cast(std::floor(point.x() / voxel_size)), + static_cast(std::floor(point.y() / voxel_size)), + static_cast(std::floor(point.z() / voxel_size))); +} +} // namespace kiss_icp + +template <> +struct std::hash { + std::size_t operator()(const kiss_icp::Voxel &voxel) const noexcept { + const uint32_t *vec = reinterpret_cast(voxel.data()); + return (vec[0] * 73856093 ^ vec[1] * 19349669 ^ vec[2] * 83492791); + } +}; From 0cbc1d9bf1f2cca9942882f2bc2005012dadd3f0 Mon Sep 17 00:00:00 2001 From: tizianoGuadagnino Date: Wed, 10 Jul 2024 11:43:22 +0200 Subject: [PATCH 06/13] Macos does not like my include --- cpp/kiss_icp/core/VoxelUtils.hpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/cpp/kiss_icp/core/VoxelUtils.hpp b/cpp/kiss_icp/core/VoxelUtils.hpp index 8778e789..7d5888fb 100644 --- a/cpp/kiss_icp/core/VoxelUtils.hpp +++ b/cpp/kiss_icp/core/VoxelUtils.hpp @@ -23,9 +23,8 @@ // #pragma once -#include - #include +#include namespace kiss_icp { using Voxel = Eigen::Vector3i; From 5d420877a6e5f984ea86957da9e670ff6aed45e4 Mon Sep 17 00:00:00 2001 From: KARTHIK TARAKA SAI BOLLA <110156560+karthikbolla@users.noreply.github.com> Date: Thu, 11 Jul 2024 02:34:57 +0530 Subject: [PATCH 07/13] Handled Zero Division Error in pipeline.py (#344) * Handled Zero Division Error * Apply suggestions from code review Co-authored-by: Benedikt Mersch * Update pipeline.py updated pipeline.py based on @benemer suggestions where the results are only appended when avg fps is greater than zero * Fix formatting --------- Co-authored-by: Benedikt Mersch Co-authored-by: tizianoGuadagnino --- python/kiss_icp/pipeline.py | 14 ++++++++------ 1 file changed, 8 insertions(+), 6 deletions(-) diff --git a/python/kiss_icp/pipeline.py b/python/kiss_icp/pipeline.py index 9994bfc3..f8dfd857 100644 --- a/python/kiss_icp/pipeline.py +++ b/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(): From 3e7abb6f69070b9a378bda1e5f21cf7daccad2cb Mon Sep 17 00:00:00 2001 From: tizianoGuadagnino Date: Thu, 11 Jul 2024 09:01:56 +0200 Subject: [PATCH 08/13] Local style include --- cpp/kiss_icp/core/Preprocessing.cpp | 3 ++- cpp/kiss_icp/core/VoxelHashMap.hpp | 3 ++- cpp/kiss_icp/core/VoxelUtils.hpp | 1 + 3 files changed, 5 insertions(+), 2 deletions(-) diff --git a/cpp/kiss_icp/core/Preprocessing.cpp b/cpp/kiss_icp/core/Preprocessing.cpp index 13203af2..60c7ecc1 100644 --- a/cpp/kiss_icp/core/Preprocessing.cpp +++ b/cpp/kiss_icp/core/Preprocessing.cpp @@ -28,10 +28,11 @@ #include #include #include -#include #include #include +#include "VoxelUtils.hpp" + namespace kiss_icp { std::vector VoxelDownsample(const std::vector &frame, double voxel_size) { diff --git a/cpp/kiss_icp/core/VoxelHashMap.hpp b/cpp/kiss_icp/core/VoxelHashMap.hpp index 071f22a4..3c9c4c36 100644 --- a/cpp/kiss_icp/core/VoxelHashMap.hpp +++ b/cpp/kiss_icp/core/VoxelHashMap.hpp @@ -29,10 +29,11 @@ #include #include -#include #include #include +#include "VoxelUtils.hpp" + namespace kiss_icp { struct VoxelHashMap { struct VoxelBlock { diff --git a/cpp/kiss_icp/core/VoxelUtils.hpp b/cpp/kiss_icp/core/VoxelUtils.hpp index 7d5888fb..710ade77 100644 --- a/cpp/kiss_icp/core/VoxelUtils.hpp +++ b/cpp/kiss_icp/core/VoxelUtils.hpp @@ -24,6 +24,7 @@ #pragma once #include +#include #include namespace kiss_icp { From 07f97b19f1b2b5c0e548b38349a9a7aa16f73bde Mon Sep 17 00:00:00 2001 From: tizianoGuadagnino Date: Thu, 11 Jul 2024 09:30:11 +0200 Subject: [PATCH 09/13] With move --- cpp/kiss_icp/core/Preprocessing.cpp | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/cpp/kiss_icp/core/Preprocessing.cpp b/cpp/kiss_icp/core/Preprocessing.cpp index 60c7ecc1..89c70f2d 100644 --- a/cpp/kiss_icp/core/Preprocessing.cpp +++ b/cpp/kiss_icp/core/Preprocessing.cpp @@ -28,6 +28,7 @@ #include #include #include +#include #include #include @@ -43,8 +44,10 @@ std::vector VoxelDownsample(const std::vector if (!grid.contains(voxel)) grid.insert({voxel, point}); }); std::vector frame_dowsampled(grid.size()); - std::transform(grid.begin(), grid.end(), frame_dowsampled.begin(), - [](const auto &voxel_and_point) { return voxel_and_point.second; }); + std::transform(std::make_move_iterator(grid.begin()), // + std::make_move_iterator(grid.end()), // + frame_dowsampled.begin(), + [](auto &&voxel_and_point) { return voxel_and_point.second; }); return frame_dowsampled; } From 9f87b9b66ebb444b298c3795abed74b4de3870bc Mon Sep 17 00:00:00 2001 From: tizianoGuadagnino Date: Thu, 11 Jul 2024 10:46:14 +0200 Subject: [PATCH 10/13] As of benchmark --- cpp/kiss_icp/core/Preprocessing.cpp | 6 ++---- 1 file changed, 2 insertions(+), 4 deletions(-) diff --git a/cpp/kiss_icp/core/Preprocessing.cpp b/cpp/kiss_icp/core/Preprocessing.cpp index 89c70f2d..fd1d9481 100644 --- a/cpp/kiss_icp/core/Preprocessing.cpp +++ b/cpp/kiss_icp/core/Preprocessing.cpp @@ -44,10 +44,8 @@ std::vector VoxelDownsample(const std::vector if (!grid.contains(voxel)) grid.insert({voxel, point}); }); std::vector frame_dowsampled(grid.size()); - std::transform(std::make_move_iterator(grid.begin()), // - std::make_move_iterator(grid.end()), // - frame_dowsampled.begin(), - [](auto &&voxel_and_point) { return voxel_and_point.second; }); + std::transform(grid.cbegin(), grid.cend(), frame_dowsampled.begin(), + [](const auto &voxel_and_point) { return voxel_and_point.second; }); return frame_dowsampled; } From 2fc877f466f351cdffee4f5fefdd2eaadab94917 Mon Sep 17 00:00:00 2001 From: tizianoGuadagnino Date: Thu, 11 Jul 2024 10:52:05 +0200 Subject: [PATCH 11/13] For each for the win --- cpp/kiss_icp/core/Preprocessing.cpp | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) diff --git a/cpp/kiss_icp/core/Preprocessing.cpp b/cpp/kiss_icp/core/Preprocessing.cpp index fd1d9481..c5fe4549 100644 --- a/cpp/kiss_icp/core/Preprocessing.cpp +++ b/cpp/kiss_icp/core/Preprocessing.cpp @@ -43,9 +43,11 @@ std::vector VoxelDownsample(const std::vector const auto voxel = PointToVoxel(point, voxel_size); if (!grid.contains(voxel)) grid.insert({voxel, point}); }); - std::vector frame_dowsampled(grid.size()); - std::transform(grid.cbegin(), grid.cend(), frame_dowsampled.begin(), - [](const auto &voxel_and_point) { return voxel_and_point.second; }); + std::vector 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; } From aeb7b2167da42fc6e77b78054130ef54da1a2f3a Mon Sep 17 00:00:00 2001 From: tizianoGuadagnino Date: Thu, 11 Jul 2024 10:58:35 +0200 Subject: [PATCH 12/13] Remove unnecessary include --- cpp/kiss_icp/core/Preprocessing.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/cpp/kiss_icp/core/Preprocessing.cpp b/cpp/kiss_icp/core/Preprocessing.cpp index c5fe4549..5486de2d 100644 --- a/cpp/kiss_icp/core/Preprocessing.cpp +++ b/cpp/kiss_icp/core/Preprocessing.cpp @@ -28,7 +28,6 @@ #include #include #include -#include #include #include From 37d520c22a31be4fb0b146f77f6577d296026a5f Mon Sep 17 00:00:00 2001 From: Luca Lobefaro Date: Thu, 11 Jul 2024 11:20:14 +0200 Subject: [PATCH 13/13] Const correcteness --- cpp/kiss_icp/core/Preprocessing.cpp | 2 +- cpp/kiss_icp/core/Preprocessing.hpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/cpp/kiss_icp/core/Preprocessing.cpp b/cpp/kiss_icp/core/Preprocessing.cpp index 5486de2d..732a8720 100644 --- a/cpp/kiss_icp/core/Preprocessing.cpp +++ b/cpp/kiss_icp/core/Preprocessing.cpp @@ -35,7 +35,7 @@ namespace kiss_icp { std::vector VoxelDownsample(const std::vector &frame, - double voxel_size) { + const double voxel_size) { tsl::robin_map grid; grid.reserve(frame.size()); std::for_each(frame.cbegin(), frame.cend(), [&](const auto &point) { diff --git a/cpp/kiss_icp/core/Preprocessing.hpp b/cpp/kiss_icp/core/Preprocessing.hpp index fd752fe7..0042837a 100644 --- a/cpp/kiss_icp/core/Preprocessing.hpp +++ b/cpp/kiss_icp/core/Preprocessing.hpp @@ -30,7 +30,7 @@ namespace kiss_icp { /// Voxelize point cloud keeping the original coordinates std::vector VoxelDownsample(const std::vector &frame, - double voxel_size); + const double voxel_size); /// Crop the frame with max/min ranges std::vector Preprocess(const std::vector &frame,