From 1fdd09b89d34ce01757aac0dc8ac1ec303383047 Mon Sep 17 00:00:00 2001 From: tizianoGuadagnino Date: Wed, 4 Dec 2024 09:31:07 +0100 Subject: [PATCH 1/6] Zero additional allocations --- cpp/kiss_icp/core/Registration.cpp | 23 ++++++----------------- cpp/kiss_icp/core/VoxelHashMap.cpp | 1 + 2 files changed, 7 insertions(+), 17 deletions(-) diff --git a/cpp/kiss_icp/core/Registration.cpp b/cpp/kiss_icp/core/Registration.cpp index ded9fb61..6bd1fb75 100644 --- a/cpp/kiss_icp/core/Registration.cpp +++ b/cpp/kiss_icp/core/Registration.cpp @@ -23,8 +23,10 @@ #include "Registration.hpp" #include +#include #include #include +#include #include #include @@ -44,7 +46,7 @@ using Matrix3_6d = Eigen::Matrix; using Vector6d = Eigen::Matrix; } // namespace Eigen -using Correspondences = std::vector>; +using Correspondences = tbb::concurrent_vector>; using LinearSystem = std::pair; namespace { @@ -61,30 +63,17 @@ Correspondences DataAssociation(const std::vector &points, using points_iterator = std::vector::const_iterator; Correspondences correspondences; correspondences.reserve(points.size()); - correspondences = tbb::parallel_reduce( + tbb::parallel_for( // Range tbb::blocked_range{points.cbegin(), points.cend()}, - // Identity - correspondences, - // 1st lambda: Parallel computation - [&](const tbb::blocked_range &r, Correspondences res) -> Correspondences { - res.reserve(r.size()); + [&](const tbb::blocked_range &r) { std::for_each(r.begin(), r.end(), [&](const auto &point) { const auto &[closest_neighbor, distance] = voxel_map.GetClosestNeighbor(point); if (distance < max_correspondance_distance) { - res.emplace_back(point, closest_neighbor); + correspondences.emplace_back(point, closest_neighbor); } }); - return res; - }, - // 2nd lambda: Parallel reduction - [](Correspondences a, const Correspondences &b) -> Correspondences { - a.insert(a.end(), // - std::make_move_iterator(b.cbegin()), // - std::make_move_iterator(b.cend())); - return a; }); - return correspondences; } diff --git a/cpp/kiss_icp/core/VoxelHashMap.cpp b/cpp/kiss_icp/core/VoxelHashMap.cpp index 4089a017..9f881409 100644 --- a/cpp/kiss_icp/core/VoxelHashMap.cpp +++ b/cpp/kiss_icp/core/VoxelHashMap.cpp @@ -34,6 +34,7 @@ using kiss_icp::Voxel; std::vector GetAdjacentVoxels(const Voxel &voxel, int adjacent_voxels = 1) { std::vector voxel_neighborhood; + voxel_neighborhood.reserve(27); for (int i = voxel.x() - adjacent_voxels; i < voxel.x() + adjacent_voxels + 1; ++i) { for (int j = voxel.y() - adjacent_voxels; j < voxel.y() + adjacent_voxels + 1; ++j) { for (int k = voxel.z() - adjacent_voxels; k < voxel.z() + adjacent_voxels + 1; ++k) { From a741512b65d57263a058a023c222c3793891ae69 Mon Sep 17 00:00:00 2001 From: tizianoGuadagnino Date: Wed, 4 Dec 2024 09:46:23 +0100 Subject: [PATCH 2/6] We know this shifts --- cpp/kiss_icp/core/VoxelHashMap.cpp | 23 +++++++---------------- 1 file changed, 7 insertions(+), 16 deletions(-) diff --git a/cpp/kiss_icp/core/VoxelHashMap.cpp b/cpp/kiss_icp/core/VoxelHashMap.cpp index 9f881409..b70d3dd6 100644 --- a/cpp/kiss_icp/core/VoxelHashMap.cpp +++ b/cpp/kiss_icp/core/VoxelHashMap.cpp @@ -31,19 +31,11 @@ namespace { using kiss_icp::Voxel; - -std::vector GetAdjacentVoxels(const Voxel &voxel, int adjacent_voxels = 1) { - std::vector voxel_neighborhood; - voxel_neighborhood.reserve(27); - for (int i = voxel.x() - adjacent_voxels; i < voxel.x() + adjacent_voxels + 1; ++i) { - for (int j = voxel.y() - adjacent_voxels; j < voxel.y() + adjacent_voxels + 1; ++j) { - for (int k = voxel.z() - adjacent_voxels; k < voxel.z() + adjacent_voxels + 1; ++k) { - voxel_neighborhood.emplace_back(i, j, k); - } - } - } - return voxel_neighborhood; -} +static const std::array shifts{ + {{0, 0, 0}, {1, 0, 0}, {-1, 0, 0}, {0, 1, 0}, {0, -1, 0}, {0, 0, 1}, {0, 0, -1}, + {1, 1, 0}, {1, -1, 0}, {-1, 1, 0}, {-1, -1, 0}, {1, 0, 1}, {1, 0, -1}, {-1, 0, 1}, + {-1, 0, -1}, {0, 1, 1}, {0, 1, -1}, {0, -1, 1}, {0, -1, -1}, {1, 1, 1}, {1, 1, -1}, + {1, -1, 1}, {1, -1, -1}, {-1, 1, 1}, {-1, 1, -1}, {-1, -1, 1}, {-1, -1, -1}}}; } // namespace namespace kiss_icp { @@ -52,12 +44,11 @@ std::tuple VoxelHashMap::GetClosestNeighbor( const Eigen::Vector3d &query) const { // Convert the point to voxel coordinates const auto &voxel = PointToVoxel(query, voxel_size_); - // Get nearby voxels on the map - const auto &query_voxels = GetAdjacentVoxels(voxel); // Find the nearest neighbor Eigen::Vector3d closest_neighbor = Eigen::Vector3d::Zero(); double closest_distance = std::numeric_limits::max(); - std::for_each(query_voxels.cbegin(), query_voxels.cend(), [&](const auto &query_voxel) { + std::for_each(shifts.cbegin(), shifts.cend(), [&](const auto &voxel_shift) { + const auto &query_voxel = voxel + voxel_shift; auto search = map_.find(query_voxel); if (search != map_.end()) { const auto &points = search.value(); From 5ae32838815f510b20fa6d060fce575bc7746f96 Mon Sep 17 00:00:00 2001 From: tizianoGuadagnino Date: Wed, 4 Dec 2024 18:53:23 +0100 Subject: [PATCH 3/6] Revert VoxelHashMap change -> Allocations go in a separate PR --- cpp/kiss_icp/core/VoxelHashMap.cpp | 22 +++++++++++++++------- 1 file changed, 15 insertions(+), 7 deletions(-) diff --git a/cpp/kiss_icp/core/VoxelHashMap.cpp b/cpp/kiss_icp/core/VoxelHashMap.cpp index b70d3dd6..4089a017 100644 --- a/cpp/kiss_icp/core/VoxelHashMap.cpp +++ b/cpp/kiss_icp/core/VoxelHashMap.cpp @@ -31,11 +31,18 @@ namespace { using kiss_icp::Voxel; -static const std::array shifts{ - {{0, 0, 0}, {1, 0, 0}, {-1, 0, 0}, {0, 1, 0}, {0, -1, 0}, {0, 0, 1}, {0, 0, -1}, - {1, 1, 0}, {1, -1, 0}, {-1, 1, 0}, {-1, -1, 0}, {1, 0, 1}, {1, 0, -1}, {-1, 0, 1}, - {-1, 0, -1}, {0, 1, 1}, {0, 1, -1}, {0, -1, 1}, {0, -1, -1}, {1, 1, 1}, {1, 1, -1}, - {1, -1, 1}, {1, -1, -1}, {-1, 1, 1}, {-1, 1, -1}, {-1, -1, 1}, {-1, -1, -1}}}; + +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) { + for (int j = voxel.y() - adjacent_voxels; j < voxel.y() + adjacent_voxels + 1; ++j) { + for (int k = voxel.z() - adjacent_voxels; k < voxel.z() + adjacent_voxels + 1; ++k) { + voxel_neighborhood.emplace_back(i, j, k); + } + } + } + return voxel_neighborhood; +} } // namespace namespace kiss_icp { @@ -44,11 +51,12 @@ std::tuple VoxelHashMap::GetClosestNeighbor( const Eigen::Vector3d &query) const { // Convert the point to voxel coordinates const auto &voxel = PointToVoxel(query, voxel_size_); + // Get nearby voxels on the map + const auto &query_voxels = GetAdjacentVoxels(voxel); // Find the nearest neighbor Eigen::Vector3d closest_neighbor = Eigen::Vector3d::Zero(); double closest_distance = std::numeric_limits::max(); - std::for_each(shifts.cbegin(), shifts.cend(), [&](const auto &voxel_shift) { - const auto &query_voxel = voxel + voxel_shift; + std::for_each(query_voxels.cbegin(), query_voxels.cend(), [&](const auto &query_voxel) { auto search = map_.find(query_voxel); if (search != map_.end()) { const auto &points = search.value(); From 4521454a0c4dc3b827a68153b047e8455b75c8c7 Mon Sep 17 00:00:00 2001 From: tizianoGuadagnino Date: Wed, 4 Dec 2024 18:56:29 +0100 Subject: [PATCH 4/6] Revert "Revert VoxelHashMap change -> Allocations go in a separate PR" This reverts commit 5ae32838815f510b20fa6d060fce575bc7746f96. --- cpp/kiss_icp/core/VoxelHashMap.cpp | 22 +++++++--------------- 1 file changed, 7 insertions(+), 15 deletions(-) diff --git a/cpp/kiss_icp/core/VoxelHashMap.cpp b/cpp/kiss_icp/core/VoxelHashMap.cpp index 4089a017..b70d3dd6 100644 --- a/cpp/kiss_icp/core/VoxelHashMap.cpp +++ b/cpp/kiss_icp/core/VoxelHashMap.cpp @@ -31,18 +31,11 @@ namespace { using 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) { - for (int j = voxel.y() - adjacent_voxels; j < voxel.y() + adjacent_voxels + 1; ++j) { - for (int k = voxel.z() - adjacent_voxels; k < voxel.z() + adjacent_voxels + 1; ++k) { - voxel_neighborhood.emplace_back(i, j, k); - } - } - } - return voxel_neighborhood; -} +static const std::array shifts{ + {{0, 0, 0}, {1, 0, 0}, {-1, 0, 0}, {0, 1, 0}, {0, -1, 0}, {0, 0, 1}, {0, 0, -1}, + {1, 1, 0}, {1, -1, 0}, {-1, 1, 0}, {-1, -1, 0}, {1, 0, 1}, {1, 0, -1}, {-1, 0, 1}, + {-1, 0, -1}, {0, 1, 1}, {0, 1, -1}, {0, -1, 1}, {0, -1, -1}, {1, 1, 1}, {1, 1, -1}, + {1, -1, 1}, {1, -1, -1}, {-1, 1, 1}, {-1, 1, -1}, {-1, -1, 1}, {-1, -1, -1}}}; } // namespace namespace kiss_icp { @@ -51,12 +44,11 @@ std::tuple VoxelHashMap::GetClosestNeighbor( const Eigen::Vector3d &query) const { // Convert the point to voxel coordinates const auto &voxel = PointToVoxel(query, voxel_size_); - // Get nearby voxels on the map - const auto &query_voxels = GetAdjacentVoxels(voxel); // Find the nearest neighbor Eigen::Vector3d closest_neighbor = Eigen::Vector3d::Zero(); double closest_distance = std::numeric_limits::max(); - std::for_each(query_voxels.cbegin(), query_voxels.cend(), [&](const auto &query_voxel) { + std::for_each(shifts.cbegin(), shifts.cend(), [&](const auto &voxel_shift) { + const auto &query_voxel = voxel + voxel_shift; auto search = map_.find(query_voxel); if (search != map_.end()) { const auto &points = search.value(); From 6e56ac740034110685473f5757cab55f2d1fe2a6 Mon Sep 17 00:00:00 2001 From: tizianoGuadagnino Date: Wed, 4 Dec 2024 18:57:11 +0100 Subject: [PATCH 5/6] Revert concurrent vector change --- cpp/kiss_icp/core/Registration.cpp | 23 +++++++++++++++++------ 1 file changed, 17 insertions(+), 6 deletions(-) diff --git a/cpp/kiss_icp/core/Registration.cpp b/cpp/kiss_icp/core/Registration.cpp index 6bd1fb75..ded9fb61 100644 --- a/cpp/kiss_icp/core/Registration.cpp +++ b/cpp/kiss_icp/core/Registration.cpp @@ -23,10 +23,8 @@ #include "Registration.hpp" #include -#include #include #include -#include #include #include @@ -46,7 +44,7 @@ using Matrix3_6d = Eigen::Matrix; using Vector6d = Eigen::Matrix; } // namespace Eigen -using Correspondences = tbb::concurrent_vector>; +using Correspondences = std::vector>; using LinearSystem = std::pair; namespace { @@ -63,17 +61,30 @@ Correspondences DataAssociation(const std::vector &points, using points_iterator = std::vector::const_iterator; Correspondences correspondences; correspondences.reserve(points.size()); - tbb::parallel_for( + correspondences = tbb::parallel_reduce( // Range tbb::blocked_range{points.cbegin(), points.cend()}, - [&](const tbb::blocked_range &r) { + // Identity + correspondences, + // 1st lambda: Parallel computation + [&](const tbb::blocked_range &r, Correspondences res) -> Correspondences { + res.reserve(r.size()); std::for_each(r.begin(), r.end(), [&](const auto &point) { const auto &[closest_neighbor, distance] = voxel_map.GetClosestNeighbor(point); if (distance < max_correspondance_distance) { - correspondences.emplace_back(point, closest_neighbor); + res.emplace_back(point, closest_neighbor); } }); + return res; + }, + // 2nd lambda: Parallel reduction + [](Correspondences a, const Correspondences &b) -> Correspondences { + a.insert(a.end(), // + std::make_move_iterator(b.cbegin()), // + std::make_move_iterator(b.cend())); + return a; }); + return correspondences; } From a0946951b0142cdf5522fb86fe7a1db2e992bf24 Mon Sep 17 00:00:00 2001 From: tizianoGuadagnino Date: Thu, 5 Dec 2024 12:40:57 +0100 Subject: [PATCH 6/6] Some renaming for clarity --- cpp/kiss_icp/core/VoxelHashMap.cpp | 15 +++++++++------ 1 file changed, 9 insertions(+), 6 deletions(-) diff --git a/cpp/kiss_icp/core/VoxelHashMap.cpp b/cpp/kiss_icp/core/VoxelHashMap.cpp index b70d3dd6..69a87ec4 100644 --- a/cpp/kiss_icp/core/VoxelHashMap.cpp +++ b/cpp/kiss_icp/core/VoxelHashMap.cpp @@ -24,6 +24,7 @@ #include #include +#include #include #include @@ -31,11 +32,13 @@ namespace { using kiss_icp::Voxel; -static const std::array shifts{ - {{0, 0, 0}, {1, 0, 0}, {-1, 0, 0}, {0, 1, 0}, {0, -1, 0}, {0, 0, 1}, {0, 0, -1}, - {1, 1, 0}, {1, -1, 0}, {-1, 1, 0}, {-1, -1, 0}, {1, 0, 1}, {1, 0, -1}, {-1, 0, 1}, - {-1, 0, -1}, {0, 1, 1}, {0, 1, -1}, {0, -1, 1}, {0, -1, -1}, {1, 1, 1}, {1, 1, -1}, - {1, -1, 1}, {1, -1, -1}, {-1, 1, 1}, {-1, 1, -1}, {-1, -1, 1}, {-1, -1, -1}}}; +static const std::array voxel_shifts{ + {Voxel{0, 0, 0}, Voxel{1, 0, 0}, Voxel{-1, 0, 0}, Voxel{0, 1, 0}, Voxel{0, -1, 0}, + Voxel{0, 0, 1}, Voxel{0, 0, -1}, Voxel{1, 1, 0}, Voxel{1, -1, 0}, Voxel{-1, 1, 0}, + Voxel{-1, -1, 0}, Voxel{1, 0, 1}, Voxel{1, 0, -1}, Voxel{-1, 0, 1}, Voxel{-1, 0, -1}, + Voxel{0, 1, 1}, Voxel{0, 1, -1}, Voxel{0, -1, 1}, Voxel{0, -1, -1}, Voxel{1, 1, 1}, + Voxel{1, 1, -1}, Voxel{1, -1, 1}, Voxel{1, -1, -1}, Voxel{-1, 1, 1}, Voxel{-1, 1, -1}, + Voxel{-1, -1, 1}, Voxel{-1, -1, -1}}}; } // namespace namespace kiss_icp { @@ -47,7 +50,7 @@ std::tuple VoxelHashMap::GetClosestNeighbor( // Find the nearest neighbor Eigen::Vector3d closest_neighbor = Eigen::Vector3d::Zero(); double closest_distance = std::numeric_limits::max(); - std::for_each(shifts.cbegin(), shifts.cend(), [&](const auto &voxel_shift) { + std::for_each(voxel_shifts.cbegin(), voxel_shifts.cend(), [&](const auto &voxel_shift) { const auto &query_voxel = voxel + voxel_shift; auto search = map_.find(query_voxel); if (search != map_.end()) {