From 70c3e8dd44edda31018bc6ea360c02b79cc0a40a Mon Sep 17 00:00:00 2001 From: Saurabh Gupta Date: Fri, 12 Jul 2024 09:19:19 +0000 Subject: [PATCH 1/2] Fix include headers (#365) * Make voxel computation consistent across all source code, also use std::floor to have explicit control over type casting * Use available function for conversion * Make it a one liner * remove numeric from includes * New proposal * remove numeric from include * shrink diff * Consistency as always * Fix include header, follow IWYU --------- Co-authored-by: tizianoGuadagnino Co-authored-by: Ignacio Vizzo --- 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 From a9f527daacd350511fdb315e338d5e11315644df Mon Sep 17 00:00:00 2001 From: Tiziano Guadagnino <37455909+tizianoGuadagnino@users.noreply.github.com> Date: Sat, 20 Jul 2024 10:23:18 +0200 Subject: [PATCH 2/2] Revise Voxel functionalities in VoxelHashMap and Preprocessing (#362) * Unify Voxel functionalities, now everything comes from VoxelHashMap * This looks like an even better unification * Modernize VoxelDownsample * VoxelUtils is born, now everthing is liteally in one place, and we have some side effect code reduction * Macos does not like my include * Local style include * With move * As of benchmark * For each for the win * Remove unnecessary include * Const correcteness * Refresh changes --------- Co-authored-by: Luca Lobefaro Co-authored-by: Ignacio Vizzo --- cpp/kiss_icp/core/Preprocessing.cpp | 17 +---------- cpp/kiss_icp/core/Registration.cpp | 5 ++-- cpp/kiss_icp/core/VoxelHashMap.cpp | 4 ++- cpp/kiss_icp/core/VoxelHashMap.hpp | 14 ++------- cpp/kiss_icp/core/VoxelUtils.hpp | 45 +++++++++++++++++++++++++++++ 5 files changed, 54 insertions(+), 31 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 d9e1d805..0e74910b 100644 --- a/cpp/kiss_icp/core/Preprocessing.cpp +++ b/cpp/kiss_icp/core/Preprocessing.cpp @@ -31,22 +31,7 @@ #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); - } -}; - -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 +#include "VoxelUtils.hpp" namespace kiss_icp { std::vector VoxelDownsample(const std::vector &frame, diff --git a/cpp/kiss_icp/core/Registration.cpp b/cpp/kiss_icp/core/Registration.cpp index 137fa50c..b8acea10 100644 --- a/cpp/kiss_icp/core/Registration.cpp +++ b/cpp/kiss_icp/core/Registration.cpp @@ -36,6 +36,7 @@ #include #include "VoxelHashMap.hpp" +#include "VoxelUtils.hpp" namespace Eigen { using Matrix6d = Eigen::Matrix; @@ -54,7 +55,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) { @@ -70,7 +71,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 6b3b061b..e6445ebd 100644 --- a/cpp/kiss_icp/core/VoxelHashMap.cpp +++ b/cpp/kiss_icp/core/VoxelHashMap.cpp @@ -27,6 +27,8 @@ #include #include +#include "VoxelUtils.hpp" + namespace kiss_icp { std::vector VoxelHashMap::GetPoints(const std::vector &query_voxels) const { @@ -74,7 +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 = 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..a49b7096 100644 --- a/cpp/kiss_icp/core/VoxelHashMap.hpp +++ b/cpp/kiss_icp/core/VoxelHashMap.hpp @@ -32,9 +32,10 @@ #include #include +#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 points; @@ -43,12 +44,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), @@ -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(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); diff --git a/cpp/kiss_icp/core/VoxelUtils.hpp b/cpp/kiss_icp/core/VoxelUtils.hpp new file mode 100644 index 00000000..331a2ce1 --- /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))); +} + +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); + } +}; +} // namespace kiss_icp