From 609ebb5d068c99d8dbc50e5c8e2c4f475b8fc43f Mon Sep 17 00:00:00 2001 From: tizianoGuadagnino <frevo93@gmail.com> Date: Tue, 9 Jul 2024 18:28:56 +0200 Subject: [PATCH 01/12] 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 <Eigen/Core> #include <algorithm> #include <cmath> +#include <kiss_icp/core/VoxelHashMap.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); - } -}; +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<int>(std::floor(point.x() / voxel_size)), From 8875319e0f5fe3bf45f7da81ff3653f3cbad1be8 Mon Sep 17 00:00:00 2001 From: tizianoGuadagnino <frevo93@gmail.com> Date: Tue, 9 Jul 2024 18:36:57 +0200 Subject: [PATCH 02/12] 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 <sophus/se3.hpp> #include <vector> -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<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; - 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<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; + VoxelHashMap grid(voxel_size, 1.0, 1); + grid.AddPoints(frame); + return grid.Pointcloud(); } std::vector<Eigen::Vector3d> Preprocess(const std::vector<Eigen::Vector3d> &frame, From 409e2e851bcdf70b00cbc9dd68ffa8ff9a29d155 Mon Sep 17 00:00:00 2001 From: tizianoGuadagnino <frevo93@gmail.com> Date: Wed, 10 Jul 2024 11:28:40 +0200 Subject: [PATCH 03/12] 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 <sophus/se3.hpp> #include <vector> +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<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) { - VoxelHashMap grid(voxel_size, 1.0, 1); - grid.AddPoints(frame); - return grid.Pointcloud(); + tsl::robin_map<Voxel, Eigen::Vector3d, VoxelHash> 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(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<Eigen::Vector3d> Preprocess(const std::vector<Eigen::Vector3d> &frame, From fa5c9b8fe1901c7b789fc81008a8f2caaa8650d1 Mon Sep 17 00:00:00 2001 From: tizianoGuadagnino <frevo93@gmail.com> Date: Wed, 10 Jul 2024 11:40:08 +0200 Subject: [PATCH 04/12] 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 <Eigen/Core> #include <algorithm> #include <cmath> -#include <kiss_icp/core/VoxelHashMap.hpp> +#include <kiss_icp/core/VoxelUtils.hpp> #include <sophus/se3.hpp> #include <vector> -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<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()); std::for_each(frame.cbegin(), frame.cend(), [&](const auto &point) { const auto voxel = PointToVoxel(point, voxel_size); @@ -54,7 +43,7 @@ std::vector<Eigen::Vector3d> VoxelDownsample(const std::vector<Eigen::Vector3d> }); 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.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 <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 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<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 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<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(); 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 <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,13 +43,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 (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<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 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 <bits/functional_hash.h> + +#include <Eigen/Core> + +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); + } +}; From 0cbc1d9bf1f2cca9942882f2bc2005012dadd3f0 Mon Sep 17 00:00:00 2001 From: tizianoGuadagnino <frevo93@gmail.com> Date: Wed, 10 Jul 2024 11:43:22 +0200 Subject: [PATCH 05/12] 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 <bits/functional_hash.h> - #include <Eigen/Core> +#include <memory> namespace kiss_icp { using Voxel = Eigen::Vector3i; From 3e7abb6f69070b9a378bda1e5f21cf7daccad2cb Mon Sep 17 00:00:00 2001 From: tizianoGuadagnino <frevo93@gmail.com> Date: Thu, 11 Jul 2024 09:01:56 +0200 Subject: [PATCH 06/12] 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 <Eigen/Core> #include <algorithm> #include <cmath> -#include <kiss_icp/core/VoxelUtils.hpp> #include <sophus/se3.hpp> #include <vector> +#include "VoxelUtils.hpp" + namespace kiss_icp { std::vector<Eigen::Vector3d> VoxelDownsample(const std::vector<Eigen::Vector3d> &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 <tsl/robin_map.h> #include <Eigen/Core> -#include <kiss_icp/core/VoxelUtils.hpp> #include <sophus/se3.hpp> #include <vector> +#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 <Eigen/Core> +#include <cmath> #include <memory> namespace kiss_icp { From 07f97b19f1b2b5c0e548b38349a9a7aa16f73bde Mon Sep 17 00:00:00 2001 From: tizianoGuadagnino <frevo93@gmail.com> Date: Thu, 11 Jul 2024 09:30:11 +0200 Subject: [PATCH 07/12] 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 <Eigen/Core> #include <algorithm> #include <cmath> +#include <iterator> #include <sophus/se3.hpp> #include <vector> @@ -43,8 +44,10 @@ std::vector<Eigen::Vector3d> VoxelDownsample(const std::vector<Eigen::Vector3d> 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; }); + 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 <frevo93@gmail.com> Date: Thu, 11 Jul 2024 10:46:14 +0200 Subject: [PATCH 08/12] 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<Eigen::Vector3d> VoxelDownsample(const std::vector<Eigen::Vector3d> if (!grid.contains(voxel)) grid.insert({voxel, point}); }); std::vector<Eigen::Vector3d> 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 <frevo93@gmail.com> Date: Thu, 11 Jul 2024 10:52:05 +0200 Subject: [PATCH 09/12] 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<Eigen::Vector3d> VoxelDownsample(const std::vector<Eigen::Vector3d> const auto voxel = PointToVoxel(point, voxel_size); if (!grid.contains(voxel)) grid.insert({voxel, point}); }); - std::vector<Eigen::Vector3d> 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<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; } From aeb7b2167da42fc6e77b78054130ef54da1a2f3a Mon Sep 17 00:00:00 2001 From: tizianoGuadagnino <frevo93@gmail.com> Date: Thu, 11 Jul 2024 10:58:35 +0200 Subject: [PATCH 10/12] 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 <Eigen/Core> #include <algorithm> #include <cmath> -#include <iterator> #include <sophus/se3.hpp> #include <vector> From 37d520c22a31be4fb0b146f77f6577d296026a5f Mon Sep 17 00:00:00 2001 From: Luca Lobefaro <luca.lobefaro95@gmail.com> Date: Thu, 11 Jul 2024 11:20:14 +0200 Subject: [PATCH 11/12] 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<Eigen::Vector3d> VoxelDownsample(const std::vector<Eigen::Vector3d> &frame, - double voxel_size) { + 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) { 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<Eigen::Vector3d> VoxelDownsample(const std::vector<Eigen::Vector3d> &frame, - double voxel_size); + const double voxel_size); /// Crop the frame with max/min ranges std::vector<Eigen::Vector3d> Preprocess(const std::vector<Eigen::Vector3d> &frame, From 9b2b2cd216b397fc511e81305b0e1916f7a8a9c1 Mon Sep 17 00:00:00 2001 From: Ignacio Vizzo <ignacio@dexory.com> Date: Thu, 11 Jul 2024 13:13:21 +0200 Subject: [PATCH 12/12] Refresh changes --- cpp/kiss_icp/core/Preprocessing.cpp | 18 ++++++++++-------- cpp/kiss_icp/core/Preprocessing.hpp | 6 +++--- cpp/kiss_icp/core/Registration.cpp | 1 + cpp/kiss_icp/core/VoxelHashMap.cpp | 2 ++ cpp/kiss_icp/core/VoxelHashMap.hpp | 3 ++- cpp/kiss_icp/core/VoxelUtils.hpp | 10 +++++----- 6 files changed, 23 insertions(+), 17 deletions(-) diff --git a/cpp/kiss_icp/core/Preprocessing.cpp b/cpp/kiss_icp/core/Preprocessing.cpp index 732a8720..95b82643 100644 --- a/cpp/kiss_icp/core/Preprocessing.cpp +++ b/cpp/kiss_icp/core/Preprocessing.cpp @@ -35,18 +35,20 @@ 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; + double voxel_size) { + tsl::robin_map<Voxel, Eigen::Vector3d, VoxelHash> grid; grid.reserve(frame.size()); - std::for_each(frame.cbegin(), frame.cend(), [&](const auto &point) { + for (const auto &point : frame) { const auto voxel = PointToVoxel(point, voxel_size); - if (!grid.contains(voxel)) grid.insert({voxel, point}); - }); + if (grid.contains(voxel)) continue; + 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); - }); + for (const auto &[voxel, point] : grid) { + (void)voxel; + frame_dowsampled.emplace_back(point); + } return frame_dowsampled; } diff --git a/cpp/kiss_icp/core/Preprocessing.hpp b/cpp/kiss_icp/core/Preprocessing.hpp index 0042837a..e919a422 100644 --- a/cpp/kiss_icp/core/Preprocessing.hpp +++ b/cpp/kiss_icp/core/Preprocessing.hpp @@ -28,9 +28,6 @@ #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, @@ -42,4 +39,7 @@ 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 diff --git a/cpp/kiss_icp/core/Registration.cpp b/cpp/kiss_icp/core/Registration.cpp index 78168197..b8acea10 100644 --- a/cpp/kiss_icp/core/Registration.cpp +++ b/cpp/kiss_icp/core/Registration.cpp @@ -36,6 +36,7 @@ #include <tuple> #include "VoxelHashMap.hpp" +#include "VoxelUtils.hpp" namespace Eigen { using Matrix6d = Eigen::Matrix<double, 6, 6>; diff --git a/cpp/kiss_icp/core/VoxelHashMap.cpp b/cpp/kiss_icp/core/VoxelHashMap.cpp index 8701d110..b58457d2 100644 --- a/cpp/kiss_icp/core/VoxelHashMap.cpp +++ b/cpp/kiss_icp/core/VoxelHashMap.cpp @@ -28,6 +28,8 @@ #include <utility> #include <vector> +#include "VoxelUtils.hpp" + namespace kiss_icp { std::vector<Eigen::Vector3d> VoxelHashMap::GetPoints(const std::vector<Voxel> &query_voxels) const { diff --git a/cpp/kiss_icp/core/VoxelHashMap.hpp b/cpp/kiss_icp/core/VoxelHashMap.hpp index 3c9c4c36..a49b7096 100644 --- a/cpp/kiss_icp/core/VoxelHashMap.hpp +++ b/cpp/kiss_icp/core/VoxelHashMap.hpp @@ -44,6 +44,7 @@ struct VoxelHashMap { if (points.size() < static_cast<size_t>(num_points_)) points.push_back(point); } }; + explicit VoxelHashMap(double voxel_size, double max_distance, int max_points_per_voxel) : voxel_size_(voxel_size), max_distance_(max_distance), @@ -61,6 +62,6 @@ struct VoxelHashMap { double voxel_size_; double max_distance_; int max_points_per_voxel_; - tsl::robin_map<Voxel, VoxelBlock> map_; + tsl::robin_map<Voxel, VoxelBlock, VoxelHash> map_; }; } // namespace kiss_icp diff --git a/cpp/kiss_icp/core/VoxelUtils.hpp b/cpp/kiss_icp/core/VoxelUtils.hpp index 710ade77..331a2ce1 100644 --- a/cpp/kiss_icp/core/VoxelUtils.hpp +++ b/cpp/kiss_icp/core/VoxelUtils.hpp @@ -25,21 +25,21 @@ #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 { +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); } }; +} // namespace kiss_icp