From b0e9b16cc40adcf266fd4e7d153f387097977666 Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@gmail.com>
Date: Wed, 15 Jan 2025 18:09:01 -0500
Subject: [PATCH] expmap now returns Matrix3

---
 gtsam/geometry/SO3.cpp           | 12 ++++++------
 gtsam/geometry/SO3.h             |  2 +-
 gtsam/geometry/tests/testSO3.cpp |  8 ++++----
 3 files changed, 11 insertions(+), 11 deletions(-)

diff --git a/gtsam/geometry/SO3.cpp b/gtsam/geometry/SO3.cpp
index 41f7ce810..a1947953b 100644
--- a/gtsam/geometry/SO3.cpp
+++ b/gtsam/geometry/SO3.cpp
@@ -92,7 +92,7 @@ ExpmapFunctor::ExpmapFunctor(const Vector3& axis, double angle,
   init(nearZeroApprox);
 }
 
-SO3 ExpmapFunctor::expmap() const { return SO3(I_3x3 + A * W + B * WW); }
+Matrix3 ExpmapFunctor::expmap() const { return I_3x3 + A * W + B * WW; }
 
 DexpFunctor::DexpFunctor(const Vector3& omega, bool nearZeroApprox)
     : ExpmapFunctor(omega, nearZeroApprox), omega(omega) {
@@ -193,7 +193,7 @@ Vector3 DexpFunctor::applyLeftJacobianInverse(const Vector3& v,
 template <>
 GTSAM_EXPORT
 SO3 SO3::AxisAngle(const Vector3& axis, double theta) {
-  return so3::ExpmapFunctor(axis, theta).expmap();
+  return SO3(so3::ExpmapFunctor(axis, theta).expmap());
 }
 
 //******************************************************************************
@@ -251,11 +251,11 @@ template <>
 GTSAM_EXPORT
 SO3 SO3::Expmap(const Vector3& omega, ChartJacobian H) {
   if (H) {
-    so3::DexpFunctor impl(omega);
-    *H = impl.dexp();
-    return impl.expmap();
+    so3::DexpFunctor local(omega);
+    *H = local.dexp();
+    return SO3(local.expmap());
   } else {
-    return so3::ExpmapFunctor(omega).expmap();
+    return SO3(so3::ExpmapFunctor(omega).expmap());
   }
 }
 
diff --git a/gtsam/geometry/SO3.h b/gtsam/geometry/SO3.h
index 36329a19d..1bdcd82c7 100644
--- a/gtsam/geometry/SO3.h
+++ b/gtsam/geometry/SO3.h
@@ -150,7 +150,7 @@ struct GTSAM_EXPORT ExpmapFunctor {
   ExpmapFunctor(const Vector3& axis, double angle, bool nearZeroApprox = false);
 
   /// Rodrigues formula
-  SO3 expmap() const;
+  Matrix3 expmap() const;
 
  protected:
   void init(bool nearZeroApprox = false);
diff --git a/gtsam/geometry/tests/testSO3.cpp b/gtsam/geometry/tests/testSO3.cpp
index c6fd52161..41777ae3a 100644
--- a/gtsam/geometry/tests/testSO3.cpp
+++ b/gtsam/geometry/tests/testSO3.cpp
@@ -163,22 +163,22 @@ TEST(SO3, ExpmapFunctor) {
 
   // axis angle version
   so3::ExpmapFunctor f1(axis, angle);
-  SO3 actual1 = f1.expmap();
+  SO3 actual1(f1.expmap());
   CHECK(assert_equal(expected, actual1.matrix(), 1e-5));
 
   // axis angle version, negative angle
   so3::ExpmapFunctor f2(axis, angle - 2*M_PI);
-  SO3 actual2 = f2.expmap();
+  SO3 actual2(f2.expmap());
   CHECK(assert_equal(expected, actual2.matrix(), 1e-5));
 
   // omega version
   so3::ExpmapFunctor f3(axis * angle);
-  SO3 actual3 = f3.expmap();
+  SO3 actual3(f3.expmap());
   CHECK(assert_equal(expected, actual3.matrix(), 1e-5));
 
   // omega version, negative angle
   so3::ExpmapFunctor f4(axis * (angle - 2*M_PI));
-  SO3 actual4 = f4.expmap();
+  SO3 actual4(f4.expmap());
   CHECK(assert_equal(expected, actual4.matrix(), 1e-5));
 }