From 0c9837dcaea7b613e094b7d07aa6bb532132f1ab Mon Sep 17 00:00:00 2001
From: B1ueber2y <b1ueber2y@gmail.com>
Date: Wed, 19 Jun 2024 10:57:20 +0200
Subject: [PATCH 1/3] fix and improve stability

---
 _pyceres/factors/bindings.h | 14 ++++++++++++--
 1 file changed, 12 insertions(+), 2 deletions(-)

diff --git a/_pyceres/factors/bindings.h b/_pyceres/factors/bindings.h
index 3601b96..025cc70 100644
--- a/_pyceres/factors/bindings.h
+++ b/_pyceres/factors/bindings.h
@@ -10,7 +10,17 @@
 namespace py = pybind11;
 
 inline Eigen::MatrixXd SqrtInformation(const Eigen::MatrixXd& covariance) {
-  return covariance.inverse().llt().matrixL();
+  // LLT decomposition of Fisher information matrix with SVD
+  Eigen::JacobiSVD<Eigen::MatrixXd> svd(
+      covariance, Eigen::ComputeThinU | Eigen::ComputeThinV);
+  Eigen::MatrixXd U = svd.matrixU();
+  Eigen::VectorXd singularValues = svd.singularValues();
+  const double epsilon = 1e-12;
+  Eigen::VectorXd invSqrtSingularValues =
+      singularValues.array().max(0.).sqrt().max(epsilon).inverse();
+  Eigen::MatrixXd invSqrtSingularValuesDiag =
+      invSqrtSingularValues.asDiagonal();
+  return U * invSqrtSingularValuesDiag;
 }
 
 // Mahalanobis squared distance between two parameters.
@@ -39,7 +49,7 @@ class NormalError {
     }
     Eigen::Map<Eigen::Matrix<T, Eigen::Dynamic, 1>> residuals(residuals_ptr,
                                                               dimension);
-    residuals.applyOnTheLeft(sqrt_information_.template cast<T>());
+    residuals.applyOnTheLeft(sqrt_information_.transpose().template cast<T>());
     return true;
   }
 

From d120066d37a2168f6cc66f7624e4dd57e3c2d5ab Mon Sep 17 00:00:00 2001
From: B1ueber2y <b1ueber2y@gmail.com>
Date: Wed, 19 Jun 2024 15:06:17 +0200
Subject: [PATCH 2/3] revert to inverse + llt.

---
 _pyceres/factors/bindings.h | 12 +-----------
 1 file changed, 1 insertion(+), 11 deletions(-)

diff --git a/_pyceres/factors/bindings.h b/_pyceres/factors/bindings.h
index 025cc70..b2eaa62 100644
--- a/_pyceres/factors/bindings.h
+++ b/_pyceres/factors/bindings.h
@@ -10,17 +10,7 @@
 namespace py = pybind11;
 
 inline Eigen::MatrixXd SqrtInformation(const Eigen::MatrixXd& covariance) {
-  // LLT decomposition of Fisher information matrix with SVD
-  Eigen::JacobiSVD<Eigen::MatrixXd> svd(
-      covariance, Eigen::ComputeThinU | Eigen::ComputeThinV);
-  Eigen::MatrixXd U = svd.matrixU();
-  Eigen::VectorXd singularValues = svd.singularValues();
-  const double epsilon = 1e-12;
-  Eigen::VectorXd invSqrtSingularValues =
-      singularValues.array().max(0.).sqrt().max(epsilon).inverse();
-  Eigen::MatrixXd invSqrtSingularValuesDiag =
-      invSqrtSingularValues.asDiagonal();
-  return U * invSqrtSingularValuesDiag;
+  return covariance.inverse().llt().matrixL();
 }
 
 // Mahalanobis squared distance between two parameters.

From 23bad6e5ee1c886743caa5bd9a109b3489fdc357 Mon Sep 17 00:00:00 2001
From: B1ueber2y <b1ueber2y@gmail.com>
Date: Thu, 20 Jun 2024 20:30:28 +0200
Subject: [PATCH 3/3] align convention with ceres doc.

---
 _pyceres/factors/bindings.h | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/_pyceres/factors/bindings.h b/_pyceres/factors/bindings.h
index b2eaa62..114a38d 100644
--- a/_pyceres/factors/bindings.h
+++ b/_pyceres/factors/bindings.h
@@ -10,7 +10,7 @@
 namespace py = pybind11;
 
 inline Eigen::MatrixXd SqrtInformation(const Eigen::MatrixXd& covariance) {
-  return covariance.inverse().llt().matrixL();
+  return covariance.inverse().llt().matrixL().transpose();
 }
 
 // Mahalanobis squared distance between two parameters.
@@ -39,7 +39,7 @@ class NormalError {
     }
     Eigen::Map<Eigen::Matrix<T, Eigen::Dynamic, 1>> residuals(residuals_ptr,
                                                               dimension);
-    residuals.applyOnTheLeft(sqrt_information_.transpose().template cast<T>());
+    residuals.applyOnTheLeft(sqrt_information_.template cast<T>());
     return true;
   }