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.cpp b/cpp/map_closures/AlignRansac2D.cpp
index 63e0e39..29c49a1 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
@@ -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
diff --git a/cpp/map_closures/AlignRansac2D.hpp b/cpp/map_closures/AlignRansac2D.hpp
index b104d73..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
@@ -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(
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 36a3f92..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
@@ -57,7 +56,7 @@ 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()
@@ -65,7 +64,7 @@ DensityMap GenerateDensityMap(const std::vector<Eigen::Vector3d> &pcd,
     };
     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);
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 97004e0..f0b5c34 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
@@ -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,7 +85,11 @@ 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) {
@@ -90,7 +97,7 @@ std::pair<Vector3dVector, Sophus::SE3d> SampleGroundPoints(const Vector3dVector
                               }) /
         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;
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 840d265..7b1d24f 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
@@ -38,20 +37,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 +68,7 @@ MapClosures::MapClosures(const Config &config) : config_(config) {
 
 void MapClosures::MatchAndAddToDatabase(const int id,
                                         const std::vector<Eigen::Vector3d> &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;
@@ -77,34 +76,31 @@ 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<std::vector<cv::DMatch>> 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<Matchable *> hbst_matchable;
     hbst_matchable.reserve(orb_descriptors.rows);
     std::for_each(
         self_matches.cbegin(), self_matches.cend(), [&](const std::vector<cv::DMatch> &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<float>(density_map.lower_bound.y());
                 keypoint.pt.y = keypoint.pt.y + static_cast<float>(density_map.lower_bound.x());
                 hbst_matchable.emplace_back(
                     new Matchable(keypoint, orb_descriptors.row(index_descriptor), id));
             }
         });
-    hbst_matchable.shrink_to_fit();
 
     hbst_binary_tree_->matchAndAdd(hbst_matchable, descriptor_matches_,
                                    config_.hamming_distance_threshold,
                                    srrg_hbst::SplittingStrategy::SplitEven);
 
     density_maps_.emplace(id, std::move(density_map));
-    ground_alignments_.emplace(id, T_ground);
+    ground_alignments_.emplace(id, std::move(T_ground));
 }
 
 void MapClosures::Match(const std::vector<Eigen::Vector3d> &local_map) {
@@ -116,28 +112,24 @@ void MapClosures::Match(const std::vector<Eigen::Vector3d> &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<std::vector<cv::DMatch>> 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<Matchable *> hbst_matchable;
     hbst_matchable.reserve(orb_descriptors.rows);
     std::for_each(
         self_matches.cbegin(), self_matches.cend(), [&](const std::vector<cv::DMatch> &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<float>(density_map.lower_bound.y());
                 keypoint.pt.y = keypoint.pt.y + static_cast<float>(density_map.lower_bound.x());
                 hbst_matchable.emplace_back(
                     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);
 }
@@ -163,7 +155,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,16 +179,19 @@ std::vector<ClosureCandidate> 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();
-
-        if (k != -1) {
+        if (k != -1 && !closures.empty()) {
+            const int top_k = std::min(k, static_cast<int>(closures.size()));
+            if (top_k < static_cast<int>(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<int>(closures.size())));
         }
     }
     return closures;
diff --git a/cpp/map_closures/MapClosures.hpp b/cpp/map_closures/MapClosures.hpp
index e2e99ef..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
@@ -35,7 +34,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<cv::KeyPoint, descriptor_size_bits>;
 using Node = srrg_hbst::BinaryNode<Matchable>;
 using Tree = srrg_hbst::BinaryTree<Node>;
@@ -62,11 +61,11 @@ class MapClosures {
 
     ClosureCandidate GetBestClosure(const int query_id,
                                     const std::vector<Eigen::Vector3d> &local_map) {
-        const auto &closures = GetTopKClosures(query_id, local_map, 1);
+        std::vector<ClosureCandidate> closures = GetTopKClosures(query_id, local_map, 1);
         if (closures.empty()) {
             return ClosureCandidate();
         }
-        return closures.front();
+        return std::move(closures.front());
     }
     std::vector<ClosureCandidate> GetTopKClosures(const int query_id,
                                                   const std::vector<Eigen::Vector3d> &local_map,
@@ -99,5 +98,6 @@ class MapClosures {
     std::unordered_map<int, Eigen::Matrix4d> ground_alignments_;
     std::unique_ptr<Tree> hbst_binary_tree_ = std::make_unique<Tree>();
     cv::Ptr<cv::DescriptorExtractor> 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 43029f5..d63f7a9 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
@@ -43,22 +42,22 @@ static constexpr unsigned int min_points_for_covariance_computation = 10;
 std::tuple<Eigen::Vector3d, Eigen::Vector3d> ComputeMeanAndNormal(
     const map_closures::VoxelBlock &coordinates) {
     const double num_points = static_cast<double>(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<Eigen::Matrix3d>(),
                               [&mean](const Eigen::Vector3d &point) {
-                                  const Eigen::Vector3d &centered = 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<Eigen::Matrix3d> solver(covariance);
-    const Eigen::Vector3d normal = solver.eigenvectors().col(0);
-    return std::make_tuple(mean, normal);
+    const Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d> solver(covariance);
+    Eigen::Vector3d normal = solver.eigenvectors().col(0);
+    return {std::move(mean), std::move(normal)};
 }
 
 }  // namespace
@@ -74,13 +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<double>(std::sqrt(max_points_per_normal_computation))),
+                      (std::sqrt(static_cast<double>(max_points_per_normal_computation)))),
       max_distance_(max_distance) {}
 
 void VoxelMap::IntegrateFrame(const Vector3dVector &points, const Eigen::Matrix4d &pose) {
-    Vector3dVector points_transformed(points.size());
-    const Eigen::Matrix3d &R = pose.block<3, 3>(0, 0);
-    const Eigen::Vector3d &t = pose.block<3, 1>(0, 3);
+    const Eigen::Matrix3d R = pose.block<3, 3>(0, 0);
+    const Eigen::Vector3d t = pose.block<3, 1>(0, 3);
+    std::vector<Eigen::Vector3d> points_transformed(points.size());
     std::transform(points.cbegin(), points.cend(), points_transformed.begin(),
                    [&](const auto &point) { return R * point + t; });
     AddPoints(points_transformed);
@@ -89,7 +88,7 @@ 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;
             if (voxel_block.size() == max_points_per_normal_computation ||
@@ -112,7 +111,6 @@ Vector3dVector VoxelMap::Pointcloud() const {
         std::for_each(voxel_block.cbegin(), voxel_block.cend(),
                       [&](const Eigen::Vector3d &p) { points.emplace_back(p); });
     });
-    points.shrink_to_fit();
     return points;
 }
 
@@ -124,22 +122,19 @@ std::tuple<Vector3dVector, Vector3dVector> VoxelMap::PerVoxelMeanAndNormal() con
     std::for_each(map_.cbegin(), map_.cend(), [&](const auto &map_element) {
         const VoxelBlock &voxel_block = map_element.second;
         if (voxel_block.size() >= min_points_for_covariance_computation) {
-            const auto &[mean, normal] = ComputeMeanAndNormal(voxel_block);
+            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)) {
+        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..229bda4 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
@@ -38,7 +37,7 @@ struct std::hash<Eigen::Vector3i> {
 };
 
 // Same default as Open3d
-static constexpr unsigned int max_points_per_normal_computation = 20;
+constexpr unsigned int max_points_per_normal_computation = 20;
 
 namespace map_closures {
 using Voxel = Eigen::Vector3i;
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/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 fa75e05..27d5cfe 100644
--- a/python/map_closures/datasets/helipr.py
+++ b/python/map_closures/datasets/helipr.py
@@ -34,7 +34,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 +48,51 @@ 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):
+        import open3d as o3d
+
+        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):
@@ -104,14 +124,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)
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 6bcabe6..32410fb 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
@@ -113,14 +112,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 +167,36 @@ def _log_to_file(self):
             os.path.join(self._results_dir, "kiss_poses_kitti.txt"),
             self.odom_poses[:, :3].reshape(-1, 12),
         )
+        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"))
         self.results.log_to_file(self._results_dir)
 
     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):
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 88b6b18..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
@@ -88,7 +87,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 +106,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}"
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
diff --git a/python/pyproject.toml b/python/pyproject.toml
index 0ca28ec..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" }]
-requires-python = ">=3.8"
+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"