From e9e839f6a51a37e88c077e99148ca606a676adca Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@gmail.com>
Date: Fri, 18 Apr 2025 09:13:05 -0400
Subject: [PATCH] Use inverse() beyond pi

---
 gtsam/geometry/SO3.cpp           | 14 ++++++++++++--
 gtsam/geometry/SO3.h             |  6 ++++--
 gtsam/geometry/tests/testSO3.cpp |  4 +++-
 3 files changed, 19 insertions(+), 5 deletions(-)

diff --git a/gtsam/geometry/SO3.cpp b/gtsam/geometry/SO3.cpp
index 668c135ff..41c99925a 100644
--- a/gtsam/geometry/SO3.cpp
+++ b/gtsam/geometry/SO3.cpp
@@ -144,6 +144,16 @@ DexpFunctor::DexpFunctor(const Vector3& omega, double nearZeroThresholdSq, doubl
 DexpFunctor::DexpFunctor(const Vector3& omega)
   : DexpFunctor(omega, kNearZeroThresholdSq, kNearPiThresholdSq) {}
 
+Matrix3 DexpFunctor::rightJacobianInverse() const {
+  if (theta > M_PI) return rightJacobian().inverse();
+  return I_3x3 + 0.5 * W + D * WW;
+}
+
+Matrix3 DexpFunctor::leftJacobianInverse() const {
+  if (theta > M_PI) return leftJacobian().inverse();
+  return I_3x3 - 0.5 * W + D * WW;
+}
+
 // Multiplies v with left Jacobian through vector operations only.
 Vector3 DexpFunctor::applyRightJacobian(const Vector3& v, OptionalJacobian<3, 3> H1,
   OptionalJacobian<3, 3> H2) const {
@@ -177,7 +187,7 @@ Vector3 DexpFunctor::applyRightJacobianInverse(const Vector3& v, OptionalJacobia
   return c;
 }
 
-Vector3 so3::DexpFunctor::applyLeftJacobian(const Vector3& v,
+Vector3 DexpFunctor::applyLeftJacobian(const Vector3& v,
   OptionalJacobian<3, 3> H1, OptionalJacobian<3, 3> H2) const {
   const Vector3 Wv = gtsam::cross(omega, v);
 
@@ -196,7 +206,7 @@ Vector3 so3::DexpFunctor::applyLeftJacobian(const Vector3& v,
   return v + B * Wv + C * WWv;
 }
 
-Vector3 so3::DexpFunctor::applyLeftJacobianInverse(const Vector3& v,
+Vector3 DexpFunctor::applyLeftJacobianInverse(const Vector3& v,
   OptionalJacobian<3, 3> H1, OptionalJacobian<3, 3> H2) const {
   const Matrix3 invJl = leftJacobianInverse();
   const Vector3 c = invJl * v;
diff --git a/gtsam/geometry/SO3.h b/gtsam/geometry/SO3.h
index b4d00d92c..e491cca9a 100644
--- a/gtsam/geometry/SO3.h
+++ b/gtsam/geometry/SO3.h
@@ -193,10 +193,12 @@ struct GTSAM_EXPORT DexpFunctor : public ExpmapFunctor {
   Matrix3 leftJacobian() const { return I_3x3 + B * W + C * WW; }
 
   /// Inverse of right Jacobian
-  Matrix3 rightJacobianInverse() const { return I_3x3 + 0.5 * W + D * WW; }
+  /// For |omega|>pi uses rightJacobian().inverse(), as unstable beyond pi!
+  Matrix3 rightJacobianInverse() const;
 
   // Inverse of left Jacobian
-  Matrix3 leftJacobianInverse() const { return I_3x3 - 0.5 * W + D * WW; }
+  /// For |omega|>pi uses leftJacobian().inverse(), as unstable beyond pi!
+  Matrix3 leftJacobianInverse() const;
 
   /// Multiplies with rightJacobian(), with optional derivatives
   Vector3 applyRightJacobian(const Vector3& v,
diff --git a/gtsam/geometry/tests/testSO3.cpp b/gtsam/geometry/tests/testSO3.cpp
index ca3e55e43..f0b3cad65 100644
--- a/gtsam/geometry/tests/testSO3.cpp
+++ b/gtsam/geometry/tests/testSO3.cpp
@@ -335,7 +335,9 @@ namespace test_cases {
                              {1e-5, 0, 0}, {0, 1e-5, 0}, {0, 0, 1e-5},  //,
                              {1e-4, 0, 0}, {0, 1e-4, 0}, {0, 0, 1e-4} };
   std::vector<Vector3> large{
-      {0, 0, 0}, {1, 0, 0}, {0, 1, 0}, {0, 0, 1}, {0.1, 0.2, 0.3} };
+      {0, 0, 0}, {1, 0, 0}, {0, 1, 0}, {0, 0, 1}, {0.1, 0.2, 0.3},
+      {0.4, 0.5, 0.6}, {0.7, 0.8, 0.9}, {1.1, 1.2, 1.3}, {1.4, 1.5, 1.6},
+      {1.7, 1.8, 1.9}, {2, 2, 2}, {3, 3, 3}, {4, 4, 4}, {5, 5, 5} };
   auto omegas = [](bool nearZero) { return nearZero ? small : large; };
   std::vector<Vector3> vs{ {1, 0, 0}, {0, 1, 0}, {0, 0, 1}, {0.4, 0.3, 0.2} };
 }  // namespace test_cases