From 5ba6b0eb591e47be0e954b356af0b7aaca407f2a Mon Sep 17 00:00:00 2001 From: Saurabh Gupta Date: Thu, 20 Mar 2025 10:59:43 +0100 Subject: [PATCH 01/10] initialize instead of reserve, size known --- cpp/map_closures/GroundAlign.cpp | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/cpp/map_closures/GroundAlign.cpp b/cpp/map_closures/GroundAlign.cpp index 9fde229..56661a9 100644 --- a/cpp/map_closures/GroundAlign.cpp +++ b/cpp/map_closures/GroundAlign.cpp @@ -89,17 +89,17 @@ std::vector ComputeLowestPoints(const std::vector low_lying_points; - low_lying_points.reserve(lowest_point_hash_map.size()); - std::for_each(lowest_point_hash_map.cbegin(), lowest_point_hash_map.cend(), - [&](const auto &entry) { low_lying_points.emplace_back(entry.second); }); + 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) { low_lying_points.emplace_back(entry.second); }); return low_lying_points; } } // namespace From 009a97c0af4b29583cb8502a89d254bd517159f2 Mon Sep 17 00:00:00 2001 From: Saurabh Gupta Date: Thu, 20 Mar 2025 11:13:13 +0100 Subject: [PATCH 02/10] fix bug --- cpp/map_closures/GroundAlign.cpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/cpp/map_closures/GroundAlign.cpp b/cpp/map_closures/GroundAlign.cpp index 56661a9..79ecb02 100644 --- a/cpp/map_closures/GroundAlign.cpp +++ b/cpp/map_closures/GroundAlign.cpp @@ -98,8 +98,7 @@ std::vector ComputeLowestPoints(const 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) { low_lying_points.emplace_back(entry.second); }); + low_lying_points.begin(), [](const auto &entry) { return entry.second; }); return low_lying_points; } } // namespace From 14041a2f4d0db2d037629b9eb99279f6578d0bb1 Mon Sep 17 00:00:00 2001 From: Saurabh Gupta Date: Thu, 20 Mar 2025 12:19:36 +0100 Subject: [PATCH 03/10] reserve nfeatures for orb_keypoints --- cpp/map_closures/MapClosures.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/cpp/map_closures/MapClosures.cpp b/cpp/map_closures/MapClosures.cpp index 7d098c5..c3ae0ef 100644 --- a/cpp/map_closures/MapClosures.cpp +++ b/cpp/map_closures/MapClosures.cpp @@ -74,8 +74,10 @@ void MapClosures::MatchAndAddToDatabase(const int id, config_.density_threshold); cv::Mat orb_descriptors; std::vector orb_keypoints; + orb_keypoints.reserve(nfeatures); orb_extractor_->detectAndCompute(density_map.grid, cv::noArray(), orb_keypoints, orb_descriptors); + orb_keypoints.shrink_to_fit(); auto matcher = cv::BFMatcher(cv::NORM_HAMMING); std::vector> bf_matches; From 3200d929b21bf3dedcd1d130f86c58033978ba1f Mon Sep 17 00:00:00 2001 From: Saurabh Gupta Date: Thu, 20 Mar 2025 12:21:15 +0100 Subject: [PATCH 04/10] self-similarity: use reserve, rename vars for better read --- cpp/map_closures/MapClosures.cpp | 13 +++++++------ 1 file changed, 7 insertions(+), 6 deletions(-) diff --git a/cpp/map_closures/MapClosures.cpp b/cpp/map_closures/MapClosures.cpp index c3ae0ef..b26f8fa 100644 --- a/cpp/map_closures/MapClosures.cpp +++ b/cpp/map_closures/MapClosures.cpp @@ -79,9 +79,10 @@ void MapClosures::MatchAndAddToDatabase(const int id, orb_descriptors); orb_keypoints.shrink_to_fit(); - auto matcher = cv::BFMatcher(cv::NORM_HAMMING); - std::vector> bf_matches; - matcher.knnMatch(orb_descriptors, orb_descriptors, bf_matches, 2); + 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); std::for_each(orb_keypoints.begin(), orb_keypoints.end(), [&](cv::KeyPoint &keypoint) { keypoint.pt.x = keypoint.pt.x + static_cast(density_map.lower_bound.y()); @@ -92,9 +93,9 @@ void MapClosures::MatchAndAddToDatabase(const int id, std::vector hbst_matchable; hbst_matchable.reserve(orb_descriptors.rows); - std::for_each(bf_matches.cbegin(), bf_matches.cend(), [&](const auto &bf_match) { - if (bf_match[1].distance > self_similarity_threshold) { - auto index_descriptor = bf_match[0].queryIdx; + std::for_each(self_matches.cbegin(), self_matches.cend(), [&](const auto &self_match) { + if (self_match[1].distance > self_similarity_threshold) { + auto index_descriptor = self_match[0].queryIdx; auto keypoint = orb_keypoints[index_descriptor]; hbst_matchable.emplace_back( new Matchable(keypoint, orb_descriptors.row(index_descriptor), id)); From 495ad08a2832a08eda94505df14ee6b9559ffb18 Mon Sep 17 00:00:00 2001 From: Saurabh Gupta Date: Thu, 20 Mar 2025 12:41:02 +0100 Subject: [PATCH 05/10] resize instead of copy, simplify life --- cpp/map_closures/MapClosures.cpp | 15 +++++++-------- 1 file changed, 7 insertions(+), 8 deletions(-) diff --git a/cpp/map_closures/MapClosures.cpp b/cpp/map_closures/MapClosures.cpp index b26f8fa..caa0c3c 100644 --- a/cpp/map_closures/MapClosures.cpp +++ b/cpp/map_closures/MapClosures.cpp @@ -179,13 +179,12 @@ std::vector MapClosures::GetTopKClosures( } } }); - if (k == -1) return closures; - - std::vector top_k_closures; - top_k_closures.reserve(std::min(k, static_cast(closures.size()))); - std::sort(closures.begin(), closures.end(), compare_closure_candidates); - std::copy_n(closures.cbegin(), std::min(k, static_cast(closures.size())), - std::back_inserter(top_k_closures)); - return top_k_closures; + 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 From 79331f68e782c5c06d0d09ec8dcec8138768472e Mon Sep 17 00:00:00 2001 From: Saurabh Gupta Date: Thu, 20 Mar 2025 13:30:18 +0100 Subject: [PATCH 06/10] const some variables --- cpp/map_closures/AlignRansac2D.cpp | 2 +- cpp/map_closures/GroundAlign.cpp | 2 +- cpp/map_closures/MapClosures.cpp | 4 ++-- 3 files changed, 4 insertions(+), 4 deletions(-) diff --git a/cpp/map_closures/AlignRansac2D.cpp b/cpp/map_closures/AlignRansac2D.cpp index 729bd92..c70641b 100644 --- a/cpp/map_closures/AlignRansac2D.cpp +++ b/cpp/map_closures/AlignRansac2D.cpp @@ -93,7 +93,7 @@ std::pair RansacAlignment2D( std::sample(keypoint_pairs.begin(), keypoint_pairs.end(), sample_keypoint_pairs.begin(), 2, std::mt19937{std::random_device{}()}); - auto T = KabschUmeyamaAlignment2D(sample_keypoint_pairs); + const auto T = KabschUmeyamaAlignment2D(sample_keypoint_pairs); int index = 0; std::for_each(keypoint_pairs.cbegin(), keypoint_pairs.cend(), diff --git a/cpp/map_closures/GroundAlign.cpp b/cpp/map_closures/GroundAlign.cpp index 79ecb02..c40511a 100644 --- a/cpp/map_closures/GroundAlign.cpp +++ b/cpp/map_closures/GroundAlign.cpp @@ -86,7 +86,7 @@ std::vector ComputeLowestPoints(const std::vector &local_map) { - Eigen::Matrix4d T_ground = AlignToLocalGround(local_map, ground_alignment_resolution); + const Eigen::Matrix4d T_ground = AlignToLocalGround(local_map, ground_alignment_resolution); DensityMap density_map = GenerateDensityMap(local_map, T_ground, config_.density_map_resolution, config_.density_threshold); cv::Mat orb_descriptors; @@ -79,7 +79,7 @@ void MapClosures::MatchAndAddToDatabase(const int id, orb_descriptors); orb_keypoints.shrink_to_fit(); - auto self_matcher = cv::BFMatcher(cv::NORM_HAMMING); + 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); From 0ba64af2de4dc3d1a6b24c55110f3a6809a51f0c Mon Sep 17 00:00:00 2001 From: Saurabh Gupta Date: Thu, 20 Mar 2025 13:35:31 +0100 Subject: [PATCH 07/10] shrink vectors after filling --- cpp/map_closures/AlignRansac2D.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/cpp/map_closures/AlignRansac2D.cpp b/cpp/map_closures/AlignRansac2D.cpp index c70641b..c5aafa8 100644 --- a/cpp/map_closures/AlignRansac2D.cpp +++ b/cpp/map_closures/AlignRansac2D.cpp @@ -108,7 +108,7 @@ 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(), From c160177501ca378dd1a558e3a5d369d13ad8df66 Mon Sep 17 00:00:00 2001 From: Saurabh Gupta Date: Thu, 20 Mar 2025 18:25:49 +0100 Subject: [PATCH 08/10] use ref_id --- cpp/map_closures/MapClosures.cpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/cpp/map_closures/MapClosures.cpp b/cpp/map_closures/MapClosures.cpp index cf9fe39..d8b63bb 100644 --- a/cpp/map_closures/MapClosures.cpp +++ b/cpp/map_closures/MapClosures.cpp @@ -172,8 +172,7 @@ std::vector MapClosures::GetTopKClosures( [&](const auto &descriptor_match) { const auto ref_id = static_cast(descriptor_match.first); if (is_far_enough(ref_id, query_id)) { - ClosureCandidate closure = - ValidateClosure(descriptor_match.first, query_id); + ClosureCandidate closure = ValidateClosure(ref_id, query_id); if (closure.number_of_inliers > min_no_of_matches) { closures.emplace_back(closure); } From d886b0eb28a8b36d8042a5550d76bfccf6c94c0e Mon Sep 17 00:00:00 2001 From: Saurabh Gupta Date: Thu, 27 Mar 2025 20:04:14 +0100 Subject: [PATCH 09/10] use const references to non-atomic vars --- cpp/map_closures/AlignRansac2D.cpp | 6 +++--- cpp/map_closures/DensityMap.cpp | 2 +- cpp/map_closures/GroundAlign.cpp | 6 +++--- cpp/map_closures/MapClosures.cpp | 8 ++++---- 4 files changed, 11 insertions(+), 11 deletions(-) diff --git a/cpp/map_closures/AlignRansac2D.cpp b/cpp/map_closures/AlignRansac2D.cpp index c5aafa8..8ff28ff 100644 --- a/cpp/map_closures/AlignRansac2D.cpp +++ b/cpp/map_closures/AlignRansac2D.cpp @@ -55,7 +55,7 @@ Eigen::Isometry2d KabschUmeyamaAlignment2D( 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; @@ -93,7 +93,7 @@ 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 auto &T = KabschUmeyamaAlignment2D(sample_keypoint_pairs); int index = 0; std::for_each(keypoint_pairs.cbegin(), keypoint_pairs.cend(), @@ -114,7 +114,7 @@ std::pair RansacAlignment2D( std::transform(optimal_inlier_indices.cbegin(), optimal_inlier_indices.cend(), inlier_keypoint_pairs.begin(), [&](const auto index) { return keypoint_pairs[index]; }); - Eigen::Isometry2d T = KabschUmeyamaAlignment2D(inlier_keypoint_pairs); + 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 6477ac8..aa95271 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 auto pixel = Discretize2D(point); + const auto &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 c40511a..c8ac42a 100644 --- a/cpp/map_closures/GroundAlign.cpp +++ b/cpp/map_closures/GroundAlign.cpp @@ -86,7 +86,7 @@ std::vector ComputeLowestPoints(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); + auto &low_lying_points = ComputeLowestPoints(pointcloud, resolution); for (int iters = 0; iters < max_iterations; iters++) { const auto &[H, b] = BuildLinearSystem(low_lying_points, resolution); - const Eigen::Vector3d dx = H.ldlt().solve(-b); + 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)); diff --git a/cpp/map_closures/MapClosures.cpp b/cpp/map_closures/MapClosures.cpp index d8b63bb..01062eb 100644 --- a/cpp/map_closures/MapClosures.cpp +++ b/cpp/map_closures/MapClosures.cpp @@ -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, ground_alignment_resolution); + const Eigen::Matrix4d &T_ground = AlignToLocalGround(local_map, ground_alignment_resolution); DensityMap density_map = GenerateDensityMap(local_map, T_ground, config_.density_map_resolution, config_.density_threshold); cv::Mat orb_descriptors; @@ -95,8 +95,8 @@ void MapClosures::MatchAndAddToDatabase(const int id, 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) { - auto index_descriptor = self_match[0].queryIdx; - auto keypoint = orb_keypoints[index_descriptor]; + const auto index_descriptor = self_match[0].queryIdx; + const auto &keypoint = orb_keypoints[index_descriptor]; hbst_matchable.emplace_back( new Matchable(keypoint, orb_descriptors.row(index_descriptor), id)); } @@ -172,7 +172,7 @@ std::vector MapClosures::GetTopKClosures( [&](const auto &descriptor_match) { const auto ref_id = static_cast(descriptor_match.first); if (is_far_enough(ref_id, query_id)) { - ClosureCandidate closure = ValidateClosure(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); } From 875ba3c9518cb4884eea6cde4f28f354faebd9be Mon Sep 17 00:00:00 2001 From: Saurabh Gupta Date: Thu, 27 Mar 2025 20:06:27 +0100 Subject: [PATCH 10/10] oops, not for non-const though --- cpp/map_closures/GroundAlign.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/cpp/map_closures/GroundAlign.cpp b/cpp/map_closures/GroundAlign.cpp index c8ac42a..be65d78 100644 --- a/cpp/map_closures/GroundAlign.cpp +++ b/cpp/map_closures/GroundAlign.cpp @@ -107,7 +107,7 @@ 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); + auto low_lying_points = ComputeLowestPoints(pointcloud, resolution); for (int iters = 0; iters < max_iterations; iters++) { const auto &[H, b] = BuildLinearSystem(low_lying_points, resolution);