From d1e2bfec80323c68b969ee2ab5324ac2cf1d6330 Mon Sep 17 00:00:00 2001 From: tizianoGuadagnino Date: Thu, 8 Aug 2024 10:44:11 +0200 Subject: [PATCH 1/5] Now we can search neighbors from the voxel hash --- cpp/kiss_icp/core/Registration.cpp | 37 +--------------------- cpp/kiss_icp/core/VoxelHashMap.cpp | 49 +++++++++++++++++++++++++++--- cpp/kiss_icp/core/VoxelHashMap.hpp | 2 +- 3 files changed, 46 insertions(+), 42 deletions(-) diff --git a/cpp/kiss_icp/core/Registration.cpp b/cpp/kiss_icp/core/Registration.cpp index b8acea10..ded9fb61 100644 --- a/cpp/kiss_icp/core/Registration.cpp +++ b/cpp/kiss_icp/core/Registration.cpp @@ -55,41 +55,6 @@ void TransformPoints(const Sophus::SE3d &T, std::vector &points [&](const auto &point) { return T * point; }); } -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) { - 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; -} - -std::tuple GetClosestNeighbor(const Eigen::Vector3d &point, - const kiss_icp::VoxelHashMap &voxel_map) { - // Convert the point to voxel coordinates - 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 - const auto &neighbors = voxel_map.GetPoints(query_voxels); - - // Find the nearest neighbor - Eigen::Vector3d closest_neighbor = Eigen::Vector3d::Zero(); - double closest_distance = std::numeric_limits::max(); - std::for_each(neighbors.cbegin(), neighbors.cend(), [&](const auto &neighbor) { - double distance = (neighbor - point).norm(); - if (distance < closest_distance) { - closest_neighbor = neighbor; - closest_distance = distance; - } - }); - return std::make_tuple(closest_neighbor, closest_distance); -} - Correspondences DataAssociation(const std::vector &points, const kiss_icp::VoxelHashMap &voxel_map, const double max_correspondance_distance) { @@ -105,7 +70,7 @@ Correspondences DataAssociation(const std::vector &points, [&](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] = GetClosestNeighbor(point, voxel_map); + const auto &[closest_neighbor, distance] = voxel_map.GetClosestNeighbor(point); if (distance < max_correspondance_distance) { res.emplace_back(point, closest_neighbor); } diff --git a/cpp/kiss_icp/core/VoxelHashMap.cpp b/cpp/kiss_icp/core/VoxelHashMap.cpp index a762bf40..2fc5ba5e 100644 --- a/cpp/kiss_icp/core/VoxelHashMap.cpp +++ b/cpp/kiss_icp/core/VoxelHashMap.cpp @@ -29,14 +29,27 @@ #include "VoxelUtils.hpp" -namespace kiss_icp { +namespace { +using kiss_icp::Voxel; -std::vector VoxelHashMap::GetPoints(const std::vector &query_voxels) const { +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; +} +std::vector GetPoints(const std::vector &query_voxels, + const kiss_icp::VoxelHashMap &voxel_map) { std::vector points; - points.reserve(query_voxels.size() * static_cast(max_points_per_voxel_)); + points.reserve(query_voxels.size() * static_cast(voxel_map.max_points_per_voxel_)); std::for_each(query_voxels.cbegin(), query_voxels.cend(), [&](const auto &query) { - auto search = map_.find(query); - if (search != map_.end()) { + auto search = voxel_map.map_.find(query); + if (search != voxel_map.map_.end()) { const auto &voxel_points = search.value(); points.insert(points.end(), voxel_points.cbegin(), voxel_points.cend()); } @@ -45,6 +58,32 @@ std::vector VoxelHashMap::GetPoints(const std::vector &q return points; } +} // namespace + +namespace kiss_icp { + +std::tuple VoxelHashMap::GetClosestNeighbor( + const Eigen::Vector3d &point) const { + // Convert the point to voxel coordinates + const auto &voxel = kiss_icp::PointToVoxel(point, voxel_size_); + // Get nearby voxels on the map + const auto &query_voxels = GetAdjacentVoxels(voxel); + // Extract the points contained within the neighborhood voxels + const auto &neighbors = GetPoints(query_voxels, *this); + + // Find the nearest neighbor + Eigen::Vector3d closest_neighbor = Eigen::Vector3d::Zero(); + double closest_distance = std::numeric_limits::max(); + std::for_each(neighbors.cbegin(), neighbors.cend(), [&](const auto &neighbor) { + double distance = (neighbor - point).norm(); + if (distance < closest_distance) { + closest_neighbor = neighbor; + closest_distance = distance; + } + }); + return std::make_tuple(closest_neighbor, closest_distance); +} + std::vector VoxelHashMap::Pointcloud() const { std::vector points; points.reserve(map_.size() * static_cast(max_points_per_voxel_)); diff --git a/cpp/kiss_icp/core/VoxelHashMap.hpp b/cpp/kiss_icp/core/VoxelHashMap.hpp index 98664cd4..0c677885 100644 --- a/cpp/kiss_icp/core/VoxelHashMap.hpp +++ b/cpp/kiss_icp/core/VoxelHashMap.hpp @@ -48,7 +48,7 @@ struct VoxelHashMap { void AddPoints(const std::vector &points); void RemovePointsFarFromLocation(const Eigen::Vector3d &origin); std::vector Pointcloud() const; - std::vector GetPoints(const std::vector &query_voxels) const; + std::tuple GetClosestNeighbor(const Eigen::Vector3d &point) const; double voxel_size_; double max_distance_; From 4594af4fe1f61a0846b4d76cba3debaab91aa8fd Mon Sep 17 00:00:00 2001 From: Tiziano Guadagnino <37455909+tizianoGuadagnino@users.noreply.github.com> Date: Thu, 8 Aug 2024 12:28:24 +0200 Subject: [PATCH 2/5] Integrate Nacho's Comment Co-authored-by: Ignacio Vizzo --- cpp/kiss_icp/core/VoxelHashMap.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/cpp/kiss_icp/core/VoxelHashMap.cpp b/cpp/kiss_icp/core/VoxelHashMap.cpp index 2fc5ba5e..78f1b5a3 100644 --- a/cpp/kiss_icp/core/VoxelHashMap.cpp +++ b/cpp/kiss_icp/core/VoxelHashMap.cpp @@ -65,7 +65,7 @@ namespace kiss_icp { std::tuple VoxelHashMap::GetClosestNeighbor( const Eigen::Vector3d &point) const { // Convert the point to voxel coordinates - const auto &voxel = kiss_icp::PointToVoxel(point, voxel_size_); + const auto &voxel = PointToVoxel(point, voxel_size_); // Get nearby voxels on the map const auto &query_voxels = GetAdjacentVoxels(voxel); // Extract the points contained within the neighborhood voxels From f6812b45808f2318f7d829c9c2acfea2f5c30772 Mon Sep 17 00:00:00 2001 From: tizianoGuadagnino Date: Thu, 8 Aug 2024 12:36:19 +0200 Subject: [PATCH 3/5] Remove useless function, we now can do everything from the voxel hash map --- cpp/kiss_icp/core/VoxelHashMap.cpp | 40 +++++++++++------------------- 1 file changed, 15 insertions(+), 25 deletions(-) diff --git a/cpp/kiss_icp/core/VoxelHashMap.cpp b/cpp/kiss_icp/core/VoxelHashMap.cpp index 2fc5ba5e..c0ff1513 100644 --- a/cpp/kiss_icp/core/VoxelHashMap.cpp +++ b/cpp/kiss_icp/core/VoxelHashMap.cpp @@ -43,42 +43,32 @@ std::vector GetAdjacentVoxels(const Voxel &voxel, int adjacent_voxels = 1 } return voxel_neighborhood; } -std::vector GetPoints(const std::vector &query_voxels, - const kiss_icp::VoxelHashMap &voxel_map) { - std::vector points; - points.reserve(query_voxels.size() * static_cast(voxel_map.max_points_per_voxel_)); - std::for_each(query_voxels.cbegin(), query_voxels.cend(), [&](const auto &query) { - auto search = voxel_map.map_.find(query); - if (search != voxel_map.map_.end()) { - const auto &voxel_points = search.value(); - points.insert(points.end(), voxel_points.cbegin(), voxel_points.cend()); - } - }); - points.shrink_to_fit(); - return points; -} - } // namespace namespace kiss_icp { std::tuple VoxelHashMap::GetClosestNeighbor( - const Eigen::Vector3d &point) const { + const Eigen::Vector3d &query) const { // Convert the point to voxel coordinates - const auto &voxel = kiss_icp::PointToVoxel(point, voxel_size_); + const auto &voxel = kiss_icp::PointToVoxel(query, voxel_size_); // Get nearby voxels on the map const auto &query_voxels = GetAdjacentVoxels(voxel); - // Extract the points contained within the neighborhood voxels - const auto &neighbors = GetPoints(query_voxels, *this); - // Find the nearest neighbor Eigen::Vector3d closest_neighbor = Eigen::Vector3d::Zero(); double closest_distance = std::numeric_limits::max(); - std::for_each(neighbors.cbegin(), neighbors.cend(), [&](const auto &neighbor) { - double distance = (neighbor - point).norm(); - if (distance < closest_distance) { - closest_neighbor = neighbor; - closest_distance = distance; + auto closer = [&query](const auto &lhs, const auto &rhs) { + return (lhs - query).norm() < (rhs - query).norm(); + }; + std::for_each(query_voxels.cbegin(), query_voxels.cend(), [&](const auto &qv) { + auto search = map_.find(qv); + if (search != map_.end()) { + const auto &points = search.value(); + const auto &neighbor = *std::min_element(points.cbegin(), points.cend(), closer); + double distance = (neighbor - query).norm(); + if (distance < closest_distance) { + closest_neighbor = neighbor; + closest_distance = distance; + } } }); return std::make_tuple(closest_neighbor, closest_distance); From bb58b89cb4be8a11d93166bc05182ba39a2560c0 Mon Sep 17 00:00:00 2001 From: tizianoGuadagnino Date: Thu, 8 Aug 2024 14:09:31 +0200 Subject: [PATCH 4/5] Renaming for consistency --- cpp/kiss_icp/core/VoxelHashMap.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/cpp/kiss_icp/core/VoxelHashMap.hpp b/cpp/kiss_icp/core/VoxelHashMap.hpp index 0c677885..48a7c9de 100644 --- a/cpp/kiss_icp/core/VoxelHashMap.hpp +++ b/cpp/kiss_icp/core/VoxelHashMap.hpp @@ -48,7 +48,7 @@ struct VoxelHashMap { void AddPoints(const std::vector &points); void RemovePointsFarFromLocation(const Eigen::Vector3d &origin); std::vector Pointcloud() const; - std::tuple GetClosestNeighbor(const Eigen::Vector3d &point) const; + std::tuple GetClosestNeighbor(const Eigen::Vector3d &query) const; double voxel_size_; double max_distance_; From ade1479ac966610f510bbaf43faa1a00398d7c1e Mon Sep 17 00:00:00 2001 From: tizianoGuadagnino Date: Thu, 8 Aug 2024 15:20:49 +0200 Subject: [PATCH 5/5] Integrate Nacho's and Ben comments --- cpp/kiss_icp/core/VoxelHashMap.cpp | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/cpp/kiss_icp/core/VoxelHashMap.cpp b/cpp/kiss_icp/core/VoxelHashMap.cpp index 9cc4edd9..bd2a5403 100644 --- a/cpp/kiss_icp/core/VoxelHashMap.cpp +++ b/cpp/kiss_icp/core/VoxelHashMap.cpp @@ -56,14 +56,14 @@ std::tuple VoxelHashMap::GetClosestNeighbor( // Find the nearest neighbor Eigen::Vector3d closest_neighbor = Eigen::Vector3d::Zero(); double closest_distance = std::numeric_limits::max(); - auto closer = [&query](const auto &lhs, const auto &rhs) { - return (lhs - query).norm() < (rhs - query).norm(); - }; - std::for_each(query_voxels.cbegin(), query_voxels.cend(), [&](const auto &qv) { - auto search = map_.find(qv); + 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(); - const auto &neighbor = *std::min_element(points.cbegin(), points.cend(), closer); + const Eigen::Vector3d &neighbor = *std::min_element( + points.cbegin(), points.cend(), [&](const auto &lhs, const auto &rhs) { + return (lhs - query).norm() < (rhs - query).norm(); + }); double distance = (neighbor - query).norm(); if (distance < closest_distance) { closest_neighbor = neighbor;