From a61c11ef9adf3684265855aa30d9be976b0955cd Mon Sep 17 00:00:00 2001 From: ssg1002 Date: Tue, 2 Jul 2024 11:38:15 +0200 Subject: [PATCH 1/9] Make voxel computation consistent across all source code, also use std::floor to have explicit control over type casting --- cpp/kiss_icp/core/Preprocessing.cpp | 5 ++++- cpp/kiss_icp/core/VoxelHashMap.cpp | 5 ++++- cpp/kiss_icp/core/VoxelHashMap.hpp | 7 ++++--- 3 files changed, 12 insertions(+), 5 deletions(-) diff --git a/cpp/kiss_icp/core/Preprocessing.cpp b/cpp/kiss_icp/core/Preprocessing.cpp index 221651bf..40407356 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 @@ -47,7 +48,9 @@ std::vector VoxelDownsample(const std::vector tsl::robin_map grid; grid.reserve(frame.size()); for (const auto &point : frame) { - const auto voxel = Voxel((point / voxel_size).cast()); + const auto voxel = 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))); if (grid.contains(voxel)) continue; grid.insert({voxel, point}); } diff --git a/cpp/kiss_icp/core/VoxelHashMap.cpp b/cpp/kiss_icp/core/VoxelHashMap.cpp index 47a345b4..34cef8f3 100644 --- a/cpp/kiss_icp/core/VoxelHashMap.cpp +++ b/cpp/kiss_icp/core/VoxelHashMap.cpp @@ -24,6 +24,7 @@ #include #include +#include #include #include #include @@ -75,7 +76,9 @@ 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 = Voxel((point / voxel_size_).template cast()); + auto voxel = 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_))); 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 7eff4347..6b5f2211 100644 --- a/cpp/kiss_icp/core/VoxelHashMap.hpp +++ b/cpp/kiss_icp/core/VoxelHashMap.hpp @@ -29,6 +29,7 @@ #include #include +#include #include #include @@ -58,9 +59,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(point.x() / voxel_size_), - static_cast(point.y() / voxel_size_), - static_cast(point.z() / 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_))); } void Update(const std::vector &points, const Eigen::Vector3d &origin); void Update(const std::vector &points, const Sophus::SE3d &pose); From adf8f4d465301df3a71bda5e4bcfbe08947d6912 Mon Sep 17 00:00:00 2001 From: ssg1002 Date: Tue, 2 Jul 2024 16:17:56 +0200 Subject: [PATCH 2/9] Use available function for conversion --- cpp/kiss_icp/core/VoxelHashMap.cpp | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/cpp/kiss_icp/core/VoxelHashMap.cpp b/cpp/kiss_icp/core/VoxelHashMap.cpp index 34cef8f3..910611a5 100644 --- a/cpp/kiss_icp/core/VoxelHashMap.cpp +++ b/cpp/kiss_icp/core/VoxelHashMap.cpp @@ -76,9 +76,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 = 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_))); + auto voxel = PointToVoxel(point); auto search = map_.find(voxel); if (search != map_.end()) { auto &voxel_block = search.value(); From fd3a27e31e414dcced6ffe62646a790ed4689cb1 Mon Sep 17 00:00:00 2001 From: tizianoGuadagnino Date: Mon, 8 Jul 2024 16:06:49 +0200 Subject: [PATCH 3/9] Make it a one liner --- cpp/kiss_icp/core/Preprocessing.cpp | 4 +--- cpp/kiss_icp/core/VoxelHashMap.hpp | 4 +--- 2 files changed, 2 insertions(+), 6 deletions(-) diff --git a/cpp/kiss_icp/core/Preprocessing.cpp b/cpp/kiss_icp/core/Preprocessing.cpp index 40407356..0d4317d4 100644 --- a/cpp/kiss_icp/core/Preprocessing.cpp +++ b/cpp/kiss_icp/core/Preprocessing.cpp @@ -48,9 +48,7 @@ std::vector VoxelDownsample(const std::vector tsl::robin_map grid; grid.reserve(frame.size()); for (const auto &point : frame) { - const auto voxel = 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))); + const Voxel voxel = (point / voxel_size).array().floor().cast(); if (grid.contains(voxel)) continue; grid.insert({voxel, point}); } diff --git a/cpp/kiss_icp/core/VoxelHashMap.hpp b/cpp/kiss_icp/core/VoxelHashMap.hpp index 6b5f2211..69c77d2c 100644 --- a/cpp/kiss_icp/core/VoxelHashMap.hpp +++ b/cpp/kiss_icp/core/VoxelHashMap.hpp @@ -59,9 +59,7 @@ 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_))); + return (point / voxel_size_).array().floor().cast(); } void Update(const std::vector &points, const Eigen::Vector3d &origin); void Update(const std::vector &points, const Sophus::SE3d &pose); From 13a9e1172692c7d583982db32522333c6a09468b Mon Sep 17 00:00:00 2001 From: tizianoGuadagnino Date: Mon, 8 Jul 2024 16:10:02 +0200 Subject: [PATCH 4/9] remove numeric from includes --- cpp/kiss_icp/core/Preprocessing.cpp | 1 - cpp/kiss_icp/core/VoxelHashMap.hpp | 1 - 2 files changed, 2 deletions(-) diff --git a/cpp/kiss_icp/core/Preprocessing.cpp b/cpp/kiss_icp/core/Preprocessing.cpp index 0d4317d4..8bb6240d 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 diff --git a/cpp/kiss_icp/core/VoxelHashMap.hpp b/cpp/kiss_icp/core/VoxelHashMap.hpp index 69c77d2c..a801678e 100644 --- a/cpp/kiss_icp/core/VoxelHashMap.hpp +++ b/cpp/kiss_icp/core/VoxelHashMap.hpp @@ -29,7 +29,6 @@ #include #include -#include #include #include From d7a56c007def0dcc321d59829325c733122336d4 Mon Sep 17 00:00:00 2001 From: Ignacio Vizzo Date: Tue, 9 Jul 2024 10:23:10 +0200 Subject: [PATCH 5/9] New proposal --- cpp/kiss_icp/core/Preprocessing.cpp | 9 ++++++++- cpp/kiss_icp/core/VoxelHashMap.hpp | 5 ++++- 2 files changed, 12 insertions(+), 2 deletions(-) diff --git a/cpp/kiss_icp/core/Preprocessing.cpp b/cpp/kiss_icp/core/Preprocessing.cpp index 8bb6240d..85d14a83 100644 --- a/cpp/kiss_icp/core/Preprocessing.cpp +++ b/cpp/kiss_icp/core/Preprocessing.cpp @@ -32,6 +32,7 @@ #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 { @@ -39,6 +40,12 @@ struct VoxelHash { return ((1 << 20) - 1) & (vec[0] * 73856093 ^ vec[1] * 19349669 ^ vec[2] * 83492791); } }; + +Voxel PointToVoxel(const Eigen::Vector3d &point, double voxel_size) { + return {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 { @@ -47,7 +54,7 @@ std::vector VoxelDownsample(const std::vector tsl::robin_map grid; grid.reserve(frame.size()); for (const auto &point : frame) { - const Voxel voxel = (point / voxel_size).array().floor().cast(); + const auto voxel = PointToVoxel(point, voxel_size); if (grid.contains(voxel)) continue; grid.insert({voxel, point}); } diff --git a/cpp/kiss_icp/core/VoxelHashMap.hpp b/cpp/kiss_icp/core/VoxelHashMap.hpp index a801678e..4786cde9 100644 --- a/cpp/kiss_icp/core/VoxelHashMap.hpp +++ b/cpp/kiss_icp/core/VoxelHashMap.hpp @@ -58,8 +58,11 @@ struct VoxelHashMap { inline void Clear() { map_.clear(); } inline bool Empty() const { return map_.empty(); } inline Voxel PointToVoxel(const Eigen::Vector3d &point) const { - return (point / voxel_size_).array().floor().cast(); + 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); From ecb5ba2500b692ee70219049eba5d62127f5cd52 Mon Sep 17 00:00:00 2001 From: Ignacio Vizzo Date: Tue, 9 Jul 2024 10:58:08 +0200 Subject: [PATCH 6/9] remove numeric from include --- cpp/kiss_icp/core/VoxelHashMap.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/cpp/kiss_icp/core/VoxelHashMap.cpp b/cpp/kiss_icp/core/VoxelHashMap.cpp index 910611a5..65790fba 100644 --- a/cpp/kiss_icp/core/VoxelHashMap.cpp +++ b/cpp/kiss_icp/core/VoxelHashMap.cpp @@ -24,7 +24,6 @@ #include #include -#include #include #include #include From 11314beffc3ba7cdb685c39db1e14550ed14793e Mon Sep 17 00:00:00 2001 From: Ignacio Vizzo Date: Tue, 9 Jul 2024 10:58:47 +0200 Subject: [PATCH 7/9] shrink diff --- cpp/kiss_icp/core/VoxelHashMap.hpp | 1 - 1 file changed, 1 deletion(-) diff --git a/cpp/kiss_icp/core/VoxelHashMap.hpp b/cpp/kiss_icp/core/VoxelHashMap.hpp index 4786cde9..92a3d167 100644 --- a/cpp/kiss_icp/core/VoxelHashMap.hpp +++ b/cpp/kiss_icp/core/VoxelHashMap.hpp @@ -62,7 +62,6 @@ struct VoxelHashMap { 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); From 66ef872c96b4f51300dad1148eb009818bc345c1 Mon Sep 17 00:00:00 2001 From: Ignacio Vizzo Date: Tue, 9 Jul 2024 11:08:10 +0200 Subject: [PATCH 8/9] Consistency as always --- cpp/kiss_icp/core/Preprocessing.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/cpp/kiss_icp/core/Preprocessing.cpp b/cpp/kiss_icp/core/Preprocessing.cpp index 85d14a83..0a6a6e30 100644 --- a/cpp/kiss_icp/core/Preprocessing.cpp +++ b/cpp/kiss_icp/core/Preprocessing.cpp @@ -42,9 +42,9 @@ struct VoxelHash { }; Voxel PointToVoxel(const Eigen::Vector3d &point, double voxel_size) { - return {static_cast(std::floor(point.x() / voxel_size)), - static_cast(std::floor(point.y() / voxel_size)), - static_cast(std::floor(point.z() / 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 From 8952ca016662ad0c7f88e17b4ed45ac5eed66788 Mon Sep 17 00:00:00 2001 From: ssg1002 Date: Thu, 11 Jul 2024 15:35:32 +0200 Subject: [PATCH 9/9] Fix include header, follow IWYU --- cpp/kiss_icp/core/Preprocessing.cpp | 2 +- cpp/kiss_icp/core/Preprocessing.hpp | 2 -- cpp/kiss_icp/core/Threshold.cpp | 3 ++- cpp/kiss_icp/core/VoxelHashMap.cpp | 3 +-- cpp/kiss_icp/metrics/Metrics.cpp | 1 + 5 files changed, 5 insertions(+), 6 deletions(-) diff --git a/cpp/kiss_icp/core/Preprocessing.cpp b/cpp/kiss_icp/core/Preprocessing.cpp index 0a6a6e30..d9e1d805 100644 --- a/cpp/kiss_icp/core/Preprocessing.cpp +++ b/cpp/kiss_icp/core/Preprocessing.cpp @@ -26,9 +26,9 @@ #include #include +#include #include #include -#include #include namespace { diff --git a/cpp/kiss_icp/core/Preprocessing.hpp b/cpp/kiss_icp/core/Preprocessing.hpp index e919a422..1a8ae1ee 100644 --- a/cpp/kiss_icp/core/Preprocessing.hpp +++ b/cpp/kiss_icp/core/Preprocessing.hpp @@ -23,8 +23,6 @@ #pragma once #include -#include -#include #include namespace kiss_icp { diff --git a/cpp/kiss_icp/core/Threshold.cpp b/cpp/kiss_icp/core/Threshold.cpp index f9cb4131..1c51a511 100644 --- a/cpp/kiss_icp/core/Threshold.cpp +++ b/cpp/kiss_icp/core/Threshold.cpp @@ -22,8 +22,9 @@ // SOFTWARE. #include "Threshold.hpp" -#include +#include #include +#include namespace kiss_icp { AdaptiveThreshold::AdaptiveThreshold(double initial_threshold, diff --git a/cpp/kiss_icp/core/VoxelHashMap.cpp b/cpp/kiss_icp/core/VoxelHashMap.cpp index 65790fba..6b3b061b 100644 --- a/cpp/kiss_icp/core/VoxelHashMap.cpp +++ b/cpp/kiss_icp/core/VoxelHashMap.cpp @@ -24,8 +24,7 @@ #include #include -#include -#include +#include #include namespace kiss_icp { diff --git a/cpp/kiss_icp/metrics/Metrics.cpp b/cpp/kiss_icp/metrics/Metrics.cpp index 26f025bf..78195586 100644 --- a/cpp/kiss_icp/metrics/Metrics.cpp +++ b/cpp/kiss_icp/metrics/Metrics.cpp @@ -23,6 +23,7 @@ #include "Metrics.hpp" #include +#include #include #include #include