Refactor and optimize alignment and density map code

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

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

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

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

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
@@ -28,6 +27,7 @@
 #include <Eigen/LU>
 #include <Eigen/SVD>
 #include <algorithm>
+#include <functional>
 #include <numeric>
 #include <random>
 #include <utility>
@@ -36,40 +36,37 @@
 namespace {
 Eigen::Isometry2d KabschUmeyamaAlignment2D(
     const std::vector<map_closures::PointPair> &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<double>(keypoint_pairs.size());
-    mean.ref /= static_cast<double>(keypoint_pairs.size());
-    Eigen::Matrix2d covariance_matrix = std::transform_reduce(
+                    std::plus<map_closures::PointPair>()) /
+        static_cast<double>(keypoint_pairs.size());
+    const Eigen::Matrix2d covariance_matrix = std::transform_reduce(
         keypoint_pairs.cbegin(), keypoint_pairs.cend(), Eigen::Matrix2d().setZero(),
         std::plus<Eigen::Matrix2d>(), [&](const map_closures::PointPair &keypoint_pair) {
             return (keypoint_pair.ref - mean.ref) *
                    ((keypoint_pair.query - mean.query).transpose());
         });
 
-    Eigen::JacobiSVD<Eigen::Matrix2d> svd(covariance_matrix,
-                                          Eigen::ComputeFullU | Eigen::ComputeFullV);
+    const Eigen::JacobiSVD<Eigen::Matrix2d> 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;
 
     return T;
 }
 
-static constexpr double inliers_distance_threshold = 3.0;
+constexpr double inliers_distance_threshold = 3.0;
+constexpr double sq_inliers_distance_threshold =
+    inliers_distance_threshold * inliers_distance_threshold;
 
 // 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;
+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 {
@@ -80,7 +77,7 @@ std::pair<Eigen::Isometry2d, std::size_t> RansacAlignment2D(
     const std::vector<PointPair> &keypoint_pairs) {
     const size_t max_inliers = keypoint_pairs.size();
 
-    std::vector<PointPair> sample_keypoint_pairs(2);
+    std::vector<PointPair> sample_keypoint_pairs(min_points);
     std::vector<int> inlier_indices;
     inlier_indices.reserve(max_inliers);
 
@@ -91,15 +88,15 @@ std::pair<Eigen::Isometry2d, std::size_t> 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(),
                       [&](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++;
                       });
@@ -108,13 +105,12 @@ std::pair<Eigen::Isometry2d, std::size_t> RansacAlignment2D(
             optimal_inlier_indices = inlier_indices;
         }
     }
-    optimal_inlier_indices.shrink_to_fit();
     const std::size_t num_inliers = optimal_inlier_indices.size();
     std::vector<PointPair> 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

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
@@ -35,6 +34,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<Eigen::Isometry2d, std::size_t> RansacAlignment2D(

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

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
@@ -57,15 +56,15 @@ DensityMap GenerateDensityMap(const std::vector<Eigen::Vector3d> &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()
             .cast<int>();
     };
     std::vector<Eigen::Array2i> 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;
@@ -77,9 +76,10 @@ DensityMap GenerateDensityMap(const std::vector<Eigen::Vector3d> &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<double>(px.x(), px.y()) += 1;
-        max_points = std::max(max_points, counting_grid.at<double>(px.x(), px.y()));
-        min_points = std::min(min_points, counting_grid.at<double>(px.x(), px.y()));
+        double &count = counting_grid.at<double>(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);

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

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
@@ -36,6 +35,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<Eigen::Vector3d>;
 using LinearSystem = std::pair<Eigen::Matrix3d, Eigen::Vector3d>;
 
@@ -64,13 +67,13 @@ auto PointToPixel = [](const Eigen::Vector3d &pt) -> Eigen::Vector2i {
 std::pair<Vector3dVector, Sophus::SE3d> SampleGroundPoints(const Vector3dVector &voxel_means,
                                                            const Vector3dVector &voxel_normals) {
     std::unordered_map<Eigen::Vector2i, VoxelMeanAndNormal, PixelHash> 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];
+        const Eigen::Vector3d mean = voxel_means[index];
+        const Eigen::Vector3d normal = voxel_normals[index];
         const Eigen::Vector2i pixel = PointToPixel(mean);
 
-        auto it = lowest_voxel_hash_map.find(pixel);
+        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,15 +85,19 @@ std::pair<Vector3dVector, Sophus::SE3d> 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 =
+    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<Eigen::Matrix3d>(),
                               [&](const VoxelMeanAndNormal &voxel) {
                                   return voxel.normal * voxel.normal.transpose();
                               }) /
         static_cast<double>(low_lying_voxels.size() - 1);
 
-    Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d> eigensolver(covariance_matrix);
+    const Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d> 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,67 +112,62 @@ std::pair<Vector3dVector, Sophus::SE3d> 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);
-                      }
-                  });
-    ground_samples.shrink_to_fit();
+    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);
+            }
+        });
+    if (ground_samples.empty()) {
+        return {Vector3dVector(), Sophus::SE3d()};
+    }
     ground_centroid /= static_cast<double>(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 {std::move(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) {
-        const double residual = point.z();
-        Eigen::Matrix<double, 1, 3> J;
-        J(0, 0) = 1.0;
-        J(0, 1) = point.y();
-        J(0, 2) = -point.x();
-        return std::make_pair(J, residual);
+    auto compute_jacobian_and_residual =
+        [](const Eigen::Vector3d &point) -> std::pair<Eigen::Matrix<double, 1, 3>, double> {
+        return {Eigen::Matrix<double, 1, 3>(1.0, point.y(), -point.x()), point.z()};
     };
 
-    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;
     };
 
-    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;
 }
-
-static constexpr double convergence_threshold = 1e-3;
-static constexpr int max_iterations = 10;
 }  // namespace
 
 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);
+    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);
-        const Eigen::Vector3d &dx = H.ldlt().solve(-b);
+        const auto [H, b] = BuildLinearSystem(ground_samples);
+        const Eigen::Vector3d dx = H.ldlt().solve(-b);
         Eigen::Matrix<double, 6, 1> se3 = Eigen::Matrix<double, 6, 1>::Zero();
-        se3.block<3, 1>(2, 0) = dx;
-        Sophus::SE3d estimation(Sophus::SE3d::exp(se3));
+        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;

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