From 5ccf9dbd14b8c4e7e60b2c1ad75c0be30afb95fc Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Mon, 10 Mar 2025 15:50:09 -0400
Subject: [PATCH] fix Sim2 Expmap and Logmap

---
 gtsam/geometry/Similarity2.cpp | 15 ++++++++-------
 1 file changed, 8 insertions(+), 7 deletions(-)

diff --git a/gtsam/geometry/Similarity2.cpp b/gtsam/geometry/Similarity2.cpp
index ec52f01c0..15bb23210 100644
--- a/gtsam/geometry/Similarity2.cpp
+++ b/gtsam/geometry/Similarity2.cpp
@@ -223,11 +223,11 @@ Matrix2 Similarity2::GetV(double theta, double lambda) {
 
 Vector4 Similarity2::Logmap(const Similarity2& S,  //
                             OptionalJacobian<4, 4> Hm) {
-  const Vector2 u = S.t_;
   const Vector1 w = Rot2::Logmap(S.R_);
-  const double s = log(S.s_);
+  const double lambda = log(S.s_);
+  // In Expmap, t = V * u -> in Logmap, u = V^{-1} * t
   Vector4 result;
-  result << u, w, s;
+  result << GetV(w[0], lambda).inverse() * S.t_, w, lambda;
   if (Hm) {
     throw std::runtime_error("Similarity2::Logmap: derivative not implemented");
   }
@@ -236,13 +236,14 @@ Vector4 Similarity2::Logmap(const Similarity2& S,  //
 
 Similarity2 Similarity2::Expmap(const Vector4& v,  //
                                 OptionalJacobian<4, 4> Hm) {
-  const Vector2 t = v.head<2>();
-  const Rot2 R = Rot2::Expmap(v.segment<1>(2));
-  const double s = v[3];
+  const Vector2 u = v.head<2>();
+  const double theta = v[2];
+  const double lambda = v[3];
   if (Hm) {
     throw std::runtime_error("Similarity2::Expmap: derivative not implemented");
   }
-  return Similarity2(R, t, s);
+  const Matrix2 V = GetV(theta, lambda);
+  return Similarity2(Rot2::Expmap(v.segment<1>(2)), V * u, exp(lambda));
 }
 
 Matrix4 Similarity2::AdjointMap() const {