From 1c28d1776eca0c91c561d31a605addb31176e3d3 Mon Sep 17 00:00:00 2001 From: Ignacio Vizzo Date: Mon, 22 Jul 2024 14:42:17 +0200 Subject: [PATCH] Style changes in voxelization and preprocessing utilities (#364) * 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 * Add style changes for a separate PR * Add missing headers * Remove more raw for loops * Move Voxeldownsample into VoxelUtils * Cmake needs to know * Miss cpp * Remove duplicate * cmake-format * Format * We never used noexcept anywhere, why adding it now? * We always use frame, if we want to move point_cloud, let's do it in a seprate PR * put back whitespaces * reduce diff --------- Co-authored-by: tizianoGuadagnino Co-authored-by: Luca Lobefaro Co-authored-by: Benedikt Mersch --- cpp/kiss_icp/core/CMakeLists.txt | 3 ++- cpp/kiss_icp/core/Preprocessing.cpp | 18 ------------------ cpp/kiss_icp/core/Preprocessing.hpp | 3 --- cpp/kiss_icp/core/VoxelHashMap.cpp | 12 ++++-------- cpp/kiss_icp/core/VoxelHashMap.hpp | 2 +- cpp/kiss_icp/core/VoxelUtils.cpp | 23 +++++++++++++++++++++++ cpp/kiss_icp/core/VoxelUtils.hpp | 14 ++++++++++---- 7 files changed, 40 insertions(+), 35 deletions(-) create mode 100644 cpp/kiss_icp/core/VoxelUtils.cpp diff --git a/cpp/kiss_icp/core/CMakeLists.txt b/cpp/kiss_icp/core/CMakeLists.txt index ce83625a..fc627bf4 100644 --- a/cpp/kiss_icp/core/CMakeLists.txt +++ b/cpp/kiss_icp/core/CMakeLists.txt @@ -21,6 +21,7 @@ # OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE # SOFTWARE. add_library(kiss_icp_core STATIC) -target_sources(kiss_icp_core PRIVATE Registration.cpp Deskew.cpp VoxelHashMap.cpp Preprocessing.cpp Threshold.cpp) +target_sources(kiss_icp_core PRIVATE Registration.cpp Deskew.cpp VoxelHashMap.cpp VoxelUtils.cpp Preprocessing.cpp + Threshold.cpp) target_link_libraries(kiss_icp_core PUBLIC Eigen3::Eigen tsl::robin_map TBB::tbb Sophus::Sophus) set_global_target_properties(kiss_icp_core) diff --git a/cpp/kiss_icp/core/Preprocessing.cpp b/cpp/kiss_icp/core/Preprocessing.cpp index 0e74910b..d7975497 100644 --- a/cpp/kiss_icp/core/Preprocessing.cpp +++ b/cpp/kiss_icp/core/Preprocessing.cpp @@ -34,24 +34,6 @@ #include "VoxelUtils.hpp" 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; -} - std::vector Preprocess(const std::vector &frame, double max_range, double min_range) { diff --git a/cpp/kiss_icp/core/Preprocessing.hpp b/cpp/kiss_icp/core/Preprocessing.hpp index 1a8ae1ee..3cff777f 100644 --- a/cpp/kiss_icp/core/Preprocessing.hpp +++ b/cpp/kiss_icp/core/Preprocessing.hpp @@ -37,7 +37,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/VoxelHashMap.cpp b/cpp/kiss_icp/core/VoxelHashMap.cpp index e6445ebd..c9b7313b 100644 --- a/cpp/kiss_icp/core/VoxelHashMap.cpp +++ b/cpp/kiss_icp/core/VoxelHashMap.cpp @@ -37,9 +37,8 @@ std::vector VoxelHashMap::GetPoints(const std::vector &q std::for_each(query_voxels.cbegin(), query_voxels.cend(), [&](const auto &query) { auto search = map_.find(query); if (search != map_.end()) { - for (const auto &point : search->second.points) { - points.emplace_back(point); - } + const auto &voxel_points = search->second.points; + points.insert(points.end(), voxel_points.cbegin(), voxel_points.cend()); } }); points.shrink_to_fit(); @@ -50,11 +49,8 @@ std::vector VoxelHashMap::Pointcloud() const { std::vector points; points.reserve(map_.size() * static_cast(max_points_per_voxel_)); std::for_each(map_.cbegin(), map_.cend(), [&](const auto &map_element) { - const auto &[voxel, voxel_block] = map_element; - (void)voxel; - for (const auto &point : voxel_block.points) { - points.emplace_back(point); - } + const auto &voxel_points = map_element.second.points; + points.insert(points.end(), voxel_points.cbegin(), voxel_points.cend()); }); points.shrink_to_fit(); return points; diff --git a/cpp/kiss_icp/core/VoxelHashMap.hpp b/cpp/kiss_icp/core/VoxelHashMap.hpp index a49b7096..1b3dbb59 100644 --- a/cpp/kiss_icp/core/VoxelHashMap.hpp +++ b/cpp/kiss_icp/core/VoxelHashMap.hpp @@ -62,6 +62,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.cpp b/cpp/kiss_icp/core/VoxelUtils.cpp new file mode 100644 index 00000000..f8f8ea8c --- /dev/null +++ b/cpp/kiss_icp/core/VoxelUtils.cpp @@ -0,0 +1,23 @@ +#include "VoxelUtils.hpp" + +#include + +namespace kiss_icp { + +std::vector VoxelDownsample(const std::vector &frame, + const double voxel_size) { + 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; + 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; +} + +} // namespace kiss_icp diff --git a/cpp/kiss_icp/core/VoxelUtils.hpp b/cpp/kiss_icp/core/VoxelUtils.hpp index 331a2ce1..580a2521 100644 --- a/cpp/kiss_icp/core/VoxelUtils.hpp +++ b/cpp/kiss_icp/core/VoxelUtils.hpp @@ -25,21 +25,27 @@ #include #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 { +/// Voxelize a point cloud keeping the original coordinates +std::vector VoxelDownsample(const std::vector &frame, + const double voxel_size); + +} // namespace kiss_icp + +template <> +struct std::hash { + std::size_t operator()(const kiss_icp::Voxel &voxel) const { const uint32_t *vec = reinterpret_cast(voxel.data()); return (vec[0] * 73856093 ^ vec[1] * 19349669 ^ vec[2] * 83492791); } }; -} // namespace kiss_icp