From 7a56473d8faf01daf59f0878afc681594f27d063 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Sat, 30 Apr 2022 05:12:43 -0400
Subject: [PATCH] add Lie group operations

---
 gtsam/geometry/Similarity2.cpp | 26 +++++++++++++++++++++++++-
 gtsam/geometry/Similarity2.h   | 25 ++++++++++++++++++++++++-
 2 files changed, 49 insertions(+), 2 deletions(-)

diff --git a/gtsam/geometry/Similarity2.cpp b/gtsam/geometry/Similarity2.cpp
index 3c780e907..258039b5b 100644
--- a/gtsam/geometry/Similarity2.cpp
+++ b/gtsam/geometry/Similarity2.cpp
@@ -12,7 +12,7 @@
 /**
  * @file   Similarity2.cpp
  * @brief  Implementation of Similarity2 transform
- * @author John Lambert
+ * @author John Lambert, Varun Agrawal
  */
 
 #include <gtsam/base/Manifold.h>
@@ -198,6 +198,30 @@ Similarity2 Similarity2::Align(const Pose2Pairs& abPosePairs) {
   return internal::AlignGivenR(abPointPairs, aRb_estimate);
 }
 
+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_);
+  Vector4 result;
+  result << u, w, s;
+  if (Hm) {
+    throw std::runtime_error("Similarity2::Logmap: derivative not implemented");
+  }
+  return result;
+}
+
+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];
+  if (Hm) {
+    throw std::runtime_error("Similarity2::Expmap: derivative not implemented");
+  }
+  return Similarity2(R, t, s);
+}
+
 std::ostream& operator<<(std::ostream& os, const Similarity2& p) {
   os << "[" << p.rotation().theta() << " " << p.translation().transpose() << " "
      << p.scale() << "]\';";
diff --git a/gtsam/geometry/Similarity2.h b/gtsam/geometry/Similarity2.h
index a09dc1858..0453758e0 100644
--- a/gtsam/geometry/Similarity2.h
+++ b/gtsam/geometry/Similarity2.h
@@ -12,7 +12,7 @@
 /**
  * @file   Similarity2.h
  * @brief  Implementation of Similarity2 transform
- * @author John Lambert
+ * @author John Lambert, Varun Agrawal
  */
 
 #pragma once
@@ -138,6 +138,29 @@ class GTSAM_EXPORT Similarity2 : public LieGroup<Similarity2, 4> {
   /// @name Lie Group
   /// @{
 
+  /**
+   * Log map at the identity
+   * \f$ [t_x, t_y, \delta, \lambda] \f$
+   */
+  static Vector4 Logmap(const Similarity2& S,  //
+                        OptionalJacobian<4, 4> Hm = boost::none);
+
+  /// Exponential map at the identity
+  static Similarity2 Expmap(const Vector4& v,  //
+                            OptionalJacobian<4, 4> Hm = boost::none);
+
+  /// Chart at the origin
+  struct ChartAtOrigin {
+    static Similarity2 Retract(const Vector4& v,
+                               ChartJacobian H = boost::none) {
+      return Similarity2::Expmap(v, H);
+    }
+    static Vector4 Local(const Similarity2& other,
+                         ChartJacobian H = boost::none) {
+      return Similarity2::Logmap(other, H);
+    }
+  };
+
   using LieGroup<Similarity2, 4>::inverse;
 
   /// @}