diff --git a/cpp/map_closures/AlignRansac2D.cpp b/cpp/map_closures/AlignRansac2D.cpp index 8ff28ff..f6f9ea3 100644 --- a/cpp/map_closures/AlignRansac2D.cpp +++ b/cpp/map_closures/AlignRansac2D.cpp @@ -38,7 +38,7 @@ Eigen::Isometry2d KabschUmeyamaAlignment2D( const std::vector &keypoint_pairs) { map_closures::PointPair mean = std::reduce(keypoint_pairs.cbegin(), keypoint_pairs.cend(), map_closures::PointPair(), - [](auto lhs, const auto &rhs) { + [](map_closures::PointPair lhs, const map_closures::PointPair &rhs) { lhs.ref += rhs.ref; lhs.query += rhs.query; return lhs; @@ -47,7 +47,7 @@ Eigen::Isometry2d KabschUmeyamaAlignment2D( mean.ref /= static_cast(keypoint_pairs.size()); Eigen::Matrix2d covariance_matrix = std::transform_reduce( keypoint_pairs.cbegin(), keypoint_pairs.cend(), Eigen::Matrix2d().setZero(), - std::plus(), [&](const auto &keypoint_pair) { + std::plus(), [&](const map_closures::PointPair &keypoint_pair) { return (keypoint_pair.ref - mean.ref) * ((keypoint_pair.query - mean.query).transpose()); }); @@ -93,11 +93,11 @@ std::pair RansacAlignment2D( std::sample(keypoint_pairs.begin(), keypoint_pairs.end(), sample_keypoint_pairs.begin(), 2, std::mt19937{std::random_device{}()}); - const auto &T = KabschUmeyamaAlignment2D(sample_keypoint_pairs); + const Eigen::Isometry2d &T = KabschUmeyamaAlignment2D(sample_keypoint_pairs); int index = 0; std::for_each(keypoint_pairs.cbegin(), keypoint_pairs.cend(), - [&](const auto &keypoint_pair) { + [&](const PointPair &keypoint_pair) { if ((T * keypoint_pair.ref - keypoint_pair.query).norm() < inliers_distance_threshold) inlier_indices.emplace_back(index); diff --git a/cpp/map_closures/CMakeLists.txt b/cpp/map_closures/CMakeLists.txt index 8032645..c008659 100644 --- a/cpp/map_closures/CMakeLists.txt +++ b/cpp/map_closures/CMakeLists.txt @@ -22,7 +22,7 @@ # SOFTWARE. add_library(map_closures STATIC) -target_sources(map_closures PRIVATE DensityMap.cpp AlignRansac2D.cpp GroundAlign.cpp +target_sources(map_closures PRIVATE VoxelMap.cpp DensityMap.cpp AlignRansac2D.cpp GroundAlign.cpp MapClosures.cpp) target_include_directories(map_closures PUBLIC ${hbst_SOURCE_DIR} ${CMAKE_CURRENT_SOURCE_DIR}/..) target_link_libraries(map_closures PUBLIC Eigen3::Eigen ${OpenCV_LIBS} Sophus::Sophus) diff --git a/cpp/map_closures/DensityMap.cpp b/cpp/map_closures/DensityMap.cpp index aa95271..36a3f92 100644 --- a/cpp/map_closures/DensityMap.cpp +++ b/cpp/map_closures/DensityMap.cpp @@ -65,18 +65,18 @@ DensityMap GenerateDensityMap(const std::vector &pcd, }; std::vector pixels(pcd.size()); std::transform(pcd.cbegin(), pcd.cend(), pixels.begin(), [&](const Eigen::Vector3d &point) { - const auto &pixel = Discretize2D(point); + const Eigen::Array2i &pixel = Discretize2D(point); lower_bound_coordinates = lower_bound_coordinates.min(pixel); upper_bound_coordinates = upper_bound_coordinates.max(pixel); return pixel; }); - const auto rows_and_columns = upper_bound_coordinates - lower_bound_coordinates; - const auto n_rows = rows_and_columns.x() + 1; - const auto n_cols = rows_and_columns.y() + 1; + const Eigen::Array2i rows_and_columns = upper_bound_coordinates - lower_bound_coordinates; + const int n_rows = rows_and_columns.x() + 1; + const int n_cols = rows_and_columns.y() + 1; cv::Mat counting_grid(n_rows, n_cols, CV_64FC1, 0.0); - std::for_each(pixels.cbegin(), pixels.cend(), [&](const auto &pixel) { - const auto px = pixel - lower_bound_coordinates; + std::for_each(pixels.cbegin(), pixels.cend(), [&](const Eigen::Array2i &pixel) { + const Eigen::Array2i px = pixel - lower_bound_coordinates; counting_grid.at(px.x(), px.y()) += 1; max_points = std::max(max_points, counting_grid.at(px.x(), px.y())); min_points = std::min(min_points, counting_grid.at(px.x(), px.y())); @@ -84,7 +84,7 @@ DensityMap GenerateDensityMap(const std::vector &pcd, DensityMap density_map(n_rows, n_cols, density_map_resolution, lower_bound_coordinates); counting_grid.forEach([&](const double count, const int pos[]) { - auto density = (count - min_points) / (max_points - min_points); + double density = (count - min_points) / (max_points - min_points); density = density > density_threshold ? density : 0.0; density_map(pos[0], pos[1]) = static_cast(255 * density); }); diff --git a/cpp/map_closures/DensityMap.hpp b/cpp/map_closures/DensityMap.hpp index 95df5e3..0f3d84b 100644 --- a/cpp/map_closures/DensityMap.hpp +++ b/cpp/map_closures/DensityMap.hpp @@ -38,7 +38,7 @@ struct DensityMap { DensityMap(DensityMap &&other) = default; DensityMap &operator=(DensityMap &&other) = default; DensityMap &operator=(const DensityMap &other) = delete; - inline auto &operator()(const int x, const int y) { return grid.at(x, y); } + uint8_t &operator()(const int x, const int y) { return grid.at(x, y); } Eigen::Vector2i lower_bound; double resolution; cv::Mat grid; diff --git a/cpp/map_closures/GroundAlign.cpp b/cpp/map_closures/GroundAlign.cpp index 188d97b..3e5e8c9 100644 --- a/cpp/map_closures/GroundAlign.cpp +++ b/cpp/map_closures/GroundAlign.cpp @@ -24,6 +24,8 @@ #include "GroundAlign.hpp" #include +#include +#include #include #include #include @@ -32,6 +34,9 @@ #include namespace { +using Vector3dVector = std::vector; +using LinearSystem = std::pair; + struct PixelHash { size_t operator()(const Eigen::Vector2i &pixel) const { const uint32_t *vec = reinterpret_cast(pixel.data()); @@ -39,15 +44,82 @@ struct PixelHash { } }; -void TransformPoints(const Sophus::SE3d &T, std::vector &pointcloud) { +void TransformPoints(const Sophus::SE3d &T, Vector3dVector &pointcloud) { std::transform(pointcloud.cbegin(), pointcloud.cend(), pointcloud.begin(), - [&](const auto &point) { return T * point; }); + [&](const Eigen::Vector3d &point) { return T * point; }); } -using LinearSystem = std::pair; -LinearSystem BuildLinearSystem(const std::vector &points, - const double resolution) { - auto compute_jacobian_and_residual = [](const auto &point) { +struct VoxelMeanAndNormal { + Eigen::Vector3d mean; + Eigen::Vector3d normal; +}; + +auto PointToPixel = [](const Eigen::Vector3d &pt) -> Eigen::Vector2i { + return Eigen::Vector2i(static_cast(std::floor(pt.x())), + static_cast(std::floor(pt.y()))); +}; + +std::pair SampleGroundPoints(const Vector3dVector &voxel_means, + const Vector3dVector &voxel_normals) { + std::unordered_map lowest_voxel_hash_map; + + for (size_t index = 0; index < voxel_means.size(); ++index) { + const Eigen::Vector3d &mean = voxel_means[index]; + const Eigen::Vector3d &normal = voxel_normals[index]; + const Eigen::Vector2i pixel = PointToPixel(mean); + + auto it = lowest_voxel_hash_map.find(pixel); + if (it == lowest_voxel_hash_map.end()) { + lowest_voxel_hash_map.emplace(pixel, VoxelMeanAndNormal{mean, normal}); + } else if (mean.z() < it->second.mean.z()) { + it->second = VoxelMeanAndNormal{mean, normal}; + } + } + + std::vector low_lying_voxels(lowest_voxel_hash_map.size()); + std::transform(lowest_voxel_hash_map.cbegin(), lowest_voxel_hash_map.cend(), + low_lying_voxels.begin(), [](const auto &entry) { return entry.second; }); + + const Eigen::Matrix3d covariance_matrix = + std::transform_reduce(low_lying_voxels.cbegin(), low_lying_voxels.cend(), + Eigen::Matrix3d().setZero(), std::plus(), + [&](const VoxelMeanAndNormal &voxel) { + return voxel.normal * voxel.normal.transpose(); + }) / + static_cast(low_lying_voxels.size() - 1); + + Eigen::SelfAdjointEigenSolver eigensolver(covariance_matrix); + Eigen::Vector3d largest_eigenvector = eigensolver.eigenvectors().col(2); + largest_eigenvector = + (largest_eigenvector.z() < 0) ? -1.0 * largest_eigenvector : largest_eigenvector; + const Eigen::Vector3d axis = largest_eigenvector.cross(Eigen::Vector3d(0.0, 0.0, 1.0)); + const double angle = std::acos(std::clamp(largest_eigenvector.z(), -1.0, 1.0)); + const double axis_norm = axis.norm(); + + const Eigen::Matrix3d R = axis_norm > 1e-3 + ? Eigen::AngleAxisd(angle, axis / axis_norm).toRotationMatrix() + : Eigen::Matrix3d::Identity(); + + Eigen::Vector3d ground_centroid(0.0, 0.0, 0.0); + Vector3dVector ground_samples; + ground_samples.reserve(low_lying_voxels.size()); + std::for_each(low_lying_voxels.cbegin(), low_lying_voxels.cend(), + [&](const VoxelMeanAndNormal &voxel) { + if (std::abs(voxel.normal.dot(largest_eigenvector)) > 0.95) { + ground_centroid += voxel.mean; + ground_samples.emplace_back(voxel.mean); + } + }); + ground_samples.shrink_to_fit(); + ground_centroid /= static_cast(ground_samples.size()); + + const double z_shift = R.row(2) * ground_centroid; + const Sophus::SE3d T_init(R, Eigen::Vector3d(0.0, 0.0, -1.0 * z_shift)); + return std::make_pair(ground_samples, T_init); +} + +LinearSystem BuildLinearSystem(const Vector3dVector &points) { + auto compute_jacobian_and_residual = [](const Eigen::Vector3d &point) { const double residual = point.z(); Eigen::Matrix J; J(0, 0) = 1.0; @@ -65,9 +137,9 @@ LinearSystem BuildLinearSystem(const std::vector &points, const auto &[H, b] = std::transform_reduce(points.cbegin(), points.cend(), LinearSystem(Eigen::Matrix3d::Zero(), Eigen::Vector3d::Zero()), - sum_linear_systems, [&](const auto &point) { + sum_linear_systems, [&](const Eigen::Vector3d &point) { const auto &[J, residual] = compute_jacobian_and_residual(point); - const double w = std::abs(residual) <= resolution ? 1.0 : 0.0; + const double w = std::exp(-1.0 * residual * residual); return LinearSystem(J.transpose() * w * J, // JTJ J.transpose() * w * residual); // JTr }); @@ -75,45 +147,21 @@ LinearSystem BuildLinearSystem(const std::vector &points, } static constexpr double convergence_threshold = 1e-3; -static constexpr int max_iterations = 20; - -std::vector ComputeLowestPoints(const std::vector &pointcloud, - const double resolution) { - std::unordered_map lowest_point_hash_map; - auto PointToPixel = [&resolution](const Eigen::Vector3d &pt) -> Eigen::Vector2i { - return Eigen::Vector2i(static_cast(std::floor(pt.x() / resolution)), - static_cast(std::floor(pt.y() / resolution))); - }; - - std::for_each(pointcloud.cbegin(), pointcloud.cend(), [&](const Eigen::Vector3d &point) { - const auto &pixel = PointToPixel(point); - if (lowest_point_hash_map.find(pixel) == lowest_point_hash_map.cend()) { - lowest_point_hash_map.emplace(pixel, point); - } else if (point.z() < lowest_point_hash_map[pixel].z()) { - lowest_point_hash_map[pixel] = point; - } - }); - - std::vector low_lying_points(lowest_point_hash_map.size()); - std::transform(lowest_point_hash_map.cbegin(), lowest_point_hash_map.cend(), - low_lying_points.begin(), [](const auto &entry) { return entry.second; }); - return low_lying_points; -} +static constexpr int max_iterations = 10; } // namespace namespace map_closures { -Eigen::Matrix4d AlignToLocalGround(const std::vector &pointcloud, - const double resolution) { - Sophus::SE3d T = Sophus::SE3d(Eigen::Matrix3d::Identity(), Eigen::Vector3d::Zero()); - auto low_lying_points = ComputeLowestPoints(pointcloud, resolution); - +Eigen::Matrix4d AlignToLocalGround(const Vector3dVector &voxel_means, + const Vector3dVector &voxel_normals) { + auto [ground_samples, T] = SampleGroundPoints(voxel_means, voxel_normals); + TransformPoints(T, ground_samples); for (int iters = 0; iters < max_iterations; iters++) { - const auto &[H, b] = BuildLinearSystem(low_lying_points, resolution); + const auto &[H, b] = BuildLinearSystem(ground_samples); const Eigen::Vector3d &dx = H.ldlt().solve(-b); Eigen::Matrix se3 = Eigen::Matrix::Zero(); se3.block<3, 1>(2, 0) = dx; Sophus::SE3d estimation(Sophus::SE3d::exp(se3)); - TransformPoints(estimation, low_lying_points); + TransformPoints(estimation, ground_samples); T = estimation * T; if (dx.norm() < convergence_threshold) break; } diff --git a/cpp/map_closures/GroundAlign.hpp b/cpp/map_closures/GroundAlign.hpp index 04067db..d88d11a 100644 --- a/cpp/map_closures/GroundAlign.hpp +++ b/cpp/map_closures/GroundAlign.hpp @@ -27,6 +27,6 @@ #include namespace map_closures { -Eigen::Matrix4d AlignToLocalGround(const std::vector &pointcloud, - const double resolution); +Eigen::Matrix4d AlignToLocalGround(const std::vector &voxel_means, + const std::vector &voxel_normals); } // namespace map_closures diff --git a/cpp/map_closures/MapClosures.cpp b/cpp/map_closures/MapClosures.cpp index 1d583e6..a6f8c62 100644 --- a/cpp/map_closures/MapClosures.cpp +++ b/cpp/map_closures/MapClosures.cpp @@ -28,6 +28,7 @@ #include #include #include +#include #include #include @@ -39,8 +40,7 @@ namespace { static constexpr int min_no_of_matches = 2; static constexpr int no_of_local_maps_to_skip = 3; -static constexpr double ground_alignment_resolution = 5.0; -static constexpr double self_similarity_threshold = 35.0; +static constexpr int self_similarity_threshold = 35; // fixed parameters for OpenCV ORB Features static constexpr float scale_factor = 1.00f; @@ -68,8 +68,10 @@ MapClosures::MapClosures(const Config &config) : config_(config) { } void MapClosures::MatchAndAddToDatabase(const int id, - const std::vector &local_map) { - const Eigen::Matrix4d &T_ground = AlignToLocalGround(local_map, ground_alignment_resolution); + const std::vector &local_map, + const std::vector &voxel_means, + const std::vector &voxel_normals) { + const Eigen::Matrix4d T_ground = AlignToLocalGround(voxel_means, voxel_normals); DensityMap density_map = GenerateDensityMap(local_map, T_ground, config_.density_map_resolution, config_.density_threshold); cv::Mat orb_descriptors; @@ -84,26 +86,27 @@ void MapClosures::MatchAndAddToDatabase(const int id, self_matches.reserve(orb_keypoints.size()); self_matcher.knnMatch(orb_descriptors, orb_descriptors, self_matches, 2); - density_maps_.emplace(id, std::move(density_map)); - ground_alignments_.emplace(id, T_ground); - std::vector hbst_matchable; hbst_matchable.reserve(orb_descriptors.rows); - std::for_each(self_matches.cbegin(), self_matches.cend(), [&](const auto &self_match) { - if (self_match[1].distance > self_similarity_threshold) { - const auto index_descriptor = self_match[0].queryIdx; - cv::KeyPoint keypoint = orb_keypoints[index_descriptor]; - keypoint.pt.x = keypoint.pt.x + static_cast(density_map.lower_bound.y()); - keypoint.pt.y = keypoint.pt.y + static_cast(density_map.lower_bound.x()); - hbst_matchable.emplace_back( - new Matchable(keypoint, orb_descriptors.row(index_descriptor), id)); - } - }); + std::for_each( + self_matches.cbegin(), self_matches.cend(), [&](const std::vector &self_match) { + if (self_match[1].distance > self_similarity_threshold) { + const auto index_descriptor = self_match[0].queryIdx; + cv::KeyPoint &keypoint = orb_keypoints[index_descriptor]; + keypoint.pt.x = keypoint.pt.x + static_cast(density_map.lower_bound.y()); + keypoint.pt.y = keypoint.pt.y + static_cast(density_map.lower_bound.x()); + hbst_matchable.emplace_back( + new Matchable(keypoint, orb_descriptors.row(index_descriptor), id)); + } + }); hbst_matchable.shrink_to_fit(); hbst_binary_tree_->matchAndAdd(hbst_matchable, descriptor_matches_, config_.hamming_distance_threshold, srrg_hbst::SplittingStrategy::SplitEven); + + density_maps_.emplace(id, std::move(density_map)); + ground_alignments_.emplace(id, T_ground); } ClosureCandidate MapClosures::ValidateClosure(const int reference_id, const int query_id) const { @@ -115,10 +118,10 @@ ClosureCandidate MapClosures::ValidateClosure(const int reference_id, const int std::vector keypoint_pairs(num_matches); std::transform(matches.cbegin(), matches.cend(), keypoint_pairs.begin(), [&](const Tree::Match &match) { - auto query_point = + const Eigen::Vector2d query_point = Eigen::Vector2d(match.object_query.pt.y, match.object_query.pt.x); - auto ref_point = Eigen::Vector2d(match.object_references[0].pt.y, - match.object_references[0].pt.x); + const Eigen::Vector2d ref_point = Eigen::Vector2d( + match.object_references[0].pt.y, match.object_references[0].pt.x); return PointPair(ref_point, query_point); }); @@ -134,54 +137,35 @@ ClosureCandidate MapClosures::ValidateClosure(const int reference_id, const int return closure; } -ClosureCandidate MapClosures::GetBestClosure(const int query_id, - const std::vector &local_map) { - MatchAndAddToDatabase(query_id, local_map); - auto compare_closure_candidates = [](ClosureCandidate a, - const ClosureCandidate &b) -> ClosureCandidate { - return a.number_of_inliers > b.number_of_inliers ? a : b; - }; - auto is_far_enough = [](const int ref_id, const int query_id) { - return std::abs(query_id - ref_id) > no_of_local_maps_to_skip; - }; - const auto &closure = std::transform_reduce( - descriptor_matches_.cbegin(), descriptor_matches_.cend(), ClosureCandidate(), - compare_closure_candidates, [&](const auto &descriptor_match) { - const auto ref_id = static_cast(descriptor_match.first); - return is_far_enough(ref_id, query_id) ? ValidateClosure(ref_id, query_id) - : ClosureCandidate(); - }); - return closure; -} - std::vector MapClosures::GetTopKClosures( - const int query_id, const std::vector &local_map, const int k) { - MatchAndAddToDatabase(query_id, local_map); + const int query_id, + const std::vector &local_map, + const std::vector &voxel_means, + const std::vector &voxel_normals, + const int k) { + MatchAndAddToDatabase(query_id, local_map, voxel_means, voxel_normals); auto compare_closure_candidates = [](const ClosureCandidate &a, const ClosureCandidate &b) { return a.number_of_inliers >= b.number_of_inliers; }; - auto is_far_enough = [](const int ref_id, const int query_id) { - return std::abs(query_id - ref_id) > no_of_local_maps_to_skip; - }; std::vector closures; - closures.reserve(query_id); - std::for_each(descriptor_matches_.cbegin(), descriptor_matches_.cend(), - [&](const auto &descriptor_match) { - const auto ref_id = static_cast(descriptor_match.first); - if (is_far_enough(ref_id, query_id)) { - const ClosureCandidate &closure = ValidateClosure(ref_id, query_id); - if (closure.number_of_inliers > min_no_of_matches) { - closures.emplace_back(closure); - } - } - }); - closures.shrink_to_fit(); - - if (k != -1) { - std::sort(closures.begin(), closures.end(), compare_closure_candidates); - closures.resize(std::min(k, static_cast(closures.size()))); + const int num_of_potential_closures = query_id - no_of_local_maps_to_skip; + if (num_of_potential_closures > 0) { + closures.reserve(num_of_potential_closures); + for (int ref_id = 0; ref_id < num_of_potential_closures; ++ref_id) { + const ClosureCandidate &closure = ValidateClosure(ref_id, query_id); + if (closure.number_of_inliers > 2) { + closures.emplace_back(closure); + } + } + closures.shrink_to_fit(); + + if (k != -1) { + std::sort(closures.begin(), closures.end(), compare_closure_candidates); + closures.resize(std::min(k, static_cast(closures.size()))); + } } return closures; } + } // namespace map_closures diff --git a/cpp/map_closures/MapClosures.hpp b/cpp/map_closures/MapClosures.hpp index b34f830..39b7c66 100644 --- a/cpp/map_closures/MapClosures.hpp +++ b/cpp/map_closures/MapClosures.hpp @@ -27,6 +27,7 @@ #include #include #include +#include #include #include #include @@ -60,20 +61,44 @@ class MapClosures { ~MapClosures() = default; ClosureCandidate GetBestClosure(const int query_id, - const std::vector &local_map); + const std::vector &local_map, + const std::vector &voxel_means, + const std::vector &voxel_normals) { + const auto &closures = GetTopKClosures(query_id, local_map, voxel_means, voxel_normals, 1); + if (closures.empty()) { + return ClosureCandidate(); + } + return closures.front(); + } std::vector GetTopKClosures(const int query_id, const std::vector &local_map, + const std::vector &voxel_means, + const std::vector &voxel_normals, const int k); std::vector GetClosures(const int query_id, - const std::vector &local_map) { - return GetTopKClosures(query_id, local_map, -1); + const std::vector &local_map, + const std::vector &voxel_means, + const std::vector &voxel_normals) { + return GetTopKClosures(query_id, local_map, voxel_means, voxel_normals, -1); } - const DensityMap &getDensityMapFromId(const int &map_id) const { + + const DensityMap &getDensityMapFromId(const int map_id) const { return density_maps_.at(map_id); } + const Eigen::Matrix4d &getGroundAlignmentFromId(const int map_id) const { + return ground_alignments_.at(map_id); + } + + void SaveHbstDatabase(const std::string &database_path) const { + hbst_binary_tree_->write(database_path); + } + protected: - void MatchAndAddToDatabase(const int id, const std::vector &local_map); + void MatchAndAddToDatabase(const int id, + const std::vector &local_map, + const std::vector &voxel_means, + const std::vector &voxel_normals); ClosureCandidate ValidateClosure(const int reference_id, const int query_id) const; Config config_; diff --git a/cpp/map_closures/VoxelMap.cpp b/cpp/map_closures/VoxelMap.cpp new file mode 100644 index 0000000..43029f5 --- /dev/null +++ b/cpp/map_closures/VoxelMap.cpp @@ -0,0 +1,149 @@ +// MIT License + +// Copyright (c) 2025 Saurabh Gupta, Tiziano Guadagnino, Benedikt Mersch, +// Niklas Trekel, Meher Malladi, and 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. +#include "VoxelMap.hpp" + +#include +#include +#include +#include +#include +#include +#include + +namespace { + +inline Eigen::Vector3i ToVoxelCoordinates(const Eigen::Vector3d &point, const double voxel_size) { + return Eigen::Vector3i(static_cast(std::floor(point.x() / voxel_size)), + static_cast(std::floor(point.y() / voxel_size)), + static_cast(std::floor(point.z() / voxel_size))); +} + +static constexpr unsigned int min_points_for_covariance_computation = 10; + +std::tuple ComputeMeanAndNormal( + const map_closures::VoxelBlock &coordinates) { + const double num_points = static_cast(coordinates.size()); + const Eigen::Vector3d &mean = + std::reduce(coordinates.cbegin(), coordinates.cend(), Eigen::Vector3d().setZero()) / + num_points; + + const Eigen::Matrix3d &covariance = + std::transform_reduce(coordinates.cbegin(), coordinates.cend(), Eigen::Matrix3d().setZero(), + std::plus(), + [&mean](const Eigen::Vector3d &point) { + const Eigen::Vector3d ¢ered = point - mean; + const Eigen::Matrix3d S = centered * centered.transpose(); + return S; + }) / + (num_points - 1); + Eigen::SelfAdjointEigenSolver solver(covariance); + const Eigen::Vector3d normal = solver.eigenvectors().col(0); + return std::make_tuple(mean, normal); +} + +} // namespace + +namespace map_closures { +void VoxelBlock::emplace_back(const Eigen::Vector3d &p) { + if (size() < max_points_per_normal_computation) { + points.at(num_points) = p; + ++num_points; + } +} + +VoxelMap::VoxelMap(const double voxel_size, const double max_distance) + : voxel_size_(voxel_size), + map_resolution_(voxel_size / + static_cast(std::sqrt(max_points_per_normal_computation))), + max_distance_(max_distance) {} + +void VoxelMap::IntegrateFrame(const Vector3dVector &points, const Eigen::Matrix4d &pose) { + Vector3dVector points_transformed(points.size()); + const Eigen::Matrix3d &R = pose.block<3, 3>(0, 0); + const Eigen::Vector3d &t = pose.block<3, 1>(0, 3); + std::transform(points.cbegin(), points.cend(), points_transformed.begin(), + [&](const auto &point) { return R * point + t; }); + AddPoints(points_transformed); +} + +void VoxelMap::AddPoints(const Vector3dVector &points) { + std::for_each(points.cbegin(), points.cend(), [&](const Eigen::Vector3d &point) { + const Voxel voxel = ToVoxelCoordinates(point, voxel_size_); + const auto &[it, inserted] = map_.insert({voxel, VoxelBlock()}); + if (!inserted) { + const VoxelBlock &voxel_block = it->second; + if (voxel_block.size() == max_points_per_normal_computation || + std::any_of(voxel_block.cbegin(), voxel_block.cend(), + [&](const Eigen::Vector3d &voxel_point) { + return (voxel_point - point).norm() < map_resolution_; + })) { + return; + } + } + it->second.emplace_back(point); + }); +} + +Vector3dVector VoxelMap::Pointcloud() const { + Vector3dVector points; + points.reserve(map_.size() * max_points_per_normal_computation); + std::for_each(map_.cbegin(), map_.cend(), [&](const auto &map_element) { + const VoxelBlock &voxel_block = map_element.second; + std::for_each(voxel_block.cbegin(), voxel_block.cend(), + [&](const Eigen::Vector3d &p) { points.emplace_back(p); }); + }); + points.shrink_to_fit(); + return points; +} + +std::tuple VoxelMap::PerVoxelMeanAndNormal() const { + Vector3dVector voxel_means; + voxel_means.reserve(map_.size()); + Vector3dVector voxel_normals; + voxel_normals.reserve(map_.size()); + std::for_each(map_.cbegin(), map_.cend(), [&](const auto &map_element) { + const VoxelBlock &voxel_block = map_element.second; + if (voxel_block.size() >= min_points_for_covariance_computation) { + const auto &[mean, normal] = ComputeMeanAndNormal(voxel_block); + voxel_means.emplace_back(mean); + voxel_normals.emplace_back(normal); + } + }); + voxel_means.shrink_to_fit(); + voxel_normals.shrink_to_fit(); + return std::make_tuple(voxel_means, voxel_normals); +} + +void VoxelMap::RemovePointsFarFromLocation(const Eigen::Vector3d &origin) { + const double max_distance2 = max_distance_ * max_distance_; + for (auto it = map_.begin(); it != map_.end();) { + const auto &[voxel, voxel_points] = *it; + const Eigen::Vector3d &pt = voxel_points.front(); + if ((pt - origin).squaredNorm() >= (max_distance2)) { + it = map_.erase(it); + } else { + ++it; + } + } +} +} // namespace map_closures diff --git a/cpp/map_closures/VoxelMap.hpp b/cpp/map_closures/VoxelMap.hpp new file mode 100644 index 0000000..f995bac --- /dev/null +++ b/cpp/map_closures/VoxelMap.hpp @@ -0,0 +1,76 @@ +// MIT License + +// Copyright (c) 2025 Saurabh Gupta, Tiziano Guadagnino, Benedikt Mersch, +// Niklas Trekel, Meher Malladi, and 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 +#include +#include +#include +#include + +template <> +struct std::hash { + std::size_t operator()(const Eigen::Vector3i &voxel) const { + const uint32_t *vec = reinterpret_cast(voxel.data()); + return (vec[0] * 73856093 ^ vec[1] * 19349669 ^ vec[2] * 83492791); + } +}; + +// Same default as Open3d +static constexpr unsigned int max_points_per_normal_computation = 20; + +namespace map_closures { +using Voxel = Eigen::Vector3i; +using Vector3dVector = std::vector; + +struct VoxelBlock { + void emplace_back(const Eigen::Vector3d &point); + size_t size() const { return num_points; } + auto cbegin() const { return points.cbegin(); } + auto cend() const { return std::next(points.cbegin(), num_points); } + Eigen::Vector3d front() const { return points.front(); } + std::array points; + size_t num_points = 0; +}; + +struct VoxelMap { + explicit VoxelMap(const double voxel_size, const double max_distance); + + void Clear() { map_.clear(); } + bool Empty() const { return map_.empty(); } + size_t NumVoxels() const { return map_.size(); } + + void IntegrateFrame(const Vector3dVector &points, const Eigen::Matrix4d &pose); + void AddPoints(const Vector3dVector &points); + + Vector3dVector Pointcloud() const; + std::tuple PerVoxelMeanAndNormal() const; + void RemovePointsFarFromLocation(const Eigen::Vector3d &origin); + + double voxel_size_; + double map_resolution_; + double max_distance_; + std::unordered_map map_; +}; +} // namespace map_closures diff --git a/python/map_closures/map_closures.py b/python/map_closures/map_closures.py index f678096..6f93ed2 100644 --- a/python/map_closures/map_closures.py +++ b/python/map_closures/map_closures.py @@ -27,6 +27,7 @@ from map_closures.config import MapClosuresConfig from map_closures.pybind import map_closures_pybind +from map_closures.pybind.map_closures_pybind import _Vector3dVector as Vector3dVector ClosureCandidate: TypeAlias = map_closures_pybind._ClosureCandidate @@ -36,29 +37,58 @@ def __init__(self, config: MapClosuresConfig = MapClosuresConfig()): self._config = config self._pipeline = map_closures_pybind._MapClosures(self._config.model_dump()) - def get_best_closure(self, query_idx: int, local_map: np.ndarray) -> ClosureCandidate: - pcd = map_closures_pybind._Vector3dVector(local_map) - closure = self._pipeline._GetBestClosure(query_idx, pcd) + def get_best_closure( + self, + query_idx: int, + local_map: np.ndarray, + voxel_means: np.ndarray, + voxel_normals: np.ndarray, + ) -> ClosureCandidate: + closure = self._pipeline._GetBestClosure( + query_idx, + Vector3dVector(local_map), + Vector3dVector(voxel_means), + Vector3dVector(voxel_normals), + ) return closure def get_top_k_closures( - self, query_idx: int, local_map: np.ndarray, k: int + self, + query_idx: int, + local_map: np.ndarray, + voxel_means: np.ndarray, + voxel_normals: np.ndarray, + k: int, ) -> List[ClosureCandidate]: - pcd = map_closures_pybind._Vector3dVector(local_map) - top_k_closures = self._pipeline._GetTopKClosures(query_idx, pcd, k) + top_k_closures = self._pipeline._GetTopKClosures( + query_idx, + Vector3dVector(local_map), + Vector3dVector(voxel_means), + Vector3dVector(voxel_normals), + k, + ) return top_k_closures - def get_closures(self, query_idx: int, local_map: np.ndarray) -> List[ClosureCandidate]: - pcd = map_closures_pybind._Vector3dVector(local_map) - closures = self._pipeline._GetClosures(query_idx, pcd) + def get_closures( + self, + query_idx: int, + local_map: np.ndarray, + voxel_means: np.ndarray, + voxel_normals: np.ndarray, + ) -> List[ClosureCandidate]: + closures = self._pipeline._GetClosures( + query_idx, + Vector3dVector(local_map), + Vector3dVector(voxel_means), + Vector3dVector(voxel_normals), + ) return closures def get_density_map_from_id(self, map_id: int) -> np.ndarray: return self._pipeline._getDensityMapFromId(map_id) + def get_ground_alignment_from_id(self, map_id: int) -> np.ndarray: + return np.asarray(self._pipeline._getGroundAlignmentFromId(map_id)) -def align_map_to_local_ground(pointcloud, resolution): - return map_closures_pybind._align_map_to_local_ground( - map_closures_pybind._Vector3dVector(pointcloud), - resolution, - ) + def save_hbst_database(self, database_path: str): + self._pipeline._SaveHbstDatabase(database_path) diff --git a/python/map_closures/pipeline.py b/python/map_closures/pipeline.py index b572209..2a04230 100644 --- a/python/map_closures/pipeline.py +++ b/python/map_closures/pipeline.py @@ -28,19 +28,14 @@ import numpy as np from kiss_icp.config import KISSConfig from kiss_icp.kiss_icp import KissICP -from kiss_icp.mapping import VoxelHashMap from tqdm.auto import trange from map_closures.config import load_config, write_config from map_closures.map_closures import MapClosures -from map_closures.tools.evaluation import EvaluationPipeline, LocalMap, StubEvaluation +from map_closures.tools.evaluation import EvaluationPipeline, StubEvaluation +from map_closures.tools.utils import Data, pose_inv from map_closures.visualizer.visualizer import StubVisualizer, Visualizer - - -def transform_points(pcd, T): - R = T[:3, :3] - t = T[:3, -1] - return pcd @ R.T + t +from map_closures.voxel_map import VoxelMap class MapClosurePipeline: @@ -71,37 +66,33 @@ def __init__( self.kiss_config.data.max_range = 100.0 self.odometry = KissICP(self.kiss_config) - self.voxel_local_map = VoxelHashMap( + self.voxel_local_map = VoxelMap( voxel_size=self.closure_config.density_map_resolution, max_distance=self.kiss_config.data.max_range, - max_points_per_voxel=20, ) self._map_range = self.kiss_config.data.max_range - self.closures = [] - self.local_maps = [] - self.density_maps = [] + self.data = Data() self.odom_poses = np.zeros((self._n_scans, 4, 4)) - self.closure_distance_threshold = 6.0 self.results = StubEvaluation() if self._eval: gt_closures_path = ( - os.path.join(self._dataset.sequence_dir, "loop_closure", "gt_closures.txt") + os.path.join( + self._dataset.sequence_dir, "loop_closure", "local_map_gt_closures.txt" + ) if hasattr(self._dataset, "sequence_dir") else None ) if gt_closures_path is not None and os.path.exists(gt_closures_path): self.gt_closures = np.loadtxt(gt_closures_path, dtype=int) - self.results = EvaluationPipeline( - self.gt_closures, self._dataset_name, self.closure_distance_threshold - ) + self.results = EvaluationPipeline(self.gt_closures, self._dataset_name) self.visualizer = Visualizer() if self._vis else StubVisualizer() def run(self): self._run_pipeline() - self.results.compute_closures_and_metrics() + self.results.compute_metrics() self._save_config() self._log_to_file() self._log_to_console() @@ -110,76 +101,53 @@ def run(self): def _run_pipeline(self): map_idx = 0 - poses_in_local_map = [] - scan_indices_in_local_map = [] - + local_map_start_scan_idx = 0 current_map_pose = np.eye(4) + for scan_idx in trange( 0, self._n_scans, ncols=8, unit=" frames", dynamic_ncols=True, - desc="Processing for Loop Closures", + desc="Processing Scans for Loop Closures", ): raw_frame, timestamps = self._dataset[scan_idx] - source, keypoints = self.odometry.register_frame(raw_frame, timestamps) + source, _ = self.odometry.register_frame(raw_frame, timestamps) self.odom_poses[scan_idx] = self.odometry.last_pose current_frame_pose = self.odometry.last_pose - frame_to_map_pose = np.linalg.inv(current_map_pose) @ current_frame_pose - self.voxel_local_map.add_points(transform_points(source, frame_to_map_pose)) + frame_to_map_pose = pose_inv(current_map_pose) @ current_frame_pose + self.voxel_local_map.integrate_frame(source, frame_to_map_pose) self.visualizer.update_registration( - source, - self.odometry.local_map.point_cloud(), - current_frame_pose, + source, self.odometry.local_map.point_cloud(), current_frame_pose ) if np.linalg.norm(frame_to_map_pose[:3, -1]) > self._map_range or ( scan_idx == self._n_scans - 1 ): local_map_pointcloud = self.voxel_local_map.point_cloud() - closures = self.map_closures.get_closures(map_idx, local_map_pointcloud) - - scan_indices_in_local_map.append(scan_idx) - poses_in_local_map.append(current_frame_pose) - - self.local_maps.append( - LocalMap( - local_map_pointcloud, - self.map_closures.get_density_map_from_id(map_idx), - np.copy(scan_indices_in_local_map), - np.copy(poses_in_local_map), - ) + points, normals = self.voxel_local_map.per_voxel_mean_and_normal() + closures = self.map_closures.get_closures( + map_idx, local_map_pointcloud, points, normals ) - self.visualizer.update_data( - self.local_maps[-1].pointcloud, - self.local_maps[-1].density_map, - current_map_pose, + + density_map = self.map_closures.get_density_map_from_id(map_idx) + self.data.append_localmap( + local_map_pointcloud, + density_map, + self.map_closures.get_ground_alignment_from_id(map_idx), + [local_map_start_scan_idx, scan_idx + 1], ) + + self.visualizer.update_data(local_map_pointcloud, density_map, current_map_pose) + for closure in closures: if closure.number_of_inliers > self.closure_config.inliers_threshold: - reference_local_map = self.local_maps[closure.source_id] - query_local_map = self.local_maps[closure.target_id] - self.closures.append( - np.r_[ - closure.source_id, - closure.target_id, - reference_local_map.scan_indices[0], - query_local_map.scan_indices[0], - closure.pose.flatten(), - ] + self.data.append_closure(closure) + self.results.append( + closure.source_id, closure.target_id, closure.number_of_inliers ) - - if self._eval: - self.results.append( - reference_local_map, - query_local_map, - closure.pose, - self.closure_distance_threshold, - closure.number_of_inliers, - ) - self.visualizer.update_closures( np.asarray(closure.pose), [closure.source_id, closure.target_id] ) @@ -187,24 +155,21 @@ def _run_pipeline(self): self.voxel_local_map.remove_far_away_points(frame_to_map_pose[:3, -1]) pts_to_keep = self.voxel_local_map.point_cloud() self.voxel_local_map.clear() - self.voxel_local_map.add_points( - transform_points( - pts_to_keep, np.linalg.inv(current_frame_pose) @ current_map_pose - ) + self.voxel_local_map.integrate_frame( + pts_to_keep, pose_inv(current_frame_pose) @ current_map_pose ) current_map_pose = np.copy(current_frame_pose) - scan_indices_in_local_map.clear() - poses_in_local_map.clear() + frame_to_map_pose = pose_inv(current_map_pose) @ current_frame_pose + local_map_start_scan_idx = scan_idx map_idx += 1 - scan_indices_in_local_map.append(scan_idx) - poses_in_local_map.append(current_frame_pose) - def _log_to_file(self): - np.savetxt(os.path.join(self._results_dir, "map_closures.txt"), np.asarray(self.closures)) + np.savetxt( + os.path.join(self._results_dir, "map_closures.txt"), np.asarray(self.data.closures) + ) np.savetxt( os.path.join(self._results_dir, "kiss_poses_kitti.txt"), - np.asarray(self.odom_poses)[:, :3].reshape(-1, 12), + self.odom_poses[:, :3].reshape(-1, 12), ) self.results.log_to_file(self._results_dir) @@ -219,42 +184,15 @@ def _log_to_console(self): table.add_column("# MapClosure", justify="left", style="cyan") table.add_column("Ref Map Index", justify="left", style="magenta") table.add_column("Query Map Index", justify="left", style="magenta") - table.add_column("Relative Translation 2D", justify="right", style="green") - table.add_column("Relative Rotation 2D", justify="right", style="green") - for i, closure in enumerate(self.closures): - table.add_row( - f"{i+1}", - f"{int(closure[0])}", - f"{int(closure[1])}", - f"[{closure[7]:.4f}, {closure[11]:.4f}] m", - f"{(np.arctan2(closure[8], closure[9]) * 180 / np.pi):.4f} deg", - ) + for i, closure in enumerate(self.data.closures): + table.add_row(f"{i+1}", f"{int(closure[0])}", f"{int(closure[1])}") console.print(table) def _save_config(self): self._results_dir = self._create_results_dir() write_config(self.closure_config, os.path.join(self._results_dir, "config")) - def _write_data_to_disk(self): - import open3d as o3d - from matplotlib import pyplot as plt - - local_map_dir = os.path.join(self._results_dir, "local_maps") - density_map_dir = os.path.join(self._results_dir, "density_maps") - os.makedirs(density_map_dir, exist_ok=True) - os.makedirs(local_map_dir, exist_ok=True) - for i, local_map in enumerate(self.local_maps): - plt.imsave( - os.path.join(density_map_dir, f"{i:06d}.png"), - 255 - local_map.density_map, - cmap="gray", - ) - o3d.io.write_point_cloud( - os.path.join(local_map_dir, f"{i:06d}.ply"), - o3d.geometry.PointCloud(o3d.utility.Vector3dVector(local_map.pointcloud)), - ) - def _create_results_dir(self) -> Path: def get_timestamp() -> str: return datetime.datetime.now().strftime("%Y-%m-%d_%H-%M-%S") @@ -271,3 +209,20 @@ def get_timestamp() -> str: os.symlink(results_dir, latest_dir) return results_dir + + def write_data_to_disk(self): + import open3d as o3d + from matplotlib import pyplot as plt + + local_map_dir = os.path.join(self._results_dir, "local_maps") + os.makedirs(local_map_dir, exist_ok=True) + density_map_dir = os.path.join(self._results_dir, "density_maps") + os.makedirs(density_map_dir, exist_ok=True) + for i, [local_map, density_map] in enumerate( + zip(self.data.local_maps, self.data.density_maps) + ): + o3d.io.write_point_cloud( + os.path.join(local_map_dir, f"{i:06d}.ply"), + o3d.geometry.PointCloud(o3d.utility.Vector3dVector(local_map)), + ) + plt.imsave(os.path.join(density_map_dir, f"{i:06d}.png"), density_map, cmap="gray") diff --git a/python/map_closures/pybind/map_closures_pybind.cpp b/python/map_closures/pybind/map_closures_pybind.cpp index a8c4233..c8ceb6e 100644 --- a/python/map_closures/pybind/map_closures_pybind.cpp +++ b/python/map_closures/pybind/map_closures_pybind.cpp @@ -31,11 +31,10 @@ #include #include #include -#include #include -#include "map_closures/GroundAlign.hpp" #include "map_closures/MapClosures.hpp" +#include "map_closures/VoxelMap.hpp" #include "stl_vector_eigen.h" PYBIND11_MAKE_OPAQUE(std::vector); @@ -78,10 +77,23 @@ PYBIND11_MODULE(map_closures_pybind, m) { cv::cv2eigen(density_map.grid, density_map_eigen); return density_map_eigen; }) - .def("_GetBestClosure", &MapClosures::GetBestClosure, "query_id"_a, "local_map"_a) - .def("_GetTopKClosures", &MapClosures::GetTopKClosures, "query_id"_a, "local_map"_a, "k"_a) - .def("_GetClosures", &MapClosures::GetClosures, "query_id"_a, "local_map"_a); + .def("_getGroundAlignmentFromId", &MapClosures::getGroundAlignmentFromId, "map_id"_a) + .def("_GetBestClosure", &MapClosures::GetBestClosure, "query_id"_a, "local_map"_a, + "voxel_means"_a, "voxel_normals"_a) + .def("_GetTopKClosures", &MapClosures::GetTopKClosures, "query_id"_a, "local_map"_a, + "voxel_means"_a, "voxel_normals"_a, "k"_a) + .def("_GetClosures", &MapClosures::GetClosures, "query_id"_a, "local_map"_a, + "voxel_means"_a, "voxel_normals"_a) + .def("_SaveHbstDatabase", &MapClosures::SaveHbstDatabase, "database_path"_a); - m.def("_align_map_to_local_ground", &AlignToLocalGround, "pointcloud"_a, "resolution"_a); + py::class_ internal_map(m, "_VoxelMap", "Don't use this"); + internal_map.def(py::init(), "voxel_size"_a, "max_distance"_a) + .def("_clear", &VoxelMap::Clear) + .def("_num_voxels", &VoxelMap::NumVoxels) + .def("_add_points", &VoxelMap::AddPoints, "points"_a) + .def("_integrate_frame", &VoxelMap::IntegrateFrame, "points"_a, "pose"_a) + .def("_point_cloud", &VoxelMap::Pointcloud) + .def("_per_voxel_mean_and_normal", &VoxelMap::PerVoxelMeanAndNormal) + .def("_remove_far_away_points", &VoxelMap::RemovePointsFarFromLocation, "origin"_a); } } // namespace map_closures diff --git a/python/map_closures/tools/evaluation.py b/python/map_closures/tools/evaluation.py index ae1a261..88b6b18 100644 --- a/python/map_closures/tools/evaluation.py +++ b/python/map_closures/tools/evaluation.py @@ -22,51 +22,14 @@ # SOFTWARE. import os from abc import ABC -from dataclasses import dataclass -from typing import Dict, List, Set, Tuple +from typing import List, Set, Tuple import numpy as np -from numpy.linalg import inv, norm from rich import box from rich.console import Console from rich.table import Table -@dataclass -class LocalMap: - pointcloud: np.ndarray - density_map: np.ndarray - scan_indices: np.ndarray - scan_poses: np.ndarray - - -def compute_closure_indices( - ref_indices: np.ndarray, - query_indices: np.ndarray, - ref_scan_poses: np.ndarray, - query_scan_poses: np.ndarray, - relative_tf: np.ndarray, - distance_threshold: float, -): - T_query_world = inv(query_scan_poses[0]) - T_ref_world = inv(ref_scan_poses[0]) - - # bring all poses to a common frame at the query map - query_locs = (T_query_world @ query_scan_poses)[:, :3, -1].squeeze() - ref_locs = (relative_tf @ T_ref_world @ ref_scan_poses)[:, :3, -1].squeeze() - - closure_indices = [] - query_id_start = query_indices[0] - ref_id_start = ref_indices[0] - qq, rr = np.meshgrid(query_indices, ref_indices) - distances = norm(query_locs[qq - query_id_start] - ref_locs[rr - ref_id_start], axis=2) - ids = np.where(distances < distance_threshold) - for r_id, q_id in zip(ids[0] + ref_id_start, ids[1] + query_id_start): - closure_indices.append((r_id, q_id)) - closure_indices = set(map(lambda x: tuple(sorted(x)), closure_indices)) - return closure_indices - - class EvaluationMetrics: def __init__(self, true_positives, false_positives, false_negatives): self.tp = true_positives @@ -99,7 +62,7 @@ def print(self): def append(self, *kwargs): pass - def compute_closures_and_metrics(self): + def compute_metrics(self): pass def log_to_file(self, *kwargs): @@ -111,16 +74,13 @@ def __init__( self, gt_closures: np.ndarray, dataset_name: str, - closure_distance_threshold: float, ): self._dataset_name = dataset_name - self._closure_distance_threshold = closure_distance_threshold - self.closure_indices_list: List[Set[Tuple]] = [] - self.inliers_count_list: List = [] + self.closure_list: List[Tuple[int]] = [] + self.inliers_count_list: List[int] = [] - self.predicted_closures: Dict[int, Set[Tuple[int]]] = {} - self.metrics: Dict[int, EvaluationMetrics] = {} + self.metrics = [] gt_closures = gt_closures if gt_closures.shape[1] == 2 else gt_closures.T self.gt_closures: Set[Tuple[int]] = set(map(lambda x: tuple(sorted(x)), gt_closures)) @@ -128,42 +88,22 @@ def __init__( def print(self): self._log_to_console() - def append( - self, - ref_local_map: LocalMap, - query_local_map: LocalMap, - relative_pose: np.ndarray, - distance_threshold: float, - inliers_count: int, - ): - closure_indices = compute_closure_indices( - ref_local_map.scan_indices, - query_local_map.scan_indices, - ref_local_map.scan_poses, - query_local_map.scan_poses, - relative_pose, - distance_threshold, - ) - self.closure_indices_list.append(closure_indices) + def append(self, source_id: int, target_id: int, inliers_count: int): + self.closure_list.append((source_id, target_id)) self.inliers_count_list.append(inliers_count) - def compute_closures_and_metrics( - self, - ): + def compute_metrics(self): print("[INFO] Computing Loop Closure Evaluation Metrics") - for inliers_threshold in range(5, 15): + for inliers_threshold in range(3, np.max(self.inliers_count_list)): closures = set() - for closure_indices, inliers_count in zip( - self.closure_indices_list, self.inliers_count_list - ): + for closure_indices, inliers_count in zip(self.closure_list, self.inliers_count_list): if inliers_count >= inliers_threshold: - closures = closures.union(closure_indices) + closures.add(closure_indices) tp = len(self.gt_closures.intersection(closures)) fp = len(closures) - tp fn = len(self.gt_closures) - tp - self.metrics[inliers_threshold] = EvaluationMetrics(tp, fp, fn) - self.predicted_closures[inliers_threshold] = closures + self.metrics.append(EvaluationMetrics(tp, fp, fn)) def _rich_table_pr(self, table_format: box.Box = box.HORIZONTALS) -> Table: table = Table(box=table_format) @@ -175,15 +115,9 @@ def _rich_table_pr(self, table_format: box.Box = box.HORIZONTALS) -> Table: table.add_column("Precision", justify="left", style="green") table.add_column("Recall", justify="left", style="green") table.add_column("F1 score", justify="left", style="green") - for [inliers, metric] in self.metrics.items(): + for i, metric in enumerate(self.metrics): table.add_row( - f"{inliers}", - f"{metric.tp}", - f"{metric.fp}", - f"{metric.fn}", - f"{metric.precision:.4f}", - f"{metric.recall:.4f}", - f"{metric.F1:.4f}", + f"{i + 3} {metric.tp} {metric.fp} {metric.fn} {metric.precision:.4f} {metric.recall:.4f} {metric.F1:.4f}" ) return table @@ -196,4 +130,6 @@ def log_to_file(self, results_dir): console = Console(file=logfile, width=100, force_jupyter=False) table = self._rich_table_pr(table_format=box.ASCII_DOUBLE_HEAD) console.print(table) - np.save(os.path.join(results_dir, "scan_level_closures.npy"), self.predicted_closures) + np.savetxt( + os.path.join(results_dir, "closure_indices.txt"), np.concatenate(self.closure_list) + ) diff --git a/python/map_closures/tools/utils.py b/python/map_closures/tools/utils.py new file mode 100644 index 0000000..51a72d7 --- /dev/null +++ b/python/map_closures/tools/utils.py @@ -0,0 +1,37 @@ +from dataclasses import dataclass, field +from typing import List + +import numpy as np + +from map_closures.map_closures import ClosureCandidate + + +def pose_inv(T): + T_inv = np.eye(4) + T_inv[:3, :3] = T[:3, :3].T + T_inv[:3, -1] = -T[:3, :3].T @ T[:3, -1] + return T_inv + + +@dataclass +class Data: + local_maps: list = field(default_factory=list) + density_maps: list = field(default_factory=list) + local_maps_scan_index_range: list = field(default_factory=list) + ground_alignment_transforms: list = field(default_factory=list) + closures: list = field(default_factory=list) + + def append_localmap( + self, + local_map: np.ndarray, + density_map: np.ndarray, + ground_alignment_transform: np.ndarray, + scan_index_range: List[int], + ): + self.local_maps.append(local_map) + self.density_maps.append(density_map) + self.ground_alignment_transforms.append(ground_alignment_transform) + self.local_maps_scan_index_range.append(scan_index_range) + + def append_closure(self, closure: ClosureCandidate): + self.closures.append(np.r_[closure.source_id, closure.target_id, closure.pose.flatten()]) diff --git a/python/map_closures/voxel_map.py b/python/map_closures/voxel_map.py new file mode 100644 index 0000000..50778d9 --- /dev/null +++ b/python/map_closures/voxel_map.py @@ -0,0 +1,55 @@ +# MIT License +# +# Copyright (c) 2025 Tiziano Guadagnino, Benedikt Mersch, Saurabh Gupta, 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. +import numpy as np + +from map_closures.pybind import map_closures_pybind +from map_closures.pybind.map_closures_pybind import _Vector3dVector as Vector3dVector + + +class VoxelMap: + def __init__(self, voxel_size: float, max_distance: float): + self.map = map_closures_pybind._VoxelMap(voxel_size, max_distance) + + def integrate_frame(self, points: np.ndarray, pose: np.ndarray): + vector3dvector = Vector3dVector(points.astype(np.float64)) + self.map._integrate_frame(vector3dvector, pose) + + def add_points(self, points: np.ndarray): + vector3dvector = Vector3dVector(points.astype(np.float64)) + self.map._add_points(vector3dvector) + + def point_cloud(self): + return np.asarray(self.map._point_cloud()).astype(np.float64) + + def clear(self): + self.map._clear() + + def num_voxels(self): + return self.map._num_voxels() + + def remove_far_away_points(self, origin): + self.map._remove_far_away_points(origin) + + def per_voxel_mean_and_normal(self): + means, normals = self.map._per_voxel_mean_and_normal() + return np.asarray(means, np.float64), np.asarray(normals, np.float64)