From 23b96d468063ae07c9dc6b8fff7bd988e0bd470d Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Mon, 10 Mar 2025 15:49:35 -0400
Subject: [PATCH] method to get Expmap coefficients

---
 gtsam/geometry/Similarity2.cpp | 27 +++++++++++++++++++++++++++
 gtsam/geometry/Similarity2.h   |  3 +++
 2 files changed, 30 insertions(+)

diff --git a/gtsam/geometry/Similarity2.cpp b/gtsam/geometry/Similarity2.cpp
index a48a6b6ef..ec52f01c0 100644
--- a/gtsam/geometry/Similarity2.cpp
+++ b/gtsam/geometry/Similarity2.cpp
@@ -194,6 +194,33 @@ Similarity2 Similarity2::Align(const Pose2Pairs& abPosePairs) {
   return internal::AlignGivenR(abPointPairs, aRb_estimate);
 }
 
+Matrix2 Similarity2::GetV(double theta, double lambda) {
+  // Derivation from https://ethaneade.com/lie_groups.pdf page 6
+  const double lambda2 = lambda * lambda, theta2 = theta * theta;
+  double A, B, C;
+  if (theta2 > 1e-9) {
+    A = sin(theta) / theta;
+    B = (1 - cos(theta)) / theta2;
+    C = (1 - A) / theta2;
+  } else {
+    // Taylor series expansion for theta=0
+    A = 1.0;
+    B = 0.5 - theta2 / 24.0;
+    C = 1.0 / 6.0 - theta2 / 120.0;
+  }
+  double alpha = 1.0 / (1.0 + theta2 / lambda2);
+  const double s = exp(lambda);
+
+  double s_inv = 1.0 / s;
+  double X = alpha * (1 - s_inv) / lambda + (1 - alpha) * (A - lambda * B);
+  double Y =
+      alpha * (s_inv - 1 + lambda) / lambda2 + (1 - alpha) * (B - lambda * C);
+
+  Matrix2 V;
+  V << X, -theta * Y, theta * Y, X;
+  return V;
+}
+
 Vector4 Similarity2::Logmap(const Similarity2& S,  //
                             OptionalJacobian<4, 4> Hm) {
   const Vector2 u = S.t_;
diff --git a/gtsam/geometry/Similarity2.h b/gtsam/geometry/Similarity2.h
index cc4ffc792..39aa4363a 100644
--- a/gtsam/geometry/Similarity2.h
+++ b/gtsam/geometry/Similarity2.h
@@ -141,6 +141,9 @@ class GTSAM_EXPORT Similarity2 : public LieGroup<Similarity2, 4> {
 
   using LieAlgebra = Matrix3;
 
+  /// Calculate expmap and logmap coefficients.
+  static Matrix2 GetV(double theta, double lambda);
+
   /**
    * Log map at the identity
    * \f$ [t_x, t_y, \delta, \lambda] \f$