From 6da404912026690e5a62800029945f60f8f9f740 Mon Sep 17 00:00:00 2001
From: tizianoGuadagnino <frevo93@gmail.com>
Date: Mon, 25 Mar 2024 13:16:33 +0100
Subject: [PATCH 01/39] First trial, this is very ugly

---
 cpp/kiss_icp/core/Registration.cpp | 64 +++++++++++++++++-------------
 cpp/kiss_icp/core/Registration.hpp | 21 +++++++---
 cpp/kiss_icp/pipeline/KissICP.cpp  | 34 ++++++++--------
 cpp/kiss_icp/pipeline/KissICP.hpp  |  4 +-
 4 files changed, 72 insertions(+), 51 deletions(-)

diff --git a/cpp/kiss_icp/core/Registration.cpp b/cpp/kiss_icp/core/Registration.cpp
index b3d8d1cf..047c91e6 100644
--- a/cpp/kiss_icp/core/Registration.cpp
+++ b/cpp/kiss_icp/core/Registration.cpp
@@ -29,6 +29,7 @@
 
 #include <algorithm>
 #include <cmath>
+#include <functional>
 #include <numeric>
 #include <sophus/se3.hpp>
 #include <sophus/so3.hpp>
@@ -36,13 +37,9 @@
 
 #include "VoxelHashMap.hpp"
 
-namespace Eigen {
-using Matrix6d = Eigen::Matrix<double, 6, 6>;
-using Matrix3_6d = Eigen::Matrix<double, 3, 6>;
-using Vector6d = Eigen::Matrix<double, 6, 1>;
-}  // namespace Eigen
-using Associations = std::vector<std::pair<Eigen::Vector3d, Eigen::Vector3d>>;
-using LinearSystem = std::pair<Eigen::Matrix6d, Eigen::Vector6d>;
+using Correspondence = std::pair<Eigen::Vector3d, Eigen::Vector3d>;
+using Associations = std::vector<Correspondence>;
+using LinearSystem = std::tuple<Eigen::Matrix6d, Eigen::Vector6d, double>;
 
 namespace {
 inline double square(double x) { return x * x; }
@@ -120,7 +117,8 @@ Associations FindAssociations(const std::vector<Eigen::Vector3d> &points,
     return associations;
 }
 
-LinearSystem BuildLinearSystem(const Associations &associations, double kernel) {
+LinearSystem BuildLinearSystem(const Associations &associations,
+                               std::function<double_t(double_t)> kernel) {
     auto compute_jacobian_and_residual = [](auto association) {
         const auto &[source, target] = association;
         const Eigen::Vector3d residual = source - target;
@@ -131,33 +129,33 @@ LinearSystem BuildLinearSystem(const Associations &associations, double kernel)
     };
 
     auto sum_linear_systems = [](LinearSystem a, const LinearSystem &b) {
-        a.first += b.first;
-        a.second += b.second;
+        std::get<Eigen::Matrix6d>(a) += std::get<Eigen::Matrix6d>(b);
+        std::get<Eigen::Vector6d>(a) += std::get<Eigen::Vector6d>(b);
+        std::get<double>(a) += std::get<double>(b);
         return a;
     };
 
-    auto GM_weight = [&](double residual2) { return square(kernel) / square(kernel + residual2); };
-
     using associations_iterator = Associations::const_iterator;
-    const auto &[JTJ, JTr] = tbb::parallel_reduce(
+    const auto &[JTJ, JTr, chi_square] = tbb::parallel_reduce(
         // Range
         tbb::blocked_range<associations_iterator>{associations.cbegin(), associations.cend()},
         // Identity
-        LinearSystem(Eigen::Matrix6d::Zero(), Eigen::Vector6d::Zero()),
+        LinearSystem(Eigen::Matrix6d::Zero(), Eigen::Vector6d::Zero(), 0.0),
         // 1st Lambda: Parallel computation
         [&](const tbb::blocked_range<associations_iterator> &r, LinearSystem J) -> LinearSystem {
             return std::transform_reduce(
                 r.begin(), r.end(), J, sum_linear_systems, [&](const auto &association) {
                     const auto &[J_r, residual] = compute_jacobian_and_residual(association);
-                    const double w = GM_weight(residual.squaredNorm());
-                    return LinearSystem(J_r.transpose() * w * J_r,        // JTJ
-                                        J_r.transpose() * w * residual);  // JTr
+                    const double chi_square = residual.squaredNorm();
+                    const double w = kernel(chi_square);
+                    return LinearSystem(J_r.transpose() * w * J_r,                    // JTJ
+                                        J_r.transpose() * w * residual, chi_square);  // JTr
                 });
         },
         // 2nd Lambda: Parallel reduction of the private Jacboians
         sum_linear_systems);
 
-    return {JTJ, JTr};
+    return {JTJ, JTr, chi_square};
 }
 }  // namespace
 
@@ -174,24 +172,28 @@ Registration::Registration(int max_num_iteration, double convergence_criterion,
         tbb::global_control::max_allowed_parallelism, static_cast<size_t>(max_num_threads_));
 }
 
-Sophus::SE3d Registration::AlignPointsToMap(const std::vector<Eigen::Vector3d> &frame,
-                                            const VoxelHashMap &voxel_map,
-                                            const Sophus::SE3d &initial_guess,
-                                            double max_correspondence_distance,
-                                            double kernel) {
+Estimate Registration::AlignPointsToMap(const std::vector<Eigen::Vector3d> &frame,
+                                        const VoxelHashMap &voxel_map,
+                                        const Estimate &initial_guess,
+                                        double max_correspondence_distance,
+                                        double kernel_threshold) {
     if (voxel_map.Empty()) return initial_guess;
 
+    auto GM = [&](double residual2) {
+        return square(kernel_threshold) / square(kernel_threshold + residual2);
+    };
     // Equation (9)
     std::vector<Eigen::Vector3d> source = frame;
-    TransformPoints(initial_guess, source);
+    TransformPoints(initial_guess.pose, source);
 
     // ICP-loop
     Sophus::SE3d T_icp = Sophus::SE3d();
+    Associations associations;
     for (int j = 0; j < max_num_iterations_; ++j) {
         // Equation (10)
-        const auto associations = FindAssociations(source, voxel_map, max_correspondence_distance);
+        associations = FindAssociations(source, voxel_map, max_correspondence_distance);
         // Equation (11)
-        const auto &[JTJ, JTr] = BuildLinearSystem(associations, kernel);
+        const auto &[JTJ, JTr, chi_square] = BuildLinearSystem(associations, GM);
         const Eigen::Vector6d dx = JTJ.ldlt().solve(-JTr);
         const Sophus::SE3d estimation = Sophus::SE3d::exp(dx);
         // Equation (12)
@@ -201,8 +203,16 @@ Sophus::SE3d Registration::AlignPointsToMap(const std::vector<Eigen::Vector3d> &
         // Termination criteria
         if (dx.norm() < convergence_criterion_) break;
     }
+    const auto &[information_matrix, information_vector, chi_square] =
+        BuildLinearSystem(associations, [](double x) { return x / x; });
+    Estimate estimate;
+    estimate.pose = T_icp * initial_guess.pose;
+    const auto A = T_icp.inverse().Adj();
+    estimate.covariance =
+        A * initial_guess.covariance * A.transpose() +
+        (chi_square / static_cast<double>(associations.size()) * information_matrix).inverse();
     // Spit the final transformation
-    return T_icp * initial_guess;
+    return estimate;
 }
 
 }  // namespace kiss_icp
diff --git a/cpp/kiss_icp/core/Registration.hpp b/cpp/kiss_icp/core/Registration.hpp
index d878c2fc..3ea90753 100644
--- a/cpp/kiss_icp/core/Registration.hpp
+++ b/cpp/kiss_icp/core/Registration.hpp
@@ -28,16 +28,27 @@
 
 #include "VoxelHashMap.hpp"
 
+namespace Eigen {
+using Matrix6d = Eigen::Matrix<double, 6, 6>;
+using Matrix3_6d = Eigen::Matrix<double, 3, 6>;
+using Vector6d = Eigen::Matrix<double, 6, 1>;
+}  // namespace Eigen
+
 namespace kiss_icp {
 
+struct Estimate {
+    Sophus::SE3d pose;
+    Eigen::Matrix6d covariance = Eigen::Matrix6d::Zero();
+};
+
 struct Registration {
     explicit Registration(int max_num_iteration, double convergence_criterion, int max_num_threads);
 
-    Sophus::SE3d AlignPointsToMap(const std::vector<Eigen::Vector3d> &frame,
-                                  const VoxelHashMap &voxel_map,
-                                  const Sophus::SE3d &initial_guess,
-                                  double max_correspondence_distance,
-                                  double kernel);
+    Estimate AlignPointsToMap(const std::vector<Eigen::Vector3d> &frame,
+                              const VoxelHashMap &voxel_map,
+                              const Estimate &initial_guess,
+                              double max_correspondence_distance,
+                              double kernel_threshold);
 
     int max_num_iterations_;
     double convergence_criterion_;
diff --git a/cpp/kiss_icp/pipeline/KissICP.cpp b/cpp/kiss_icp/pipeline/KissICP.cpp
index 283f0b7d..b2818040 100644
--- a/cpp/kiss_icp/pipeline/KissICP.cpp
+++ b/cpp/kiss_icp/pipeline/KissICP.cpp
@@ -45,8 +45,8 @@ KissICP::Vector3dVectorTuple KissICP::RegisterFrame(const std::vector<Eigen::Vec
         if (N <= 2) return frame;
 
         // Estimate linear and angular velocities
-        const auto &start_pose = poses_[N - 2];
-        const auto &finish_pose = poses_[N - 1];
+        const auto &start_pose = estimates_[N - 2].pose;
+        const auto &finish_pose = estimates_[N - 1].pose;
         return DeSkewScan(frame, timestamps, start_pose, finish_pose);
     }();
     return RegisterFrame(deskew_frame);
@@ -64,19 +64,18 @@ KissICP::Vector3dVectorTuple KissICP::RegisterFrame(const std::vector<Eigen::Vec
 
     // Compute initial_guess for ICP
     const auto prediction = GetPredictionModel();
-    const auto last_pose = !poses_.empty() ? poses_.back() : Sophus::SE3d();
-    const auto initial_guess = last_pose * prediction;
-
+    auto initial_guess = !estimates_.empty() ? estimates_.back() : Estimate();
+    initial_guess.pose = initial_guess.pose * prediction;
     // Run icp
-    const Sophus::SE3d new_pose = registration_.AlignPointsToMap(source,         //
-                                                                 local_map_,     //
-                                                                 initial_guess,  //
-                                                                 3.0 * sigma,    //
-                                                                 sigma / 3.0);
-    const auto model_deviation = initial_guess.inverse() * new_pose;
+    const auto new_estimate = registration_.AlignPointsToMap(source,         //
+                                                             local_map_,     //
+                                                             initial_guess,  //
+                                                             3.0 * sigma,    //
+                                                             sigma / 3.0);
+    const auto model_deviation = initial_guess.pose.inverse() * new_estimate.pose;
     adaptive_threshold_.UpdateModelDeviation(model_deviation);
-    local_map_.Update(frame_downsample, new_pose);
-    poses_.push_back(new_pose);
+    local_map_.Update(frame_downsample, new_estimate.pose);
+    estimates_.push_back(new_estimate);
     return {frame, source};
 }
 
@@ -96,14 +95,15 @@ double KissICP::GetAdaptiveThreshold() {
 
 Sophus::SE3d KissICP::GetPredictionModel() const {
     Sophus::SE3d pred = Sophus::SE3d();
-    const size_t N = poses_.size();
+    const size_t N = estimates_.size();
     if (N < 2) return pred;
-    return poses_[N - 2].inverse() * poses_[N - 1];
+    return estimates_[N - 2].pose.inverse() * estimates_[N - 1].pose;
 }
 
 bool KissICP::HasMoved() {
-    if (poses_.empty()) return false;
-    const double motion = (poses_.front().inverse() * poses_.back()).translation().norm();
+    if (estimates_.empty()) return false;
+    const double motion =
+        (estimates_.front().pose.inverse() * estimates_.back().pose).translation().norm();
     return motion > 5.0 * config_.min_motion_th;
 }
 
diff --git a/cpp/kiss_icp/pipeline/KissICP.hpp b/cpp/kiss_icp/pipeline/KissICP.hpp
index 69b5830e..4b22124a 100644
--- a/cpp/kiss_icp/pipeline/KissICP.hpp
+++ b/cpp/kiss_icp/pipeline/KissICP.hpp
@@ -77,11 +77,11 @@ class KissICP {
 public:
     // Extra C++ API to facilitate ROS debugging
     std::vector<Eigen::Vector3d> LocalMap() const { return local_map_.Pointcloud(); };
-    std::vector<Sophus::SE3d> poses() const { return poses_; };
+    std::vector<Estimate> poses() const { return estimates_; };
 
 private:
     // KISS-ICP pipeline modules
-    std::vector<Sophus::SE3d> poses_;
+    std::vector<Estimate> estimates_;
     KISSConfig config_;
     Registration registration_;
     VoxelHashMap local_map_;

From e26b2f59dc0c8621f458d2f42241b549079e59a2 Mon Sep 17 00:00:00 2001
From: tizianoGuadagnino <frevo93@gmail.com>
Date: Mon, 25 Mar 2024 13:31:56 +0100
Subject: [PATCH 02/39] Start explosing covariance in ros...WIP

---
 cpp/kiss_icp/pipeline/KissICP.hpp |  2 +-
 ros/src/OdometryServer.cpp        | 21 +++++++++++++--------
 ros/src/OdometryServer.hpp        |  4 ----
 3 files changed, 14 insertions(+), 13 deletions(-)

diff --git a/cpp/kiss_icp/pipeline/KissICP.hpp b/cpp/kiss_icp/pipeline/KissICP.hpp
index 4b22124a..bbeb9532 100644
--- a/cpp/kiss_icp/pipeline/KissICP.hpp
+++ b/cpp/kiss_icp/pipeline/KissICP.hpp
@@ -77,7 +77,7 @@ class KissICP {
 public:
     // Extra C++ API to facilitate ROS debugging
     std::vector<Eigen::Vector3d> LocalMap() const { return local_map_.Pointcloud(); };
-    std::vector<Estimate> poses() const { return estimates_; };
+    std::vector<Estimate> estimates() const { return estimates_; };
 
 private:
     // KISS-ICP pipeline modules
diff --git a/ros/src/OdometryServer.cpp b/ros/src/OdometryServer.cpp
index 83563896..ee8023e4 100644
--- a/ros/src/OdometryServer.cpp
+++ b/ros/src/OdometryServer.cpp
@@ -28,6 +28,7 @@
 
 // KISS-ICP-ROS
 #include "OdometryServer.hpp"
+#include "Registration.hpp"
 #include "Utils.hpp"
 
 // KISS-ICP
@@ -135,24 +136,28 @@ void OdometryServer::RegisterFrame(const sensor_msgs::msg::PointCloud2::ConstSha
     const auto &[frame, keypoints] = kiss_icp_->RegisterFrame(points, timestamps);
 
     // Compute the pose using KISS, ego-centric to the LiDAR
-    const Sophus::SE3d kiss_pose = kiss_icp_->poses().back();
+    const auto kiss_estimate = kiss_icp_->estimates().back();
 
     // If necessary, transform the ego-centric pose to the specified base_link/base_footprint frame
-    const auto pose = [&]() -> Sophus::SE3d {
-        if (egocentric_estimation) return kiss_pose;
+    const auto estimate = [&]() -> kiss_icp::Estimate {
+        if (egocentric_estimation) return kiss_estimate;
+        kiss_icp::Estimate transformed;
         const Sophus::SE3d cloud2base = LookupTransform(base_frame_, cloud_frame_id);
-        return cloud2base * kiss_pose * cloud2base.inverse();
+        transformed.pose = cloud2base * kiss_estimate.pose * cloud2base.inverse();
+        transformed.covariance =
+            cloud2base.Adj() * kiss_estimate.covariance * cloud2base.Adj().transpose();
+        return transformed;
     }();
 
     // Spit the current estimated pose to ROS msgs
-    PublishOdometry(pose, msg->header.stamp, cloud_frame_id);
+    PublishOdometry(estimate, msg->header.stamp, cloud_frame_id);
     // Publishing this clouds is a bit costly, so do it only if we are debugging
     if (publish_debug_clouds_) {
         PublishClouds(frame, keypoints, msg->header.stamp, cloud_frame_id);
     }
 }
 
-void OdometryServer::PublishOdometry(const Sophus::SE3d &pose,
+void OdometryServer::PublishOdometry(const kiss_icp::Estimate &estimate,
                                      const rclcpp::Time &stamp,
                                      const std::string &cloud_frame_id) {
     // Broadcast the tf ---
@@ -161,7 +166,7 @@ void OdometryServer::PublishOdometry(const Sophus::SE3d &pose,
         transform_msg.header.stamp = stamp;
         transform_msg.header.frame_id = odom_frame_;
         transform_msg.child_frame_id = base_frame_.empty() ? cloud_frame_id : base_frame_;
-        transform_msg.transform = tf2::sophusToTransform(pose);
+        transform_msg.transform = tf2::sophusToTransform(estimate.pose);
         tf_broadcaster_->sendTransform(transform_msg);
     }
 
@@ -169,7 +174,7 @@ void OdometryServer::PublishOdometry(const Sophus::SE3d &pose,
     nav_msgs::msg::Odometry odom_msg;
     odom_msg.header.stamp = stamp;
     odom_msg.header.frame_id = odom_frame_;
-    odom_msg.pose.pose = tf2::sophusToPose(pose);
+    odom_msg.pose.pose = tf2::sophusToPose(estimate.pose);
     odom_msg.pose.covariance.fill(0.0);
     odom_msg.pose.covariance[0] = position_covariance_;
     odom_msg.pose.covariance[7] = position_covariance_;
diff --git a/ros/src/OdometryServer.hpp b/ros/src/OdometryServer.hpp
index 87f44aea..7ca7ff8c 100644
--- a/ros/src/OdometryServer.hpp
+++ b/ros/src/OdometryServer.hpp
@@ -85,10 +85,6 @@ class OdometryServer : public rclcpp::Node {
     /// Global/map coordinate frame.
     std::string odom_frame_{"odom"};
     std::string base_frame_{};
-
-    /// Covariance diagonal
-    double position_covariance_;
-    double orientation_covariance_;
 };
 
 }  // namespace kiss_icp_ros

From 3eb20698da0dfa7e3b9aa39f4dfea9da36545e0c Mon Sep 17 00:00:00 2001
From: Ignacio Vizzo <ignaciovizzo@gmail.com>
Date: Mon, 25 Mar 2024 13:42:55 +0100
Subject: [PATCH 03/39] Fix and improve ROS visualization (#285)

* Fix this madness

Simplify implementation by debugging in an ego-centric way always.

Since the KISS-ICP internal map is on the global coordinates, fetch the
last ego-centric pose and apply it to the map. Seen from the
cloud_frame_id (which is the sensor frame) everything should always work
in terms of visualization, no matter the multi-sensor setup.

* Now is safe to disable this by default

* Simplify, borrow the header from the input pointcloud msg

This actually makes the visualization closer to the Python visualizer

* Disable path/odom visualization by default

In the case where we are not computing the poses in an egocentric world
(base_frame != "") and we are not publishing to the TF tree, then the
visualization wouldn't make sense. Therefore, disable it by default

* Changed my mind

If someone doesn't have that particular frame defined, then the
visualization won't work. Leave this default

* Move responsability of handling tf frames out of Registration (#288)

* Move responsability of handling tf frames out of Registration

Since with this new changes PublishOdometry is the only member that
requieres to know the user specified target-frame, it is not necesary to
handle all this bits.

This makes the implementation cleaner, easier to read and reduces the
lines of code. Now RegisterFrame is a simple callback as few months ago
one can read in seconds.

* typo

* Easier to read

* We need this for LookupTransform

* Remove unused variable

* Revert "Remove unused variable"

This reverts commit 424ee901c5442ef1f1651f48079c8cbf5a3f10bf.

* Remove unnecessary check

* Remove unnecessary exposed utility function from ROS API

With this change this function is not exposed (which was never the
intention to) to the header. This way we can also "hide" this into a
private unnamed namesapces and benefit from inlining the simple function
into the translation unit

* Revert "Remove unnecessary exposed utility function from ROS API"

This reverts commit 23cd7efafd133c4f209f8575d079dee2d14702ae.

* Revert "Remove unnecessary check"

This reverts commit d1dcb48762392aae879e84d849c8f4b9c6e6cc10.

* merge conflicts :0

* Remove unnecessary exposed utility function from ROS API (#289)

* Move responsability of handling tf frames out of Registration

Since with this new changes PublishOdometry is the only member that
requieres to know the user specified target-frame, it is not necesary to
handle all this bits.

This makes the implementation cleaner, easier to read and reduces the
lines of code. Now RegisterFrame is a simple callback as few months ago
one can read in seconds.

* typo

* Easier to read

* We need this for LookupTransform

* Remove unused variable

* Revert "Remove unused variable"

This reverts commit 424ee901c5442ef1f1651f48079c8cbf5a3f10bf.

* Remove unnecessary check

* Remove unnecessary exposed utility function from ROS API

With this change this function is not exposed (which was never the
intention to) to the header. This way we can also "hide" this into a
private unnamed namesapces and benefit from inlining the simple function
into the translation unit

* Revert "Remove unnecessary exposed utility function from ROS API"

This reverts commit 23cd7efafd133c4f209f8575d079dee2d14702ae.

* Remove unnecessary exposed utility function from ROS API

With this change this function is not exposed (which was never the
intention to) to the header. This way we can also "hide" this into a
private unnamed namesapces and benefit from inlining the simple function
into the translation unit

* Revert "Remove unnecessary check"

This reverts commit d1dcb48762392aae879e84d849c8f4b9c6e6cc10.

---------

Co-authored-by: tizianoGuadagnino <frevo93@gmail.com>

* too many merges

* Merge Rviz and Python colors

* Just make the default construction more clear

---------

Co-authored-by: tizianoGuadagnino <frevo93@gmail.com>
---
 python/kiss_icp/tools/visualizer.py |   6 +-
 ros/rviz/kiss_icp.rviz              |  30 +++----
 ros/src/OdometryServer.cpp          | 119 +++++++++++-----------------
 ros/src/OdometryServer.hpp          |  12 +--
 4 files changed, 67 insertions(+), 100 deletions(-)

diff --git a/python/kiss_icp/tools/visualizer.py b/python/kiss_icp/tools/visualizer.py
index ad0900d0..3e9c9257 100644
--- a/python/kiss_icp/tools/visualizer.py
+++ b/python/kiss_icp/tools/visualizer.py
@@ -29,7 +29,7 @@
 
 import numpy as np
 
-YELLOW = np.array([1, 0.706, 0])
+CYAN = np.array([0.24, 0.898, 1])
 RED = np.array([128, 0, 0]) / 255.0
 BLACK = np.array([0, 0, 0]) / 255.0
 BLUE = np.array([0.4, 0.5, 0.9])
@@ -198,7 +198,7 @@ def _update_geometries(self, source, keypoints, target, pose):
         # Source hot frame
         if self.render_source:
             self.source.points = self.o3d.utility.Vector3dVector(source)
-            self.source.paint_uniform_color(YELLOW)
+            self.source.paint_uniform_color(CYAN)
             if self.global_view:
                 self.source.transform(pose)
         else:
@@ -207,7 +207,7 @@ def _update_geometries(self, source, keypoints, target, pose):
         # Keypoints
         if self.render_keypoints:
             self.keypoints.points = self.o3d.utility.Vector3dVector(keypoints)
-            self.keypoints.paint_uniform_color(YELLOW)
+            self.keypoints.paint_uniform_color(CYAN)
             if self.global_view:
                 self.keypoints.transform(pose)
         else:
diff --git a/ros/rviz/kiss_icp.rviz b/ros/rviz/kiss_icp.rviz
index 7320bdd0..0bf30841 100644
--- a/ros/rviz/kiss_icp.rviz
+++ b/ros/rviz/kiss_icp.rviz
@@ -23,7 +23,7 @@ Panels:
     Experimental: false
     Name: Time
     SyncMode: 0
-    SyncSource: frame_deskew
+    SyncSource: ""
 Visualization Manager:
   Class: ""
   Displays:
@@ -38,7 +38,7 @@ Visualization Manager:
           Axis: Z
           Channel Name: intensity
           Class: rviz_default_plugins/PointCloud2
-          Color: 255; 255; 0
+          Color: 61; 229; 255
           Color Transformer: FlatColor
           Decay Time: 0
           Enabled: true
@@ -52,7 +52,7 @@ Visualization Manager:
           Selectable: true
           Size (Pixels): 3
           Size (m): 0.019999999552965164
-          Style: Flat Squares
+          Style: Points
           Topic:
             Depth: 5
             Durability Policy: Volatile
@@ -84,8 +84,8 @@ Visualization Manager:
           Name: kiss_keypoints
           Position Transformer: XYZ
           Selectable: true
-          Size (Pixels): 10
-          Size (m): 1
+          Size (Pixels): 3
+          Size (m): 0.20000000298023224
           Style: Points
           Topic:
             Depth: 5
@@ -100,14 +100,14 @@ Visualization Manager:
         - Alpha: 1
           Autocompute Intensity Bounds: true
           Autocompute Value Bounds:
-            Max Value: 98.31531524658203
-            Min Value: 0.6587954163551331
+            Max Value: 14.369325637817383
+            Min Value: 0.20239250361919403
             Value: true
           Axis: Z
           Channel Name: intensity
           Class: rviz_default_plugins/PointCloud2
-          Color: 255; 255; 255
-          Color Transformer: AxisColor
+          Color: 102; 102; 102
+          Color Transformer: FlatColor
           Decay Time: 0
           Enabled: true
           Invert Rainbow: false
@@ -174,7 +174,7 @@ Visualization Manager:
             Reliability Policy: Best Effort
             Value: /kiss/odometry
           Value: true
-      Enabled: true
+      Enabled: false
       Name: odometry
     - Class: rviz_default_plugins/Axes
       Enabled: true
@@ -185,7 +185,7 @@ Visualization Manager:
       Value: true
   Enabled: true
   Global Options:
-    Background Color: 48; 48; 48
+    Background Color: 0; 0; 0
     Fixed Frame: odom
     Frame Rate: 30
   Name: root
@@ -229,16 +229,16 @@ Visualization Manager:
   Views:
     Current:
       Class: rviz_default_plugins/Orbit
-      Distance: 39.488563537597656
+      Distance: 178.319580078125
       Enable Stereo Rendering:
         Stereo Eye Separation: 0.05999999865889549
         Stereo Focal Distance: 1
         Swap Stereo Eyes: false
         Value: false
       Focal Point:
-        X: 3.482842445373535
-        Y: 4.3972296714782715
-        Z: 10.242613792419434
+        X: -68.01193237304688
+        Y: 258.05126953125
+        Z: -27.22064781188965
       Focal Shape Fixed Size: false
       Focal Shape Size: 0.05000000074505806
       Invert Z Axis: false
diff --git a/ros/src/OdometryServer.cpp b/ros/src/OdometryServer.cpp
index ee8023e4..0b2b7b20 100644
--- a/ros/src/OdometryServer.cpp
+++ b/ros/src/OdometryServer.cpp
@@ -46,6 +46,26 @@
 #include <sensor_msgs/msg/point_cloud2.hpp>
 #include <std_msgs/msg/string.hpp>
 
+namespace {
+Sophus::SE3d LookupTransform(const std::string &target_frame,
+                             const std::string &source_frame,
+                             const std::unique_ptr<tf2_ros::Buffer> &tf2_buffer) {
+    std::string err_msg;
+    if (tf2_buffer->canTransform(target_frame, source_frame, tf2::TimePointZero, &err_msg)) {
+        try {
+            auto tf = tf2_buffer->lookupTransform(target_frame, source_frame, tf2::TimePointZero);
+            return tf2::transformToSophus(tf);
+        } catch (tf2::TransformException &ex) {
+            RCLCPP_WARN(rclcpp::get_logger("LookupTransform"), "%s", ex.what());
+        }
+    }
+    RCLCPP_WARN(rclcpp::get_logger("LookupTransform"), "Failed to find tf. Reason=%s",
+                err_msg.c_str());
+    // default construction is the identity
+    return Sophus::SE3d();
+}
+}  // namespace
+
 namespace kiss_icp_ros {
 
 using utils::EigenToPointCloud2;
@@ -109,70 +129,49 @@ OdometryServer::OdometryServer(const rclcpp::NodeOptions &options)
     RCLCPP_INFO(this->get_logger(), "KISS-ICP ROS 2 odometry node initialized");
 }
 
-Sophus::SE3d OdometryServer::LookupTransform(const std::string &target_frame,
-                                             const std::string &source_frame) const {
-    std::string err_msg;
-    if (tf2_buffer_->_frameExists(source_frame) &&  //
-        tf2_buffer_->_frameExists(target_frame) &&  //
-        tf2_buffer_->canTransform(target_frame, source_frame, tf2::TimePointZero, &err_msg)) {
-        try {
-            auto tf = tf2_buffer_->lookupTransform(target_frame, source_frame, tf2::TimePointZero);
-            return tf2::transformToSophus(tf);
-        } catch (tf2::TransformException &ex) {
-            RCLCPP_WARN(this->get_logger(), "%s", ex.what());
-        }
-    }
-    RCLCPP_WARN(this->get_logger(), "Failed to find tf. Reason=%s", err_msg.c_str());
-    return {};
-}
-
 void OdometryServer::RegisterFrame(const sensor_msgs::msg::PointCloud2::ConstSharedPtr &msg) {
     const auto cloud_frame_id = msg->header.frame_id;
     const auto points = PointCloud2ToEigen(msg);
     const auto timestamps = GetTimestamps(msg);
-    const auto egocentric_estimation = (base_frame_.empty() || base_frame_ == cloud_frame_id);
 
     // Register frame, main entry point to KISS-ICP pipeline
     const auto &[frame, keypoints] = kiss_icp_->RegisterFrame(points, timestamps);
 
-    // Compute the pose using KISS, ego-centric to the LiDAR
-    const auto kiss_estimate = kiss_icp_->estimates().back();
-
-    // If necessary, transform the ego-centric pose to the specified base_link/base_footprint frame
-    const auto estimate = [&]() -> kiss_icp::Estimate {
-        if (egocentric_estimation) return kiss_estimate;
-        kiss_icp::Estimate transformed;
-        const Sophus::SE3d cloud2base = LookupTransform(base_frame_, cloud_frame_id);
-        transformed.pose = cloud2base * kiss_estimate.pose * cloud2base.inverse();
-        transformed.covariance =
-            cloud2base.Adj() * kiss_estimate.covariance * cloud2base.Adj().transpose();
-        return transformed;
-    }();
+    // Extract the last KISS-ICP pose, ego-centric to the LiDAR
+    const Sophus::SE3d kiss_pose = kiss_icp_->poses().back();
 
-    // Spit the current estimated pose to ROS msgs
-    PublishOdometry(estimate, msg->header.stamp, cloud_frame_id);
-    // Publishing this clouds is a bit costly, so do it only if we are debugging
+    // Spit the current estimated pose to ROS msgs handling the desired target frame
+    PublishOdometry(kiss_pose, msg->header);
+    // Publishing these clouds is a bit costly, so do it only if we are debugging
     if (publish_debug_clouds_) {
-        PublishClouds(frame, keypoints, msg->header.stamp, cloud_frame_id);
+        PublishClouds(frame, keypoints, msg->header);
     }
 }
 
-void OdometryServer::PublishOdometry(const kiss_icp::Estimate &estimate,
-                                     const rclcpp::Time &stamp,
-                                     const std::string &cloud_frame_id) {
+void OdometryServer::PublishOdometry(const Sophus::SE3d &kiss_pose,
+                                     const std_msgs::msg::Header &header) {
+    // If necessary, transform the ego-centric pose to the specified base_link/base_footprint frame
+    const auto cloud_frame_id = header.frame_id;
+    const auto egocentric_estimation = (base_frame_.empty() || base_frame_ == cloud_frame_id);
+    const auto pose = [&]() -> Sophus::SE3d {
+        if (egocentric_estimation) return kiss_pose;
+        const Sophus::SE3d cloud2base = LookupTransform(base_frame_, cloud_frame_id, tf2_buffer_);
+        return cloud2base * kiss_pose * cloud2base.inverse();
+    }();
+
     // Broadcast the tf ---
     if (publish_odom_tf_) {
         geometry_msgs::msg::TransformStamped transform_msg;
-        transform_msg.header.stamp = stamp;
+        transform_msg.header.stamp = header.stamp;
         transform_msg.header.frame_id = odom_frame_;
-        transform_msg.child_frame_id = base_frame_.empty() ? cloud_frame_id : base_frame_;
-        transform_msg.transform = tf2::sophusToTransform(estimate.pose);
+        transform_msg.child_frame_id = egocentric_estimation ? cloud_frame_id : base_frame_;
+        transform_msg.transform = tf2::sophusToTransform(pose);
         tf_broadcaster_->sendTransform(transform_msg);
     }
 
     // publish odometry msg
     nav_msgs::msg::Odometry odom_msg;
-    odom_msg.header.stamp = stamp;
+    odom_msg.header.stamp = header.stamp;
     odom_msg.header.frame_id = odom_frame_;
     odom_msg.pose.pose = tf2::sophusToPose(estimate.pose);
     odom_msg.pose.covariance.fill(0.0);
@@ -187,39 +186,13 @@ void OdometryServer::PublishOdometry(const kiss_icp::Estimate &estimate,
 
 void OdometryServer::PublishClouds(const std::vector<Eigen::Vector3d> frame,
                                    const std::vector<Eigen::Vector3d> keypoints,
-                                   const rclcpp::Time &stamp,
-                                   const std::string &cloud_frame_id) {
-    std_msgs::msg::Header odom_header;
-    odom_header.stamp = stamp;
-    odom_header.frame_id = odom_frame_;
-
-    // Publish map
+                                   const std_msgs::msg::Header &header) {
     const auto kiss_map = kiss_icp_->LocalMap();
+    const auto kiss_pose = kiss_icp_->poses().back().inverse();
 
-    if (!publish_odom_tf_) {
-        // debugging happens in an egocentric world
-        std_msgs::msg::Header cloud_header;
-        cloud_header.stamp = stamp;
-        cloud_header.frame_id = cloud_frame_id;
-
-        frame_publisher_->publish(std::move(EigenToPointCloud2(frame, cloud_header)));
-        kpoints_publisher_->publish(std::move(EigenToPointCloud2(keypoints, cloud_header)));
-        map_publisher_->publish(std::move(EigenToPointCloud2(kiss_map, odom_header)));
-
-        return;
-    }
-
-    // If transmitting to tf tree we know where the clouds are exactly
-    const auto cloud2odom = LookupTransform(odom_frame_, cloud_frame_id);
-    frame_publisher_->publish(std::move(EigenToPointCloud2(frame, cloud2odom, odom_header)));
-    kpoints_publisher_->publish(std::move(EigenToPointCloud2(keypoints, cloud2odom, odom_header)));
-
-    if (!base_frame_.empty()) {
-        const Sophus::SE3d cloud2base = LookupTransform(base_frame_, cloud_frame_id);
-        map_publisher_->publish(std::move(EigenToPointCloud2(kiss_map, cloud2base, odom_header)));
-    } else {
-        map_publisher_->publish(std::move(EigenToPointCloud2(kiss_map, odom_header)));
-    }
+    frame_publisher_->publish(std::move(EigenToPointCloud2(frame, header)));
+    kpoints_publisher_->publish(std::move(EigenToPointCloud2(keypoints, header)));
+    map_publisher_->publish(std::move(EigenToPointCloud2(kiss_map, kiss_pose, header)));
 }
 }  // namespace kiss_icp_ros
 
diff --git a/ros/src/OdometryServer.hpp b/ros/src/OdometryServer.hpp
index 7ca7ff8c..07d8a29f 100644
--- a/ros/src/OdometryServer.hpp
+++ b/ros/src/OdometryServer.hpp
@@ -33,6 +33,7 @@
 #include <nav_msgs/msg/odometry.hpp>
 #include <rclcpp/rclcpp.hpp>
 #include <sensor_msgs/msg/point_cloud2.hpp>
+#include <std_msgs/msg/header.hpp>
 #include <string>
 
 namespace kiss_icp_ros {
@@ -48,19 +49,12 @@ class OdometryServer : public rclcpp::Node {
     void RegisterFrame(const sensor_msgs::msg::PointCloud2::ConstSharedPtr &msg);
 
     /// Stream the estimated pose to ROS
-    void PublishOdometry(const Sophus::SE3d &pose,
-                         const rclcpp::Time &stamp,
-                         const std::string &cloud_frame_id);
+    void PublishOdometry(const Sophus::SE3d &kiss_pose, const std_msgs::msg::Header &header);
 
     /// Stream the debugging point clouds for visualization (if required)
     void PublishClouds(const std::vector<Eigen::Vector3d> frame,
                        const std::vector<Eigen::Vector3d> keypoints,
-                       const rclcpp::Time &stamp,
-                       const std::string &cloud_frame_id);
-
-    /// Utility function to compute transformation using tf tree
-    Sophus::SE3d LookupTransform(const std::string &target_frame,
-                                 const std::string &source_frame) const;
+                       const std_msgs::msg::Header &header);
 
 private:
     /// Tools for broadcasting TFs.

From 747b28f6497b9ef7f9255f411e741aaf7942de0b Mon Sep 17 00:00:00 2001
From: tizianoGuadagnino <frevo93@gmail.com>
Date: Mon, 25 Mar 2024 13:31:56 +0100
Subject: [PATCH 04/39] Start explosing covariance in ros...WIP

---
 ros/src/OdometryServer.cpp | 39 ++++++++++++++++++++------------------
 1 file changed, 21 insertions(+), 18 deletions(-)

diff --git a/ros/src/OdometryServer.cpp b/ros/src/OdometryServer.cpp
index 0b2b7b20..9d87c1dc 100644
--- a/ros/src/OdometryServer.cpp
+++ b/ros/src/OdometryServer.cpp
@@ -137,35 +137,38 @@ void OdometryServer::RegisterFrame(const sensor_msgs::msg::PointCloud2::ConstSha
     // Register frame, main entry point to KISS-ICP pipeline
     const auto &[frame, keypoints] = kiss_icp_->RegisterFrame(points, timestamps);
 
-    // Extract the last KISS-ICP pose, ego-centric to the LiDAR
-    const Sophus::SE3d kiss_pose = kiss_icp_->poses().back();
+    // Compute the pose using KISS, ego-centric to the LiDAR
+    const auto kiss_estimate = kiss_icp_->estimates().back();
 
-    // Spit the current estimated pose to ROS msgs handling the desired target frame
-    PublishOdometry(kiss_pose, msg->header);
-    // Publishing these clouds is a bit costly, so do it only if we are debugging
+    // If necessary, transform the ego-centric pose to the specified base_link/base_footprint frame
+    const auto estimate = [&]() -> kiss_icp::Estimate {
+        if (egocentric_estimation) return kiss_estimate;
+        kiss_icp::Estimate transformed;
+        const Sophus::SE3d cloud2base = LookupTransform(base_frame_, cloud_frame_id);
+        transformed.pose = cloud2base * kiss_estimate.pose * cloud2base.inverse();
+        transformed.covariance =
+            cloud2base.Adj() * kiss_estimate.covariance * cloud2base.Adj().transpose();
+        return transformed;
+    }();
+
+    // Spit the current estimated pose to ROS msgs
+    PublishOdometry(estimate, msg->header.stamp, cloud_frame_id);
+    // Publishing this clouds is a bit costly, so do it only if we are debugging
     if (publish_debug_clouds_) {
         PublishClouds(frame, keypoints, msg->header);
     }
 }
 
-void OdometryServer::PublishOdometry(const Sophus::SE3d &kiss_pose,
-                                     const std_msgs::msg::Header &header) {
-    // If necessary, transform the ego-centric pose to the specified base_link/base_footprint frame
-    const auto cloud_frame_id = header.frame_id;
-    const auto egocentric_estimation = (base_frame_.empty() || base_frame_ == cloud_frame_id);
-    const auto pose = [&]() -> Sophus::SE3d {
-        if (egocentric_estimation) return kiss_pose;
-        const Sophus::SE3d cloud2base = LookupTransform(base_frame_, cloud_frame_id, tf2_buffer_);
-        return cloud2base * kiss_pose * cloud2base.inverse();
-    }();
-
+void OdometryServer::PublishOdometry(const kiss_icp::Estimate &estimate,
+                                     const rclcpp::Time &stamp,
+                                     const std::string &cloud_frame_id) {
     // Broadcast the tf ---
     if (publish_odom_tf_) {
         geometry_msgs::msg::TransformStamped transform_msg;
         transform_msg.header.stamp = header.stamp;
         transform_msg.header.frame_id = odom_frame_;
-        transform_msg.child_frame_id = egocentric_estimation ? cloud_frame_id : base_frame_;
-        transform_msg.transform = tf2::sophusToTransform(pose);
+        transform_msg.child_frame_id = base_frame_.empty() ? cloud_frame_id : base_frame_;
+        transform_msg.transform = tf2::sophusToTransform(estimate.pose);
         tf_broadcaster_->sendTransform(transform_msg);
     }
 

From 1836b341cd7d58c06fe13e2160e2133642ab668d Mon Sep 17 00:00:00 2001
From: tizianoGuadagnino <frevo93@gmail.com>
Date: Mon, 25 Mar 2024 14:44:15 +0100
Subject: [PATCH 05/39] Copy over the covariance

---
 cpp/kiss_icp/pipeline/KissICP.cpp |  2 +-
 ros/src/OdometryServer.cpp        | 11 +++++------
 2 files changed, 6 insertions(+), 7 deletions(-)

diff --git a/cpp/kiss_icp/pipeline/KissICP.cpp b/cpp/kiss_icp/pipeline/KissICP.cpp
index b2818040..5eb17072 100644
--- a/cpp/kiss_icp/pipeline/KissICP.cpp
+++ b/cpp/kiss_icp/pipeline/KissICP.cpp
@@ -41,7 +41,7 @@ KissICP::Vector3dVectorTuple KissICP::RegisterFrame(const std::vector<Eigen::Vec
         // TODO(Nacho) Add some asserts here to sanitize the timestamps
 
         //  If not enough poses for the estimation, do not de-skew
-        const size_t N = poses().size();
+        const size_t N = estimates().size();
         if (N <= 2) return frame;
 
         // Estimate linear and angular velocities
diff --git a/ros/src/OdometryServer.cpp b/ros/src/OdometryServer.cpp
index 9d87c1dc..1d45fc94 100644
--- a/ros/src/OdometryServer.cpp
+++ b/ros/src/OdometryServer.cpp
@@ -178,12 +178,11 @@ void OdometryServer::PublishOdometry(const kiss_icp::Estimate &estimate,
     odom_msg.header.frame_id = odom_frame_;
     odom_msg.pose.pose = tf2::sophusToPose(estimate.pose);
     odom_msg.pose.covariance.fill(0.0);
-    odom_msg.pose.covariance[0] = position_covariance_;
-    odom_msg.pose.covariance[7] = position_covariance_;
-    odom_msg.pose.covariance[14] = position_covariance_;
-    odom_msg.pose.covariance[21] = orientation_covariance_;
-    odom_msg.pose.covariance[28] = orientation_covariance_;
-    odom_msg.pose.covariance[35] = orientation_covariance_;
+    for (int r = 0; r < 6; ++r) {
+        for (int c = 0; c < 6; ++c) {
+            odom_msg.pose.covariance[r * 6 + c] = estimate.covariance(r, c);
+        }
+    }
     odom_publisher_->publish(std::move(odom_msg));
 }
 

From c0b45cf4e64b61af3f87589447f15d996c928c81 Mon Sep 17 00:00:00 2001
From: tizianoGuadagnino <frevo93@gmail.com>
Date: Mon, 25 Mar 2024 15:30:10 +0100
Subject: [PATCH 06/39] Fix merge

---
 ros/src/OdometryServer.cpp | 31 ++++++++++++++-----------------
 ros/src/OdometryServer.hpp |  4 +++-
 2 files changed, 17 insertions(+), 18 deletions(-)

diff --git a/ros/src/OdometryServer.cpp b/ros/src/OdometryServer.cpp
index 1d45fc94..a61f0963 100644
--- a/ros/src/OdometryServer.cpp
+++ b/ros/src/OdometryServer.cpp
@@ -28,8 +28,8 @@
 
 // KISS-ICP-ROS
 #include "OdometryServer.hpp"
-#include "Registration.hpp"
 #include "Utils.hpp"
+#include "kiss_icp/core/Registration.hpp"
 
 // KISS-ICP
 #include "kiss_icp/pipeline/KissICP.hpp"
@@ -78,8 +78,6 @@ OdometryServer::OdometryServer(const rclcpp::NodeOptions &options)
     odom_frame_ = declare_parameter<std::string>("odom_frame", odom_frame_);
     publish_odom_tf_ = declare_parameter<bool>("publish_odom_tf", publish_odom_tf_);
     publish_debug_clouds_ = declare_parameter<bool>("publish_debug_clouds", publish_debug_clouds_);
-    position_covariance_ = declare_parameter<double>("position_covariance", 0.1);
-    orientation_covariance_ = declare_parameter<double>("orientation_covariance", 0.1);
 
     kiss_icp::pipeline::KISSConfig config;
     config.max_range = declare_parameter<double>("max_range", config.max_range);
@@ -140,28 +138,27 @@ void OdometryServer::RegisterFrame(const sensor_msgs::msg::PointCloud2::ConstSha
     // Compute the pose using KISS, ego-centric to the LiDAR
     const auto kiss_estimate = kiss_icp_->estimates().back();
 
+    // Spit the current estimated pose to ROS msgs
+    PublishOdometry(kiss_estimate, msg->header);
+    // Publishing this clouds is a bit costly, so do it only if we are debugging
+    if (publish_debug_clouds_) {
+        PublishClouds(frame, keypoints, msg->header);
+    }
+}
+void OdometryServer::PublishOdometry(const kiss_icp::Estimate &kiss_estimate,
+                                     const std_msgs::msg::Header &header) {
     // If necessary, transform the ego-centric pose to the specified base_link/base_footprint frame
+    const auto cloud_frame_id = header.frame_id;
+    const auto egocentric_estimation = (base_frame_.empty() || base_frame_ == cloud_frame_id);
     const auto estimate = [&]() -> kiss_icp::Estimate {
         if (egocentric_estimation) return kiss_estimate;
         kiss_icp::Estimate transformed;
-        const Sophus::SE3d cloud2base = LookupTransform(base_frame_, cloud_frame_id);
+        const Sophus::SE3d cloud2base = LookupTransform(base_frame_, cloud_frame_id, tf2_buffer_);
         transformed.pose = cloud2base * kiss_estimate.pose * cloud2base.inverse();
         transformed.covariance =
             cloud2base.Adj() * kiss_estimate.covariance * cloud2base.Adj().transpose();
         return transformed;
     }();
-
-    // Spit the current estimated pose to ROS msgs
-    PublishOdometry(estimate, msg->header.stamp, cloud_frame_id);
-    // Publishing this clouds is a bit costly, so do it only if we are debugging
-    if (publish_debug_clouds_) {
-        PublishClouds(frame, keypoints, msg->header);
-    }
-}
-
-void OdometryServer::PublishOdometry(const kiss_icp::Estimate &estimate,
-                                     const rclcpp::Time &stamp,
-                                     const std::string &cloud_frame_id) {
     // Broadcast the tf ---
     if (publish_odom_tf_) {
         geometry_msgs::msg::TransformStamped transform_msg;
@@ -190,7 +187,7 @@ void OdometryServer::PublishClouds(const std::vector<Eigen::Vector3d> frame,
                                    const std::vector<Eigen::Vector3d> keypoints,
                                    const std_msgs::msg::Header &header) {
     const auto kiss_map = kiss_icp_->LocalMap();
-    const auto kiss_pose = kiss_icp_->poses().back().inverse();
+    const auto kiss_pose = kiss_icp_->estimates().back().pose.inverse();
 
     frame_publisher_->publish(std::move(EigenToPointCloud2(frame, header)));
     kpoints_publisher_->publish(std::move(EigenToPointCloud2(keypoints, header)));
diff --git a/ros/src/OdometryServer.hpp b/ros/src/OdometryServer.hpp
index 07d8a29f..370337da 100644
--- a/ros/src/OdometryServer.hpp
+++ b/ros/src/OdometryServer.hpp
@@ -23,6 +23,7 @@
 #pragma once
 
 // KISS-ICP
+#include "kiss_icp/core/Registration.hpp"
 #include "kiss_icp/pipeline/KissICP.hpp"
 
 // ROS 2
@@ -49,7 +50,8 @@ class OdometryServer : public rclcpp::Node {
     void RegisterFrame(const sensor_msgs::msg::PointCloud2::ConstSharedPtr &msg);
 
     /// Stream the estimated pose to ROS
-    void PublishOdometry(const Sophus::SE3d &kiss_pose, const std_msgs::msg::Header &header);
+    void PublishOdometry(const kiss_icp::Estimate &kiss_estimate,
+                         const std_msgs::msg::Header &header);
 
     /// Stream the debugging point clouds for visualization (if required)
     void PublishClouds(const std::vector<Eigen::Vector3d> frame,

From a5aa93058fea135cebba9bc938c75f270fe46e3a Mon Sep 17 00:00:00 2001
From: tizianoGuadagnino <frevo93@gmail.com>
Date: Tue, 26 Mar 2024 09:42:41 +0100
Subject: [PATCH 07/39] More cosmesis

---
 cpp/kiss_icp/core/Registration.cpp | 12 ++++++------
 ros/src/OdometryServer.cpp         |  1 +
 2 files changed, 7 insertions(+), 6 deletions(-)

diff --git a/cpp/kiss_icp/core/Registration.cpp b/cpp/kiss_icp/core/Registration.cpp
index 047c91e6..d3378fd9 100644
--- a/cpp/kiss_icp/core/Registration.cpp
+++ b/cpp/kiss_icp/core/Registration.cpp
@@ -188,10 +188,9 @@ Estimate Registration::AlignPointsToMap(const std::vector<Eigen::Vector3d> &fram
 
     // ICP-loop
     Sophus::SE3d T_icp = Sophus::SE3d();
-    Associations associations;
     for (int j = 0; j < max_num_iterations_; ++j) {
         // Equation (10)
-        associations = FindAssociations(source, voxel_map, max_correspondence_distance);
+        const auto associations = FindAssociations(source, voxel_map, max_correspondence_distance);
         // Equation (11)
         const auto &[JTJ, JTr, chi_square] = BuildLinearSystem(associations, GM);
         const Eigen::Vector6d dx = JTJ.ldlt().solve(-JTr);
@@ -203,14 +202,15 @@ Estimate Registration::AlignPointsToMap(const std::vector<Eigen::Vector3d> &fram
         // Termination criteria
         if (dx.norm() < convergence_criterion_) break;
     }
-    const auto &[information_matrix, information_vector, chi_square] =
+    const auto associations = FindAssociations(source, voxel_map, max_correspondence_distance);
+    const auto &[H, std::ignore, chi_square] =
         BuildLinearSystem(associations, [](double x) { return x / x; });
+    const auto icp_covariance =
+        (chi_square / static_cast<double>(associations.size()) * H).inverse();
     Estimate estimate;
     estimate.pose = T_icp * initial_guess.pose;
     const auto A = T_icp.inverse().Adj();
-    estimate.covariance =
-        A * initial_guess.covariance * A.transpose() +
-        (chi_square / static_cast<double>(associations.size()) * information_matrix).inverse();
+    estimate.covariance = A * initial_guess.covariance * A.transpose() + icp_covariance;
     // Spit the final transformation
     return estimate;
 }
diff --git a/ros/src/OdometryServer.cpp b/ros/src/OdometryServer.cpp
index a61f0963..6925b19d 100644
--- a/ros/src/OdometryServer.cpp
+++ b/ros/src/OdometryServer.cpp
@@ -155,6 +155,7 @@ void OdometryServer::PublishOdometry(const kiss_icp::Estimate &kiss_estimate,
         kiss_icp::Estimate transformed;
         const Sophus::SE3d cloud2base = LookupTransform(base_frame_, cloud_frame_id, tf2_buffer_);
         transformed.pose = cloud2base * kiss_estimate.pose * cloud2base.inverse();
+        // TODO verify equation
         transformed.covariance =
             cloud2base.Adj() * kiss_estimate.covariance * cloud2base.Adj().transpose();
         return transformed;

From 10d4cd8756c7d6840f5beec4df96745158d165c3 Mon Sep 17 00:00:00 2001
From: tizianoGuadagnino <frevo93@gmail.com>
Date: Tue, 26 Mar 2024 11:04:30 +0100
Subject: [PATCH 08/39] Pybind the estimate

---
 cpp/kiss_icp/core/Registration.cpp         |  7 ++++++-
 cpp/kiss_icp/core/Registration.hpp         |  4 +---
 python/kiss_icp/pybind/kiss_icp_pybind.cpp | 18 ++++++++++++------
 python/kiss_icp/registration.py            | 13 +++++++++++++
 4 files changed, 32 insertions(+), 10 deletions(-)

diff --git a/cpp/kiss_icp/core/Registration.cpp b/cpp/kiss_icp/core/Registration.cpp
index d3378fd9..1e173044 100644
--- a/cpp/kiss_icp/core/Registration.cpp
+++ b/cpp/kiss_icp/core/Registration.cpp
@@ -41,6 +41,11 @@ using Correspondence = std::pair<Eigen::Vector3d, Eigen::Vector3d>;
 using Associations = std::vector<Correspondence>;
 using LinearSystem = std::tuple<Eigen::Matrix6d, Eigen::Vector6d, double>;
 
+namespace Eigen {
+using Matrix3_6d = Eigen::Matrix<double, 3, 6>;
+using Vector6d = Eigen::Matrix<double, 6, 1>;
+}  // namespace Eigen
+
 namespace {
 inline double square(double x) { return x * x; }
 
@@ -203,7 +208,7 @@ Estimate Registration::AlignPointsToMap(const std::vector<Eigen::Vector3d> &fram
         if (dx.norm() < convergence_criterion_) break;
     }
     const auto associations = FindAssociations(source, voxel_map, max_correspondence_distance);
-    const auto &[H, std::ignore, chi_square] =
+    const auto &[H, b, chi_square] =
         BuildLinearSystem(associations, [](double x) { return x / x; });
     const auto icp_covariance =
         (chi_square / static_cast<double>(associations.size()) * H).inverse();
diff --git a/cpp/kiss_icp/core/Registration.hpp b/cpp/kiss_icp/core/Registration.hpp
index 3ea90753..8eacb70f 100644
--- a/cpp/kiss_icp/core/Registration.hpp
+++ b/cpp/kiss_icp/core/Registration.hpp
@@ -30,9 +30,7 @@
 
 namespace Eigen {
 using Matrix6d = Eigen::Matrix<double, 6, 6>;
-using Matrix3_6d = Eigen::Matrix<double, 3, 6>;
-using Vector6d = Eigen::Matrix<double, 6, 1>;
-}  // namespace Eigen
+}
 
 namespace kiss_icp {
 
diff --git a/python/kiss_icp/pybind/kiss_icp_pybind.cpp b/python/kiss_icp/pybind/kiss_icp_pybind.cpp
index b514a6f5..f9a9078b 100644
--- a/python/kiss_icp/pybind/kiss_icp_pybind.cpp
+++ b/python/kiss_icp/pybind/kiss_icp_pybind.cpp
@@ -72,6 +72,15 @@ PYBIND11_MODULE(kiss_icp_pybind, m) {
         .def("_remove_far_away_points", &VoxelHashMap::RemovePointsFarFromLocation, "origin"_a)
         .def("_point_cloud", &VoxelHashMap::Pointcloud);
 
+    py::class_<Estimate> estimate(m, "_Estimate",
+                                  "Estimate from the icp, it contains a pose and a 6x6 covariance");
+    estimate.def(py::init<>())
+        .def_property(
+            "pose", [](Estimate &self) { return self.pose.matrix(); },
+            [](Estimate &self, const Eigen::Matrix4d &T) { self.pose = Sophus::SE3d(T); })
+        .def_property(
+            "covariance", [](Estimate &self) { return self.covariance; },
+            [](Estimate &self, const Eigen::Matrix6d &Sigma) { self.covariance = Sigma; });
     // Point Cloud registration
     py::class_<Registration> internal_registration(m, "_Registration", "Don't use this");
     internal_registration
@@ -80,13 +89,10 @@ PYBIND11_MODULE(kiss_icp_pybind, m) {
         .def(
             "_align_points_to_map",
             [](Registration &self, const std::vector<Eigen::Vector3d> &points,
-               const VoxelHashMap &voxel_map, const Eigen::Matrix4d &T_guess,
+               const VoxelHashMap &voxel_map, const Estimate &initial_guess,
                double max_correspondence_distance, double kernel) {
-                Sophus::SE3d initial_guess(T_guess);
-                return self
-                    .AlignPointsToMap(points, voxel_map, initial_guess, max_correspondence_distance,
-                                      kernel)
-                    .matrix();
+                return self.AlignPointsToMap(points, voxel_map, initial_guess,
+                                             max_correspondence_distance, kernel);
             },
             "points"_a, "voxel_map"_a, "initial_guess"_a, "max_correspondance_distance"_a,
             "kernel"_a);
diff --git a/python/kiss_icp/registration.py b/python/kiss_icp/registration.py
index 65282dfc..0f77985f 100644
--- a/python/kiss_icp/registration.py
+++ b/python/kiss_icp/registration.py
@@ -35,6 +35,19 @@ def get_registration(config: KISSConfig):
     )
 
 
+class Estimate:
+    def __init__(self):
+        self._estimate = kiss_icp_pybind._Estimate()
+
+    @property
+    def pose(self):
+        return self._estimate.pose
+
+    @property
+    def covariance(self):
+        return self._estimate.covariance
+
+
 class Registration:
     def __init__(
         self,

From 18a290aac54e695d19db46e64eb8312e49766e08 Mon Sep 17 00:00:00 2001
From: tizianoGuadagnino <frevo93@gmail.com>
Date: Tue, 26 Mar 2024 11:06:30 +0100
Subject: [PATCH 09/39] Reshape the usings for my mental health

---
 cpp/kiss_icp/core/Registration.cpp | 9 +++++----
 cpp/kiss_icp/core/Registration.hpp | 6 ++----
 2 files changed, 7 insertions(+), 8 deletions(-)

diff --git a/cpp/kiss_icp/core/Registration.cpp b/cpp/kiss_icp/core/Registration.cpp
index 1e173044..061faae2 100644
--- a/cpp/kiss_icp/core/Registration.cpp
+++ b/cpp/kiss_icp/core/Registration.cpp
@@ -37,15 +37,16 @@
 
 #include "VoxelHashMap.hpp"
 
-using Correspondence = std::pair<Eigen::Vector3d, Eigen::Vector3d>;
-using Associations = std::vector<Correspondence>;
-using LinearSystem = std::tuple<Eigen::Matrix6d, Eigen::Vector6d, double>;
-
 namespace Eigen {
+using Matrix6d = Eigen::Matrix<double, 6, 6>;
 using Matrix3_6d = Eigen::Matrix<double, 3, 6>;
 using Vector6d = Eigen::Matrix<double, 6, 1>;
 }  // namespace Eigen
 
+using Correspondence = std::pair<Eigen::Vector3d, Eigen::Vector3d>;
+using Associations = std::vector<Correspondence>;
+using LinearSystem = std::tuple<Eigen::Matrix6d, Eigen::Vector6d, double>;
+
 namespace {
 inline double square(double x) { return x * x; }
 
diff --git a/cpp/kiss_icp/core/Registration.hpp b/cpp/kiss_icp/core/Registration.hpp
index 8eacb70f..fb377e1f 100644
--- a/cpp/kiss_icp/core/Registration.hpp
+++ b/cpp/kiss_icp/core/Registration.hpp
@@ -28,15 +28,13 @@
 
 #include "VoxelHashMap.hpp"
 
-namespace Eigen {
-using Matrix6d = Eigen::Matrix<double, 6, 6>;
-}
+using CovarianceMatrixType = Eigen::Matrix<double, 6, 6>;
 
 namespace kiss_icp {
 
 struct Estimate {
     Sophus::SE3d pose;
-    Eigen::Matrix6d covariance = Eigen::Matrix6d::Zero();
+    CovarianceMatrixType covariance = CovarianceMatrixType::Zero();
 };
 
 struct Registration {

From d1f3c86b1f0ba91e184508e18e917f48c4cf52b1 Mon Sep 17 00:00:00 2001
From: tizianoGuadagnino <frevo93@gmail.com>
Date: Tue, 26 Mar 2024 11:37:41 +0100
Subject: [PATCH 10/39] Type aliasing on pybinded struct, I love it

---
 python/kiss_icp/pybind/kiss_icp_pybind.cpp |  6 ++----
 python/kiss_icp/registration.py            | 14 +++-----------
 2 files changed, 5 insertions(+), 15 deletions(-)

diff --git a/python/kiss_icp/pybind/kiss_icp_pybind.cpp b/python/kiss_icp/pybind/kiss_icp_pybind.cpp
index f9a9078b..19c86a89 100644
--- a/python/kiss_icp/pybind/kiss_icp_pybind.cpp
+++ b/python/kiss_icp/pybind/kiss_icp_pybind.cpp
@@ -76,11 +76,9 @@ PYBIND11_MODULE(kiss_icp_pybind, m) {
                                   "Estimate from the icp, it contains a pose and a 6x6 covariance");
     estimate.def(py::init<>())
         .def_property(
-            "pose", [](Estimate &self) { return self.pose.matrix(); },
+            "pose", [](const Estimate &self) { return self.pose.matrix(); },
             [](Estimate &self, const Eigen::Matrix4d &T) { self.pose = Sophus::SE3d(T); })
-        .def_property(
-            "covariance", [](Estimate &self) { return self.covariance; },
-            [](Estimate &self, const Eigen::Matrix6d &Sigma) { self.covariance = Sigma; });
+        .def_readwrite("covariance", &Estimate::covariance);
     // Point Cloud registration
     py::class_<Registration> internal_registration(m, "_Registration", "Don't use this");
     internal_registration
diff --git a/python/kiss_icp/registration.py b/python/kiss_icp/registration.py
index 0f77985f..9e55c3d2 100644
--- a/python/kiss_icp/registration.py
+++ b/python/kiss_icp/registration.py
@@ -25,6 +25,7 @@
 from kiss_icp.config.parser import KISSConfig
 from kiss_icp.mapping import VoxelHashMap
 from kiss_icp.pybind import kiss_icp_pybind
+from typing import TypeAlias
 
 
 def get_registration(config: KISSConfig):
@@ -35,17 +36,8 @@ def get_registration(config: KISSConfig):
     )
 
 
-class Estimate:
-    def __init__(self):
-        self._estimate = kiss_icp_pybind._Estimate()
-
-    @property
-    def pose(self):
-        return self._estimate.pose
-
-    @property
-    def covariance(self):
-        return self._estimate.covariance
+# Nice trick with the type aliasing for structs
+Estimate: TypeAlias = kiss_icp_pybind._Estimate
 
 
 class Registration:

From c47b4a5143c70df58d847a42da32644c8901bfc5 Mon Sep 17 00:00:00 2001
From: tizianoGuadagnino <frevo93@gmail.com>
Date: Tue, 26 Mar 2024 15:28:40 +0100
Subject: [PATCH 11/39] Getting the python side to dont complain

---
 cpp/kiss_icp/core/Registration.cpp         | 18 +++++++-----
 cpp/kiss_icp/core/Registration.hpp         |  1 +
 python/kiss_icp/kiss_icp.py                | 32 ++++++++++++----------
 python/kiss_icp/pipeline.py                | 11 +++++---
 python/kiss_icp/pybind/kiss_icp_pybind.cpp |  1 +
 5 files changed, 37 insertions(+), 26 deletions(-)

diff --git a/cpp/kiss_icp/core/Registration.cpp b/cpp/kiss_icp/core/Registration.cpp
index 061faae2..920c802d 100644
--- a/cpp/kiss_icp/core/Registration.cpp
+++ b/cpp/kiss_icp/core/Registration.cpp
@@ -167,6 +167,13 @@ LinearSystem BuildLinearSystem(const Associations &associations,
 
 namespace kiss_icp {
 
+Estimate operator*(Estimate lhs, const Estimate &rhs) {
+    lhs.pose *= rhs.pose;
+    const auto Proj = rhs.pose.Adj().inverse();
+    lhs.covariance = Proj * lhs.covariance * Proj.transpose() + rhs.covariance;
+    return lhs;
+}
+
 Registration::Registration(int max_num_iteration, double convergence_criterion, int max_num_threads)
     : max_num_iterations_(max_num_iteration),
       convergence_criterion_(convergence_criterion),
@@ -211,14 +218,11 @@ Estimate Registration::AlignPointsToMap(const std::vector<Eigen::Vector3d> &fram
     const auto associations = FindAssociations(source, voxel_map, max_correspondence_distance);
     const auto &[H, b, chi_square] =
         BuildLinearSystem(associations, [](double x) { return x / x; });
-    const auto icp_covariance =
-        (chi_square / static_cast<double>(associations.size()) * H).inverse();
-    Estimate estimate;
-    estimate.pose = T_icp * initial_guess.pose;
-    const auto A = T_icp.inverse().Adj();
-    estimate.covariance = A * initial_guess.covariance * A.transpose() + icp_covariance;
+    Estimate delta;
+    delta.pose = T_icp;
+    delta.covariance = (chi_square / static_cast<double>(associations.size()) * H).inverse();
     // Spit the final transformation
-    return estimate;
+    return delta * initial_guess;
 }
 
 }  // namespace kiss_icp
diff --git a/cpp/kiss_icp/core/Registration.hpp b/cpp/kiss_icp/core/Registration.hpp
index fb377e1f..f60ee7bb 100644
--- a/cpp/kiss_icp/core/Registration.hpp
+++ b/cpp/kiss_icp/core/Registration.hpp
@@ -35,6 +35,7 @@ namespace kiss_icp {
 struct Estimate {
     Sophus::SE3d pose;
     CovarianceMatrixType covariance = CovarianceMatrixType::Zero();
+    friend Estimate operator*(Estimate lhs, const Estimate &rhs);
 };
 
 struct Registration {
diff --git a/python/kiss_icp/kiss_icp.py b/python/kiss_icp/kiss_icp.py
index fd894af6..dd99c55a 100644
--- a/python/kiss_icp/kiss_icp.py
+++ b/python/kiss_icp/kiss_icp.py
@@ -26,14 +26,14 @@
 from kiss_icp.deskew import get_motion_compensator
 from kiss_icp.mapping import get_voxel_hash_map
 from kiss_icp.preprocess import get_preprocessor
-from kiss_icp.registration import get_registration
+from kiss_icp.registration import Estimate, get_registration
 from kiss_icp.threshold import get_threshold_estimator
 from kiss_icp.voxelization import voxel_down_sample
 
 
 class KissICP:
     def __init__(self, config: KISSConfig):
-        self.poses = []
+        self.estimates = []
         self.config = config
         self.compensator = get_motion_compensator(config)
         self.adaptive_threshold = get_threshold_estimator(self.config)
@@ -43,7 +43,7 @@ def __init__(self, config: KISSConfig):
 
     def register_frame(self, frame, timestamps):
         # Apply motion compensation
-        frame = self.compensator.deskew_scan(frame, self.poses, timestamps)
+        frame = self.compensator.deskew_scan(frame, self.estimates, timestamps)
 
         # Preprocess the input cloud
         frame = self.preprocess(frame)
@@ -56,11 +56,11 @@ def register_frame(self, frame, timestamps):
 
         # Compute initial_guess for ICP
         prediction = self.get_prediction_model()
-        last_pose = self.poses[-1] if self.poses else np.eye(4)
-        initial_guess = last_pose @ prediction
+        initial_guess = self.estimates[-1] if self.estimates else Estimate()
+        initial_guess = initial_guess @ prediction
 
         # Run ICP
-        new_pose = self.registration.align_points_to_map(
+        new_estimate = self.registration.align_points_to_map(
             points=source,
             voxel_map=self.local_map,
             initial_guess=initial_guess,
@@ -68,9 +68,10 @@ def register_frame(self, frame, timestamps):
             kernel=sigma / 3,
         )
 
-        self.adaptive_threshold.update_model_deviation(np.linalg.inv(initial_guess) @ new_pose)
-        self.local_map.update(frame_downsample, new_pose)
-        self.poses.append(new_pose)
+        pose_deviation = np.linalg.inv(initial_guess.pose) @ new_estimate.pose
+        self.adaptive_threshold.update_model_deviation(pose_deviation)
+        self.local_map.update(frame_downsample, new_estimate.pose)
+        self.estimates.append(new_estimate)
         return frame, source
 
     def voxelize(self, iframe):
@@ -86,13 +87,14 @@ def get_adaptive_threshold(self):
         )
 
     def get_prediction_model(self):
-        if len(self.poses) < 2:
-            return np.eye(4)
-        return np.linalg.inv(self.poses[-2]) @ self.poses[-1]
+        prediction = Estimate()
+        if len(self.estimates) > 1:
+            prediction.pose = np.linalg.inv(self.estimates[-2].pose) @ self.estimates[-1].pose
+        return prediction
 
     def has_moved(self):
-        if len(self.poses) < 1:
+        if len(self.estimates) < 1:
             return False
-        compute_motion = lambda T1, T2: np.linalg.norm((np.linalg.inv(T1) @ T2)[:3, -1])
-        motion = compute_motion(self.poses[0], self.poses[-1])
+        compute_motion = lambda T1, T2: np.linalg.norm((np.linalg.inv(T1.pose) @ T2.pose)[:3, -1])
+        motion = compute_motion(self.estimates[0], self.estimates[-1])
         return motion > 5 * self.config.adaptive_threshold.min_motion_th
diff --git a/python/kiss_icp/pipeline.py b/python/kiss_icp/pipeline.py
index 8946d946..3149a82c 100644
--- a/python/kiss_icp/pipeline.py
+++ b/python/kiss_icp/pipeline.py
@@ -65,7 +65,7 @@ def __init__(
         self.odometry = KissICP(config=self.config)
         self.results = PipelineResults()
         self.times = []
-        self.poses = self.odometry.poses
+        self.estimates = self.odometry.estimates
         self.has_gt = hasattr(self._dataset, "gt_poses")
         self.gt_poses = self._dataset.gt_poses[self._first : self._last] if self.has_gt else None
         self.dataset_name = self._dataset.__class__.__name__
@@ -98,7 +98,9 @@ def _run_pipeline(self):
             start_time = time.perf_counter_ns()
             source, keypoints = self.odometry.register_frame(raw_frame, timestamps)
             self.times.append(time.perf_counter_ns() - start_time)
-            self.visualizer.update(source, keypoints, self.odometry.local_map, self.poses[-1])
+            self.visualizer.update(
+                source, keypoints, self.odometry.local_map, self.estimates[-1].pose
+            )
 
     def _next(self, idx):
         """TODO: re-arrange this logic"""
@@ -167,9 +169,10 @@ def _write_gt_poses(self):
 
     def _run_evaluation(self):
         # Run estimation metrics evaluation, only when GT data was provided
+        poses = [x.pose for x in self.estimates]
         if self.has_gt:
-            avg_tra, avg_rot = sequence_error(self.gt_poses, self.poses)
-            ate_rot, ate_trans = absolute_trajectory_error(self.gt_poses, self.poses)
+            avg_tra, avg_rot = sequence_error(self.gt_poses, poses)
+            ate_rot, ate_trans = absolute_trajectory_error(self.gt_poses, poses)
             self.results.append(desc="Average Translation Error", units="%", value=avg_tra)
             self.results.append(desc="Average Rotational Error", units="deg/m", value=avg_rot)
             self.results.append(desc="Absolute Trajectory Error (ATE)", units="m", value=ate_trans)
diff --git a/python/kiss_icp/pybind/kiss_icp_pybind.cpp b/python/kiss_icp/pybind/kiss_icp_pybind.cpp
index 19c86a89..0f499448 100644
--- a/python/kiss_icp/pybind/kiss_icp_pybind.cpp
+++ b/python/kiss_icp/pybind/kiss_icp_pybind.cpp
@@ -75,6 +75,7 @@ PYBIND11_MODULE(kiss_icp_pybind, m) {
     py::class_<Estimate> estimate(m, "_Estimate",
                                   "Estimate from the icp, it contains a pose and a 6x6 covariance");
     estimate.def(py::init<>())
+        .def("__matmul__", [](Estimate lhs, const Estimate &rhs) { return lhs * rhs; })
         .def_property(
             "pose", [](const Estimate &self) { return self.pose.matrix(); },
             [](Estimate &self, const Eigen::Matrix4d &T) { self.pose = Sophus::SE3d(T); })

From f44998a48e573bc20748c08463a37b28dde13725 Mon Sep 17 00:00:00 2001
From: tizianoGuadagnino <frevo93@gmail.com>
Date: Tue, 26 Mar 2024 15:39:33 +0100
Subject: [PATCH 12/39] Now also python doesn't cry

---
 python/kiss_icp/pipeline.py | 14 +++++++++++---
 1 file changed, 11 insertions(+), 3 deletions(-)

diff --git a/python/kiss_icp/pipeline.py b/python/kiss_icp/pipeline.py
index 3149a82c..eebbe04a 100644
--- a/python/kiss_icp/pipeline.py
+++ b/python/kiss_icp/pipeline.py
@@ -83,12 +83,14 @@ def __init__(
     # Public interface  ------
     def run(self):
         self._run_pipeline()
+        self._get_poses_and_covariances()
         self._run_evaluation()
         self._create_output_dir()
         self._write_result_poses()
         self._write_gt_poses()
         self._write_cfg()
         self._write_log()
+        self._write_covariances()
         return self.results
 
     # Private interface  ------
@@ -112,6 +114,10 @@ def _next(self, idx):
             timestamps = np.zeros(frame.shape[0])
         return frame, timestamps
 
+    def _get_poses_and_covariances(self):
+        self.poses = [x.pose for x in self.estimates]
+        self.covariances = [x.covariance for x in self.estimates]
+
     @staticmethod
     def save_poses_kitti_format(filename: str, poses: List[np.ndarray]):
         def _to_kitti_format(poses: np.ndarray) -> np.ndarray:
@@ -167,12 +173,14 @@ def _write_gt_poses(self):
             timestamps=self._get_frames_timestamps(),
         )
 
+    def _write_covariances(self):
+        np.save(f"{self.results_dir}/covariances.npy", np.stack(self.covariances))
+
     def _run_evaluation(self):
         # Run estimation metrics evaluation, only when GT data was provided
-        poses = [x.pose for x in self.estimates]
         if self.has_gt:
-            avg_tra, avg_rot = sequence_error(self.gt_poses, poses)
-            ate_rot, ate_trans = absolute_trajectory_error(self.gt_poses, poses)
+            avg_tra, avg_rot = sequence_error(self.gt_poses, self.poses)
+            ate_rot, ate_trans = absolute_trajectory_error(self.gt_poses, self.poses)
             self.results.append(desc="Average Translation Error", units="%", value=avg_tra)
             self.results.append(desc="Average Rotational Error", units="deg/m", value=avg_rot)
             self.results.append(desc="Absolute Trajectory Error (ATE)", units="m", value=ate_trans)

From d01e04173248ab409265b9a69ee79ba64683bf33 Mon Sep 17 00:00:00 2001
From: tizianoGuadagnino <frevo93@gmail.com>
Date: Wed, 27 Mar 2024 13:36:04 +0100
Subject: [PATCH 13/39] Simplify python side

---
 python/kiss_icp/pipeline.py | 9 ++++-----
 1 file changed, 4 insertions(+), 5 deletions(-)

diff --git a/python/kiss_icp/pipeline.py b/python/kiss_icp/pipeline.py
index eebbe04a..71f1a3ad 100644
--- a/python/kiss_icp/pipeline.py
+++ b/python/kiss_icp/pipeline.py
@@ -66,6 +66,8 @@ def __init__(
         self.results = PipelineResults()
         self.times = []
         self.estimates = self.odometry.estimates
+        self.poses = []
+        self.covariances = []
         self.has_gt = hasattr(self._dataset, "gt_poses")
         self.gt_poses = self._dataset.gt_poses[self._first : self._last] if self.has_gt else None
         self.dataset_name = self._dataset.__class__.__name__
@@ -83,7 +85,6 @@ def __init__(
     # Public interface  ------
     def run(self):
         self._run_pipeline()
-        self._get_poses_and_covariances()
         self._run_evaluation()
         self._create_output_dir()
         self._write_result_poses()
@@ -99,6 +100,8 @@ def _run_pipeline(self):
             raw_frame, timestamps = self._next(idx)
             start_time = time.perf_counter_ns()
             source, keypoints = self.odometry.register_frame(raw_frame, timestamps)
+            self.poses.append(self.estimates[-1].pose)
+            self.covariances.append(self.estimates[-1].covariance)
             self.times.append(time.perf_counter_ns() - start_time)
             self.visualizer.update(
                 source, keypoints, self.odometry.local_map, self.estimates[-1].pose
@@ -114,10 +117,6 @@ def _next(self, idx):
             timestamps = np.zeros(frame.shape[0])
         return frame, timestamps
 
-    def _get_poses_and_covariances(self):
-        self.poses = [x.pose for x in self.estimates]
-        self.covariances = [x.covariance for x in self.estimates]
-
     @staticmethod
     def save_poses_kitti_format(filename: str, poses: List[np.ndarray]):
         def _to_kitti_format(poses: np.ndarray) -> np.ndarray:

From 7c3442cd5be72302042ecb7c220209f093720548 Mon Sep 17 00:00:00 2001
From: tizianoGuadagnino <frevo93@gmail.com>
Date: Wed, 27 Mar 2024 14:03:04 +0100
Subject: [PATCH 14/39] Update formula, now based on Barfoot et.al

---
 cpp/kiss_icp/core/Registration.cpp | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/cpp/kiss_icp/core/Registration.cpp b/cpp/kiss_icp/core/Registration.cpp
index 920c802d..1727aa99 100644
--- a/cpp/kiss_icp/core/Registration.cpp
+++ b/cpp/kiss_icp/core/Registration.cpp
@@ -169,8 +169,8 @@ namespace kiss_icp {
 
 Estimate operator*(Estimate lhs, const Estimate &rhs) {
     lhs.pose *= rhs.pose;
-    const auto Proj = rhs.pose.Adj().inverse();
-    lhs.covariance = Proj * lhs.covariance * Proj.transpose() + rhs.covariance;
+    const auto Proj = lhs.pose.Adj();
+    lhs.covariance += Proj * rhs.covariance * Proj.transpose();
     return lhs;
 }
 

From 463fbd8aa367ae6bcf51a56010f65c774aac0b4b Mon Sep 17 00:00:00 2001
From: tizianoGuadagnino <frevo93@gmail.com>
Date: Wed, 27 Mar 2024 14:50:27 +0100
Subject: [PATCH 15/39] Add the inverse operation

---
 cpp/kiss_icp/core/Registration.cpp         | 13 +++++++++++--
 cpp/kiss_icp/core/Registration.hpp         |  5 ++++-
 cpp/kiss_icp/pipeline/KissICP.cpp          | 11 +++++------
 cpp/kiss_icp/pipeline/KissICP.hpp          |  2 +-
 python/kiss_icp/kiss_icp.py                |  6 +++---
 python/kiss_icp/pybind/kiss_icp_pybind.cpp |  1 +
 6 files changed, 25 insertions(+), 13 deletions(-)

diff --git a/cpp/kiss_icp/core/Registration.cpp b/cpp/kiss_icp/core/Registration.cpp
index 1727aa99..213cf97a 100644
--- a/cpp/kiss_icp/core/Registration.cpp
+++ b/cpp/kiss_icp/core/Registration.cpp
@@ -167,10 +167,19 @@ LinearSystem BuildLinearSystem(const Associations &associations,
 
 namespace kiss_icp {
 
+Estimate::Estimate() : pose(), covariance(Eigen::Matrix6d::Zero()) {}
+Estimate::Estimate(const Sophus::SE3d &T, const Eigen::Matrix6d &Sigma)
+    : pose(T), covariance(Sigma) {}
+
+Estimate Estimate::inverse() const {
+    const auto Adj = pose.inverse().Adj();
+    return {pose.inverse(), Adj * covariance * Adj.transpose()};
+}
+
 Estimate operator*(Estimate lhs, const Estimate &rhs) {
     lhs.pose *= rhs.pose;
-    const auto Proj = lhs.pose.Adj();
-    lhs.covariance += Proj * rhs.covariance * Proj.transpose();
+    const auto Adj = lhs.pose.Adj();
+    lhs.covariance += Adj * rhs.covariance * Adj.transpose();
     return lhs;
 }
 
diff --git a/cpp/kiss_icp/core/Registration.hpp b/cpp/kiss_icp/core/Registration.hpp
index f60ee7bb..b6a0261d 100644
--- a/cpp/kiss_icp/core/Registration.hpp
+++ b/cpp/kiss_icp/core/Registration.hpp
@@ -33,8 +33,11 @@ using CovarianceMatrixType = Eigen::Matrix<double, 6, 6>;
 namespace kiss_icp {
 
 struct Estimate {
+    Estimate();
+    Estimate(const Sophus::SE3d &T, const CovarianceMatrixType &Sigma);
     Sophus::SE3d pose;
-    CovarianceMatrixType covariance = CovarianceMatrixType::Zero();
+    CovarianceMatrixType covariance;
+    Estimate inverse() const;
     friend Estimate operator*(Estimate lhs, const Estimate &rhs);
 };
 
diff --git a/cpp/kiss_icp/pipeline/KissICP.cpp b/cpp/kiss_icp/pipeline/KissICP.cpp
index 5eb17072..bea7969d 100644
--- a/cpp/kiss_icp/pipeline/KissICP.cpp
+++ b/cpp/kiss_icp/pipeline/KissICP.cpp
@@ -65,14 +65,14 @@ KissICP::Vector3dVectorTuple KissICP::RegisterFrame(const std::vector<Eigen::Vec
     // Compute initial_guess for ICP
     const auto prediction = GetPredictionModel();
     auto initial_guess = !estimates_.empty() ? estimates_.back() : Estimate();
-    initial_guess.pose = initial_guess.pose * prediction;
+    initial_guess = initial_guess * prediction;
     // Run icp
     const auto new_estimate = registration_.AlignPointsToMap(source,         //
                                                              local_map_,     //
                                                              initial_guess,  //
                                                              3.0 * sigma,    //
                                                              sigma / 3.0);
-    const auto model_deviation = initial_guess.pose.inverse() * new_estimate.pose;
+    const auto model_deviation = (initial_guess.inverse() * new_estimate).pose;
     adaptive_threshold_.UpdateModelDeviation(model_deviation);
     local_map_.Update(frame_downsample, new_estimate.pose);
     estimates_.push_back(new_estimate);
@@ -93,11 +93,10 @@ double KissICP::GetAdaptiveThreshold() {
     return adaptive_threshold_.ComputeThreshold();
 }
 
-Sophus::SE3d KissICP::GetPredictionModel() const {
-    Sophus::SE3d pred = Sophus::SE3d();
+Estimate KissICP::GetPredictionModel() const {
     const size_t N = estimates_.size();
-    if (N < 2) return pred;
-    return estimates_[N - 2].pose.inverse() * estimates_[N - 1].pose;
+    if (N < 2) return Estimate();
+    return estimates_[N - 2].inverse() * estimates_[N - 1];
 }
 
 bool KissICP::HasMoved() {
diff --git a/cpp/kiss_icp/pipeline/KissICP.hpp b/cpp/kiss_icp/pipeline/KissICP.hpp
index bbeb9532..869ef217 100644
--- a/cpp/kiss_icp/pipeline/KissICP.hpp
+++ b/cpp/kiss_icp/pipeline/KissICP.hpp
@@ -71,7 +71,7 @@ class KissICP {
                                       const std::vector<double> &timestamps);
     Vector3dVectorTuple Voxelize(const std::vector<Eigen::Vector3d> &frame) const;
     double GetAdaptiveThreshold();
-    Sophus::SE3d GetPredictionModel() const;
+    Estimate GetPredictionModel() const;
     bool HasMoved();
 
 public:
diff --git a/python/kiss_icp/kiss_icp.py b/python/kiss_icp/kiss_icp.py
index dd99c55a..39209e14 100644
--- a/python/kiss_icp/kiss_icp.py
+++ b/python/kiss_icp/kiss_icp.py
@@ -68,7 +68,7 @@ def register_frame(self, frame, timestamps):
             kernel=sigma / 3,
         )
 
-        pose_deviation = np.linalg.inv(initial_guess.pose) @ new_estimate.pose
+        pose_deviation = (initial_guess.inverse() @ new_estimate).pose
         self.adaptive_threshold.update_model_deviation(pose_deviation)
         self.local_map.update(frame_downsample, new_estimate.pose)
         self.estimates.append(new_estimate)
@@ -89,12 +89,12 @@ def get_adaptive_threshold(self):
     def get_prediction_model(self):
         prediction = Estimate()
         if len(self.estimates) > 1:
-            prediction.pose = np.linalg.inv(self.estimates[-2].pose) @ self.estimates[-1].pose
+            prediction.pose = (self.estimates[-2].inverse() @ self.estimates[-1]).pose
         return prediction
 
     def has_moved(self):
         if len(self.estimates) < 1:
             return False
-        compute_motion = lambda T1, T2: np.linalg.norm((np.linalg.inv(T1.pose) @ T2.pose)[:3, -1])
+        compute_motion = lambda T1, T2: np.linalg.norm((T1.inverse() @ T2).pose[:3, -1])
         motion = compute_motion(self.estimates[0], self.estimates[-1])
         return motion > 5 * self.config.adaptive_threshold.min_motion_th
diff --git a/python/kiss_icp/pybind/kiss_icp_pybind.cpp b/python/kiss_icp/pybind/kiss_icp_pybind.cpp
index 0f499448..46e34872 100644
--- a/python/kiss_icp/pybind/kiss_icp_pybind.cpp
+++ b/python/kiss_icp/pybind/kiss_icp_pybind.cpp
@@ -76,6 +76,7 @@ PYBIND11_MODULE(kiss_icp_pybind, m) {
                                   "Estimate from the icp, it contains a pose and a 6x6 covariance");
     estimate.def(py::init<>())
         .def("__matmul__", [](Estimate lhs, const Estimate &rhs) { return lhs * rhs; })
+        .def("inverse", &Estimate::inverse)
         .def_property(
             "pose", [](const Estimate &self) { return self.pose.matrix(); },
             [](Estimate &self, const Eigen::Matrix4d &T) { self.pose = Sophus::SE3d(T); })

From 21d2c764f1739e5f4ae9e1bae1c2f815b47221dd Mon Sep 17 00:00:00 2001
From: tizianoGuadagnino <frevo93@gmail.com>
Date: Wed, 27 Mar 2024 14:55:24 +0100
Subject: [PATCH 16/39] We now take into account the uncertanty of the
 prediction model

---
 python/kiss_icp/kiss_icp.py | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/python/kiss_icp/kiss_icp.py b/python/kiss_icp/kiss_icp.py
index 39209e14..cf579cd7 100644
--- a/python/kiss_icp/kiss_icp.py
+++ b/python/kiss_icp/kiss_icp.py
@@ -89,7 +89,7 @@ def get_adaptive_threshold(self):
     def get_prediction_model(self):
         prediction = Estimate()
         if len(self.estimates) > 1:
-            prediction.pose = (self.estimates[-2].inverse() @ self.estimates[-1]).pose
+            prediction = self.estimates[-2].inverse() @ self.estimates[-1]
         return prediction
 
     def has_moved(self):

From a20a098049e5c31294bbda85c4c85d8aa2aa0df9 Mon Sep 17 00:00:00 2001
From: tizianoGuadagnino <frevo93@gmail.com>
Date: Wed, 27 Mar 2024 15:28:00 +0100
Subject: [PATCH 17/39] Simplify

---
 cpp/kiss_icp/core/Registration.cpp | 14 +++++++-------
 cpp/kiss_icp/pipeline/KissICP.cpp  |  4 ++--
 2 files changed, 9 insertions(+), 9 deletions(-)

diff --git a/cpp/kiss_icp/core/Registration.cpp b/cpp/kiss_icp/core/Registration.cpp
index 213cf97a..bc372fcd 100644
--- a/cpp/kiss_icp/core/Registration.cpp
+++ b/cpp/kiss_icp/core/Registration.cpp
@@ -201,6 +201,7 @@ Estimate Registration::AlignPointsToMap(const std::vector<Eigen::Vector3d> &fram
                                         double kernel_threshold) {
     if (voxel_map.Empty()) return initial_guess;
 
+    auto no_kernel = [&](double residual2) { return residual2 / residual2; };
     auto GM = [&](double residual2) {
         return square(kernel_threshold) / square(kernel_threshold + residual2);
     };
@@ -224,14 +225,13 @@ Estimate Registration::AlignPointsToMap(const std::vector<Eigen::Vector3d> &fram
         // Termination criteria
         if (dx.norm() < convergence_criterion_) break;
     }
-    const auto associations = FindAssociations(source, voxel_map, max_correspondence_distance);
-    const auto &[H, b, chi_square] =
-        BuildLinearSystem(associations, [](double x) { return x / x; });
-    Estimate delta;
-    delta.pose = T_icp;
-    delta.covariance = (chi_square / static_cast<double>(associations.size()) * H).inverse();
+    // tg UGLY PART
+    const auto associations = FindAssociations(source, voxel_map, kernel_threshold);
+    const auto &[H, b, chi_square] = BuildLinearSystem(associations, no_kernel);
+    const auto covariance_icp = chi_square / static_cast<double>(associations.size()) * H.inverse();
+    Estimate icp_correction(T_icp, covariance_icp);
     // Spit the final transformation
-    return delta * initial_guess;
+    return icp_correction * initial_guess;
 }
 
 }  // namespace kiss_icp
diff --git a/cpp/kiss_icp/pipeline/KissICP.cpp b/cpp/kiss_icp/pipeline/KissICP.cpp
index bea7969d..63ff5367 100644
--- a/cpp/kiss_icp/pipeline/KissICP.cpp
+++ b/cpp/kiss_icp/pipeline/KissICP.cpp
@@ -64,8 +64,8 @@ KissICP::Vector3dVectorTuple KissICP::RegisterFrame(const std::vector<Eigen::Vec
 
     // Compute initial_guess for ICP
     const auto prediction = GetPredictionModel();
-    auto initial_guess = !estimates_.empty() ? estimates_.back() : Estimate();
-    initial_guess = initial_guess * prediction;
+    const auto last_estimate = !estimates_.empty() ? estimates_.back() : Estimate();
+    const auto initial_guess = last_estimate * prediction;
     // Run icp
     const auto new_estimate = registration_.AlignPointsToMap(source,         //
                                                              local_map_,     //

From 257f2b3845a43c0a426d0c55346de1a8d7424596 Mon Sep 17 00:00:00 2001
From: tizianoGuadagnino <frevo93@gmail.com>
Date: Thu, 28 Mar 2024 11:54:21 +0100
Subject: [PATCH 18/39] Now it makes sense

---
 cpp/kiss_icp/core/Registration.cpp | 4 ++--
 cpp/kiss_icp/pipeline/KissICP.cpp  | 7 +++++--
 python/kiss_icp/kiss_icp.py        | 4 +++-
 3 files changed, 10 insertions(+), 5 deletions(-)

diff --git a/cpp/kiss_icp/core/Registration.cpp b/cpp/kiss_icp/core/Registration.cpp
index bc372fcd..e75b5c07 100644
--- a/cpp/kiss_icp/core/Registration.cpp
+++ b/cpp/kiss_icp/core/Registration.cpp
@@ -201,7 +201,7 @@ Estimate Registration::AlignPointsToMap(const std::vector<Eigen::Vector3d> &fram
                                         double kernel_threshold) {
     if (voxel_map.Empty()) return initial_guess;
 
-    auto no_kernel = [&](double residual2) { return residual2 / residual2; };
+    auto no_kernel = [&](double residual2) { return residual2 > 0.0 ? 1.0 : 1.0; };
     auto GM = [&](double residual2) {
         return square(kernel_threshold) / square(kernel_threshold + residual2);
     };
@@ -226,7 +226,7 @@ Estimate Registration::AlignPointsToMap(const std::vector<Eigen::Vector3d> &fram
         if (dx.norm() < convergence_criterion_) break;
     }
     // tg UGLY PART
-    const auto associations = FindAssociations(source, voxel_map, kernel_threshold);
+    const auto associations = FindAssociations(source, voxel_map, voxel_map.voxel_size_ * 0.5);
     const auto &[H, b, chi_square] = BuildLinearSystem(associations, no_kernel);
     const auto covariance_icp = chi_square / static_cast<double>(associations.size()) * H.inverse();
     Estimate icp_correction(T_icp, covariance_icp);
diff --git a/cpp/kiss_icp/pipeline/KissICP.cpp b/cpp/kiss_icp/pipeline/KissICP.cpp
index 63ff5367..f539122f 100644
--- a/cpp/kiss_icp/pipeline/KissICP.cpp
+++ b/cpp/kiss_icp/pipeline/KissICP.cpp
@@ -94,9 +94,12 @@ double KissICP::GetAdaptiveThreshold() {
 }
 
 Estimate KissICP::GetPredictionModel() const {
+    Estimate prediction;
     const size_t N = estimates_.size();
-    if (N < 2) return Estimate();
-    return estimates_[N - 2].inverse() * estimates_[N - 1];
+    if (N > 1) {
+        prediction.pose = (estimates_[N - 2].inverse() * estimates_[N - 1]).pose;
+    }
+    return prediction;
 }
 
 bool KissICP::HasMoved() {
diff --git a/python/kiss_icp/kiss_icp.py b/python/kiss_icp/kiss_icp.py
index cf579cd7..84959594 100644
--- a/python/kiss_icp/kiss_icp.py
+++ b/python/kiss_icp/kiss_icp.py
@@ -89,7 +89,9 @@ def get_adaptive_threshold(self):
     def get_prediction_model(self):
         prediction = Estimate()
         if len(self.estimates) > 1:
-            prediction = self.estimates[-2].inverse() @ self.estimates[-1]
+            pose_from = self.estimates[-2].pose
+            pose_to = self.estimates[-1].pose
+            prediction.pose = np.linalg.inv(pose_from) @ pose_to
         return prediction
 
     def has_moved(self):

From 59b02773cd8bb5fc53048bf2ffb6e8f62936579f Mon Sep 17 00:00:00 2001
From: tizianoGuadagnino <frevo93@gmail.com>
Date: Mon, 25 Mar 2024 13:16:33 +0100
Subject: [PATCH 19/39] First trial, this is very ugly

---
 cpp/kiss_icp/core/Registration.cpp | 64 +++++++++++++++++-------------
 cpp/kiss_icp/core/Registration.hpp | 21 +++++++---
 cpp/kiss_icp/pipeline/KissICP.cpp  | 34 ++++++++--------
 cpp/kiss_icp/pipeline/KissICP.hpp  |  4 +-
 4 files changed, 72 insertions(+), 51 deletions(-)

diff --git a/cpp/kiss_icp/core/Registration.cpp b/cpp/kiss_icp/core/Registration.cpp
index 46f22e74..268f05d3 100644
--- a/cpp/kiss_icp/core/Registration.cpp
+++ b/cpp/kiss_icp/core/Registration.cpp
@@ -29,6 +29,7 @@
 
 #include <algorithm>
 #include <cmath>
+#include <functional>
 #include <numeric>
 #include <sophus/se3.hpp>
 #include <sophus/so3.hpp>
@@ -36,13 +37,9 @@
 
 #include "VoxelHashMap.hpp"
 
-namespace Eigen {
-using Matrix6d = Eigen::Matrix<double, 6, 6>;
-using Matrix3_6d = Eigen::Matrix<double, 3, 6>;
-using Vector6d = Eigen::Matrix<double, 6, 1>;
-}  // namespace Eigen
-using Associations = std::vector<std::pair<Eigen::Vector3d, Eigen::Vector3d>>;
-using LinearSystem = std::pair<Eigen::Matrix6d, Eigen::Vector6d>;
+using Correspondence = std::pair<Eigen::Vector3d, Eigen::Vector3d>;
+using Associations = std::vector<Correspondence>;
+using LinearSystem = std::tuple<Eigen::Matrix6d, Eigen::Vector6d, double>;
 
 namespace {
 inline double square(double x) { return x * x; }
@@ -120,7 +117,8 @@ Associations FindAssociations(const std::vector<Eigen::Vector3d> &points,
     return associations;
 }
 
-LinearSystem BuildLinearSystem(const Associations &associations, double kernel) {
+LinearSystem BuildLinearSystem(const Associations &associations,
+                               std::function<double_t(double_t)> kernel) {
     auto compute_jacobian_and_residual = [](auto association) {
         const auto &[source, target] = association;
         const Eigen::Vector3d residual = source - target;
@@ -131,33 +129,33 @@ LinearSystem BuildLinearSystem(const Associations &associations, double kernel)
     };
 
     auto sum_linear_systems = [](LinearSystem a, const LinearSystem &b) {
-        a.first += b.first;
-        a.second += b.second;
+        std::get<Eigen::Matrix6d>(a) += std::get<Eigen::Matrix6d>(b);
+        std::get<Eigen::Vector6d>(a) += std::get<Eigen::Vector6d>(b);
+        std::get<double>(a) += std::get<double>(b);
         return a;
     };
 
-    auto GM_weight = [&](double residual2) { return square(kernel) / square(kernel + residual2); };
-
     using associations_iterator = Associations::const_iterator;
-    const auto &[JTJ, JTr] = tbb::parallel_reduce(
+    const auto &[JTJ, JTr, chi_square] = tbb::parallel_reduce(
         // Range
         tbb::blocked_range<associations_iterator>{associations.cbegin(), associations.cend()},
         // Identity
-        LinearSystem(Eigen::Matrix6d::Zero(), Eigen::Vector6d::Zero()),
+        LinearSystem(Eigen::Matrix6d::Zero(), Eigen::Vector6d::Zero(), 0.0),
         // 1st Lambda: Parallel computation
         [&](const tbb::blocked_range<associations_iterator> &r, LinearSystem J) -> LinearSystem {
             return std::transform_reduce(
                 r.begin(), r.end(), J, sum_linear_systems, [&](const auto &association) {
                     const auto &[J_r, residual] = compute_jacobian_and_residual(association);
-                    const double w = GM_weight(residual.squaredNorm());
-                    return LinearSystem(J_r.transpose() * w * J_r,        // JTJ
-                                        J_r.transpose() * w * residual);  // JTr
+                    const double chi_square = residual.squaredNorm();
+                    const double w = kernel(chi_square);
+                    return LinearSystem(J_r.transpose() * w * J_r,                    // JTJ
+                                        J_r.transpose() * w * residual, chi_square);  // JTr
                 });
         },
         // 2nd Lambda: Parallel reduction of the private Jacboians
         sum_linear_systems);
 
-    return {JTJ, JTr};
+    return {JTJ, JTr, chi_square};
 }
 }  // namespace
 
@@ -174,24 +172,28 @@ Registration::Registration(int max_num_iteration, double convergence_criterion,
         tbb::global_control::max_allowed_parallelism, static_cast<size_t>(max_num_threads_));
 }
 
-Sophus::SE3d Registration::AlignPointsToMap(const std::vector<Eigen::Vector3d> &frame,
-                                            const VoxelHashMap &voxel_map,
-                                            const Sophus::SE3d &initial_guess,
-                                            double max_correspondence_distance,
-                                            double kernel) {
+Estimate Registration::AlignPointsToMap(const std::vector<Eigen::Vector3d> &frame,
+                                        const VoxelHashMap &voxel_map,
+                                        const Estimate &initial_guess,
+                                        double max_correspondence_distance,
+                                        double kernel_threshold) {
     if (voxel_map.Empty()) return initial_guess;
 
+    auto GM = [&](double residual2) {
+        return square(kernel_threshold) / square(kernel_threshold + residual2);
+    };
     // Equation (9)
     std::vector<Eigen::Vector3d> source = frame;
-    TransformPoints(initial_guess, source);
+    TransformPoints(initial_guess.pose, source);
 
     // ICP-loop
     Sophus::SE3d T_icp = Sophus::SE3d();
+    Associations associations;
     for (int j = 0; j < max_num_iterations_; ++j) {
         // Equation (10)
-        const auto associations = FindAssociations(source, voxel_map, max_correspondence_distance);
+        associations = FindAssociations(source, voxel_map, max_correspondence_distance);
         // Equation (11)
-        const auto &[JTJ, JTr] = BuildLinearSystem(associations, kernel);
+        const auto &[JTJ, JTr, chi_square] = BuildLinearSystem(associations, GM);
         const Eigen::Vector6d dx = JTJ.ldlt().solve(-JTr);
         const Sophus::SE3d estimation = Sophus::SE3d::exp(dx);
         // Equation (12)
@@ -201,8 +203,16 @@ Sophus::SE3d Registration::AlignPointsToMap(const std::vector<Eigen::Vector3d> &
         // Termination criteria
         if (dx.norm() < convergence_criterion_) break;
     }
+    const auto &[information_matrix, information_vector, chi_square] =
+        BuildLinearSystem(associations, [](double x) { return x / x; });
+    Estimate estimate;
+    estimate.pose = T_icp * initial_guess.pose;
+    const auto A = T_icp.inverse().Adj();
+    estimate.covariance =
+        A * initial_guess.covariance * A.transpose() +
+        (chi_square / static_cast<double>(associations.size()) * information_matrix).inverse();
     // Spit the final transformation
-    return T_icp * initial_guess;
+    return estimate;
 }
 
 }  // namespace kiss_icp
diff --git a/cpp/kiss_icp/core/Registration.hpp b/cpp/kiss_icp/core/Registration.hpp
index d878c2fc..3ea90753 100644
--- a/cpp/kiss_icp/core/Registration.hpp
+++ b/cpp/kiss_icp/core/Registration.hpp
@@ -28,16 +28,27 @@
 
 #include "VoxelHashMap.hpp"
 
+namespace Eigen {
+using Matrix6d = Eigen::Matrix<double, 6, 6>;
+using Matrix3_6d = Eigen::Matrix<double, 3, 6>;
+using Vector6d = Eigen::Matrix<double, 6, 1>;
+}  // namespace Eigen
+
 namespace kiss_icp {
 
+struct Estimate {
+    Sophus::SE3d pose;
+    Eigen::Matrix6d covariance = Eigen::Matrix6d::Zero();
+};
+
 struct Registration {
     explicit Registration(int max_num_iteration, double convergence_criterion, int max_num_threads);
 
-    Sophus::SE3d AlignPointsToMap(const std::vector<Eigen::Vector3d> &frame,
-                                  const VoxelHashMap &voxel_map,
-                                  const Sophus::SE3d &initial_guess,
-                                  double max_correspondence_distance,
-                                  double kernel);
+    Estimate AlignPointsToMap(const std::vector<Eigen::Vector3d> &frame,
+                              const VoxelHashMap &voxel_map,
+                              const Estimate &initial_guess,
+                              double max_correspondence_distance,
+                              double kernel_threshold);
 
     int max_num_iterations_;
     double convergence_criterion_;
diff --git a/cpp/kiss_icp/pipeline/KissICP.cpp b/cpp/kiss_icp/pipeline/KissICP.cpp
index 283f0b7d..b2818040 100644
--- a/cpp/kiss_icp/pipeline/KissICP.cpp
+++ b/cpp/kiss_icp/pipeline/KissICP.cpp
@@ -45,8 +45,8 @@ KissICP::Vector3dVectorTuple KissICP::RegisterFrame(const std::vector<Eigen::Vec
         if (N <= 2) return frame;
 
         // Estimate linear and angular velocities
-        const auto &start_pose = poses_[N - 2];
-        const auto &finish_pose = poses_[N - 1];
+        const auto &start_pose = estimates_[N - 2].pose;
+        const auto &finish_pose = estimates_[N - 1].pose;
         return DeSkewScan(frame, timestamps, start_pose, finish_pose);
     }();
     return RegisterFrame(deskew_frame);
@@ -64,19 +64,18 @@ KissICP::Vector3dVectorTuple KissICP::RegisterFrame(const std::vector<Eigen::Vec
 
     // Compute initial_guess for ICP
     const auto prediction = GetPredictionModel();
-    const auto last_pose = !poses_.empty() ? poses_.back() : Sophus::SE3d();
-    const auto initial_guess = last_pose * prediction;
-
+    auto initial_guess = !estimates_.empty() ? estimates_.back() : Estimate();
+    initial_guess.pose = initial_guess.pose * prediction;
     // Run icp
-    const Sophus::SE3d new_pose = registration_.AlignPointsToMap(source,         //
-                                                                 local_map_,     //
-                                                                 initial_guess,  //
-                                                                 3.0 * sigma,    //
-                                                                 sigma / 3.0);
-    const auto model_deviation = initial_guess.inverse() * new_pose;
+    const auto new_estimate = registration_.AlignPointsToMap(source,         //
+                                                             local_map_,     //
+                                                             initial_guess,  //
+                                                             3.0 * sigma,    //
+                                                             sigma / 3.0);
+    const auto model_deviation = initial_guess.pose.inverse() * new_estimate.pose;
     adaptive_threshold_.UpdateModelDeviation(model_deviation);
-    local_map_.Update(frame_downsample, new_pose);
-    poses_.push_back(new_pose);
+    local_map_.Update(frame_downsample, new_estimate.pose);
+    estimates_.push_back(new_estimate);
     return {frame, source};
 }
 
@@ -96,14 +95,15 @@ double KissICP::GetAdaptiveThreshold() {
 
 Sophus::SE3d KissICP::GetPredictionModel() const {
     Sophus::SE3d pred = Sophus::SE3d();
-    const size_t N = poses_.size();
+    const size_t N = estimates_.size();
     if (N < 2) return pred;
-    return poses_[N - 2].inverse() * poses_[N - 1];
+    return estimates_[N - 2].pose.inverse() * estimates_[N - 1].pose;
 }
 
 bool KissICP::HasMoved() {
-    if (poses_.empty()) return false;
-    const double motion = (poses_.front().inverse() * poses_.back()).translation().norm();
+    if (estimates_.empty()) return false;
+    const double motion =
+        (estimates_.front().pose.inverse() * estimates_.back().pose).translation().norm();
     return motion > 5.0 * config_.min_motion_th;
 }
 
diff --git a/cpp/kiss_icp/pipeline/KissICP.hpp b/cpp/kiss_icp/pipeline/KissICP.hpp
index 69b5830e..4b22124a 100644
--- a/cpp/kiss_icp/pipeline/KissICP.hpp
+++ b/cpp/kiss_icp/pipeline/KissICP.hpp
@@ -77,11 +77,11 @@ class KissICP {
 public:
     // Extra C++ API to facilitate ROS debugging
     std::vector<Eigen::Vector3d> LocalMap() const { return local_map_.Pointcloud(); };
-    std::vector<Sophus::SE3d> poses() const { return poses_; };
+    std::vector<Estimate> poses() const { return estimates_; };
 
 private:
     // KISS-ICP pipeline modules
-    std::vector<Sophus::SE3d> poses_;
+    std::vector<Estimate> estimates_;
     KISSConfig config_;
     Registration registration_;
     VoxelHashMap local_map_;

From e26db5009efa2103c9d2a5c495e89e51915bbb45 Mon Sep 17 00:00:00 2001
From: tizianoGuadagnino <frevo93@gmail.com>
Date: Mon, 25 Mar 2024 13:31:56 +0100
Subject: [PATCH 20/39] Start explosing covariance in ros...WIP

---
 ros/src/OdometryServer.cpp | 39 ++++++++++++++++++++------------------
 1 file changed, 21 insertions(+), 18 deletions(-)

diff --git a/ros/src/OdometryServer.cpp b/ros/src/OdometryServer.cpp
index 47551e4b..04390611 100644
--- a/ros/src/OdometryServer.cpp
+++ b/ros/src/OdometryServer.cpp
@@ -136,35 +136,38 @@ void OdometryServer::RegisterFrame(const sensor_msgs::msg::PointCloud2::ConstSha
     // Register frame, main entry point to KISS-ICP pipeline
     const auto &[frame, keypoints] = kiss_icp_->RegisterFrame(points, timestamps);
 
-    // Extract the last KISS-ICP pose, ego-centric to the LiDAR
-    const Sophus::SE3d kiss_pose = kiss_icp_->poses().back();
+    // Compute the pose using KISS, ego-centric to the LiDAR
+    const auto kiss_estimate = kiss_icp_->estimates().back();
 
-    // Spit the current estimated pose to ROS msgs handling the desired target frame
-    PublishOdometry(kiss_pose, msg->header);
-    // Publishing these clouds is a bit costly, so do it only if we are debugging
+    // If necessary, transform the ego-centric pose to the specified base_link/base_footprint frame
+    const auto estimate = [&]() -> kiss_icp::Estimate {
+        if (egocentric_estimation) return kiss_estimate;
+        kiss_icp::Estimate transformed;
+        const Sophus::SE3d cloud2base = LookupTransform(base_frame_, cloud_frame_id);
+        transformed.pose = cloud2base * kiss_estimate.pose * cloud2base.inverse();
+        transformed.covariance =
+            cloud2base.Adj() * kiss_estimate.covariance * cloud2base.Adj().transpose();
+        return transformed;
+    }();
+
+    // Spit the current estimated pose to ROS msgs
+    PublishOdometry(estimate, msg->header.stamp, cloud_frame_id);
+    // Publishing this clouds is a bit costly, so do it only if we are debugging
     if (publish_debug_clouds_) {
         PublishClouds(frame, keypoints, msg->header);
     }
 }
 
-void OdometryServer::PublishOdometry(const Sophus::SE3d &kiss_pose,
-                                     const std_msgs::msg::Header &header) {
-    // If necessary, transform the ego-centric pose to the specified base_link/base_footprint frame
-    const auto cloud_frame_id = header.frame_id;
-    const auto egocentric_estimation = (base_frame_.empty() || base_frame_ == cloud_frame_id);
-    const auto pose = [&]() -> Sophus::SE3d {
-        if (egocentric_estimation) return kiss_pose;
-        const Sophus::SE3d cloud2base = LookupTransform(base_frame_, cloud_frame_id, tf2_buffer_);
-        return cloud2base * kiss_pose * cloud2base.inverse();
-    }();
-
+void OdometryServer::PublishOdometry(const kiss_icp::Estimate &estimate,
+                                     const rclcpp::Time &stamp,
+                                     const std::string &cloud_frame_id) {
     // Broadcast the tf ---
     if (publish_odom_tf_) {
         geometry_msgs::msg::TransformStamped transform_msg;
         transform_msg.header.stamp = header.stamp;
         transform_msg.header.frame_id = odom_frame_;
-        transform_msg.child_frame_id = egocentric_estimation ? cloud_frame_id : base_frame_;
-        transform_msg.transform = tf2::sophusToTransform(pose);
+        transform_msg.child_frame_id = base_frame_.empty() ? cloud_frame_id : base_frame_;
+        transform_msg.transform = tf2::sophusToTransform(estimate.pose);
         tf_broadcaster_->sendTransform(transform_msg);
     }
 

From bba7b94c62909db4c4491f3ff854a0780e6433db Mon Sep 17 00:00:00 2001
From: tizianoGuadagnino <frevo93@gmail.com>
Date: Mon, 25 Mar 2024 14:44:15 +0100
Subject: [PATCH 21/39] Copy over the covariance

---
 cpp/kiss_icp/pipeline/KissICP.cpp |  2 +-
 ros/src/OdometryServer.cpp        | 11 +++++------
 2 files changed, 6 insertions(+), 7 deletions(-)

diff --git a/cpp/kiss_icp/pipeline/KissICP.cpp b/cpp/kiss_icp/pipeline/KissICP.cpp
index b2818040..5eb17072 100644
--- a/cpp/kiss_icp/pipeline/KissICP.cpp
+++ b/cpp/kiss_icp/pipeline/KissICP.cpp
@@ -41,7 +41,7 @@ KissICP::Vector3dVectorTuple KissICP::RegisterFrame(const std::vector<Eigen::Vec
         // TODO(Nacho) Add some asserts here to sanitize the timestamps
 
         //  If not enough poses for the estimation, do not de-skew
-        const size_t N = poses().size();
+        const size_t N = estimates().size();
         if (N <= 2) return frame;
 
         // Estimate linear and angular velocities
diff --git a/ros/src/OdometryServer.cpp b/ros/src/OdometryServer.cpp
index 04390611..b7887680 100644
--- a/ros/src/OdometryServer.cpp
+++ b/ros/src/OdometryServer.cpp
@@ -177,12 +177,11 @@ void OdometryServer::PublishOdometry(const kiss_icp::Estimate &estimate,
     odom_msg.header.frame_id = odom_frame_;
     odom_msg.pose.pose = tf2::sophusToPose(pose);
     odom_msg.pose.covariance.fill(0.0);
-    odom_msg.pose.covariance[0] = position_covariance_;
-    odom_msg.pose.covariance[7] = position_covariance_;
-    odom_msg.pose.covariance[14] = position_covariance_;
-    odom_msg.pose.covariance[21] = orientation_covariance_;
-    odom_msg.pose.covariance[28] = orientation_covariance_;
-    odom_msg.pose.covariance[35] = orientation_covariance_;
+    for (int r = 0; r < 6; ++r) {
+        for (int c = 0; c < 6; ++c) {
+            odom_msg.pose.covariance[r * 6 + c] = estimate.covariance(r, c);
+        }
+    }
     odom_publisher_->publish(std::move(odom_msg));
 }
 

From ee13b4f4adeb8035cf39104b0ecffac512f6e6bf Mon Sep 17 00:00:00 2001
From: tizianoGuadagnino <frevo93@gmail.com>
Date: Mon, 25 Mar 2024 15:30:10 +0100
Subject: [PATCH 22/39] Fix merge

---
 ros/src/OdometryServer.cpp | 30 ++++++++++++++----------------
 ros/src/OdometryServer.hpp |  4 +++-
 2 files changed, 17 insertions(+), 17 deletions(-)

diff --git a/ros/src/OdometryServer.cpp b/ros/src/OdometryServer.cpp
index b7887680..672dc95b 100644
--- a/ros/src/OdometryServer.cpp
+++ b/ros/src/OdometryServer.cpp
@@ -29,6 +29,7 @@
 // KISS-ICP-ROS
 #include "OdometryServer.hpp"
 #include "Utils.hpp"
+#include "kiss_icp/core/Registration.hpp"
 
 // KISS-ICP
 #include "kiss_icp/pipeline/KissICP.hpp"
@@ -77,8 +78,6 @@ OdometryServer::OdometryServer(const rclcpp::NodeOptions &options)
     odom_frame_ = declare_parameter<std::string>("odom_frame", odom_frame_);
     publish_odom_tf_ = declare_parameter<bool>("publish_odom_tf", publish_odom_tf_);
     publish_debug_clouds_ = declare_parameter<bool>("publish_debug_clouds", publish_debug_clouds_);
-    position_covariance_ = declare_parameter<double>("position_covariance", 0.1);
-    orientation_covariance_ = declare_parameter<double>("orientation_covariance", 0.1);
 
     kiss_icp::pipeline::KISSConfig config;
     config.max_range = declare_parameter<double>("max_range", config.max_range);
@@ -139,28 +138,27 @@ void OdometryServer::RegisterFrame(const sensor_msgs::msg::PointCloud2::ConstSha
     // Compute the pose using KISS, ego-centric to the LiDAR
     const auto kiss_estimate = kiss_icp_->estimates().back();
 
+    // Spit the current estimated pose to ROS msgs
+    PublishOdometry(kiss_estimate, msg->header);
+    // Publishing this clouds is a bit costly, so do it only if we are debugging
+    if (publish_debug_clouds_) {
+        PublishClouds(frame, keypoints, msg->header);
+    }
+}
+void OdometryServer::PublishOdometry(const kiss_icp::Estimate &kiss_estimate,
+                                     const std_msgs::msg::Header &header) {
     // If necessary, transform the ego-centric pose to the specified base_link/base_footprint frame
+    const auto cloud_frame_id = header.frame_id;
+    const auto egocentric_estimation = (base_frame_.empty() || base_frame_ == cloud_frame_id);
     const auto estimate = [&]() -> kiss_icp::Estimate {
         if (egocentric_estimation) return kiss_estimate;
         kiss_icp::Estimate transformed;
-        const Sophus::SE3d cloud2base = LookupTransform(base_frame_, cloud_frame_id);
+        const Sophus::SE3d cloud2base = LookupTransform(base_frame_, cloud_frame_id, tf2_buffer_);
         transformed.pose = cloud2base * kiss_estimate.pose * cloud2base.inverse();
         transformed.covariance =
             cloud2base.Adj() * kiss_estimate.covariance * cloud2base.Adj().transpose();
         return transformed;
     }();
-
-    // Spit the current estimated pose to ROS msgs
-    PublishOdometry(estimate, msg->header.stamp, cloud_frame_id);
-    // Publishing this clouds is a bit costly, so do it only if we are debugging
-    if (publish_debug_clouds_) {
-        PublishClouds(frame, keypoints, msg->header);
-    }
-}
-
-void OdometryServer::PublishOdometry(const kiss_icp::Estimate &estimate,
-                                     const rclcpp::Time &stamp,
-                                     const std::string &cloud_frame_id) {
     // Broadcast the tf ---
     if (publish_odom_tf_) {
         geometry_msgs::msg::TransformStamped transform_msg;
@@ -189,7 +187,7 @@ void OdometryServer::PublishClouds(const std::vector<Eigen::Vector3d> frame,
                                    const std::vector<Eigen::Vector3d> keypoints,
                                    const std_msgs::msg::Header &header) {
     const auto kiss_map = kiss_icp_->LocalMap();
-    const auto kiss_pose = kiss_icp_->poses().back().inverse();
+    const auto kiss_pose = kiss_icp_->estimates().back().pose.inverse();
 
     frame_publisher_->publish(std::move(EigenToPointCloud2(frame, header)));
     kpoints_publisher_->publish(std::move(EigenToPointCloud2(keypoints, header)));
diff --git a/ros/src/OdometryServer.hpp b/ros/src/OdometryServer.hpp
index f894ef9a..4b2ddda1 100644
--- a/ros/src/OdometryServer.hpp
+++ b/ros/src/OdometryServer.hpp
@@ -23,6 +23,7 @@
 #pragma once
 
 // KISS-ICP
+#include "kiss_icp/core/Registration.hpp"
 #include "kiss_icp/pipeline/KissICP.hpp"
 
 // ROS 2
@@ -49,7 +50,8 @@ class OdometryServer : public rclcpp::Node {
     void RegisterFrame(const sensor_msgs::msg::PointCloud2::ConstSharedPtr &msg);
 
     /// Stream the estimated pose to ROS
-    void PublishOdometry(const Sophus::SE3d &kiss_pose, const std_msgs::msg::Header &header);
+    void PublishOdometry(const kiss_icp::Estimate &kiss_estimate,
+                         const std_msgs::msg::Header &header);
 
     /// Stream the debugging point clouds for visualization (if required)
     void PublishClouds(const std::vector<Eigen::Vector3d> frame,

From 177373fa6dc87d13964e70dd99dec8d891b638ad Mon Sep 17 00:00:00 2001
From: tizianoGuadagnino <frevo93@gmail.com>
Date: Tue, 26 Mar 2024 09:42:41 +0100
Subject: [PATCH 23/39] More cosmesis

---
 cpp/kiss_icp/core/Registration.cpp | 12 ++++++------
 ros/src/OdometryServer.cpp         |  1 +
 2 files changed, 7 insertions(+), 6 deletions(-)

diff --git a/cpp/kiss_icp/core/Registration.cpp b/cpp/kiss_icp/core/Registration.cpp
index 268f05d3..de0b6674 100644
--- a/cpp/kiss_icp/core/Registration.cpp
+++ b/cpp/kiss_icp/core/Registration.cpp
@@ -188,10 +188,9 @@ Estimate Registration::AlignPointsToMap(const std::vector<Eigen::Vector3d> &fram
 
     // ICP-loop
     Sophus::SE3d T_icp = Sophus::SE3d();
-    Associations associations;
     for (int j = 0; j < max_num_iterations_; ++j) {
         // Equation (10)
-        associations = FindAssociations(source, voxel_map, max_correspondence_distance);
+        const auto associations = FindAssociations(source, voxel_map, max_correspondence_distance);
         // Equation (11)
         const auto &[JTJ, JTr, chi_square] = BuildLinearSystem(associations, GM);
         const Eigen::Vector6d dx = JTJ.ldlt().solve(-JTr);
@@ -203,14 +202,15 @@ Estimate Registration::AlignPointsToMap(const std::vector<Eigen::Vector3d> &fram
         // Termination criteria
         if (dx.norm() < convergence_criterion_) break;
     }
-    const auto &[information_matrix, information_vector, chi_square] =
+    const auto associations = FindAssociations(source, voxel_map, max_correspondence_distance);
+    const auto &[H, std::ignore, chi_square] =
         BuildLinearSystem(associations, [](double x) { return x / x; });
+    const auto icp_covariance =
+        (chi_square / static_cast<double>(associations.size()) * H).inverse();
     Estimate estimate;
     estimate.pose = T_icp * initial_guess.pose;
     const auto A = T_icp.inverse().Adj();
-    estimate.covariance =
-        A * initial_guess.covariance * A.transpose() +
-        (chi_square / static_cast<double>(associations.size()) * information_matrix).inverse();
+    estimate.covariance = A * initial_guess.covariance * A.transpose() + icp_covariance;
     // Spit the final transformation
     return estimate;
 }
diff --git a/ros/src/OdometryServer.cpp b/ros/src/OdometryServer.cpp
index 672dc95b..d61b5e8f 100644
--- a/ros/src/OdometryServer.cpp
+++ b/ros/src/OdometryServer.cpp
@@ -155,6 +155,7 @@ void OdometryServer::PublishOdometry(const kiss_icp::Estimate &kiss_estimate,
         kiss_icp::Estimate transformed;
         const Sophus::SE3d cloud2base = LookupTransform(base_frame_, cloud_frame_id, tf2_buffer_);
         transformed.pose = cloud2base * kiss_estimate.pose * cloud2base.inverse();
+        // TODO verify equation
         transformed.covariance =
             cloud2base.Adj() * kiss_estimate.covariance * cloud2base.Adj().transpose();
         return transformed;

From e54ebd19ec2f80e90bbc5c78cd1a20d691dac8ed Mon Sep 17 00:00:00 2001
From: tizianoGuadagnino <frevo93@gmail.com>
Date: Tue, 26 Mar 2024 11:04:30 +0100
Subject: [PATCH 24/39] Pybind the estimate

---
 cpp/kiss_icp/core/Registration.cpp         |  7 ++++++-
 cpp/kiss_icp/core/Registration.hpp         |  4 +---
 python/kiss_icp/pybind/kiss_icp_pybind.cpp | 18 ++++++++++++------
 python/kiss_icp/registration.py            | 13 +++++++++++++
 4 files changed, 32 insertions(+), 10 deletions(-)

diff --git a/cpp/kiss_icp/core/Registration.cpp b/cpp/kiss_icp/core/Registration.cpp
index de0b6674..5482a7ff 100644
--- a/cpp/kiss_icp/core/Registration.cpp
+++ b/cpp/kiss_icp/core/Registration.cpp
@@ -41,6 +41,11 @@ using Correspondence = std::pair<Eigen::Vector3d, Eigen::Vector3d>;
 using Associations = std::vector<Correspondence>;
 using LinearSystem = std::tuple<Eigen::Matrix6d, Eigen::Vector6d, double>;
 
+namespace Eigen {
+using Matrix3_6d = Eigen::Matrix<double, 3, 6>;
+using Vector6d = Eigen::Matrix<double, 6, 1>;
+}  // namespace Eigen
+
 namespace {
 inline double square(double x) { return x * x; }
 
@@ -203,7 +208,7 @@ Estimate Registration::AlignPointsToMap(const std::vector<Eigen::Vector3d> &fram
         if (dx.norm() < convergence_criterion_) break;
     }
     const auto associations = FindAssociations(source, voxel_map, max_correspondence_distance);
-    const auto &[H, std::ignore, chi_square] =
+    const auto &[H, b, chi_square] =
         BuildLinearSystem(associations, [](double x) { return x / x; });
     const auto icp_covariance =
         (chi_square / static_cast<double>(associations.size()) * H).inverse();
diff --git a/cpp/kiss_icp/core/Registration.hpp b/cpp/kiss_icp/core/Registration.hpp
index 3ea90753..8eacb70f 100644
--- a/cpp/kiss_icp/core/Registration.hpp
+++ b/cpp/kiss_icp/core/Registration.hpp
@@ -30,9 +30,7 @@
 
 namespace Eigen {
 using Matrix6d = Eigen::Matrix<double, 6, 6>;
-using Matrix3_6d = Eigen::Matrix<double, 3, 6>;
-using Vector6d = Eigen::Matrix<double, 6, 1>;
-}  // namespace Eigen
+}
 
 namespace kiss_icp {
 
diff --git a/python/kiss_icp/pybind/kiss_icp_pybind.cpp b/python/kiss_icp/pybind/kiss_icp_pybind.cpp
index b514a6f5..f9a9078b 100644
--- a/python/kiss_icp/pybind/kiss_icp_pybind.cpp
+++ b/python/kiss_icp/pybind/kiss_icp_pybind.cpp
@@ -72,6 +72,15 @@ PYBIND11_MODULE(kiss_icp_pybind, m) {
         .def("_remove_far_away_points", &VoxelHashMap::RemovePointsFarFromLocation, "origin"_a)
         .def("_point_cloud", &VoxelHashMap::Pointcloud);
 
+    py::class_<Estimate> estimate(m, "_Estimate",
+                                  "Estimate from the icp, it contains a pose and a 6x6 covariance");
+    estimate.def(py::init<>())
+        .def_property(
+            "pose", [](Estimate &self) { return self.pose.matrix(); },
+            [](Estimate &self, const Eigen::Matrix4d &T) { self.pose = Sophus::SE3d(T); })
+        .def_property(
+            "covariance", [](Estimate &self) { return self.covariance; },
+            [](Estimate &self, const Eigen::Matrix6d &Sigma) { self.covariance = Sigma; });
     // Point Cloud registration
     py::class_<Registration> internal_registration(m, "_Registration", "Don't use this");
     internal_registration
@@ -80,13 +89,10 @@ PYBIND11_MODULE(kiss_icp_pybind, m) {
         .def(
             "_align_points_to_map",
             [](Registration &self, const std::vector<Eigen::Vector3d> &points,
-               const VoxelHashMap &voxel_map, const Eigen::Matrix4d &T_guess,
+               const VoxelHashMap &voxel_map, const Estimate &initial_guess,
                double max_correspondence_distance, double kernel) {
-                Sophus::SE3d initial_guess(T_guess);
-                return self
-                    .AlignPointsToMap(points, voxel_map, initial_guess, max_correspondence_distance,
-                                      kernel)
-                    .matrix();
+                return self.AlignPointsToMap(points, voxel_map, initial_guess,
+                                             max_correspondence_distance, kernel);
             },
             "points"_a, "voxel_map"_a, "initial_guess"_a, "max_correspondance_distance"_a,
             "kernel"_a);
diff --git a/python/kiss_icp/registration.py b/python/kiss_icp/registration.py
index 65282dfc..0f77985f 100644
--- a/python/kiss_icp/registration.py
+++ b/python/kiss_icp/registration.py
@@ -35,6 +35,19 @@ def get_registration(config: KISSConfig):
     )
 
 
+class Estimate:
+    def __init__(self):
+        self._estimate = kiss_icp_pybind._Estimate()
+
+    @property
+    def pose(self):
+        return self._estimate.pose
+
+    @property
+    def covariance(self):
+        return self._estimate.covariance
+
+
 class Registration:
     def __init__(
         self,

From 9e932198de37474f773073210751e25057df93e6 Mon Sep 17 00:00:00 2001
From: tizianoGuadagnino <frevo93@gmail.com>
Date: Tue, 26 Mar 2024 11:06:30 +0100
Subject: [PATCH 25/39] Reshape the usings for my mental health

---
 cpp/kiss_icp/core/Registration.cpp | 9 +++++----
 cpp/kiss_icp/core/Registration.hpp | 6 ++----
 2 files changed, 7 insertions(+), 8 deletions(-)

diff --git a/cpp/kiss_icp/core/Registration.cpp b/cpp/kiss_icp/core/Registration.cpp
index 5482a7ff..32db4a2e 100644
--- a/cpp/kiss_icp/core/Registration.cpp
+++ b/cpp/kiss_icp/core/Registration.cpp
@@ -37,15 +37,16 @@
 
 #include "VoxelHashMap.hpp"
 
-using Correspondence = std::pair<Eigen::Vector3d, Eigen::Vector3d>;
-using Associations = std::vector<Correspondence>;
-using LinearSystem = std::tuple<Eigen::Matrix6d, Eigen::Vector6d, double>;
-
 namespace Eigen {
+using Matrix6d = Eigen::Matrix<double, 6, 6>;
 using Matrix3_6d = Eigen::Matrix<double, 3, 6>;
 using Vector6d = Eigen::Matrix<double, 6, 1>;
 }  // namespace Eigen
 
+using Correspondence = std::pair<Eigen::Vector3d, Eigen::Vector3d>;
+using Associations = std::vector<Correspondence>;
+using LinearSystem = std::tuple<Eigen::Matrix6d, Eigen::Vector6d, double>;
+
 namespace {
 inline double square(double x) { return x * x; }
 
diff --git a/cpp/kiss_icp/core/Registration.hpp b/cpp/kiss_icp/core/Registration.hpp
index 8eacb70f..fb377e1f 100644
--- a/cpp/kiss_icp/core/Registration.hpp
+++ b/cpp/kiss_icp/core/Registration.hpp
@@ -28,15 +28,13 @@
 
 #include "VoxelHashMap.hpp"
 
-namespace Eigen {
-using Matrix6d = Eigen::Matrix<double, 6, 6>;
-}
+using CovarianceMatrixType = Eigen::Matrix<double, 6, 6>;
 
 namespace kiss_icp {
 
 struct Estimate {
     Sophus::SE3d pose;
-    Eigen::Matrix6d covariance = Eigen::Matrix6d::Zero();
+    CovarianceMatrixType covariance = CovarianceMatrixType::Zero();
 };
 
 struct Registration {

From 75d9b866aed8c84d9c8630cdaaf40ce128093749 Mon Sep 17 00:00:00 2001
From: tizianoGuadagnino <frevo93@gmail.com>
Date: Tue, 26 Mar 2024 11:37:41 +0100
Subject: [PATCH 26/39] Type aliasing on pybinded struct, I love it

---
 python/kiss_icp/pybind/kiss_icp_pybind.cpp |  6 ++----
 python/kiss_icp/registration.py            | 14 +++-----------
 2 files changed, 5 insertions(+), 15 deletions(-)

diff --git a/python/kiss_icp/pybind/kiss_icp_pybind.cpp b/python/kiss_icp/pybind/kiss_icp_pybind.cpp
index f9a9078b..19c86a89 100644
--- a/python/kiss_icp/pybind/kiss_icp_pybind.cpp
+++ b/python/kiss_icp/pybind/kiss_icp_pybind.cpp
@@ -76,11 +76,9 @@ PYBIND11_MODULE(kiss_icp_pybind, m) {
                                   "Estimate from the icp, it contains a pose and a 6x6 covariance");
     estimate.def(py::init<>())
         .def_property(
-            "pose", [](Estimate &self) { return self.pose.matrix(); },
+            "pose", [](const Estimate &self) { return self.pose.matrix(); },
             [](Estimate &self, const Eigen::Matrix4d &T) { self.pose = Sophus::SE3d(T); })
-        .def_property(
-            "covariance", [](Estimate &self) { return self.covariance; },
-            [](Estimate &self, const Eigen::Matrix6d &Sigma) { self.covariance = Sigma; });
+        .def_readwrite("covariance", &Estimate::covariance);
     // Point Cloud registration
     py::class_<Registration> internal_registration(m, "_Registration", "Don't use this");
     internal_registration
diff --git a/python/kiss_icp/registration.py b/python/kiss_icp/registration.py
index 0f77985f..9e55c3d2 100644
--- a/python/kiss_icp/registration.py
+++ b/python/kiss_icp/registration.py
@@ -25,6 +25,7 @@
 from kiss_icp.config.parser import KISSConfig
 from kiss_icp.mapping import VoxelHashMap
 from kiss_icp.pybind import kiss_icp_pybind
+from typing import TypeAlias
 
 
 def get_registration(config: KISSConfig):
@@ -35,17 +36,8 @@ def get_registration(config: KISSConfig):
     )
 
 
-class Estimate:
-    def __init__(self):
-        self._estimate = kiss_icp_pybind._Estimate()
-
-    @property
-    def pose(self):
-        return self._estimate.pose
-
-    @property
-    def covariance(self):
-        return self._estimate.covariance
+# Nice trick with the type aliasing for structs
+Estimate: TypeAlias = kiss_icp_pybind._Estimate
 
 
 class Registration:

From 21c7972c95c11576346a2dd0a742c827957a2842 Mon Sep 17 00:00:00 2001
From: tizianoGuadagnino <frevo93@gmail.com>
Date: Tue, 26 Mar 2024 15:28:40 +0100
Subject: [PATCH 27/39] Getting the python side to dont complain

---
 cpp/kiss_icp/core/Registration.cpp         | 18 +++++++-----
 cpp/kiss_icp/core/Registration.hpp         |  1 +
 python/kiss_icp/kiss_icp.py                | 32 ++++++++++++----------
 python/kiss_icp/pipeline.py                | 11 +++++---
 python/kiss_icp/pybind/kiss_icp_pybind.cpp |  1 +
 5 files changed, 37 insertions(+), 26 deletions(-)

diff --git a/cpp/kiss_icp/core/Registration.cpp b/cpp/kiss_icp/core/Registration.cpp
index 32db4a2e..903ec0c3 100644
--- a/cpp/kiss_icp/core/Registration.cpp
+++ b/cpp/kiss_icp/core/Registration.cpp
@@ -167,6 +167,13 @@ LinearSystem BuildLinearSystem(const Associations &associations,
 
 namespace kiss_icp {
 
+Estimate operator*(Estimate lhs, const Estimate &rhs) {
+    lhs.pose *= rhs.pose;
+    const auto Proj = rhs.pose.Adj().inverse();
+    lhs.covariance = Proj * lhs.covariance * Proj.transpose() + rhs.covariance;
+    return lhs;
+}
+
 Registration::Registration(int max_num_iteration, double convergence_criterion, int max_num_threads)
     : max_num_iterations_(max_num_iteration),
       convergence_criterion_(convergence_criterion),
@@ -211,14 +218,11 @@ Estimate Registration::AlignPointsToMap(const std::vector<Eigen::Vector3d> &fram
     const auto associations = FindAssociations(source, voxel_map, max_correspondence_distance);
     const auto &[H, b, chi_square] =
         BuildLinearSystem(associations, [](double x) { return x / x; });
-    const auto icp_covariance =
-        (chi_square / static_cast<double>(associations.size()) * H).inverse();
-    Estimate estimate;
-    estimate.pose = T_icp * initial_guess.pose;
-    const auto A = T_icp.inverse().Adj();
-    estimate.covariance = A * initial_guess.covariance * A.transpose() + icp_covariance;
+    Estimate delta;
+    delta.pose = T_icp;
+    delta.covariance = (chi_square / static_cast<double>(associations.size()) * H).inverse();
     // Spit the final transformation
-    return estimate;
+    return delta * initial_guess;
 }
 
 }  // namespace kiss_icp
diff --git a/cpp/kiss_icp/core/Registration.hpp b/cpp/kiss_icp/core/Registration.hpp
index fb377e1f..f60ee7bb 100644
--- a/cpp/kiss_icp/core/Registration.hpp
+++ b/cpp/kiss_icp/core/Registration.hpp
@@ -35,6 +35,7 @@ namespace kiss_icp {
 struct Estimate {
     Sophus::SE3d pose;
     CovarianceMatrixType covariance = CovarianceMatrixType::Zero();
+    friend Estimate operator*(Estimate lhs, const Estimate &rhs);
 };
 
 struct Registration {
diff --git a/python/kiss_icp/kiss_icp.py b/python/kiss_icp/kiss_icp.py
index fd894af6..dd99c55a 100644
--- a/python/kiss_icp/kiss_icp.py
+++ b/python/kiss_icp/kiss_icp.py
@@ -26,14 +26,14 @@
 from kiss_icp.deskew import get_motion_compensator
 from kiss_icp.mapping import get_voxel_hash_map
 from kiss_icp.preprocess import get_preprocessor
-from kiss_icp.registration import get_registration
+from kiss_icp.registration import Estimate, get_registration
 from kiss_icp.threshold import get_threshold_estimator
 from kiss_icp.voxelization import voxel_down_sample
 
 
 class KissICP:
     def __init__(self, config: KISSConfig):
-        self.poses = []
+        self.estimates = []
         self.config = config
         self.compensator = get_motion_compensator(config)
         self.adaptive_threshold = get_threshold_estimator(self.config)
@@ -43,7 +43,7 @@ def __init__(self, config: KISSConfig):
 
     def register_frame(self, frame, timestamps):
         # Apply motion compensation
-        frame = self.compensator.deskew_scan(frame, self.poses, timestamps)
+        frame = self.compensator.deskew_scan(frame, self.estimates, timestamps)
 
         # Preprocess the input cloud
         frame = self.preprocess(frame)
@@ -56,11 +56,11 @@ def register_frame(self, frame, timestamps):
 
         # Compute initial_guess for ICP
         prediction = self.get_prediction_model()
-        last_pose = self.poses[-1] if self.poses else np.eye(4)
-        initial_guess = last_pose @ prediction
+        initial_guess = self.estimates[-1] if self.estimates else Estimate()
+        initial_guess = initial_guess @ prediction
 
         # Run ICP
-        new_pose = self.registration.align_points_to_map(
+        new_estimate = self.registration.align_points_to_map(
             points=source,
             voxel_map=self.local_map,
             initial_guess=initial_guess,
@@ -68,9 +68,10 @@ def register_frame(self, frame, timestamps):
             kernel=sigma / 3,
         )
 
-        self.adaptive_threshold.update_model_deviation(np.linalg.inv(initial_guess) @ new_pose)
-        self.local_map.update(frame_downsample, new_pose)
-        self.poses.append(new_pose)
+        pose_deviation = np.linalg.inv(initial_guess.pose) @ new_estimate.pose
+        self.adaptive_threshold.update_model_deviation(pose_deviation)
+        self.local_map.update(frame_downsample, new_estimate.pose)
+        self.estimates.append(new_estimate)
         return frame, source
 
     def voxelize(self, iframe):
@@ -86,13 +87,14 @@ def get_adaptive_threshold(self):
         )
 
     def get_prediction_model(self):
-        if len(self.poses) < 2:
-            return np.eye(4)
-        return np.linalg.inv(self.poses[-2]) @ self.poses[-1]
+        prediction = Estimate()
+        if len(self.estimates) > 1:
+            prediction.pose = np.linalg.inv(self.estimates[-2].pose) @ self.estimates[-1].pose
+        return prediction
 
     def has_moved(self):
-        if len(self.poses) < 1:
+        if len(self.estimates) < 1:
             return False
-        compute_motion = lambda T1, T2: np.linalg.norm((np.linalg.inv(T1) @ T2)[:3, -1])
-        motion = compute_motion(self.poses[0], self.poses[-1])
+        compute_motion = lambda T1, T2: np.linalg.norm((np.linalg.inv(T1.pose) @ T2.pose)[:3, -1])
+        motion = compute_motion(self.estimates[0], self.estimates[-1])
         return motion > 5 * self.config.adaptive_threshold.min_motion_th
diff --git a/python/kiss_icp/pipeline.py b/python/kiss_icp/pipeline.py
index 8946d946..3149a82c 100644
--- a/python/kiss_icp/pipeline.py
+++ b/python/kiss_icp/pipeline.py
@@ -65,7 +65,7 @@ def __init__(
         self.odometry = KissICP(config=self.config)
         self.results = PipelineResults()
         self.times = []
-        self.poses = self.odometry.poses
+        self.estimates = self.odometry.estimates
         self.has_gt = hasattr(self._dataset, "gt_poses")
         self.gt_poses = self._dataset.gt_poses[self._first : self._last] if self.has_gt else None
         self.dataset_name = self._dataset.__class__.__name__
@@ -98,7 +98,9 @@ def _run_pipeline(self):
             start_time = time.perf_counter_ns()
             source, keypoints = self.odometry.register_frame(raw_frame, timestamps)
             self.times.append(time.perf_counter_ns() - start_time)
-            self.visualizer.update(source, keypoints, self.odometry.local_map, self.poses[-1])
+            self.visualizer.update(
+                source, keypoints, self.odometry.local_map, self.estimates[-1].pose
+            )
 
     def _next(self, idx):
         """TODO: re-arrange this logic"""
@@ -167,9 +169,10 @@ def _write_gt_poses(self):
 
     def _run_evaluation(self):
         # Run estimation metrics evaluation, only when GT data was provided
+        poses = [x.pose for x in self.estimates]
         if self.has_gt:
-            avg_tra, avg_rot = sequence_error(self.gt_poses, self.poses)
-            ate_rot, ate_trans = absolute_trajectory_error(self.gt_poses, self.poses)
+            avg_tra, avg_rot = sequence_error(self.gt_poses, poses)
+            ate_rot, ate_trans = absolute_trajectory_error(self.gt_poses, poses)
             self.results.append(desc="Average Translation Error", units="%", value=avg_tra)
             self.results.append(desc="Average Rotational Error", units="deg/m", value=avg_rot)
             self.results.append(desc="Absolute Trajectory Error (ATE)", units="m", value=ate_trans)
diff --git a/python/kiss_icp/pybind/kiss_icp_pybind.cpp b/python/kiss_icp/pybind/kiss_icp_pybind.cpp
index 19c86a89..0f499448 100644
--- a/python/kiss_icp/pybind/kiss_icp_pybind.cpp
+++ b/python/kiss_icp/pybind/kiss_icp_pybind.cpp
@@ -75,6 +75,7 @@ PYBIND11_MODULE(kiss_icp_pybind, m) {
     py::class_<Estimate> estimate(m, "_Estimate",
                                   "Estimate from the icp, it contains a pose and a 6x6 covariance");
     estimate.def(py::init<>())
+        .def("__matmul__", [](Estimate lhs, const Estimate &rhs) { return lhs * rhs; })
         .def_property(
             "pose", [](const Estimate &self) { return self.pose.matrix(); },
             [](Estimate &self, const Eigen::Matrix4d &T) { self.pose = Sophus::SE3d(T); })

From 5ec519be421a5a8df8c04a0ac14fff15a3a1d088 Mon Sep 17 00:00:00 2001
From: tizianoGuadagnino <frevo93@gmail.com>
Date: Tue, 26 Mar 2024 15:39:33 +0100
Subject: [PATCH 28/39] Now also python doesn't cry

---
 python/kiss_icp/pipeline.py | 14 +++++++++++---
 1 file changed, 11 insertions(+), 3 deletions(-)

diff --git a/python/kiss_icp/pipeline.py b/python/kiss_icp/pipeline.py
index 3149a82c..eebbe04a 100644
--- a/python/kiss_icp/pipeline.py
+++ b/python/kiss_icp/pipeline.py
@@ -83,12 +83,14 @@ def __init__(
     # Public interface  ------
     def run(self):
         self._run_pipeline()
+        self._get_poses_and_covariances()
         self._run_evaluation()
         self._create_output_dir()
         self._write_result_poses()
         self._write_gt_poses()
         self._write_cfg()
         self._write_log()
+        self._write_covariances()
         return self.results
 
     # Private interface  ------
@@ -112,6 +114,10 @@ def _next(self, idx):
             timestamps = np.zeros(frame.shape[0])
         return frame, timestamps
 
+    def _get_poses_and_covariances(self):
+        self.poses = [x.pose for x in self.estimates]
+        self.covariances = [x.covariance for x in self.estimates]
+
     @staticmethod
     def save_poses_kitti_format(filename: str, poses: List[np.ndarray]):
         def _to_kitti_format(poses: np.ndarray) -> np.ndarray:
@@ -167,12 +173,14 @@ def _write_gt_poses(self):
             timestamps=self._get_frames_timestamps(),
         )
 
+    def _write_covariances(self):
+        np.save(f"{self.results_dir}/covariances.npy", np.stack(self.covariances))
+
     def _run_evaluation(self):
         # Run estimation metrics evaluation, only when GT data was provided
-        poses = [x.pose for x in self.estimates]
         if self.has_gt:
-            avg_tra, avg_rot = sequence_error(self.gt_poses, poses)
-            ate_rot, ate_trans = absolute_trajectory_error(self.gt_poses, poses)
+            avg_tra, avg_rot = sequence_error(self.gt_poses, self.poses)
+            ate_rot, ate_trans = absolute_trajectory_error(self.gt_poses, self.poses)
             self.results.append(desc="Average Translation Error", units="%", value=avg_tra)
             self.results.append(desc="Average Rotational Error", units="deg/m", value=avg_rot)
             self.results.append(desc="Absolute Trajectory Error (ATE)", units="m", value=ate_trans)

From 7d38a34c88a715cbbcfbab46421ac6131303b1c8 Mon Sep 17 00:00:00 2001
From: tizianoGuadagnino <frevo93@gmail.com>
Date: Wed, 27 Mar 2024 13:36:04 +0100
Subject: [PATCH 29/39] Simplify python side

---
 python/kiss_icp/pipeline.py | 9 ++++-----
 1 file changed, 4 insertions(+), 5 deletions(-)

diff --git a/python/kiss_icp/pipeline.py b/python/kiss_icp/pipeline.py
index eebbe04a..71f1a3ad 100644
--- a/python/kiss_icp/pipeline.py
+++ b/python/kiss_icp/pipeline.py
@@ -66,6 +66,8 @@ def __init__(
         self.results = PipelineResults()
         self.times = []
         self.estimates = self.odometry.estimates
+        self.poses = []
+        self.covariances = []
         self.has_gt = hasattr(self._dataset, "gt_poses")
         self.gt_poses = self._dataset.gt_poses[self._first : self._last] if self.has_gt else None
         self.dataset_name = self._dataset.__class__.__name__
@@ -83,7 +85,6 @@ def __init__(
     # Public interface  ------
     def run(self):
         self._run_pipeline()
-        self._get_poses_and_covariances()
         self._run_evaluation()
         self._create_output_dir()
         self._write_result_poses()
@@ -99,6 +100,8 @@ def _run_pipeline(self):
             raw_frame, timestamps = self._next(idx)
             start_time = time.perf_counter_ns()
             source, keypoints = self.odometry.register_frame(raw_frame, timestamps)
+            self.poses.append(self.estimates[-1].pose)
+            self.covariances.append(self.estimates[-1].covariance)
             self.times.append(time.perf_counter_ns() - start_time)
             self.visualizer.update(
                 source, keypoints, self.odometry.local_map, self.estimates[-1].pose
@@ -114,10 +117,6 @@ def _next(self, idx):
             timestamps = np.zeros(frame.shape[0])
         return frame, timestamps
 
-    def _get_poses_and_covariances(self):
-        self.poses = [x.pose for x in self.estimates]
-        self.covariances = [x.covariance for x in self.estimates]
-
     @staticmethod
     def save_poses_kitti_format(filename: str, poses: List[np.ndarray]):
         def _to_kitti_format(poses: np.ndarray) -> np.ndarray:

From c251278ca2a10ae0db6eb2bb19d0d7e9c5c8df4c Mon Sep 17 00:00:00 2001
From: tizianoGuadagnino <frevo93@gmail.com>
Date: Wed, 27 Mar 2024 14:03:04 +0100
Subject: [PATCH 30/39] Update formula, now based on Barfoot et.al

---
 cpp/kiss_icp/core/Registration.cpp | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/cpp/kiss_icp/core/Registration.cpp b/cpp/kiss_icp/core/Registration.cpp
index 903ec0c3..24cc0343 100644
--- a/cpp/kiss_icp/core/Registration.cpp
+++ b/cpp/kiss_icp/core/Registration.cpp
@@ -169,8 +169,8 @@ namespace kiss_icp {
 
 Estimate operator*(Estimate lhs, const Estimate &rhs) {
     lhs.pose *= rhs.pose;
-    const auto Proj = rhs.pose.Adj().inverse();
-    lhs.covariance = Proj * lhs.covariance * Proj.transpose() + rhs.covariance;
+    const auto Proj = lhs.pose.Adj();
+    lhs.covariance += Proj * rhs.covariance * Proj.transpose();
     return lhs;
 }
 

From 39ed3ba0bcc5af850d37440b3cd4212bc66935d7 Mon Sep 17 00:00:00 2001
From: tizianoGuadagnino <frevo93@gmail.com>
Date: Wed, 27 Mar 2024 14:50:27 +0100
Subject: [PATCH 31/39] Add the inverse operation

---
 cpp/kiss_icp/core/Registration.cpp         | 13 +++++++++++--
 cpp/kiss_icp/core/Registration.hpp         |  5 ++++-
 cpp/kiss_icp/pipeline/KissICP.cpp          | 11 +++++------
 cpp/kiss_icp/pipeline/KissICP.hpp          |  2 +-
 python/kiss_icp/kiss_icp.py                |  6 +++---
 python/kiss_icp/pybind/kiss_icp_pybind.cpp |  1 +
 6 files changed, 25 insertions(+), 13 deletions(-)

diff --git a/cpp/kiss_icp/core/Registration.cpp b/cpp/kiss_icp/core/Registration.cpp
index 24cc0343..84afe776 100644
--- a/cpp/kiss_icp/core/Registration.cpp
+++ b/cpp/kiss_icp/core/Registration.cpp
@@ -167,10 +167,19 @@ LinearSystem BuildLinearSystem(const Associations &associations,
 
 namespace kiss_icp {
 
+Estimate::Estimate() : pose(), covariance(Eigen::Matrix6d::Zero()) {}
+Estimate::Estimate(const Sophus::SE3d &T, const Eigen::Matrix6d &Sigma)
+    : pose(T), covariance(Sigma) {}
+
+Estimate Estimate::inverse() const {
+    const auto Adj = pose.inverse().Adj();
+    return {pose.inverse(), Adj * covariance * Adj.transpose()};
+}
+
 Estimate operator*(Estimate lhs, const Estimate &rhs) {
     lhs.pose *= rhs.pose;
-    const auto Proj = lhs.pose.Adj();
-    lhs.covariance += Proj * rhs.covariance * Proj.transpose();
+    const auto Adj = lhs.pose.Adj();
+    lhs.covariance += Adj * rhs.covariance * Adj.transpose();
     return lhs;
 }
 
diff --git a/cpp/kiss_icp/core/Registration.hpp b/cpp/kiss_icp/core/Registration.hpp
index f60ee7bb..b6a0261d 100644
--- a/cpp/kiss_icp/core/Registration.hpp
+++ b/cpp/kiss_icp/core/Registration.hpp
@@ -33,8 +33,11 @@ using CovarianceMatrixType = Eigen::Matrix<double, 6, 6>;
 namespace kiss_icp {
 
 struct Estimate {
+    Estimate();
+    Estimate(const Sophus::SE3d &T, const CovarianceMatrixType &Sigma);
     Sophus::SE3d pose;
-    CovarianceMatrixType covariance = CovarianceMatrixType::Zero();
+    CovarianceMatrixType covariance;
+    Estimate inverse() const;
     friend Estimate operator*(Estimate lhs, const Estimate &rhs);
 };
 
diff --git a/cpp/kiss_icp/pipeline/KissICP.cpp b/cpp/kiss_icp/pipeline/KissICP.cpp
index 5eb17072..bea7969d 100644
--- a/cpp/kiss_icp/pipeline/KissICP.cpp
+++ b/cpp/kiss_icp/pipeline/KissICP.cpp
@@ -65,14 +65,14 @@ KissICP::Vector3dVectorTuple KissICP::RegisterFrame(const std::vector<Eigen::Vec
     // Compute initial_guess for ICP
     const auto prediction = GetPredictionModel();
     auto initial_guess = !estimates_.empty() ? estimates_.back() : Estimate();
-    initial_guess.pose = initial_guess.pose * prediction;
+    initial_guess = initial_guess * prediction;
     // Run icp
     const auto new_estimate = registration_.AlignPointsToMap(source,         //
                                                              local_map_,     //
                                                              initial_guess,  //
                                                              3.0 * sigma,    //
                                                              sigma / 3.0);
-    const auto model_deviation = initial_guess.pose.inverse() * new_estimate.pose;
+    const auto model_deviation = (initial_guess.inverse() * new_estimate).pose;
     adaptive_threshold_.UpdateModelDeviation(model_deviation);
     local_map_.Update(frame_downsample, new_estimate.pose);
     estimates_.push_back(new_estimate);
@@ -93,11 +93,10 @@ double KissICP::GetAdaptiveThreshold() {
     return adaptive_threshold_.ComputeThreshold();
 }
 
-Sophus::SE3d KissICP::GetPredictionModel() const {
-    Sophus::SE3d pred = Sophus::SE3d();
+Estimate KissICP::GetPredictionModel() const {
     const size_t N = estimates_.size();
-    if (N < 2) return pred;
-    return estimates_[N - 2].pose.inverse() * estimates_[N - 1].pose;
+    if (N < 2) return Estimate();
+    return estimates_[N - 2].inverse() * estimates_[N - 1];
 }
 
 bool KissICP::HasMoved() {
diff --git a/cpp/kiss_icp/pipeline/KissICP.hpp b/cpp/kiss_icp/pipeline/KissICP.hpp
index 4b22124a..ab44caf5 100644
--- a/cpp/kiss_icp/pipeline/KissICP.hpp
+++ b/cpp/kiss_icp/pipeline/KissICP.hpp
@@ -71,7 +71,7 @@ class KissICP {
                                       const std::vector<double> &timestamps);
     Vector3dVectorTuple Voxelize(const std::vector<Eigen::Vector3d> &frame) const;
     double GetAdaptiveThreshold();
-    Sophus::SE3d GetPredictionModel() const;
+    Estimate GetPredictionModel() const;
     bool HasMoved();
 
 public:
diff --git a/python/kiss_icp/kiss_icp.py b/python/kiss_icp/kiss_icp.py
index dd99c55a..39209e14 100644
--- a/python/kiss_icp/kiss_icp.py
+++ b/python/kiss_icp/kiss_icp.py
@@ -68,7 +68,7 @@ def register_frame(self, frame, timestamps):
             kernel=sigma / 3,
         )
 
-        pose_deviation = np.linalg.inv(initial_guess.pose) @ new_estimate.pose
+        pose_deviation = (initial_guess.inverse() @ new_estimate).pose
         self.adaptive_threshold.update_model_deviation(pose_deviation)
         self.local_map.update(frame_downsample, new_estimate.pose)
         self.estimates.append(new_estimate)
@@ -89,12 +89,12 @@ def get_adaptive_threshold(self):
     def get_prediction_model(self):
         prediction = Estimate()
         if len(self.estimates) > 1:
-            prediction.pose = np.linalg.inv(self.estimates[-2].pose) @ self.estimates[-1].pose
+            prediction.pose = (self.estimates[-2].inverse() @ self.estimates[-1]).pose
         return prediction
 
     def has_moved(self):
         if len(self.estimates) < 1:
             return False
-        compute_motion = lambda T1, T2: np.linalg.norm((np.linalg.inv(T1.pose) @ T2.pose)[:3, -1])
+        compute_motion = lambda T1, T2: np.linalg.norm((T1.inverse() @ T2).pose[:3, -1])
         motion = compute_motion(self.estimates[0], self.estimates[-1])
         return motion > 5 * self.config.adaptive_threshold.min_motion_th
diff --git a/python/kiss_icp/pybind/kiss_icp_pybind.cpp b/python/kiss_icp/pybind/kiss_icp_pybind.cpp
index 0f499448..46e34872 100644
--- a/python/kiss_icp/pybind/kiss_icp_pybind.cpp
+++ b/python/kiss_icp/pybind/kiss_icp_pybind.cpp
@@ -76,6 +76,7 @@ PYBIND11_MODULE(kiss_icp_pybind, m) {
                                   "Estimate from the icp, it contains a pose and a 6x6 covariance");
     estimate.def(py::init<>())
         .def("__matmul__", [](Estimate lhs, const Estimate &rhs) { return lhs * rhs; })
+        .def("inverse", &Estimate::inverse)
         .def_property(
             "pose", [](const Estimate &self) { return self.pose.matrix(); },
             [](Estimate &self, const Eigen::Matrix4d &T) { self.pose = Sophus::SE3d(T); })

From d94aae0473e5d3636ecda2767978406127b16ec9 Mon Sep 17 00:00:00 2001
From: tizianoGuadagnino <frevo93@gmail.com>
Date: Wed, 27 Mar 2024 14:55:24 +0100
Subject: [PATCH 32/39] We now take into account the uncertanty of the
 prediction model

---
 python/kiss_icp/kiss_icp.py | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/python/kiss_icp/kiss_icp.py b/python/kiss_icp/kiss_icp.py
index 39209e14..cf579cd7 100644
--- a/python/kiss_icp/kiss_icp.py
+++ b/python/kiss_icp/kiss_icp.py
@@ -89,7 +89,7 @@ def get_adaptive_threshold(self):
     def get_prediction_model(self):
         prediction = Estimate()
         if len(self.estimates) > 1:
-            prediction.pose = (self.estimates[-2].inverse() @ self.estimates[-1]).pose
+            prediction = self.estimates[-2].inverse() @ self.estimates[-1]
         return prediction
 
     def has_moved(self):

From 2a054b8e7ccdbcbab65200a6ed0c0a04982f51e4 Mon Sep 17 00:00:00 2001
From: tizianoGuadagnino <frevo93@gmail.com>
Date: Wed, 27 Mar 2024 15:28:00 +0100
Subject: [PATCH 33/39] Simplify

---
 cpp/kiss_icp/core/Registration.cpp | 14 +++++++-------
 cpp/kiss_icp/pipeline/KissICP.cpp  |  4 ++--
 2 files changed, 9 insertions(+), 9 deletions(-)

diff --git a/cpp/kiss_icp/core/Registration.cpp b/cpp/kiss_icp/core/Registration.cpp
index 84afe776..9bc7fcbd 100644
--- a/cpp/kiss_icp/core/Registration.cpp
+++ b/cpp/kiss_icp/core/Registration.cpp
@@ -201,6 +201,7 @@ Estimate Registration::AlignPointsToMap(const std::vector<Eigen::Vector3d> &fram
                                         double kernel_threshold) {
     if (voxel_map.Empty()) return initial_guess;
 
+    auto no_kernel = [&](double residual2) { return residual2 / residual2; };
     auto GM = [&](double residual2) {
         return square(kernel_threshold) / square(kernel_threshold + residual2);
     };
@@ -224,14 +225,13 @@ Estimate Registration::AlignPointsToMap(const std::vector<Eigen::Vector3d> &fram
         // Termination criteria
         if (dx.norm() < convergence_criterion_) break;
     }
-    const auto associations = FindAssociations(source, voxel_map, max_correspondence_distance);
-    const auto &[H, b, chi_square] =
-        BuildLinearSystem(associations, [](double x) { return x / x; });
-    Estimate delta;
-    delta.pose = T_icp;
-    delta.covariance = (chi_square / static_cast<double>(associations.size()) * H).inverse();
+    // tg UGLY PART
+    const auto associations = FindAssociations(source, voxel_map, kernel_threshold);
+    const auto &[H, b, chi_square] = BuildLinearSystem(associations, no_kernel);
+    const auto covariance_icp = chi_square / static_cast<double>(associations.size()) * H.inverse();
+    Estimate icp_correction(T_icp, covariance_icp);
     // Spit the final transformation
-    return delta * initial_guess;
+    return icp_correction * initial_guess;
 }
 
 }  // namespace kiss_icp
diff --git a/cpp/kiss_icp/pipeline/KissICP.cpp b/cpp/kiss_icp/pipeline/KissICP.cpp
index bea7969d..63ff5367 100644
--- a/cpp/kiss_icp/pipeline/KissICP.cpp
+++ b/cpp/kiss_icp/pipeline/KissICP.cpp
@@ -64,8 +64,8 @@ KissICP::Vector3dVectorTuple KissICP::RegisterFrame(const std::vector<Eigen::Vec
 
     // Compute initial_guess for ICP
     const auto prediction = GetPredictionModel();
-    auto initial_guess = !estimates_.empty() ? estimates_.back() : Estimate();
-    initial_guess = initial_guess * prediction;
+    const auto last_estimate = !estimates_.empty() ? estimates_.back() : Estimate();
+    const auto initial_guess = last_estimate * prediction;
     // Run icp
     const auto new_estimate = registration_.AlignPointsToMap(source,         //
                                                              local_map_,     //

From bf8c3772501ea671db6a20f5d0c02192bb7c895a Mon Sep 17 00:00:00 2001
From: tizianoGuadagnino <frevo93@gmail.com>
Date: Thu, 28 Mar 2024 11:54:21 +0100
Subject: [PATCH 34/39] Now it makes sense

---
 cpp/kiss_icp/core/Registration.cpp | 4 ++--
 cpp/kiss_icp/pipeline/KissICP.cpp  | 7 +++++--
 python/kiss_icp/kiss_icp.py        | 4 +++-
 3 files changed, 10 insertions(+), 5 deletions(-)

diff --git a/cpp/kiss_icp/core/Registration.cpp b/cpp/kiss_icp/core/Registration.cpp
index 9bc7fcbd..b2be9b0b 100644
--- a/cpp/kiss_icp/core/Registration.cpp
+++ b/cpp/kiss_icp/core/Registration.cpp
@@ -201,7 +201,7 @@ Estimate Registration::AlignPointsToMap(const std::vector<Eigen::Vector3d> &fram
                                         double kernel_threshold) {
     if (voxel_map.Empty()) return initial_guess;
 
-    auto no_kernel = [&](double residual2) { return residual2 / residual2; };
+    auto no_kernel = [&](double residual2) { return residual2 > 0.0 ? 1.0 : 1.0; };
     auto GM = [&](double residual2) {
         return square(kernel_threshold) / square(kernel_threshold + residual2);
     };
@@ -226,7 +226,7 @@ Estimate Registration::AlignPointsToMap(const std::vector<Eigen::Vector3d> &fram
         if (dx.norm() < convergence_criterion_) break;
     }
     // tg UGLY PART
-    const auto associations = FindAssociations(source, voxel_map, kernel_threshold);
+    const auto associations = FindAssociations(source, voxel_map, voxel_map.voxel_size_ * 0.5);
     const auto &[H, b, chi_square] = BuildLinearSystem(associations, no_kernel);
     const auto covariance_icp = chi_square / static_cast<double>(associations.size()) * H.inverse();
     Estimate icp_correction(T_icp, covariance_icp);
diff --git a/cpp/kiss_icp/pipeline/KissICP.cpp b/cpp/kiss_icp/pipeline/KissICP.cpp
index 63ff5367..f539122f 100644
--- a/cpp/kiss_icp/pipeline/KissICP.cpp
+++ b/cpp/kiss_icp/pipeline/KissICP.cpp
@@ -94,9 +94,12 @@ double KissICP::GetAdaptiveThreshold() {
 }
 
 Estimate KissICP::GetPredictionModel() const {
+    Estimate prediction;
     const size_t N = estimates_.size();
-    if (N < 2) return Estimate();
-    return estimates_[N - 2].inverse() * estimates_[N - 1];
+    if (N > 1) {
+        prediction.pose = (estimates_[N - 2].inverse() * estimates_[N - 1]).pose;
+    }
+    return prediction;
 }
 
 bool KissICP::HasMoved() {
diff --git a/python/kiss_icp/kiss_icp.py b/python/kiss_icp/kiss_icp.py
index cf579cd7..84959594 100644
--- a/python/kiss_icp/kiss_icp.py
+++ b/python/kiss_icp/kiss_icp.py
@@ -89,7 +89,9 @@ def get_adaptive_threshold(self):
     def get_prediction_model(self):
         prediction = Estimate()
         if len(self.estimates) > 1:
-            prediction = self.estimates[-2].inverse() @ self.estimates[-1]
+            pose_from = self.estimates[-2].pose
+            pose_to = self.estimates[-1].pose
+            prediction.pose = np.linalg.inv(pose_from) @ pose_to
         return prediction
 
     def has_moved(self):

From f512a0d29983a838b9cb18570a73c43a473b9f72 Mon Sep 17 00:00:00 2001
From: tizianoGuadagnino <frevo93@gmail.com>
Date: Thu, 28 Mar 2024 12:17:30 +0100
Subject: [PATCH 35/39] Consistent naming for the ICP system matrix

---
 cpp/kiss_icp/core/Registration.cpp | 5 +++--
 1 file changed, 3 insertions(+), 2 deletions(-)

diff --git a/cpp/kiss_icp/core/Registration.cpp b/cpp/kiss_icp/core/Registration.cpp
index b2be9b0b..20d0540d 100644
--- a/cpp/kiss_icp/core/Registration.cpp
+++ b/cpp/kiss_icp/core/Registration.cpp
@@ -227,8 +227,9 @@ Estimate Registration::AlignPointsToMap(const std::vector<Eigen::Vector3d> &fram
     }
     // tg UGLY PART
     const auto associations = FindAssociations(source, voxel_map, voxel_map.voxel_size_ * 0.5);
-    const auto &[H, b, chi_square] = BuildLinearSystem(associations, no_kernel);
-    const auto covariance_icp = chi_square / static_cast<double>(associations.size()) * H.inverse();
+    const auto &[JTJ, JTr, chi_square] = BuildLinearSystem(associations, no_kernel);
+    const auto covariance_icp =
+        chi_square / static_cast<double>(associations.size()) * JTJ.inverse();
     Estimate icp_correction(T_icp, covariance_icp);
     // Spit the final transformation
     return icp_correction * initial_guess;

From 13fcb56a7cc79754016283f84330aa6427f8ca56 Mon Sep 17 00:00:00 2001
From: tizianoGuadagnino <frevo93@gmail.com>
Date: Thu, 28 Mar 2024 12:47:31 +0100
Subject: [PATCH 36/39] Add reference for equations, still the values are too
 high

---
 cpp/kiss_icp/core/Registration.cpp |  1 +
 ros/src/OdometryServer.cpp         | 10 +++-------
 2 files changed, 4 insertions(+), 7 deletions(-)

diff --git a/cpp/kiss_icp/core/Registration.cpp b/cpp/kiss_icp/core/Registration.cpp
index 20d0540d..3ae2825c 100644
--- a/cpp/kiss_icp/core/Registration.cpp
+++ b/cpp/kiss_icp/core/Registration.cpp
@@ -171,6 +171,7 @@ Estimate::Estimate() : pose(), covariance(Eigen::Matrix6d::Zero()) {}
 Estimate::Estimate(const Sophus::SE3d &T, const Eigen::Matrix6d &Sigma)
     : pose(T), covariance(Sigma) {}
 
+// Equations from figure 3 of https://arxiv.org/pdf/1906.07795.pdf
 Estimate Estimate::inverse() const {
     const auto Adj = pose.inverse().Adj();
     return {pose.inverse(), Adj * covariance * Adj.transpose()};
diff --git a/ros/src/OdometryServer.cpp b/ros/src/OdometryServer.cpp
index 6925b19d..8997eb5c 100644
--- a/ros/src/OdometryServer.cpp
+++ b/ros/src/OdometryServer.cpp
@@ -152,13 +152,9 @@ void OdometryServer::PublishOdometry(const kiss_icp::Estimate &kiss_estimate,
     const auto egocentric_estimation = (base_frame_.empty() || base_frame_ == cloud_frame_id);
     const auto estimate = [&]() -> kiss_icp::Estimate {
         if (egocentric_estimation) return kiss_estimate;
-        kiss_icp::Estimate transformed;
-        const Sophus::SE3d cloud2base = LookupTransform(base_frame_, cloud_frame_id, tf2_buffer_);
-        transformed.pose = cloud2base * kiss_estimate.pose * cloud2base.inverse();
-        // TODO verify equation
-        transformed.covariance =
-            cloud2base.Adj() * kiss_estimate.covariance * cloud2base.Adj().transpose();
-        return transformed;
+        kiss_icp::Estimate cloud2base;
+        cloud2base.pose = LookupTransform(base_frame_, cloud_frame_id, tf2_buffer_);
+        return cloud2base * kiss_estimate * cloud2base.inverse();
     }();
     // Broadcast the tf ---
     if (publish_odom_tf_) {

From 280e9a459c6640f1dcd7da3e3ac3cf9286085cd6 Mon Sep 17 00:00:00 2001
From: tizianoGuadagnino <frevo93@gmail.com>
Date: Wed, 3 Apr 2024 10:41:47 +0200
Subject: [PATCH 37/39] Reshape and follow proper math

---
 cpp/kiss_icp/core/Registration.cpp | 34 +++++++++++++-----------------
 1 file changed, 15 insertions(+), 19 deletions(-)

diff --git a/cpp/kiss_icp/core/Registration.cpp b/cpp/kiss_icp/core/Registration.cpp
index 3ae2825c..7e2d6d47 100644
--- a/cpp/kiss_icp/core/Registration.cpp
+++ b/cpp/kiss_icp/core/Registration.cpp
@@ -123,8 +123,7 @@ Associations FindAssociations(const std::vector<Eigen::Vector3d> &points,
     return associations;
 }
 
-LinearSystem BuildLinearSystem(const Associations &associations,
-                               std::function<double_t(double_t)> kernel) {
+LinearSystem BuildLinearSystem(const Associations &associations, const double kernel_threshold) {
     auto compute_jacobian_and_residual = [](auto association) {
         const auto &[source, target] = association;
         const Eigen::Vector3d residual = source - target;
@@ -141,6 +140,10 @@ LinearSystem BuildLinearSystem(const Associations &associations,
         return a;
     };
 
+    auto GM_kernel = [&](double residual2) {
+        return square(kernel_threshold) / square(kernel_threshold + residual2);
+    };
+
     using associations_iterator = Associations::const_iterator;
     const auto &[JTJ, JTr, chi_square] = tbb::parallel_reduce(
         // Range
@@ -153,7 +156,7 @@ LinearSystem BuildLinearSystem(const Associations &associations,
                 r.begin(), r.end(), J, sum_linear_systems, [&](const auto &association) {
                     const auto &[J_r, residual] = compute_jacobian_and_residual(association);
                     const double chi_square = residual.squaredNorm();
-                    const double w = kernel(chi_square);
+                    const double w = GM_kernel(chi_square);
                     return LinearSystem(J_r.transpose() * w * J_r,                    // JTJ
                                         J_r.transpose() * w * residual, chi_square);  // JTr
                 });
@@ -202,36 +205,29 @@ Estimate Registration::AlignPointsToMap(const std::vector<Eigen::Vector3d> &fram
                                         double kernel_threshold) {
     if (voxel_map.Empty()) return initial_guess;
 
-    auto no_kernel = [&](double residual2) { return residual2 > 0.0 ? 1.0 : 1.0; };
-    auto GM = [&](double residual2) {
-        return square(kernel_threshold) / square(kernel_threshold + residual2);
-    };
     // Equation (9)
     std::vector<Eigen::Vector3d> source = frame;
     TransformPoints(initial_guess.pose, source);
 
     // ICP-loop
-    Sophus::SE3d T_icp = Sophus::SE3d();
+    Estimate icp_correction;
     for (int j = 0; j < max_num_iterations_; ++j) {
         // Equation (10)
-        const auto associations = FindAssociations(source, voxel_map, max_correspondence_distance);
+        const auto &associations = FindAssociations(source, voxel_map, max_correspondence_distance);
         // Equation (11)
-        const auto &[JTJ, JTr, chi_square] = BuildLinearSystem(associations, GM);
-        const Eigen::Vector6d dx = JTJ.ldlt().solve(-JTr);
+        const auto &[JTJ, JTr, chi_square] = BuildLinearSystem(associations, kernel_threshold);
+        const auto JTJ_inverse = JTJ.inverse();
+        const Eigen::Vector6d dx = -JTJ_inverse * JTr;
         const Sophus::SE3d estimation = Sophus::SE3d::exp(dx);
+        icp_correction.pose = estimation * icp_correction.pose;
+        // Follow Ollila et.al. https://arxiv.org/pdf/2006.10005.pdf - Eq. (2) for sample covariance
+        // using an M estimator
+        icp_correction.covariance = 1.0 / static_cast<double>(associations.size()) * JTJ_inverse;
         // Equation (12)
         TransformPoints(estimation, source);
-        // Update iterations
-        T_icp = estimation * T_icp;
         // Termination criteria
         if (dx.norm() < convergence_criterion_) break;
     }
-    // tg UGLY PART
-    const auto associations = FindAssociations(source, voxel_map, voxel_map.voxel_size_ * 0.5);
-    const auto &[JTJ, JTr, chi_square] = BuildLinearSystem(associations, no_kernel);
-    const auto covariance_icp =
-        chi_square / static_cast<double>(associations.size()) * JTJ.inverse();
-    Estimate icp_correction(T_icp, covariance_icp);
     // Spit the final transformation
     return icp_correction * initial_guess;
 }

From 19582b8fd28ebc32a766aa140f34a1a428fe4991 Mon Sep 17 00:00:00 2001
From: tizianoGuadagnino <frevo93@gmail.com>
Date: Wed, 3 Apr 2024 12:09:45 +0200
Subject: [PATCH 38/39] Fix isort

---
 python/kiss_icp/registration.py | 3 ++-
 1 file changed, 2 insertions(+), 1 deletion(-)

diff --git a/python/kiss_icp/registration.py b/python/kiss_icp/registration.py
index 9e55c3d2..5779e9e0 100644
--- a/python/kiss_icp/registration.py
+++ b/python/kiss_icp/registration.py
@@ -20,12 +20,13 @@
 # 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 typing import TypeAlias
+
 import numpy as np
 
 from kiss_icp.config.parser import KISSConfig
 from kiss_icp.mapping import VoxelHashMap
 from kiss_icp.pybind import kiss_icp_pybind
-from typing import TypeAlias
 
 
 def get_registration(config: KISSConfig):

From 3589390f0ecc4ed4e6f336c6c291524fe327fa7f Mon Sep 17 00:00:00 2001
From: benemer <benedikt.mersch@gmail.com>
Date: Mon, 8 Apr 2024 15:52:16 +0200
Subject: [PATCH 39/39] Better match comments

---
 cpp/kiss_icp/core/Registration.cpp | 5 +++--
 1 file changed, 3 insertions(+), 2 deletions(-)

diff --git a/cpp/kiss_icp/core/Registration.cpp b/cpp/kiss_icp/core/Registration.cpp
index 7e2d6d47..c9b6c7e3 100644
--- a/cpp/kiss_icp/core/Registration.cpp
+++ b/cpp/kiss_icp/core/Registration.cpp
@@ -157,8 +157,9 @@ LinearSystem BuildLinearSystem(const Associations &associations, const double ke
                     const auto &[J_r, residual] = compute_jacobian_and_residual(association);
                     const double chi_square = residual.squaredNorm();
                     const double w = GM_kernel(chi_square);
-                    return LinearSystem(J_r.transpose() * w * J_r,                    // JTJ
-                                        J_r.transpose() * w * residual, chi_square);  // JTr
+                    return LinearSystem(J_r.transpose() * w * J_r,       // JTJ
+                                        J_r.transpose() * w * residual,  // JTr
+                                        chi_square);
                 });
         },
         // 2nd Lambda: Parallel reduction of the private Jacboians