From 1f6f1356c4297270583edad1b37da7491b2ca14f Mon Sep 17 00:00:00 2001 From: ssg1002 Date: Thu, 23 Apr 2026 12:50:31 +0200 Subject: [PATCH 01/22] clean up RANSAC alignment code --- cpp/map_closures/AlignRansac2D.cpp | 32 ++++++++++++------------------ cpp/map_closures/AlignRansac2D.hpp | 8 ++++++++ 2 files changed, 21 insertions(+), 19 deletions(-) diff --git a/cpp/map_closures/AlignRansac2D.cpp b/cpp/map_closures/AlignRansac2D.cpp index 63e0e39..e180aeb 100644 --- a/cpp/map_closures/AlignRansac2D.cpp +++ b/cpp/map_closures/AlignRansac2D.cpp @@ -1,7 +1,6 @@ // MIT License // -// Copyright (c) 2024 Saurabh Gupta, Tiziano Guadagnino, Benedikt Mersch, -// Ignacio Vizzo, Cyrill Stachniss. +// Copyright (c) 2026 Saurabh Gupta. // // Permission is hereby granted, free of charge, to any person obtaining a copy // of this software and associated documentation files (the "Software"), to deal @@ -36,24 +35,19 @@ namespace { Eigen::Isometry2d KabschUmeyamaAlignment2D( const std::vector &keypoint_pairs) { - map_closures::PointPair mean = + const map_closures::PointPair &mean = std::reduce(keypoint_pairs.cbegin(), keypoint_pairs.cend(), map_closures::PointPair(), - [](map_closures::PointPair lhs, const map_closures::PointPair &rhs) { - lhs.ref += rhs.ref; - lhs.query += rhs.query; - return lhs; - }); - mean.query /= static_cast(keypoint_pairs.size()); - mean.ref /= static_cast(keypoint_pairs.size()); - Eigen::Matrix2d covariance_matrix = std::transform_reduce( + std::plus()) / + static_cast(keypoint_pairs.size()); + const Eigen::Matrix2d &covariance_matrix = std::transform_reduce( keypoint_pairs.cbegin(), keypoint_pairs.cend(), Eigen::Matrix2d().setZero(), std::plus(), [&](const map_closures::PointPair &keypoint_pair) { return (keypoint_pair.ref - mean.ref) * ((keypoint_pair.query - mean.query).transpose()); }); - Eigen::JacobiSVD svd(covariance_matrix, - Eigen::ComputeFullU | Eigen::ComputeFullV); + const Eigen::JacobiSVD svd(covariance_matrix, + Eigen::ComputeFullU | Eigen::ComputeFullV); Eigen::Isometry2d T = Eigen::Isometry2d::Identity(); const Eigen::Matrix2d &R = svd.matrixV() * svd.matrixU().transpose(); T.linear() = R.determinant() > 0 ? R : -R; @@ -62,14 +56,14 @@ Eigen::Isometry2d KabschUmeyamaAlignment2D( return T; } -static constexpr double inliers_distance_threshold = 3.0; +constexpr double inliers_distance_threshold = 3.0; // RANSAC Parameters -static constexpr double inliers_ratio = 0.1; -static constexpr double probability_success = 0.999; -static constexpr int min_points = 2; -static int kRansacTrials = std::ceil(std::log(1.0 - probability_success) / - std::log(1.0 - std::pow(inliers_ratio, min_points))); +constexpr double inliers_ratio = 0.1; +constexpr double probability_success = 0.999; +constexpr int min_points = 2; +constexpr int kRansacTrials = std::ceil(std::log(1.0 - probability_success) / + std::log(1.0 - std::pow(inliers_ratio, min_points))); } // namespace namespace map_closures { diff --git a/cpp/map_closures/AlignRansac2D.hpp b/cpp/map_closures/AlignRansac2D.hpp index b104d73..fa0200e 100644 --- a/cpp/map_closures/AlignRansac2D.hpp +++ b/cpp/map_closures/AlignRansac2D.hpp @@ -35,6 +35,14 @@ struct PointPair { PointPair(const Eigen::Vector2d &r, const Eigen::Vector2d &q); Eigen::Vector2d ref = Eigen::Vector2d::Zero(); Eigen::Vector2d query = Eigen::Vector2d::Zero(); + + PointPair operator+(const PointPair &other) const { + return PointPair(ref + other.ref, query + other.query); + } + + PointPair operator/(const double scalar) const { + return PointPair(ref / scalar, query / scalar); + } }; std::pair RansacAlignment2D( From 69f704831174928e0cc304bf4710c1a5242d58a0 Mon Sep 17 00:00:00 2001 From: ssg1002 Date: Thu, 23 Apr 2026 13:03:39 +0200 Subject: [PATCH 02/22] clean up density map code --- cpp/map_closures/DensityMap.cpp | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) diff --git a/cpp/map_closures/DensityMap.cpp b/cpp/map_closures/DensityMap.cpp index 36a3f92..57d2bce 100644 --- a/cpp/map_closures/DensityMap.cpp +++ b/cpp/map_closures/DensityMap.cpp @@ -57,7 +57,7 @@ DensityMap GenerateDensityMap(const std::vector &pcd, Eigen::Array2i upper_bound_coordinates = Eigen::Array2i::Constant(min_int); auto Discretize2D = [&](const Eigen::Vector3d &p) -> Eigen::Array2i { - return ((T_ground.block<3, 3>(0, 0) * p + T_ground.block<3, 1>(0, 3)).head<2>() / + return ((T_ground.block<2, 3>(0, 0) * p + T_ground.block<2, 1>(0, 3)) / density_map_resolution) .array() .floor() @@ -77,9 +77,10 @@ DensityMap GenerateDensityMap(const std::vector &pcd, cv::Mat counting_grid(n_rows, n_cols, CV_64FC1, 0.0); 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())); + double &count = counting_grid.at(px.x(), px.y()); + count += 1.0; + max_points = std::max(max_points, count); + min_points = std::min(min_points, count); }); DensityMap density_map(n_rows, n_cols, density_map_resolution, lower_bound_coordinates); From 7d373f4a2c05cad28c572ecb7ed7ed379ecb4af2 Mon Sep 17 00:00:00 2001 From: ssg1002 Date: Thu, 23 Apr 2026 17:25:20 +0200 Subject: [PATCH 03/22] clean up ground alignment code --- cpp/map_closures/GroundAlign.cpp | 48 +++++++++++++++----------------- 1 file changed, 23 insertions(+), 25 deletions(-) diff --git a/cpp/map_closures/GroundAlign.cpp b/cpp/map_closures/GroundAlign.cpp index 97004e0..716ddfc 100644 --- a/cpp/map_closures/GroundAlign.cpp +++ b/cpp/map_closures/GroundAlign.cpp @@ -36,6 +36,10 @@ #include "VoxelMap.hpp" namespace { +constexpr double normal_filter_threshold = 0.95; +constexpr double convergence_threshold = 1e-3; +constexpr int max_iterations = 10; + using Vector3dVector = std::vector; using LinearSystem = std::pair; @@ -68,9 +72,9 @@ std::pair SampleGroundPoints(const Vector3dVector 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); + const Eigen::Vector2i &pixel = PointToPixel(mean); - auto it = lowest_voxel_hash_map.find(pixel); + const 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()) { @@ -82,7 +86,7 @@ std::pair SampleGroundPoints(const Vector3dVector 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 = + const Eigen::Matrix3d normals_covariance_matrix = std::transform_reduce(low_lying_voxels.cbegin(), low_lying_voxels.cend(), Eigen::Matrix3d().setZero(), std::plus(), [&](const VoxelMeanAndNormal &voxel) { @@ -90,7 +94,7 @@ std::pair SampleGroundPoints(const Vector3dVector }) / static_cast(low_lying_voxels.size() - 1); - Eigen::SelfAdjointEigenSolver eigensolver(covariance_matrix); + const Eigen::SelfAdjointEigenSolver eigensolver(normals_covariance_matrix); Eigen::Vector3d largest_eigenvector = eigensolver.eigenvectors().col(2); largest_eigenvector = (largest_eigenvector.z() < 0) ? -1.0 * largest_eigenvector : largest_eigenvector; @@ -105,32 +109,30 @@ std::pair SampleGroundPoints(const Vector3dVector 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); - } - }); + std::for_each( + low_lying_voxels.cbegin(), low_lying_voxels.cend(), [&](const VoxelMeanAndNormal &voxel) { + if (std::abs(voxel.normal.dot(largest_eigenvector)) > normal_filter_threshold) { + 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); + return {ground_samples, T_init}; } LinearSystem BuildLinearSystem(const Vector3dVector &points) { - auto compute_jacobian_and_residual = [](const Eigen::Vector3d &point) { + auto compute_jacobian_and_residual = + [](const Eigen::Vector3d &point) -> std::pair, double> { const double residual = point.z(); - Eigen::Matrix J; - J(0, 0) = 1.0; - J(0, 1) = point.y(); - J(0, 2) = -point.x(); - return std::make_pair(J, residual); + const Eigen::Matrix J(1.0, point.y(), -point.x()); + return {J, residual}; }; - auto sum_linear_systems = [](LinearSystem a, const LinearSystem &b) { + auto sum_linear_systems = [](LinearSystem a, const LinearSystem &b) -> LinearSystem { a.first += b.first; a.second += b.second; return a; @@ -147,9 +149,6 @@ LinearSystem BuildLinearSystem(const Vector3dVector &points) { }); return {H, b}; } - -static constexpr double convergence_threshold = 1e-3; -static constexpr int max_iterations = 10; } // namespace namespace map_closures { @@ -163,9 +162,8 @@ Eigen::Matrix4d AlignToLocalGround(const Vector3dVector &pointcloud, const doubl for (int iters = 0; iters < max_iterations; iters++) { 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)); + const Eigen::Matrix se3(0.0, 0.0, dx.x(), dx.y(), dx.z(), 0.0); + const Sophus::SE3d estimation(Sophus::SE3d::exp(se3)); TransformPoints(estimation, ground_samples); T = estimation * T; if (dx.norm() < convergence_threshold) break; From 97e16f9c2f3e19b8427f6049eaab506da8468b1c Mon Sep 17 00:00:00 2001 From: ssg1002 Date: Fri, 24 Apr 2026 15:28:55 +0200 Subject: [PATCH 04/22] understand and try to use RVO, NRVO --- cpp/map_closures/AlignRansac2D.cpp | 18 ++++++------ cpp/map_closures/DensityMap.cpp | 2 +- cpp/map_closures/GroundAlign.cpp | 25 +++++++--------- cpp/map_closures/MapClosures.cpp | 46 +++++++++++++++--------------- cpp/map_closures/MapClosures.hpp | 6 ++-- cpp/map_closures/VoxelMap.cpp | 38 ++++++++++++------------ cpp/map_closures/VoxelMap.hpp | 4 ++- 7 files changed, 68 insertions(+), 71 deletions(-) diff --git a/cpp/map_closures/AlignRansac2D.cpp b/cpp/map_closures/AlignRansac2D.cpp index e180aeb..c77135b 100644 --- a/cpp/map_closures/AlignRansac2D.cpp +++ b/cpp/map_closures/AlignRansac2D.cpp @@ -35,11 +35,11 @@ namespace { Eigen::Isometry2d KabschUmeyamaAlignment2D( const std::vector &keypoint_pairs) { - const map_closures::PointPair &mean = + const map_closures::PointPair mean = std::reduce(keypoint_pairs.cbegin(), keypoint_pairs.cend(), map_closures::PointPair(), std::plus()) / static_cast(keypoint_pairs.size()); - const Eigen::Matrix2d &covariance_matrix = std::transform_reduce( + const Eigen::Matrix2d covariance_matrix = std::transform_reduce( keypoint_pairs.cbegin(), keypoint_pairs.cend(), Eigen::Matrix2d().setZero(), std::plus(), [&](const map_closures::PointPair &keypoint_pair) { return (keypoint_pair.ref - mean.ref) * @@ -49,7 +49,7 @@ Eigen::Isometry2d KabschUmeyamaAlignment2D( const Eigen::JacobiSVD svd(covariance_matrix, Eigen::ComputeFullU | Eigen::ComputeFullV); Eigen::Isometry2d T = Eigen::Isometry2d::Identity(); - const Eigen::Matrix2d &R = svd.matrixV() * svd.matrixU().transpose(); + const Eigen::Matrix2d R = svd.matrixV() * svd.matrixU().transpose(); T.linear() = R.determinant() > 0 ? R : -R; T.translation() = mean.query - R * mean.ref; @@ -74,7 +74,7 @@ std::pair RansacAlignment2D( const std::vector &keypoint_pairs) { const size_t max_inliers = keypoint_pairs.size(); - std::vector sample_keypoint_pairs(2); + std::vector sample_keypoint_pairs(min_points); std::vector inlier_indices; inlier_indices.reserve(max_inliers); @@ -85,9 +85,9 @@ std::pair RansacAlignment2D( while (iter++ < kRansacTrials) { inlier_indices.clear(); - std::sample(keypoint_pairs.begin(), keypoint_pairs.end(), sample_keypoint_pairs.begin(), 2, - std::mt19937{std::random_device{}()}); - const Eigen::Isometry2d &T = KabschUmeyamaAlignment2D(sample_keypoint_pairs); + std::sample(keypoint_pairs.begin(), keypoint_pairs.end(), sample_keypoint_pairs.begin(), + min_points, std::mt19937{std::random_device{}()}); + const Eigen::Isometry2d T = KabschUmeyamaAlignment2D(sample_keypoint_pairs); int index = 0; std::for_each(keypoint_pairs.cbegin(), keypoint_pairs.cend(), @@ -107,8 +107,8 @@ std::pair RansacAlignment2D( std::vector inlier_keypoint_pairs(num_inliers); std::transform(optimal_inlier_indices.cbegin(), optimal_inlier_indices.cend(), inlier_keypoint_pairs.begin(), - [&](const auto index) { return keypoint_pairs[index]; }); - const Eigen::Isometry2d &T = KabschUmeyamaAlignment2D(inlier_keypoint_pairs); + [&](const int index) { return keypoint_pairs[index]; }); + const Eigen::Isometry2d T = KabschUmeyamaAlignment2D(inlier_keypoint_pairs); return {T, num_inliers}; } } // namespace map_closures diff --git a/cpp/map_closures/DensityMap.cpp b/cpp/map_closures/DensityMap.cpp index 57d2bce..dea69b4 100644 --- a/cpp/map_closures/DensityMap.cpp +++ b/cpp/map_closures/DensityMap.cpp @@ -65,7 +65,7 @@ DensityMap GenerateDensityMap(const std::vector &pcd, }; std::vector pixels(pcd.size()); std::transform(pcd.cbegin(), pcd.cend(), pixels.begin(), [&](const Eigen::Vector3d &point) { - const Eigen::Array2i &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; diff --git a/cpp/map_closures/GroundAlign.cpp b/cpp/map_closures/GroundAlign.cpp index 716ddfc..14e4fac 100644 --- a/cpp/map_closures/GroundAlign.cpp +++ b/cpp/map_closures/GroundAlign.cpp @@ -70,9 +70,9 @@ std::pair SampleGroundPoints(const Vector3dVector 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); + const Eigen::Vector3d mean = voxel_means[index]; + const Eigen::Vector3d normal = voxel_normals[index]; + const Eigen::Vector2i pixel = PointToPixel(mean); const auto it = lowest_voxel_hash_map.find(pixel); if (it == lowest_voxel_hash_map.end()) { @@ -120,16 +120,13 @@ std::pair SampleGroundPoints(const Vector3dVector 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 {ground_samples, T_init}; + return {ground_samples, Sophus::SE3d(R, Eigen::Vector3d(0.0, 0.0, -1.0 * z_shift))}; } LinearSystem BuildLinearSystem(const Vector3dVector &points) { auto compute_jacobian_and_residual = [](const Eigen::Vector3d &point) -> std::pair, double> { - const double residual = point.z(); - const Eigen::Matrix J(1.0, point.y(), -point.x()); - return {J, residual}; + return {Eigen::Matrix(1.0, point.y(), -point.x()), point.z()}; }; auto sum_linear_systems = [](LinearSystem a, const LinearSystem &b) -> LinearSystem { @@ -138,16 +135,16 @@ LinearSystem BuildLinearSystem(const Vector3dVector &points) { return a; }; - const auto &[H, b] = + const auto linear_system = std::transform_reduce(points.cbegin(), points.cend(), LinearSystem(Eigen::Matrix3d::Zero(), Eigen::Vector3d::Zero()), sum_linear_systems, [&](const Eigen::Vector3d &point) { - const auto &[J, residual] = compute_jacobian_and_residual(point); + const auto [J, residual] = compute_jacobian_and_residual(point); const double w = std::exp(-1.0 * residual * residual); return LinearSystem(J.transpose() * w * J, // JTJ J.transpose() * w * residual); // JTr }); - return {H, b}; + return linear_system; } } // namespace @@ -155,13 +152,13 @@ namespace map_closures { Eigen::Matrix4d AlignToLocalGround(const Vector3dVector &pointcloud, const double resolution) { VoxelMap voxel_map(resolution, 100.0); voxel_map.AddPoints(pointcloud); - const auto &[voxel_means, voxel_normals] = voxel_map.PerVoxelMeanAndNormal(); + const auto [voxel_means, voxel_normals] = voxel_map.PerVoxelMeanAndNormal(); 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(ground_samples); - const Eigen::Vector3d &dx = H.ldlt().solve(-b); + const auto [H, b] = BuildLinearSystem(ground_samples); + const Eigen::Vector3d dx = H.ldlt().solve(-b); const Eigen::Matrix se3(0.0, 0.0, dx.x(), dx.y(), dx.z(), 0.0); const Sophus::SE3d estimation(Sophus::SE3d::exp(se3)); TransformPoints(estimation, ground_samples); diff --git a/cpp/map_closures/MapClosures.cpp b/cpp/map_closures/MapClosures.cpp index 840d265..69833a2 100644 --- a/cpp/map_closures/MapClosures.cpp +++ b/cpp/map_closures/MapClosures.cpp @@ -38,20 +38,20 @@ #include "srrg_hbst/types/binary_tree.hpp" namespace { -static constexpr int min_no_of_matches = 2; -static constexpr int no_of_local_maps_to_skip = 3; -static constexpr int self_similarity_threshold = 35; +constexpr int min_no_of_matches = 2; +constexpr int no_of_local_maps_to_skip = 3; +constexpr int self_similarity_threshold = 35; // fixed parameters for OpenCV ORB Features -static constexpr float scale_factor = 1.00f; -static constexpr int n_levels = 1; -static constexpr int first_level = 0; -static constexpr int WTA_K = 2; -static constexpr int nfeatures = 500; -static constexpr int edge_threshold = 31; -static constexpr int score_type = 0; -static constexpr int patch_size = 31; -static constexpr int fast_threshold = 35; +constexpr float scale_factor = 1.00f; +constexpr int n_levels = 1; +constexpr int first_level = 0; +constexpr int WTA_K = 2; +constexpr int nfeatures = 500; +constexpr int edge_threshold = 31; +constexpr int score_type = 0; +constexpr int patch_size = 31; +constexpr int fast_threshold = 35; } // namespace namespace map_closures { @@ -69,7 +69,7 @@ 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, config_.density_map_resolution); + Eigen::Matrix4d T_ground = AlignToLocalGround(local_map, config_.density_map_resolution); DensityMap density_map = GenerateDensityMap(local_map, T_ground, config_.density_map_resolution, config_.density_threshold); cv::Mat orb_descriptors; @@ -89,8 +89,8 @@ void MapClosures::MatchAndAddToDatabase(const int 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]; + const int 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( @@ -104,7 +104,7 @@ void MapClosures::MatchAndAddToDatabase(const int id, srrg_hbst::SplittingStrategy::SplitEven); density_maps_.emplace(id, std::move(density_map)); - ground_alignments_.emplace(id, T_ground); + ground_alignments_.emplace(id, std::move(T_ground)); } void MapClosures::Match(const std::vector &local_map) { @@ -128,8 +128,8 @@ void MapClosures::Match(const std::vector &local_map) { 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]; + const int 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( @@ -148,7 +148,7 @@ ClosureCandidate MapClosures::ValidateClosure(const int reference_id, const int return ClosureCandidate(); } - const Tree::MatchVector &matches = it->second; + const Tree::MatchVector matches = it->second; const size_t num_matches = matches.size(); ClosureCandidate closure; @@ -163,7 +163,7 @@ ClosureCandidate MapClosures::ValidateClosure(const int reference_id, const int return PointPair(ref_point, query_point); }); - const auto &[pose2d, number_of_inliers] = RansacAlignment2D(keypoint_pairs); + const auto [pose2d, number_of_inliers] = RansacAlignment2D(keypoint_pairs); closure.source_id = reference_id; closure.target_id = query_id; closure.pose.block<2, 2>(0, 0) = pose2d.linear(); @@ -187,9 +187,9 @@ std::vector MapClosures::GetTopKClosures( 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); + ClosureCandidate closure = ValidateClosure(ref_id, query_id); + if (closure.number_of_inliers > min_no_of_matches) { + closures.emplace_back(std::move(closure)); } } closures.shrink_to_fit(); diff --git a/cpp/map_closures/MapClosures.hpp b/cpp/map_closures/MapClosures.hpp index e2e99ef..2bf363b 100644 --- a/cpp/map_closures/MapClosures.hpp +++ b/cpp/map_closures/MapClosures.hpp @@ -35,7 +35,7 @@ #include "DensityMap.hpp" #include "srrg_hbst/types/binary_tree.hpp" -static constexpr int descriptor_size_bits = 256; +constexpr int descriptor_size_bits = 256; using Matchable = srrg_hbst::BinaryMatchable; using Node = srrg_hbst::BinaryNode; using Tree = srrg_hbst::BinaryTree; @@ -62,11 +62,11 @@ class MapClosures { ClosureCandidate GetBestClosure(const int query_id, const std::vector &local_map) { - const auto &closures = GetTopKClosures(query_id, local_map, 1); + std::vector closures = GetTopKClosures(query_id, local_map, 1); if (closures.empty()) { return ClosureCandidate(); } - return closures.front(); + return std::move(closures.front()); } std::vector GetTopKClosures(const int query_id, const std::vector &local_map, diff --git a/cpp/map_closures/VoxelMap.cpp b/cpp/map_closures/VoxelMap.cpp index 43029f5..5107702 100644 --- a/cpp/map_closures/VoxelMap.cpp +++ b/cpp/map_closures/VoxelMap.cpp @@ -43,22 +43,22 @@ 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 = + Eigen::Vector3d mean = std::reduce(coordinates.cbegin(), coordinates.cend(), Eigen::Vector3d().setZero()) / num_points; - const Eigen::Matrix3d &covariance = + 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(); + Eigen::Vector3d centered = point - mean; + 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); + const Eigen::SelfAdjointEigenSolver solver(covariance); + Eigen::Vector3d normal = solver.eigenvectors().col(0); + return {std::move(mean), std::move(normal)}; } } // namespace @@ -73,14 +73,13 @@ void VoxelBlock::emplace_back(const Eigen::Vector3d &p) { 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))), + map_resolution_(voxel_size / max_points_per_normal_computation_sqrt), 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); + 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); @@ -89,9 +88,9 @@ void VoxelMap::IntegrateFrame(const Vector3dVector &points, const Eigen::Matrix4 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()}); + const auto [it, inserted] = map_.try_emplace(voxel, VoxelBlock()); if (!inserted) { - const VoxelBlock &voxel_block = it->second; + 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) { @@ -108,7 +107,7 @@ 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; + const VoxelBlock voxel_block = map_element.second; std::for_each(voxel_block.cbegin(), voxel_block.cend(), [&](const Eigen::Vector3d &p) { points.emplace_back(p); }); }); @@ -122,24 +121,23 @@ std::tuple VoxelMap::PerVoxelMeanAndNormal() con 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; + const VoxelBlock voxel_block = map_element.second; if (voxel_block.size() >= min_points_for_covariance_computation) { - const auto &[mean, normal] = ComputeMeanAndNormal(voxel_block); + 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); + return {std::move(voxel_means), std::move(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)) { + const auto [voxel, voxel_points] = *it; + if ((voxel_points.front() - origin).squaredNorm() >= (max_distance2)) { it = map_.erase(it); } else { ++it; diff --git a/cpp/map_closures/VoxelMap.hpp b/cpp/map_closures/VoxelMap.hpp index f995bac..2be9da1 100644 --- a/cpp/map_closures/VoxelMap.hpp +++ b/cpp/map_closures/VoxelMap.hpp @@ -38,7 +38,9 @@ struct std::hash { }; // Same default as Open3d -static constexpr unsigned int max_points_per_normal_computation = 20; +constexpr unsigned int max_points_per_normal_computation = 20; +constexpr double max_points_per_normal_computation_sqrt = + static_cast(std::sqrt(max_points_per_normal_computation)); namespace map_closures { using Voxel = Eigen::Vector3i; From c1456d5071ade7b2c5d6d9361565ee1bf116f953 Mon Sep 17 00:00:00 2001 From: ssg1002 Date: Fri, 24 Apr 2026 16:14:21 +0200 Subject: [PATCH 05/22] optimize code further, remove shrink_to_fits to avoid reallocations --- cpp/map_closures/AlignRansac2D.cpp | 7 ++++--- cpp/map_closures/GroundAlign.cpp | 3 +-- cpp/map_closures/MapClosures.cpp | 20 +++++++------------- cpp/map_closures/MapClosures.hpp | 1 + cpp/map_closures/VoxelMap.cpp | 9 +++------ 5 files changed, 16 insertions(+), 24 deletions(-) diff --git a/cpp/map_closures/AlignRansac2D.cpp b/cpp/map_closures/AlignRansac2D.cpp index c77135b..1672067 100644 --- a/cpp/map_closures/AlignRansac2D.cpp +++ b/cpp/map_closures/AlignRansac2D.cpp @@ -57,6 +57,8 @@ Eigen::Isometry2d KabschUmeyamaAlignment2D( } constexpr double inliers_distance_threshold = 3.0; +constexpr double sq_inliers_distance_threshold = + inliers_distance_threshold * inliers_distance_threshold; // RANSAC Parameters constexpr double inliers_ratio = 0.1; @@ -92,8 +94,8 @@ std::pair RansacAlignment2D( int index = 0; std::for_each(keypoint_pairs.cbegin(), keypoint_pairs.cend(), [&](const PointPair &keypoint_pair) { - if ((T * keypoint_pair.ref - keypoint_pair.query).norm() < - inliers_distance_threshold) + if ((T * keypoint_pair.ref - keypoint_pair.query).squaredNorm() < + sq_inliers_distance_threshold) inlier_indices.emplace_back(index); index++; }); @@ -102,7 +104,6 @@ std::pair RansacAlignment2D( optimal_inlier_indices = inlier_indices; } } - optimal_inlier_indices.shrink_to_fit(); const std::size_t num_inliers = optimal_inlier_indices.size(); std::vector inlier_keypoint_pairs(num_inliers); std::transform(optimal_inlier_indices.cbegin(), optimal_inlier_indices.cend(), diff --git a/cpp/map_closures/GroundAlign.cpp b/cpp/map_closures/GroundAlign.cpp index 14e4fac..b113293 100644 --- a/cpp/map_closures/GroundAlign.cpp +++ b/cpp/map_closures/GroundAlign.cpp @@ -68,7 +68,7 @@ auto PointToPixel = [](const Eigen::Vector3d &pt) -> Eigen::Vector2i { std::pair SampleGroundPoints(const Vector3dVector &voxel_means, const Vector3dVector &voxel_normals) { std::unordered_map lowest_voxel_hash_map; - + lowest_voxel_hash_map.reserve(voxel_means.size()); for (size_t index = 0; index < voxel_means.size(); ++index) { const Eigen::Vector3d mean = voxel_means[index]; const Eigen::Vector3d normal = voxel_normals[index]; @@ -116,7 +116,6 @@ std::pair SampleGroundPoints(const Vector3dVector 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; diff --git a/cpp/map_closures/MapClosures.cpp b/cpp/map_closures/MapClosures.cpp index 69833a2..fdc43de 100644 --- a/cpp/map_closures/MapClosures.cpp +++ b/cpp/map_closures/MapClosures.cpp @@ -77,12 +77,10 @@ void MapClosures::MatchAndAddToDatabase(const int id, orb_keypoints.reserve(nfeatures); orb_extractor_->detectAndCompute(density_map.grid, cv::noArray(), orb_keypoints, orb_descriptors); - orb_keypoints.shrink_to_fit(); - const auto self_matcher = cv::BFMatcher(cv::NORM_HAMMING); std::vector> self_matches; self_matches.reserve(orb_keypoints.size()); - self_matcher.knnMatch(orb_descriptors, orb_descriptors, self_matches, 2); + self_matcher_.knnMatch(orb_descriptors, orb_descriptors, self_matches, 2); std::vector hbst_matchable; hbst_matchable.reserve(orb_descriptors.rows); @@ -97,7 +95,6 @@ void MapClosures::MatchAndAddToDatabase(const int id, 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, @@ -116,12 +113,10 @@ void MapClosures::Match(const std::vector &local_map) { orb_keypoints.reserve(nfeatures); orb_extractor_->detectAndCompute(density_map.grid, cv::noArray(), orb_keypoints, orb_descriptors); - orb_keypoints.shrink_to_fit(); - const auto self_matcher = cv::BFMatcher(cv::NORM_HAMMING); std::vector> self_matches; self_matches.reserve(orb_keypoints.size()); - self_matcher.knnMatch(orb_descriptors, orb_descriptors, self_matches, 2); + self_matcher_.knnMatch(orb_descriptors, orb_descriptors, self_matches, 2); std::vector hbst_matchable; hbst_matchable.reserve(orb_descriptors.rows); @@ -136,8 +131,6 @@ void MapClosures::Match(const std::vector &local_map) { new Matchable(keypoint, orb_descriptors.row(index_descriptor))); } }); - hbst_matchable.shrink_to_fit(); - hbst_binary_tree_->match(hbst_matchable, descriptor_matches_, config_.hamming_distance_threshold); } @@ -192,11 +185,12 @@ std::vector MapClosures::GetTopKClosures( closures.emplace_back(std::move(closure)); } } - closures.shrink_to_fit(); - - if (k != -1) { + if (k != -1 && !closures.empty()) { + const int top_k = std::min(k, static_cast(closures.size())); + const auto kth = closures.begin() + top_k; + std::nth_element(closures.begin(), kth, closures.end(), compare_closure_candidates); + closures.resize(top_k); std::sort(closures.begin(), closures.end(), compare_closure_candidates); - closures.resize(std::min(k, static_cast(closures.size()))); } } return closures; diff --git a/cpp/map_closures/MapClosures.hpp b/cpp/map_closures/MapClosures.hpp index 2bf363b..5aaaff7 100644 --- a/cpp/map_closures/MapClosures.hpp +++ b/cpp/map_closures/MapClosures.hpp @@ -99,5 +99,6 @@ class MapClosures { std::unordered_map ground_alignments_; std::unique_ptr hbst_binary_tree_ = std::make_unique(); cv::Ptr orb_extractor_; + cv::BFMatcher self_matcher_ = cv::BFMatcher(cv::NORM_HAMMING); }; } // namespace map_closures diff --git a/cpp/map_closures/VoxelMap.cpp b/cpp/map_closures/VoxelMap.cpp index 5107702..4160538 100644 --- a/cpp/map_closures/VoxelMap.cpp +++ b/cpp/map_closures/VoxelMap.cpp @@ -90,7 +90,7 @@ void VoxelMap::AddPoints(const Vector3dVector &points) { const Voxel voxel = ToVoxelCoordinates(point, voxel_size_); const auto [it, inserted] = map_.try_emplace(voxel, VoxelBlock()); if (!inserted) { - const VoxelBlock voxel_block = it->second; + 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) { @@ -107,11 +107,10 @@ 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; + 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; } @@ -121,15 +120,13 @@ std::tuple VoxelMap::PerVoxelMeanAndNormal() con 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; + const VoxelBlock &voxel_block = map_element.second; if (voxel_block.size() >= min_points_for_covariance_computation) { 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::move(voxel_means), std::move(voxel_normals)}; } From 8906eb16d1fae760e5eb260d2ae99b8045c51bf5 Mon Sep 17 00:00:00 2001 From: ssg1002 Date: Fri, 24 Apr 2026 16:21:24 +0200 Subject: [PATCH 06/22] add divide by zero guards --- cpp/map_closures/GroundAlign.cpp | 9 ++++++++- 1 file changed, 8 insertions(+), 1 deletion(-) diff --git a/cpp/map_closures/GroundAlign.cpp b/cpp/map_closures/GroundAlign.cpp index b113293..40f3079 100644 --- a/cpp/map_closures/GroundAlign.cpp +++ b/cpp/map_closures/GroundAlign.cpp @@ -86,6 +86,10 @@ std::pair SampleGroundPoints(const Vector3dVector std::transform(lowest_voxel_hash_map.cbegin(), lowest_voxel_hash_map.cend(), low_lying_voxels.begin(), [](const auto &entry) { return entry.second; }); + if (low_lying_voxels.size() < 2) { + return {Vector3dVector(), Sophus::SE3d()}; + } + const Eigen::Matrix3d normals_covariance_matrix = std::transform_reduce(low_lying_voxels.cbegin(), low_lying_voxels.cend(), Eigen::Matrix3d().setZero(), std::plus(), @@ -116,10 +120,13 @@ std::pair SampleGroundPoints(const Vector3dVector ground_samples.emplace_back(voxel.mean); } }); + if (ground_samples.empty()) { + return {Vector3dVector(), Sophus::SE3d()}; + } ground_centroid /= static_cast(ground_samples.size()); const double z_shift = R.row(2) * ground_centroid; - return {ground_samples, Sophus::SE3d(R, Eigen::Vector3d(0.0, 0.0, -1.0 * z_shift))}; + return {std::move(ground_samples), Sophus::SE3d(R, Eigen::Vector3d(0.0, 0.0, -1.0 * z_shift))}; } LinearSystem BuildLinearSystem(const Vector3dVector &points) { From 1b33b7417c112135c53471808b2bf585874321a1 Mon Sep 17 00:00:00 2001 From: ssg1002 Date: Fri, 24 Apr 2026 17:34:28 +0200 Subject: [PATCH 07/22] fix helipr dataloader --- cpp/map_closures/VoxelMap.cpp | 20 ++++++-- python/map_closures/datasets/helipr.py | 69 ++++++++++++++++---------- 2 files changed, 60 insertions(+), 29 deletions(-) diff --git a/cpp/map_closures/VoxelMap.cpp b/cpp/map_closures/VoxelMap.cpp index 4160538..46bfb63 100644 --- a/cpp/map_closures/VoxelMap.cpp +++ b/cpp/map_closures/VoxelMap.cpp @@ -77,12 +77,24 @@ VoxelMap::VoxelMap(const double voxel_size, const double max_distance) 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); + std::for_each(points.cbegin(), points.cend(), [&](const Eigen::Vector3d &point) { + const Eigen::Vector3d transformed_point = R * point + t; + const Voxel voxel = ToVoxelCoordinates(transformed_point, voxel_size_); + const auto [it, inserted] = map_.try_emplace(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 - transformed_point).norm() < map_resolution_; + })) { + return; + } + } + it->second.emplace_back(transformed_point); + }); } void VoxelMap::AddPoints(const Vector3dVector &points) { diff --git a/python/map_closures/datasets/helipr.py b/python/map_closures/datasets/helipr.py index fa75e05..9991096 100644 --- a/python/map_closures/datasets/helipr.py +++ b/python/map_closures/datasets/helipr.py @@ -27,6 +27,7 @@ from pathlib import Path import numpy as np +import open3d as o3d class HeLiPRDataset: @@ -34,7 +35,11 @@ def __init__(self, data_dir: Path, sequence: str, *_, **__): self.sequence_id = sequence self.sequence_dir = os.path.realpath(data_dir) self.scans_dir = os.path.join(self.sequence_dir, "LiDAR", self.sequence_id) - self.scan_files = sorted(glob.glob(self.scans_dir + "/*.bin")) + self.bin_format = False + self.scan_files = sorted(glob.glob(self.scans_dir + "/*.ply")) + if len(self.scan_files) == 0: + self.bin_format = True + self.scan_files = sorted(glob.glob(self.scans_dir + "/*.bin")) self.gt_file = os.path.join( self.sequence_dir, "LiDAR_GT", f"global_{self.sequence_id}_gt.txt" @@ -44,35 +49,49 @@ def __init__(self, data_dir: Path, sequence: str, *_, **__): if len(self.scan_files) == 0: raise ValueError(f"Tried to read point cloud files in {data_dir} but none found") - # Obtain the pointcloud reader for the given data folder - if self.sequence_id == "Avia": - self.format_string = "fffBBBL" - self.intensity_channel = None - self.time_channel = 6 - elif self.sequence_id == "Aeva": - self.format_string = "ffffflBf" - self.format_string_no_intensity = "ffffflB" - self.intensity_channel = 7 - self.time_channel = 5 - elif self.sequence_id == "Ouster": - self.format_string = "ffffIHHH" - self.intensity_channel = 3 - self.time_channel = 4 - elif self.sequence_id == "Velodyne": - self.format_string = "ffffHf" - self.intensity_channel = 3 - self.time_channel = 5 - else: - print("[ERROR] Unsupported LiDAR Type") - sys.exit(1) + if self.bin_format: + # Obtain the pointcloud reader for the given data folder + if self.sequence_id == "Avia": + self.format_string = "fffBBBL" + self.intensity_channel = None + self.time_channel = 6 + elif self.sequence_id == "Aeva": + self.format_string = "ffffflBf" + self.format_string_no_intensity = "ffffflB" + self.intensity_channel = 7 + self.time_channel = 5 + elif self.sequence_id == "Ouster": + self.format_string = "ffffIHHH" + self.intensity_channel = 3 + self.time_channel = 4 + elif self.sequence_id == "Velodyne": + self.format_string = "ffffHf" + self.intensity_channel = 3 + self.time_channel = 5 + else: + print("[ERROR] Unsupported LiDAR Type") + sys.exit(1) def __len__(self): return len(self.scan_files) def __getitem__(self, idx): - data = self.get_data(idx) - points = self.read_point_cloud(data) - timestamps = self.read_timestamps(data) + if self.bin_format: + data = self.get_data(idx) + points = self.read_point_cloud(data) + timestamps = self.read_timestamps(data) + return points, timestamps + else: + return self.get_ply_data(idx) + + def get_ply_data(self, idx: int): + file_path = self.scan_files[idx] + pcd = o3d.t.io.read_point_cloud(file_path) + points = pcd.point.positions.numpy() + try: + timestamps = pcd.point.timestamps.numpy() + except KeyError: + timestamps = np.array([]) return points, timestamps def get_data(self, idx: int): From b5c7dd217a07868b546ca2fba8bae0fd41462c75 Mon Sep 17 00:00:00 2001 From: ssg1002 Date: Fri, 24 Apr 2026 17:47:38 +0200 Subject: [PATCH 08/22] fix pipeline --- python/map_closures/pipeline.py | 27 ++++++++++++++++++++------- 1 file changed, 20 insertions(+), 7 deletions(-) diff --git a/python/map_closures/pipeline.py b/python/map_closures/pipeline.py index 6bcabe6..f29c281 100644 --- a/python/map_closures/pipeline.py +++ b/python/map_closures/pipeline.py @@ -113,14 +113,14 @@ def _run_pipeline(self): desc="Processing Scans for Loop Closures", ): raw_frame, timestamps = self._dataset[scan_idx] - source, _ = self.odometry.register_frame(raw_frame, timestamps) + deskewed_frame, _ = 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 = pose_inv(current_map_pose) @ current_frame_pose - self.voxel_local_map.integrate_frame(source, frame_to_map_pose) + self.voxel_local_map.integrate_frame(deskewed_frame, frame_to_map_pose) self.visualizer.update_registration( - source, self.odometry.local_map.point_cloud(), current_frame_pose + deskewed_frame, self.odometry.local_map.point_cloud(), current_frame_pose ) if np.linalg.norm(frame_to_map_pose[:3, -1]) > self._map_range or ( @@ -168,22 +168,35 @@ def _log_to_file(self): os.path.join(self._results_dir, "kiss_poses_kitti.txt"), self.odom_poses[:, :3].reshape(-1, 12), ) - self.results.log_to_file(self._results_dir) + np.save( + os.path.join(self._results_dir, "ground_alignment_transforms.npy"), + self.data.ground_alignment_transforms, + ) + self.map_closures.save_hbst_database(os.path.join(self._results_dir, "database.bin")) def _log_to_console(self): from rich import box from rich.console import Console from rich.table import Table + from scipy.spatial.transform import Rotation console = Console() table = Table(box=box.HORIZONTALS) - table.caption = f"Loop Closures Detected Between Local Maps\n" + table.caption = f"Loop Closures" 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", justify="right", style="green") + table.add_column("Relative Rotation", justify="right", style="green") for i, closure in enumerate(self.data.closures): - table.add_row(f"{i+1}", f"{int(closure[0])}", f"{int(closure[1])}") + rpy = Rotation.from_matrix(closure[2:].reshape(4, 4)[:3, :3]).as_euler("xyz", True) + table.add_row( + f"{i+1}", + f"{int(closure[0])}", + f"{int(closure[1])}", + f"[{closure[5]:.4f}, {closure[9]:.4f}, {closure[13]:.4f}] m", + f"[{rpy[0]:.2f}, {rpy[1]:.2f}, {rpy[2]:.2f}] deg", + ) console.print(table) def _save_config(self): From 9ddc0cbe83ad53cf57cc6eb6bbfc763cf5962dbf Mon Sep 17 00:00:00 2001 From: ssg1002 Date: Mon, 27 Apr 2026 14:00:20 +0200 Subject: [PATCH 09/22] remove in-loop transformation of pointcloud --- cpp/map_closures/VoxelMap.cpp | 20 ++++---------------- 1 file changed, 4 insertions(+), 16 deletions(-) diff --git a/cpp/map_closures/VoxelMap.cpp b/cpp/map_closures/VoxelMap.cpp index 46bfb63..903d8df 100644 --- a/cpp/map_closures/VoxelMap.cpp +++ b/cpp/map_closures/VoxelMap.cpp @@ -79,22 +79,10 @@ VoxelMap::VoxelMap(const double voxel_size, const double max_distance) void VoxelMap::IntegrateFrame(const Vector3dVector &points, const Eigen::Matrix4d &pose) { const Eigen::Matrix3d R = pose.block<3, 3>(0, 0); const Eigen::Vector3d t = pose.block<3, 1>(0, 3); - std::for_each(points.cbegin(), points.cend(), [&](const Eigen::Vector3d &point) { - const Eigen::Vector3d transformed_point = R * point + t; - const Voxel voxel = ToVoxelCoordinates(transformed_point, voxel_size_); - const auto [it, inserted] = map_.try_emplace(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 - transformed_point).norm() < map_resolution_; - })) { - return; - } - } - it->second.emplace_back(transformed_point); - }); + std::vector points_transformed(points.size()); + 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) { From 11cae927a943218c80a239efaee3b1acc84b5431 Mon Sep 17 00:00:00 2001 From: ssg1002 Date: Mon, 27 Apr 2026 14:00:41 +0200 Subject: [PATCH 10/22] minor fixes --- python/map_closures/tools/evaluation.py | 10 +--------- 1 file changed, 1 insertion(+), 9 deletions(-) diff --git a/python/map_closures/tools/evaluation.py b/python/map_closures/tools/evaluation.py index 88b6b18..a955713 100644 --- a/python/map_closures/tools/evaluation.py +++ b/python/map_closures/tools/evaluation.py @@ -88,7 +88,7 @@ def __init__( def print(self): self._log_to_console() - def append(self, source_id: int, target_id: int, inliers_count: int): + def append(self, source_id, target_id, inliers_count): self.closure_list.append((source_id, target_id)) self.inliers_count_list.append(inliers_count) @@ -107,14 +107,6 @@ def compute_metrics(self): def _rich_table_pr(self, table_format: box.Box = box.HORIZONTALS) -> Table: table = Table(box=table_format) - table.caption = f"Loop Closure Evaluation Metrics\n" - table.add_column("RANSAC #Inliers", justify="center", style="cyan") - table.add_column("True Positives", justify="center", style="magenta") - table.add_column("False Positives", justify="center", style="magenta") - table.add_column("False Negatives", justify="center", style="magenta") - 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 i, metric in enumerate(self.metrics): table.add_row( f"{i + 3} {metric.tp} {metric.fp} {metric.fn} {metric.precision:.4f} {metric.recall:.4f} {metric.F1:.4f}" From 27002895b9ac9f2ecfcff23ffa060678e84af36f Mon Sep 17 00:00:00 2001 From: ssg1002 Date: Mon, 27 Apr 2026 14:01:22 +0200 Subject: [PATCH 11/22] bumpy python version --- python/pyproject.toml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/python/pyproject.toml b/python/pyproject.toml index 0ca28ec..43ea78c 100644 --- a/python/pyproject.toml +++ b/python/pyproject.toml @@ -8,7 +8,7 @@ version = "2.0.2" description = "Effectively Detecting Loop Closures using Point Cloud Density Maps" readme = "README.md" authors = [{ name = "Saurabh Gupta" }, { name = "Tiziano Guadagnino" }] -requires-python = ">=3.8" +requires-python = ">=3.10" keywords = ["Loop Closures", "Localization", "SLAM", "LiDAR"] dependencies = [ "kiss-icp>=1.2.3", From c55ee8bc06762ba073293afe4c0d894c2a4d73a7 Mon Sep 17 00:00:00 2001 From: ssg1002 Date: Mon, 27 Apr 2026 14:13:54 +0200 Subject: [PATCH 12/22] remove use of pyquaternion library, not maintained regularly --- python/map_closures/datasets/apollo.py | 6 ++---- python/map_closures/datasets/helipr.py | 6 ++---- python/map_closures/datasets/ncd.py | 11 +++-------- 3 files changed, 7 insertions(+), 16 deletions(-) diff --git a/python/map_closures/datasets/apollo.py b/python/map_closures/datasets/apollo.py index 857214b..fc9edb3 100644 --- a/python/map_closures/datasets/apollo.py +++ b/python/map_closures/datasets/apollo.py @@ -57,13 +57,11 @@ def get_scan(self, scan_file: str): @staticmethod def read_poses(file): - from pyquaternion import Quaternion + from scipy.spatial.transform import Rotation as R data = np.loadtxt(file) _, _, translations, qxyzw = np.split(data, [1, 2, 5], axis=1) - rotations = np.array( - [Quaternion(x=x, y=y, z=z, w=w).rotation_matrix for x, y, z, w in qxyzw] - ) + rotations = np.array([R.from_quat(q).as_matrix() for q in qxyzw]) poses = np.zeros([rotations.shape[0], 4, 4]) poses[:, :3, -1] = translations poses[:, :3, :3] = rotations diff --git a/python/map_closures/datasets/helipr.py b/python/map_closures/datasets/helipr.py index 9991096..46ca657 100644 --- a/python/map_closures/datasets/helipr.py +++ b/python/map_closures/datasets/helipr.py @@ -123,14 +123,12 @@ def read_point_cloud(self, data: np.ndarray) -> np.ndarray: return data[:, :3] def load_poses(self, poses_file): - from pyquaternion import Quaternion + from scipy.spatial.transform import Rotation as R poses = np.loadtxt(poses_file, delimiter=" ") xyz = poses[:, 1:4] - rotations = np.array( - [Quaternion(x=x, y=y, z=z, w=w).rotation_matrix for x, y, z, w in poses[:, 4:]] - ) + rotations = np.array([R.from_quat(q).as_matrix() for q in poses[:, 4:]]) poses = np.eye(4, dtype=np.float64).reshape(1, 4, 4).repeat(self.__len__(), axis=0) poses[:, :3, :3] = rotations poses[:, :3, 3] = xyz diff --git a/python/map_closures/datasets/ncd.py b/python/map_closures/datasets/ncd.py index 4fe49f1..78ef7a3 100644 --- a/python/map_closures/datasets/ncd.py +++ b/python/map_closures/datasets/ncd.py @@ -27,7 +27,7 @@ from pathlib import Path import numpy as np -from pyquaternion import Quaternion +from scipy.spatial.transform import Rotation as R class NewerCollegeDataset: @@ -89,12 +89,7 @@ def load_gt_poses(file_path: str): """Taken from pyLiDAR-SLAM/blob/master/slam/dataset/nhcd_dataset.py""" ground_truth_df = np.genfromtxt(str(file_path), delimiter=",", dtype=np.float64) xyz = ground_truth_df[:, 2:5] - rotations = np.array( - [ - Quaternion(x=x, y=y, z=z, w=w).rotation_matrix - for x, y, z, w in ground_truth_df[:, 5:] - ] - ) + rotations = np.array([R.from_quat(q).as_matrix() for q in ground_truth_df[:, 5:]]) num_poses = rotations.shape[0] poses = np.eye(4, dtype=np.float64).reshape(1, 4, 4).repeat(num_poses, axis=0) @@ -102,7 +97,7 @@ def load_gt_poses(file_path: str): poses[:, :3, 3] = xyz T_CL = np.eye(4, dtype=np.float32) - T_CL[:3, :3] = Quaternion(x=0, y=0, z=0.924, w=0.383).rotation_matrix + T_CL[:3, :3] = R.from_quat([0, 0, 0.924, 0.383]).as_matrix() T_CL[:3, 3] = np.array([-0.084, -0.025, 0.050], dtype=np.float32) poses = np.einsum("nij,jk->nik", poses, T_CL) poses = np.einsum("ij,njk->nik", np.linalg.inv(poses[0]), poses) From 367e8cbb81054aff6819e72fa7fdb558edc3fc4d Mon Sep 17 00:00:00 2001 From: ssg1002 Date: Mon, 27 Apr 2026 14:36:51 +0200 Subject: [PATCH 13/22] fix pyproject toml configs for deps, make python>=3.10 --- python/pyproject.toml | 42 +++++++++++++++++++++++++++++------------- 1 file changed, 29 insertions(+), 13 deletions(-) diff --git a/python/pyproject.toml b/python/pyproject.toml index 43ea78c..eb95d16 100644 --- a/python/pyproject.toml +++ b/python/pyproject.toml @@ -6,28 +6,44 @@ build-backend = "scikit_build_core.build" name = "map_closures" version = "2.0.2" description = "Effectively Detecting Loop Closures using Point Cloud Density Maps" -readme = "README.md" authors = [{ name = "Saurabh Gupta" }, { name = "Tiziano Guadagnino" }] +readme = "README.md" requires-python = ">=3.10" +license = { text = "MIT" } + +classifiers = [ + "Intended Audience :: Developers", + "Intended Audience :: Education", + "Intended Audience :: Other Audience", + "Intended Audience :: Science/Research", + "Programming Language :: C++", + "Programming Language :: Python :: 3", + "Programming Language :: Python :: 3.10", + "Programming Language :: Python :: 3.11", + "Programming Language :: Python :: 3.12", + "Programming Language :: Python :: 3.13", + "Programming Language :: Python :: 3.14", +] + keywords = ["Loop Closures", "Localization", "SLAM", "LiDAR"] + dependencies = [ - "kiss-icp>=1.2.3", - "numpy", - "pyquaternion", - "pydantic>=2", - "pydantic-settings", - "tqdm", - "typer[all]>=0.6.0", - "rich", + "kiss-icp>=1.2.3, <=1.3.0", + "numpy>=2.1.0, <2.3.0", + "scipy>=1.14.0, <1.18.0", + "pydantic>=2.11,<2.14", + "pydantic-settings>=2.12,<2.15", + "typer[all]>=0.24.0, <0.26.0", "typing_extensions", + "tqdm~=4.67" ] [project.optional-dependencies] all = [ - "open3d>=0.19.0", - "PyYAML", + "open3d>=0.19,<0.20", + "pyyaml~=6.0", "mcap-ros2-support", - "rosbags", + "rosbags>=0.10,<0.12", "trimesh", "ouster-sdk", "pyntcloud", @@ -61,7 +77,7 @@ max-line-length = "100" [tool.cibuildwheel] archs = ["auto64"] -skip = ["*-musllinux*", "pp*", "cp36-*"] +skip = ["*-musllinux*", "pp*", "cp36-*", "cp37-*", "cp38-*", "cp39-*"] [tool.cibuildwheel.macos] environment = "MACOSX_DEPLOYMENT_TARGET=11.00" From b32ac5525b14395b441b98d0e299e598c84be622 Mon Sep 17 00:00:00 2001 From: ssg1002 Date: Mon, 27 Apr 2026 14:43:52 +0200 Subject: [PATCH 14/22] modify licenses --- LICENSE | 2 +- cpp/3rdparty/find_dependencies.cmake | 3 +-- cpp/CMakeLists.txt | 3 +-- cpp/LICENSE | 3 +-- cpp/map_closures/AlignRansac2D.hpp | 3 +-- cpp/map_closures/CMakeLists.txt | 3 +-- cpp/map_closures/DensityMap.cpp | 3 +-- cpp/map_closures/DensityMap.hpp | 3 +-- cpp/map_closures/GroundAlign.cpp | 3 +-- cpp/map_closures/GroundAlign.hpp | 3 +-- cpp/map_closures/MapClosures.cpp | 3 +-- cpp/map_closures/MapClosures.hpp | 3 +-- cpp/map_closures/VoxelMap.cpp | 3 +-- cpp/map_closures/VoxelMap.hpp | 3 +-- python/CMakeLists.txt | 3 +-- python/LICENSE | 2 +- python/map_closures/__init__.py | 3 +-- python/map_closures/config/__init__.py | 3 +-- python/map_closures/config/config.py | 3 +-- python/map_closures/map_closures.py | 3 +-- python/map_closures/pipeline.py | 3 +-- python/map_closures/pybind/CMakeLists.txt | 3 +-- .../pybind/map_closures_pybind.cpp | 3 +-- python/map_closures/tools/__init__.py | 3 +-- python/map_closures/tools/cmd.py | 3 +-- python/map_closures/tools/evaluation.py | 3 +-- python/map_closures/tools/utils.py | 21 +++++++++++++++++++ .../visualizer/closures_visualizer.py | 3 +-- .../visualizer/local_maps_visualizer.py | 3 +-- .../visualizer/registration_visualizer.py | 3 +-- python/map_closures/visualizer/visualizer.py | 3 +-- python/map_closures/voxel_map.py | 3 +-- 32 files changed, 52 insertions(+), 60 deletions(-) diff --git a/LICENSE b/LICENSE index d29ab91..74d6089 100644 --- a/LICENSE +++ b/LICENSE @@ -1,6 +1,6 @@ MIT License -Copyright (c) 2024 Saurabh Gupta, Tiziano Guadagnino, Benedikt Mersch, Ignacio Vizzo, Cyrill Stachniss +Copyright (c) 2026 Saurabh Gupta Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"), to deal diff --git a/cpp/3rdparty/find_dependencies.cmake b/cpp/3rdparty/find_dependencies.cmake index 991e663..f86f0a3 100644 --- a/cpp/3rdparty/find_dependencies.cmake +++ b/cpp/3rdparty/find_dependencies.cmake @@ -1,7 +1,6 @@ # MIT License # -# Copyright (c) 2024 Saurabh Gupta, Tiziano Guadagnino, Benedikt Mersch, -# Ignacio Vizzo, Cyrill Stachniss. +# Copyright (c) 2026 Saurabh Gupta # # Permission is hereby granted, free of charge, to any person obtaining a copy # of this software and associated documentation files (the "Software"), to deal diff --git a/cpp/CMakeLists.txt b/cpp/CMakeLists.txt index 9bcddbe..f1aaf0b 100644 --- a/cpp/CMakeLists.txt +++ b/cpp/CMakeLists.txt @@ -1,7 +1,6 @@ # MIT License # -# Copyright (c) 2024 Saurabh Gupta, Tiziano Guadagnino, Benedikt Mersch, -# Ignacio Vizzo, Cyrill Stachniss. +# Copyright (c) 2026 Saurabh Gupta # # Permission is hereby granted, free of charge, to any person obtaining a copy # of this software and associated documentation files (the "Software"), to deal diff --git a/cpp/LICENSE b/cpp/LICENSE index 87e174a..74d6089 100644 --- a/cpp/LICENSE +++ b/cpp/LICENSE @@ -1,7 +1,6 @@ MIT License -Copyright (c) 2024 Saurabh Gupta, Tiziano Guadagnino, Benedikt Mersch, -Ignacio Vizzo, Cyrill Stachniss. +Copyright (c) 2026 Saurabh Gupta Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"), to deal diff --git a/cpp/map_closures/AlignRansac2D.hpp b/cpp/map_closures/AlignRansac2D.hpp index fa0200e..0ce2d92 100644 --- a/cpp/map_closures/AlignRansac2D.hpp +++ b/cpp/map_closures/AlignRansac2D.hpp @@ -1,7 +1,6 @@ // MIT License // -// Copyright (c) 2024 Saurabh Gupta, Tiziano Guadagnino, Benedikt Mersch, -// Ignacio Vizzo, Cyrill Stachniss. +// Copyright (c) 2026 Saurabh Gupta // // Permission is hereby granted, free of charge, to any person obtaining a copy // of this software and associated documentation files (the "Software"), to deal diff --git a/cpp/map_closures/CMakeLists.txt b/cpp/map_closures/CMakeLists.txt index c008659..c92cd70 100644 --- a/cpp/map_closures/CMakeLists.txt +++ b/cpp/map_closures/CMakeLists.txt @@ -1,7 +1,6 @@ # MIT License # -# Copyright (c) 2024 Saurabh Gupta, Tiziano Guadagnino, Benedikt Mersch, -# Ignacio Vizzo, Cyrill Stachniss. +# Copyright (c) 2026 Saurabh Gupta # # Permission is hereby granted, free of charge, to any person obtaining a copy # of this software and associated documentation files (the "Software"), to deal diff --git a/cpp/map_closures/DensityMap.cpp b/cpp/map_closures/DensityMap.cpp index dea69b4..9d6c70d 100644 --- a/cpp/map_closures/DensityMap.cpp +++ b/cpp/map_closures/DensityMap.cpp @@ -1,7 +1,6 @@ // MIT License // -// Copyright (c) 2024 Saurabh Gupta, Tiziano Guadagnino, Benedikt Mersch, -// Ignacio Vizzo, Cyrill Stachniss. +// Copyright (c) 2026 Saurabh Gupta // // Permission is hereby granted, free of charge, to any person obtaining a copy // of this software and associated documentation files (the "Software"), to deal diff --git a/cpp/map_closures/DensityMap.hpp b/cpp/map_closures/DensityMap.hpp index 0f3d84b..1c026bf 100644 --- a/cpp/map_closures/DensityMap.hpp +++ b/cpp/map_closures/DensityMap.hpp @@ -1,7 +1,6 @@ // MIT License // -// Copyright (c) 2024 Saurabh Gupta, Tiziano Guadagnino, Benedikt Mersch, -// Ignacio Vizzo, Cyrill Stachniss. +// Copyright (c) 2026 Saurabh Gupta // // Permission is hereby granted, free of charge, to any person obtaining a copy // of this software and associated documentation files (the "Software"), to deal diff --git a/cpp/map_closures/GroundAlign.cpp b/cpp/map_closures/GroundAlign.cpp index 40f3079..de290d7 100644 --- a/cpp/map_closures/GroundAlign.cpp +++ b/cpp/map_closures/GroundAlign.cpp @@ -1,7 +1,6 @@ // MIT License // -// Copyright (c) 2025 Saurabh Gupta, Tiziano Guadagnino, Benedikt Mersch, -// Niklas Trekel, Meher Malladi, and Cyrill Stachniss. +// Copyright (c) 2026 Saurabh Gupta // // Permission is hereby granted, free of charge, to any person obtaining a copy // of this software and associated documentation files (the "Software"), to deal diff --git a/cpp/map_closures/GroundAlign.hpp b/cpp/map_closures/GroundAlign.hpp index 04067db..98ef956 100644 --- a/cpp/map_closures/GroundAlign.hpp +++ b/cpp/map_closures/GroundAlign.hpp @@ -1,7 +1,6 @@ // MIT License // -// Copyright (c) 2025 Saurabh Gupta, Tiziano Guadagnino, Benedikt Mersch, -// Niklas Trekel, Meher Malladi, and Cyrill Stachniss. +// Copyright (c) 2026 Saurabh Gupta // // Permission is hereby granted, free of charge, to any person obtaining a copy // of this software and associated documentation files (the "Software"), to deal diff --git a/cpp/map_closures/MapClosures.cpp b/cpp/map_closures/MapClosures.cpp index fdc43de..b72e547 100644 --- a/cpp/map_closures/MapClosures.cpp +++ b/cpp/map_closures/MapClosures.cpp @@ -1,7 +1,6 @@ // MIT License // -// Copyright (c) 2024 Saurabh Gupta, Tiziano Guadagnino, Benedikt Mersch, -// Ignacio Vizzo, Cyrill Stachniss. +// Copyright (c) 2026 Saurabh Gupta // // Permission is hereby granted, free of charge, to any person obtaining a copy // of this software and associated documentation files (the "Software"), to deal diff --git a/cpp/map_closures/MapClosures.hpp b/cpp/map_closures/MapClosures.hpp index 5aaaff7..eaa07c0 100644 --- a/cpp/map_closures/MapClosures.hpp +++ b/cpp/map_closures/MapClosures.hpp @@ -1,7 +1,6 @@ // MIT License // -// Copyright (c) 2024 Saurabh Gupta, Tiziano Guadagnino, Benedikt Mersch, -// Ignacio Vizzo, Cyrill Stachniss. +// Copyright (c) 2026 Saurabh Gupta // // Permission is hereby granted, free of charge, to any person obtaining a copy // of this software and associated documentation files (the "Software"), to deal diff --git a/cpp/map_closures/VoxelMap.cpp b/cpp/map_closures/VoxelMap.cpp index 903d8df..8d27d15 100644 --- a/cpp/map_closures/VoxelMap.cpp +++ b/cpp/map_closures/VoxelMap.cpp @@ -1,7 +1,6 @@ // MIT License -// Copyright (c) 2025 Saurabh Gupta, Tiziano Guadagnino, Benedikt Mersch, -// Niklas Trekel, Meher Malladi, and Cyrill Stachniss. +// Copyright (c) 2026 Saurabh Gupta // Permission is hereby granted, free of charge, to any person obtaining a copy // of this software and associated documentation files (the "Software"), to deal diff --git a/cpp/map_closures/VoxelMap.hpp b/cpp/map_closures/VoxelMap.hpp index 2be9da1..b8c7354 100644 --- a/cpp/map_closures/VoxelMap.hpp +++ b/cpp/map_closures/VoxelMap.hpp @@ -1,7 +1,6 @@ // MIT License -// Copyright (c) 2025 Saurabh Gupta, Tiziano Guadagnino, Benedikt Mersch, -// Niklas Trekel, Meher Malladi, and Cyrill Stachniss. +// Copyright (c) 2026 Saurabh Gupta // Permission is hereby granted, free of charge, to any person obtaining a copy // of this software and associated documentation files (the "Software"), to deal diff --git a/python/CMakeLists.txt b/python/CMakeLists.txt index 6a852bf..9b51e8c 100644 --- a/python/CMakeLists.txt +++ b/python/CMakeLists.txt @@ -1,7 +1,6 @@ # MIT License # -# Copyright (c) 2022 Ignacio Vizzo, Tiziano Guadagnino, Benedikt Mersch, Cyrill -# Stachniss. +# Copyright (c) 2026 Saurabh Gupta # # Permission is hereby granted, free of charge, to any person obtaining a copy # of this software and associated documentation files (the "Software"), to deal diff --git a/python/LICENSE b/python/LICENSE index d29ab91..74d6089 100644 --- a/python/LICENSE +++ b/python/LICENSE @@ -1,6 +1,6 @@ MIT License -Copyright (c) 2024 Saurabh Gupta, Tiziano Guadagnino, Benedikt Mersch, Ignacio Vizzo, Cyrill Stachniss +Copyright (c) 2026 Saurabh Gupta Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"), to deal diff --git a/python/map_closures/__init__.py b/python/map_closures/__init__.py index ea6c3a3..31977a8 100644 --- a/python/map_closures/__init__.py +++ b/python/map_closures/__init__.py @@ -1,7 +1,6 @@ # MIT License # -# Copyright (c) 2024 Saurabh Gupta, Tiziano Guadagnino, Benedikt Mersch, -# Ignacio Vizzo, Cyrill Stachniss. +# Copyright (c) 2026 Saurabh Gupta # # Permission is hereby granted, free of charge, to any person obtaining a copy # of this software and associated documentation files (the "Software"), to deal diff --git a/python/map_closures/config/__init__.py b/python/map_closures/config/__init__.py index f5d8dae..a52f0e6 100644 --- a/python/map_closures/config/__init__.py +++ b/python/map_closures/config/__init__.py @@ -1,7 +1,6 @@ # MIT License # -# Copyright (c) 2023 Saurabh Gupta, Ignacio Vizzo, Tiziano Guadagnino, Benedikt Mersch, -# Cyrill Stachniss. +# Copyright (c) 2026 Saurabh Gupta # # Permission is hereby granted, free of charge, to any person obtaining a copy # of this software and associated documentation files (the "Software"), to deal diff --git a/python/map_closures/config/config.py b/python/map_closures/config/config.py index e0ca7dd..9aa24ea 100644 --- a/python/map_closures/config/config.py +++ b/python/map_closures/config/config.py @@ -1,7 +1,6 @@ # MIT License # -# Copyright (c) 2023 Saurabh Gupta, Ignacio Vizzo, Tiziano Guadagnino, Benedikt Mersch, -# Cyrill Stachniss. +# Copyright (c) 2026 Saurabh Gupta # # Permission is hereby granted, free of charge, to any person obtaining a copy # of this software and associated documentation files (the "Software"), to deal diff --git a/python/map_closures/map_closures.py b/python/map_closures/map_closures.py index 1b6a713..04f704f 100644 --- a/python/map_closures/map_closures.py +++ b/python/map_closures/map_closures.py @@ -1,7 +1,6 @@ # MIT License # -# Copyright (c) 2024 Saurabh Gupta, Tiziano Guadagnino, Benedikt Mersch, -# Ignacio Vizzo, Cyrill Stachniss. +# Copyright (c) 2026 Saurabh Gupta # # Permission is hereby granted, free of charge, to any person obtaining a copy # of this software and associated documentation files (the "Software"), to deal diff --git a/python/map_closures/pipeline.py b/python/map_closures/pipeline.py index f29c281..1d5f311 100644 --- a/python/map_closures/pipeline.py +++ b/python/map_closures/pipeline.py @@ -1,7 +1,6 @@ # MIT License # -# Copyright (c) 2023 Saurabh Gupta, Ignacio Vizzo, Tiziano Guadagnino, Benedikt Mersch, -# Cyrill Stachniss. +# Copyright (c) 2026 Saurabh Gupta # # Permission is hereby granted, free of charge, to any person obtaining a copy # of this software and associated documentation files (the "Software"), to deal diff --git a/python/map_closures/pybind/CMakeLists.txt b/python/map_closures/pybind/CMakeLists.txt index 75cb584..9c9a84a 100644 --- a/python/map_closures/pybind/CMakeLists.txt +++ b/python/map_closures/pybind/CMakeLists.txt @@ -1,7 +1,6 @@ # MIT License # -# Copyright (c) 2024 Saurabh Gupta, Tiziano Guadagnino, Benedikt Mersch, -# Ignacio Vizzo, Cyrill Stachniss. +# Copyright (c) 2026 Saurabh Gupta # # Permission is hereby granted, free of charge, to any person obtaining a copy # of this software and associated documentation files (the "Software"), to deal diff --git a/python/map_closures/pybind/map_closures_pybind.cpp b/python/map_closures/pybind/map_closures_pybind.cpp index c6b9c01..cafac30 100644 --- a/python/map_closures/pybind/map_closures_pybind.cpp +++ b/python/map_closures/pybind/map_closures_pybind.cpp @@ -1,7 +1,6 @@ // MIT License // -// Copyright (c) 2024 Saurabh Gupta, Tiziano Guadagnino, Benedikt Mersch, -// Ignacio Vizzo, Cyrill Stachniss. +// Copyright (c) 2026 Saurabh Gupta // // Permission is hereby granted, free of charge, to any person obtaining a copy // of this software and associated documentation files (the "Software"), to deal diff --git a/python/map_closures/tools/__init__.py b/python/map_closures/tools/__init__.py index 7037915..6ea09c6 100644 --- a/python/map_closures/tools/__init__.py +++ b/python/map_closures/tools/__init__.py @@ -1,7 +1,6 @@ # MIT License # -# Copyright (c) 2023 Saurabh Gupta, Ignacio Vizzo, Tiziano Guadagnino, Benedikt Mersch, -# Cyrill Stachniss. +# Copyright (c) 2026 Saurabh Gupta # # Permission is hereby granted, free of charge, to any person obtaining a copy # of this software and associated documentation files (the "Software"), to deal diff --git a/python/map_closures/tools/cmd.py b/python/map_closures/tools/cmd.py index f0da82b..9e262fd 100644 --- a/python/map_closures/tools/cmd.py +++ b/python/map_closures/tools/cmd.py @@ -1,7 +1,6 @@ # MIT License # -# Copyright (c) 2024 Saurabh Gupta, Tiziano Guadagnino, Benedikt Mersch, -# Ignacio Vizzo, Cyrill Stachniss. +# Copyright (c) 2026 Saurabh Gupta # # Permission is hereby granted, free of charge, to any person obtaining a copy # of this software and associated documentation files (the "Software"), to deal diff --git a/python/map_closures/tools/evaluation.py b/python/map_closures/tools/evaluation.py index a955713..b444bf6 100644 --- a/python/map_closures/tools/evaluation.py +++ b/python/map_closures/tools/evaluation.py @@ -1,7 +1,6 @@ # MIT License # -# Copyright (c) 2024 Saurabh Gupta, Tiziano Guadagnino, Benedikt Mersch, -# Ignacio Vizzo, Cyrill Stachniss. +# Copyright (c) 2026 Saurabh Gupta # # Permission is hereby granted, free of charge, to any person obtaining a copy # of this software and associated documentation files (the "Software"), to deal diff --git a/python/map_closures/tools/utils.py b/python/map_closures/tools/utils.py index 51a72d7..9346417 100644 --- a/python/map_closures/tools/utils.py +++ b/python/map_closures/tools/utils.py @@ -1,3 +1,24 @@ +# MIT License +# +# Copyright (c) 2026 Saurabh Gupta +# +# 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. from dataclasses import dataclass, field from typing import List diff --git a/python/map_closures/visualizer/closures_visualizer.py b/python/map_closures/visualizer/closures_visualizer.py index 34ae05f..90d86f0 100644 --- a/python/map_closures/visualizer/closures_visualizer.py +++ b/python/map_closures/visualizer/closures_visualizer.py @@ -1,7 +1,6 @@ # MIT License # -# Copyright (c) 2024 Saurabh Gupta, Tiziano Guadagnino, Benedikt Mersch, -# Ignacio Vizzo, Cyrill Stachniss. +# Copyright (c) 2026 Saurabh Gupta # # Permission is hereby granted, free of charge, to any person obtaining a copy # of this software and associated documentation files (the "Software"), to deal diff --git a/python/map_closures/visualizer/local_maps_visualizer.py b/python/map_closures/visualizer/local_maps_visualizer.py index a6d5be3..fbc1d51 100644 --- a/python/map_closures/visualizer/local_maps_visualizer.py +++ b/python/map_closures/visualizer/local_maps_visualizer.py @@ -1,7 +1,6 @@ # MIT License # -# Copyright (c) 2024 Saurabh Gupta, Tiziano Guadagnino, Benedikt Mersch, -# Ignacio Vizzo, Cyrill Stachniss. +# Copyright (c) 2026 Saurabh Gupta # # Permission is hereby granted, free of charge, to any person obtaining a copy # of this software and associated documentation files (the "Software"), to deal diff --git a/python/map_closures/visualizer/registration_visualizer.py b/python/map_closures/visualizer/registration_visualizer.py index 6539e11..79ba651 100644 --- a/python/map_closures/visualizer/registration_visualizer.py +++ b/python/map_closures/visualizer/registration_visualizer.py @@ -1,7 +1,6 @@ # MIT License # -# Copyright (c) 2024 Saurabh Gupta, Tiziano Guadagnino, Benedikt Mersch, -# Ignacio Vizzo, Cyrill Stachniss. +# Copyright (c) 2026 Saurabh Gupta # # Permission is hereby granted, free of charge, to any person obtaining a copy # of this software and associated documentation files (the "Software"), to deal diff --git a/python/map_closures/visualizer/visualizer.py b/python/map_closures/visualizer/visualizer.py index acc6974..bac2f5c 100644 --- a/python/map_closures/visualizer/visualizer.py +++ b/python/map_closures/visualizer/visualizer.py @@ -1,7 +1,6 @@ # MIT License # -# Copyright (c) 2024 Saurabh Gupta, Tiziano Guadagnino, Benedikt Mersch, -# Ignacio Vizzo, Cyrill Stachniss. +# Copyright (c) 2026 Saurabh Gupta # # Permission is hereby granted, free of charge, to any person obtaining a copy # of this software and associated documentation files (the "Software"), to deal diff --git a/python/map_closures/voxel_map.py b/python/map_closures/voxel_map.py index 50778d9..1876f7a 100644 --- a/python/map_closures/voxel_map.py +++ b/python/map_closures/voxel_map.py @@ -1,7 +1,6 @@ # MIT License # -# Copyright (c) 2025 Tiziano Guadagnino, Benedikt Mersch, Saurabh Gupta, Cyrill -# Stachniss. +# Copyright (c) 2026 Saurabh Gupta # # Permission is hereby granted, free of charge, to any person obtaining a copy # of this software and associated documentation files (the "Software"), to deal From 547fe137c2e410f0baaa65f46fd672ead3dc8c09 Mon Sep 17 00:00:00 2001 From: ssg1002 Date: Mon, 27 Apr 2026 15:05:27 +0200 Subject: [PATCH 15/22] remove some constexprs, not working with appleclang compiler --- cpp/map_closures/AlignRansac2D.cpp | 4 ++-- cpp/map_closures/VoxelMap.cpp | 3 ++- cpp/map_closures/VoxelMap.hpp | 2 -- 3 files changed, 4 insertions(+), 5 deletions(-) diff --git a/cpp/map_closures/AlignRansac2D.cpp b/cpp/map_closures/AlignRansac2D.cpp index 1672067..45bf03a 100644 --- a/cpp/map_closures/AlignRansac2D.cpp +++ b/cpp/map_closures/AlignRansac2D.cpp @@ -64,8 +64,8 @@ constexpr double sq_inliers_distance_threshold = constexpr double inliers_ratio = 0.1; constexpr double probability_success = 0.999; constexpr int min_points = 2; -constexpr int kRansacTrials = std::ceil(std::log(1.0 - probability_success) / - std::log(1.0 - std::pow(inliers_ratio, min_points))); +const int kRansacTrials = std::ceil(std::log(1.0 - probability_success) / + std::log(1.0 - std::pow(inliers_ratio, min_points))); } // namespace namespace map_closures { diff --git a/cpp/map_closures/VoxelMap.cpp b/cpp/map_closures/VoxelMap.cpp index 8d27d15..2f54fff 100644 --- a/cpp/map_closures/VoxelMap.cpp +++ b/cpp/map_closures/VoxelMap.cpp @@ -72,7 +72,8 @@ void VoxelBlock::emplace_back(const Eigen::Vector3d &p) { VoxelMap::VoxelMap(const double voxel_size, const double max_distance) : voxel_size_(voxel_size), - map_resolution_(voxel_size / max_points_per_normal_computation_sqrt), + map_resolution_(voxel_size / + (std::sqrt(static_cast(max_points_per_normal_computation)))), max_distance_(max_distance) {} void VoxelMap::IntegrateFrame(const Vector3dVector &points, const Eigen::Matrix4d &pose) { diff --git a/cpp/map_closures/VoxelMap.hpp b/cpp/map_closures/VoxelMap.hpp index b8c7354..229bda4 100644 --- a/cpp/map_closures/VoxelMap.hpp +++ b/cpp/map_closures/VoxelMap.hpp @@ -38,8 +38,6 @@ struct std::hash { // Same default as Open3d constexpr unsigned int max_points_per_normal_computation = 20; -constexpr double max_points_per_normal_computation_sqrt = - static_cast(std::sqrt(max_points_per_normal_computation)); namespace map_closures { using Voxel = Eigen::Vector3i; From 7584789d00b59590e1744638fa4c1f94c752ffac Mon Sep 17 00:00:00 2001 From: Saurabh Gupta Date: Mon, 27 Apr 2026 15:08:12 +0200 Subject: [PATCH 16/22] fix std::nth_element edge case Co-authored-by: Copilot Autofix powered by AI <175728472+Copilot@users.noreply.github.com> --- cpp/map_closures/MapClosures.cpp | 9 ++++++--- 1 file changed, 6 insertions(+), 3 deletions(-) diff --git a/cpp/map_closures/MapClosures.cpp b/cpp/map_closures/MapClosures.cpp index b72e547..e00894f 100644 --- a/cpp/map_closures/MapClosures.cpp +++ b/cpp/map_closures/MapClosures.cpp @@ -186,9 +186,12 @@ std::vector MapClosures::GetTopKClosures( } if (k != -1 && !closures.empty()) { const int top_k = std::min(k, static_cast(closures.size())); - const auto kth = closures.begin() + top_k; - std::nth_element(closures.begin(), kth, closures.end(), compare_closure_candidates); - closures.resize(top_k); + if (top_k < static_cast(closures.size())) { + const auto kth = closures.begin() + top_k; + std::nth_element( + closures.begin(), kth, closures.end(), compare_closure_candidates); + closures.resize(top_k); + } std::sort(closures.begin(), closures.end(), compare_closure_candidates); } } From 53be5e779bcda00e02bb6db3ab1c9eaa30de1a28 Mon Sep 17 00:00:00 2001 From: Saurabh Gupta Date: Mon, 27 Apr 2026 15:09:50 +0200 Subject: [PATCH 17/22] log the results to file Co-authored-by: Copilot Autofix powered by AI <175728472+Copilot@users.noreply.github.com> --- python/map_closures/pipeline.py | 1 + 1 file changed, 1 insertion(+) diff --git a/python/map_closures/pipeline.py b/python/map_closures/pipeline.py index 1d5f311..32410fb 100644 --- a/python/map_closures/pipeline.py +++ b/python/map_closures/pipeline.py @@ -172,6 +172,7 @@ def _log_to_file(self): self.data.ground_alignment_transforms, ) self.map_closures.save_hbst_database(os.path.join(self._results_dir, "database.bin")) + self.results.log_to_file(self._results_dir) def _log_to_console(self): from rich import box From f5a3ad2ba130f7e7c64aeb86fe8ef55241b87e69 Mon Sep 17 00:00:00 2001 From: "copilot-swe-agent[bot]" <198982749+Copilot@users.noreply.github.com> Date: Mon, 27 Apr 2026 13:14:25 +0000 Subject: [PATCH 18/22] Add missing #include to AlignRansac2D.cpp Agent-Logs-Url: https://github.com/PRBonn/MapClosures/sessions/e336eb2a-b3e4-4578-8d20-33af4cf07afc Co-authored-by: saurabh1002 <28734882+saurabh1002@users.noreply.github.com> --- cpp/map_closures/AlignRansac2D.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/cpp/map_closures/AlignRansac2D.cpp b/cpp/map_closures/AlignRansac2D.cpp index 45bf03a..29c49a1 100644 --- a/cpp/map_closures/AlignRansac2D.cpp +++ b/cpp/map_closures/AlignRansac2D.cpp @@ -27,6 +27,7 @@ #include #include #include +#include #include #include #include From 1f5f128e5a984ed550b0a1190edfbd326bd0de70 Mon Sep 17 00:00:00 2001 From: Saurabh Gupta Date: Mon, 27 Apr 2026 15:15:05 +0200 Subject: [PATCH 19/22] fix issue with vector6d init Co-authored-by: Copilot Autofix powered by AI <175728472+Copilot@users.noreply.github.com> --- cpp/map_closures/GroundAlign.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/cpp/map_closures/GroundAlign.cpp b/cpp/map_closures/GroundAlign.cpp index de290d7..ee1b431 100644 --- a/cpp/map_closures/GroundAlign.cpp +++ b/cpp/map_closures/GroundAlign.cpp @@ -164,7 +164,8 @@ Eigen::Matrix4d AlignToLocalGround(const Vector3dVector &pointcloud, const doubl for (int iters = 0; iters < max_iterations; iters++) { const auto [H, b] = BuildLinearSystem(ground_samples); const Eigen::Vector3d dx = H.ldlt().solve(-b); - const Eigen::Matrix se3(0.0, 0.0, dx.x(), dx.y(), dx.z(), 0.0); + Eigen::Matrix se3 = Eigen::Matrix::Zero(); + se3 << 0.0, 0.0, dx.x(), dx.y(), dx.z(), 0.0; const Sophus::SE3d estimation(Sophus::SE3d::exp(se3)); TransformPoints(estimation, ground_samples); T = estimation * T; From 80b7a598a2393045f2e21b6ba51d7a414fff74a8 Mon Sep 17 00:00:00 2001 From: "copilot-swe-agent[bot]" <198982749+Copilot@users.noreply.github.com> Date: Mon, 27 Apr 2026 13:19:54 +0000 Subject: [PATCH 20/22] Add early return in AlignToLocalGround when ground_samples is empty Agent-Logs-Url: https://github.com/PRBonn/MapClosures/sessions/b706290d-4bc2-49cf-9ea4-52d48689b110 Co-authored-by: saurabh1002 <28734882+saurabh1002@users.noreply.github.com> --- cpp/map_closures/GroundAlign.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/cpp/map_closures/GroundAlign.cpp b/cpp/map_closures/GroundAlign.cpp index ee1b431..f0b5c34 100644 --- a/cpp/map_closures/GroundAlign.cpp +++ b/cpp/map_closures/GroundAlign.cpp @@ -160,6 +160,7 @@ Eigen::Matrix4d AlignToLocalGround(const Vector3dVector &pointcloud, const doubl const auto [voxel_means, voxel_normals] = voxel_map.PerVoxelMeanAndNormal(); auto [ground_samples, T] = SampleGroundPoints(voxel_means, voxel_normals); + if (ground_samples.empty()) return Eigen::Matrix4d::Identity(); TransformPoints(T, ground_samples); for (int iters = 0; iters < max_iterations; iters++) { const auto [H, b] = BuildLinearSystem(ground_samples); From b8fd8526ce437769a6bde8b881998affe09ce262 Mon Sep 17 00:00:00 2001 From: ssg1002 Date: Mon, 27 Apr 2026 15:22:43 +0200 Subject: [PATCH 21/22] fix refs, fix formatting --- cpp/map_closures/MapClosures.cpp | 5 ++--- cpp/map_closures/VoxelMap.cpp | 2 +- 2 files changed, 3 insertions(+), 4 deletions(-) diff --git a/cpp/map_closures/MapClosures.cpp b/cpp/map_closures/MapClosures.cpp index e00894f..7b1d24f 100644 --- a/cpp/map_closures/MapClosures.cpp +++ b/cpp/map_closures/MapClosures.cpp @@ -140,7 +140,7 @@ ClosureCandidate MapClosures::ValidateClosure(const int reference_id, const int return ClosureCandidate(); } - const Tree::MatchVector matches = it->second; + const Tree::MatchVector &matches = it->second; const size_t num_matches = matches.size(); ClosureCandidate closure; @@ -188,8 +188,7 @@ std::vector MapClosures::GetTopKClosures( const int top_k = std::min(k, static_cast(closures.size())); if (top_k < static_cast(closures.size())) { const auto kth = closures.begin() + top_k; - std::nth_element( - closures.begin(), kth, closures.end(), compare_closure_candidates); + std::nth_element(closures.begin(), kth, closures.end(), compare_closure_candidates); closures.resize(top_k); } std::sort(closures.begin(), closures.end(), compare_closure_candidates); diff --git a/cpp/map_closures/VoxelMap.cpp b/cpp/map_closures/VoxelMap.cpp index 2f54fff..d63f7a9 100644 --- a/cpp/map_closures/VoxelMap.cpp +++ b/cpp/map_closures/VoxelMap.cpp @@ -133,7 +133,7 @@ std::tuple VoxelMap::PerVoxelMeanAndNormal() con 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 auto &[voxel, voxel_points] = *it; if ((voxel_points.front() - origin).squaredNorm() >= (max_distance2)) { it = map_.erase(it); } else { From 76b4859a8aba29e716922ce6862c8663d0ebbc17 Mon Sep 17 00:00:00 2001 From: "copilot-swe-agent[bot]" <198982749+Copilot@users.noreply.github.com> Date: Mon, 27 Apr 2026 13:25:31 +0000 Subject: [PATCH 22/22] Move open3d import inside get_ply_data to avoid unconditional optional dependency Agent-Logs-Url: https://github.com/PRBonn/MapClosures/sessions/65ae623c-2cb0-4347-91f2-cae487a75ac0 Co-authored-by: saurabh1002 <28734882+saurabh1002@users.noreply.github.com> --- python/map_closures/datasets/helipr.py | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/python/map_closures/datasets/helipr.py b/python/map_closures/datasets/helipr.py index 46ca657..27d5cfe 100644 --- a/python/map_closures/datasets/helipr.py +++ b/python/map_closures/datasets/helipr.py @@ -27,7 +27,6 @@ from pathlib import Path import numpy as np -import open3d as o3d class HeLiPRDataset: @@ -85,6 +84,8 @@ def __getitem__(self, idx): return self.get_ply_data(idx) def get_ply_data(self, idx: int): + import open3d as o3d + file_path = self.scan_files[idx] pcd = o3d.t.io.read_point_cloud(file_path) points = pcd.point.positions.numpy()