From d5918dcb810982a41659a35b71427869b3f16edd Mon Sep 17 00:00:00 2001
From: John Lambert <johnlambert@gatech.edu>
Date: Sat, 6 Nov 2021 15:31:33 -0400
Subject: [PATCH 01/35] Create Similarity2.h

---
 gtsam/geometry/Similarity2.h | 189 +++++++++++++++++++++++++++++++++++
 1 file changed, 189 insertions(+)
 create mode 100644 gtsam/geometry/Similarity2.h

diff --git a/gtsam/geometry/Similarity2.h b/gtsam/geometry/Similarity2.h
new file mode 100644
index 0000000000..120e6690ab
--- /dev/null
+++ b/gtsam/geometry/Similarity2.h
@@ -0,0 +1,189 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation,
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
+/**
+ * @file   Similarity2.h
+ * @brief  Implementation of Similarity2 transform
+ * @author John Lambert
+ */
+
+#pragma once
+
+#include <gtsam/geometry/Rot2.h>
+#include <gtsam/geometry/Point2.h>
+#include <gtsam/geometry/Pose2.h>
+#include <gtsam/base/Lie.h>
+#include <gtsam/base/Manifold.h>
+#include <gtsam/dllexport.h>
+
+
+namespace gtsam {
+
+// Forward declarations
+class Pose2;
+
+/**
+ * 2D similarity transform
+ */
+class Similarity2: public LieGroup<Similarity2, 4> {
+
+  /// @name Pose Concept
+  /// @{
+  typedef Rot2 Rotation;
+  typedef Point2 Translation;
+  /// @}
+
+private:
+  Rot2 R_;
+  Point2 t_;
+  double s_;
+
+public:
+
+  /// @name Constructors
+  /// @{
+
+  /// Default constructor
+  GTSAM_EXPORT Similarity2();
+
+  /// Construct pure scaling
+  GTSAM_EXPORT Similarity2(double s);
+
+  /// Construct from GTSAM types
+  GTSAM_EXPORT Similarity2(const Rot2& R, const Point2& t, double s);
+
+  /// Construct from Eigen types
+  GTSAM_EXPORT Similarity2(const Matrix2& R, const Vector2& t, double s);
+
+  /// Construct from matrix [R t; 0 s^-1]
+  GTSAM_EXPORT Similarity2(const Matrix3& T);
+
+  /// @}
+  /// @name Testable
+  /// @{
+
+  /// Compare with tolerance
+  GTSAM_EXPORT bool equals(const Similarity2& sim, double tol) const;
+
+  /// Exact equality
+  GTSAM_EXPORT bool operator==(const Similarity2& other) const;
+
+  /// Print with optional string
+  GTSAM_EXPORT void print(const std::string& s) const;
+
+  GTSAM_EXPORT friend std::ostream &operator<<(std::ostream &os, const Similarity2& p);
+
+  /// @}
+  /// @name Group
+  /// @{
+
+  /// Return an identity transform
+  GTSAM_EXPORT static Similarity2 identity();
+
+  /// Composition
+  GTSAM_EXPORT Similarity2 operator*(const Similarity2& S) const;
+
+  /// Return the inverse
+  GTSAM_EXPORT Similarity2 inverse() const;
+
+  /// @}
+  /// @name Group action on Point2
+  /// @{
+
+  /// Action on a point p is s*(R*p+t)
+  GTSAM_EXPORT Point2 transformFrom(const Point2& p) const;
+
+  /** 
+   * Action on a pose T.
+   * |Rs  ts|   |R t|   |Rs*R Rs*t+ts| 
+   * |0  1/s| * |0 1| = | 0      1/s |, the result is still a Sim2 object.
+   * To retrieve a Pose2, we normalized the scale value into 1.
+   * |Rs*R Rs*t+ts|   |Rs*R s(Rs*t+ts)|
+   * | 0      1/s | = |  0       1    |
+   * 
+   * This group action satisfies the compatibility condition. 
+   * For more details, refer to: https://en.wikipedia.org/wiki/Group_action
+   */
+  GTSAM_EXPORT Pose2 transformFrom(const Pose2& T) const;
+
+  /** syntactic sugar for transformFrom */
+  GTSAM_EXPORT Point2 operator*(const Point2& p) const;
+
+  /**
+   *  Create Similarity2 by aligning at least two point pairs
+   */
+  GTSAM_EXPORT static Similarity2 Align(const std::vector<Point2Pair>& abPointPairs);
+  
+  /**
+   * Create the Similarity2 object that aligns at least two pose pairs.
+   * Each pair is of the form (aTi, bTi).
+   * Given a list of pairs in frame a, and a list of pairs in frame b, Align()
+   * will compute the best-fit Similarity2 aSb transformation to align them.
+   * First, the rotation aRb will be computed as the average (Karcher mean) of
+   * many estimates aRb (from each pair). Afterwards, the scale factor will be computed
+   * using the algorithm described here:
+   * http://www5.informatik.uni-erlangen.de/Forschung/Publikationen/2005/Zinsser05-PSR.pdf
+   */
+  GTSAM_EXPORT static Similarity2 Align(const std::vector<Pose2Pair>& abPosePairs);
+
+  /// @}
+  /// @name Lie Group
+  /// @{
+
+  using LieGroup<Similarity2, 4>::inverse;
+
+  /// @}
+  /// @name Standard interface
+  /// @{
+
+  /// Calculate 4*4 matrix group equivalent
+  GTSAM_EXPORT const Matrix3 matrix() const;
+
+  /// Return a GTSAM rotation
+  const Rot2& rotation() const {
+    return R_;
+  }
+
+  /// Return a GTSAM translation
+  const Point2& translation() const {
+    return t_;
+  }
+
+  /// Return the scale
+  double scale() const {
+    return s_;
+  }
+
+  /// Convert to a rigid body pose (R, s*t)
+  /// TODO(frank): why is this here? Red flag! Definitely don't have it as a cast.
+  GTSAM_EXPORT operator Pose2() const;
+
+  /// Dimensionality of tangent space = 4 DOF - used to autodetect sizes
+  inline static size_t Dim() {
+    return 4;
+  }
+
+  /// Dimensionality of tangent space = 4 DOF
+  inline size_t dim() const {
+    return 4;
+  }
+
+  /// @}
+};
+
+
+template<>
+struct traits<Similarity2> : public internal::LieGroup<Similarity2> {};
+
+template<>
+struct traits<const Similarity2> : public internal::LieGroup<Similarity2> {};
+
+} // namespace gtsam

From 6b7b31a9121385890efcc395e6fb48a443a214c4 Mon Sep 17 00:00:00 2001
From: John Lambert <johnlambert@gatech.edu>
Date: Sat, 6 Nov 2021 15:32:52 -0400
Subject: [PATCH 02/35] Create Similarity2.cpp

---
 gtsam/geometry/Similarity2.cpp | 203 +++++++++++++++++++++++++++++++++
 1 file changed, 203 insertions(+)
 create mode 100644 gtsam/geometry/Similarity2.cpp

diff --git a/gtsam/geometry/Similarity2.cpp b/gtsam/geometry/Similarity2.cpp
new file mode 100644
index 0000000000..7970b31902
--- /dev/null
+++ b/gtsam/geometry/Similarity2.cpp
@@ -0,0 +1,203 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation,
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
+/**
+ * @file   Similarity2.cpp
+ * @brief  Implementation of Similarity2 transform
+ * @author John Lambert
+ */
+
+#include <gtsam/geometry/Similarity2.h>
+
+#include <gtsam/geometry/Pose2.h>
+#include <gtsam/base/Manifold.h>
+#include <gtsam/slam/KarcherMeanFactor-inl.h>
+
+namespace gtsam {
+
+using std::vector;
+
+namespace {
+/// Subtract centroids from point pairs.
+static Point2Pairs subtractCentroids(const Point2Pairs &abPointPairs,
+                                    const Point2Pair &centroids) {
+  Point2Pairs d_abPointPairs;
+  for (const Point2Pair& abPair : abPointPairs) {
+    Point2 da = abPair.first - centroids.first;
+    Point2 db = abPair.second - centroids.second;
+    d_abPointPairs.emplace_back(da, db);
+  }
+  return d_abPointPairs;
+}
+
+/// Form inner products x and y and calculate scale.
+static const double calculateScale(const Point2Pairs &d_abPointPairs,
+                                   const Rot2 &aRb) {
+  double x = 0, y = 0;
+  Point2 da, db;
+  for (const Point2Pair& d_abPair : d_abPointPairs) {
+    std::tie(da, db) = d_abPair;
+    const Vector2 da_prime = aRb * db;
+    y += da.transpose() * da_prime;
+    x += da_prime.transpose() * da_prime;
+  }
+  const double s = y / x;
+  return s;
+}
+
+/// Form outer product H.
+static Matrix2 calculateH(const Point2Pairs &d_abPointPairs) {
+  Matrix2 H = Z_2x2;
+  for (const Point2Pair& d_abPair : d_abPointPairs) {
+    H += d_abPair.first * d_abPair.second.transpose();
+  }
+  return H;
+}
+
+/// This method estimates the similarity transform from differences point pairs,
+// given a known or estimated rotation and point centroids.
+static Similarity2 align(const Point2Pairs &d_abPointPairs, const Rot2 &aRb,
+                         const Point2Pair &centroids) {
+  const double s = calculateScale(d_abPointPairs, aRb);
+  // dividing aTb by s is required because the registration cost function
+  // minimizes ||a - sRb - t||, whereas Sim(2) computes s(Rb + t)
+  const Point2 aTb = (centroids.first - s * (aRb * centroids.second)) / s;
+  return Similarity2(aRb, aTb, s);
+}
+
+/// This method estimates the similarity transform from point pairs, given a known or estimated rotation.
+// Refer to: http://www5.informatik.uni-erlangen.de/Forschung/Publikationen/2005/Zinsser05-PSR.pdf Chapter 3
+static Similarity2 alignGivenR(const Point2Pairs &abPointPairs,
+                               const Rot2 &aRb) {
+  auto centroids = means(abPointPairs);
+  auto d_abPointPairs = subtractCentroids(abPointPairs, centroids);
+  return align(d_abPointPairs, aRb, centroids);
+}
+}  // namespace
+
+Similarity2::Similarity2() :
+    t_(0,0), s_(1) {
+}
+
+Similarity2::Similarity2(double s) :
+    t_(0,0), s_(s) {
+}
+
+Similarity2::Similarity2(const Rot2& R, const Point2& t, double s) :
+    R_(R), t_(t), s_(s) {
+}
+
+Similarity2::Similarity2(const Matrix2& R, const Vector2& t, double s) :
+    R_(R), t_(t), s_(s) {
+}
+
+Similarity2::Similarity2(const Matrix3& T) :
+    R_(T.topLeftCorner<2, 2>()), t_(T.topRightCorner<2, 1>()), s_(1.0 / T(2, 2)) {
+}
+
+bool Similarity2::equals(const Similarity2& other, double tol) const {
+  return R_.equals(other.R_, tol) && traits<Point3>::Equals(t_, other.t_, tol)
+      && s_ < (other.s_ + tol) && s_ > (other.s_ - tol);
+}
+
+bool Similarity2::operator==(const Similarity2& other) const {
+  return R_.matrix() == other.R_.matrix() && t_ == other.t_ && s_ == other.s_;
+}
+
+void Similarity2::print(const std::string& s) const {
+  std::cout << std::endl;
+  std::cout << s;
+  rotation().print("\nR:\n");
+  std::cout << "t: " << translation().transpose() << " s: " << scale() << std::endl;
+}
+
+Similarity2 Similarity2::identity() {
+  return Similarity2();
+}
+Similarity2 Similarity2::operator*(const Similarity2& S) const {
+  return Similarity2(R_ * S.R_, ((1.0 / S.s_) * t_) + R_ * S.t_, s_ * S.s_);
+}
+
+Similarity2 Similarity2::inverse() const {
+  const Rot2 Rt = R_.inverse();
+  const Point2 sRt = Rt * (-s_ * t_);
+  return Similarity2(Rt, sRt, 1.0 / s_);
+}
+
+Point3 Similarity2::transformFrom(const Point2& p) const {
+  const Point2 q = R_ * p + t_;
+  return s_ * q;
+}
+
+Pose2 Similarity2::transformFrom(const Pose2& T) const {
+  Rot2 R = R_.compose(T.rotation());
+  Point2 t = Point2(s_ * (R_ * T.translation() + t_));
+  return Pose2(R, t);
+}
+
+Point2 Similarity2::operator*(const Point2& p) const {
+  return transformFrom(p);
+}
+
+Similarity2 Similarity2::Align(const Point2Pairs &abPointPairs) {
+  // Refer to Chapter 3 of
+  // http://www5.informatik.uni-erlangen.de/Forschung/Publikationen/2005/Zinsser05-PSR.pdf
+  if (abPointPairs.size() < 2)
+    throw std::runtime_error("input should have at least 2 pairs of points");
+  auto centroids = means(abPointPairs);
+  auto d_abPointPairs = subtractCentroids(abPointPairs, centroids);
+  Matrix3 H = calculateH(d_abPointPairs);
+  // ClosestTo finds rotation matrix closest to H in Frobenius sense
+  Rot2 aRb = Rot2::ClosestTo(H);
+  return align(d_abPointPairs, aRb, centroids);
+}
+
+Similarity2 Similarity2::Align(const vector<Pose3Pair> &abPosePairs) {
+  const size_t n = abPosePairs.size();
+  if (n < 2)
+    throw std::runtime_error("input should have at least 2 pairs of poses");
+
+  // calculate rotation
+  vector<Rot2> rotations;
+  Point2Pairs abPointPairs;
+  rotations.reserve(n);
+  abPointPairs.reserve(n);
+  // Below denotes the pose of the i'th object/camera/etc in frame "a" or frame "b"
+  Pose2 aTi, bTi;
+  for (const Pose2Pair &abPair : abPosePairs) {
+    std::tie(aTi, bTi) = abPair;
+    const Rot2 aRb = aTi.rotation().compose(bTi.rotation().inverse());
+    rotations.emplace_back(aRb);
+    abPointPairs.emplace_back(aTi.translation(), bTi.translation());
+  }
+  const Rot2 aRb_estimate = FindKarcherMean<Rot2>(rotations);
+
+  return alignGivenR(abPointPairs, aRb_estimate);
+}
+
+std::ostream &operator<<(std::ostream &os, const Similarity2& p) {
+  os << "[" << p.rotation().xyz().transpose() << " "
+      << p.translation().transpose() << " " << p.scale() << "]\';";
+  return os;
+}
+
+const Matrix3 Similarity2::matrix() const {
+  Matrix3 T;
+  T.topRows<2>() << R_.matrix(), t_;
+  T.bottomRows<1>() << 0, 0, 1.0 / s_;
+  return T;
+}
+
+Similarity2::operator Pose2() const {
+  return Pose2(R_, s_ * t_);
+}
+
+} // namespace gtsam

From e48704d7855e111da019861dbbdb8b9224d5e522 Mon Sep 17 00:00:00 2001
From: John Lambert <johnlambert@gatech.edu>
Date: Sat, 6 Nov 2021 15:44:40 -0400
Subject: [PATCH 03/35] add basic Python interface to .i file

---
 gtsam/geometry/geometry.i | 22 ++++++++++++++++++++++
 1 file changed, 22 insertions(+)

diff --git a/gtsam/geometry/geometry.i b/gtsam/geometry/geometry.i
index 9baa49e8e8..151c421555 100644
--- a/gtsam/geometry/geometry.i
+++ b/gtsam/geometry/geometry.i
@@ -890,6 +890,28 @@ class PinholeCamera {
   // enable pickling in python
   void pickle() const;
 };
+  
+#include <gtsam/geometry/Similarity2.h>
+class Similarity2 {
+  // Standard Constructors
+  Similarity2();
+  Similarity2(double s);
+  Similarity2(const gtsam::Rot2& R, const gtsam::Point2& t, double s);
+  Similarity2(const Matrix& R, const Vector& t, double s);
+  Similarity2(const Matrix& T);
+
+  gtsam::Point2 transformFrom(const gtsam::Point2& p) const;
+  gtsam::Pose2 transformFrom(const gtsam::Pose2& T);
+
+  static gtsam::Similarity2 Align(const gtsam::Point2Pairs& abPointPairs);
+  static gtsam::Similarity2 Align(const gtsam::Pose2Pairs& abPosePairs);
+
+  // Standard Interface
+  const Matrix matrix() const;
+  const gtsam::Rot2& rotation();
+  const gtsam::Point2& translation();
+  double scale() const;
+};
 
 #include <gtsam/geometry/Similarity3.h>
 class Similarity3 {

From 60053906a6016f0ec9e079c41a5c76cf18b70675 Mon Sep 17 00:00:00 2001
From: John Lambert <johnlambert@gatech.edu>
Date: Sat, 6 Nov 2021 16:15:20 -0400
Subject: [PATCH 04/35] add python unit tests

---
 python/gtsam/tests/test_Sim2.py | 196 ++++++++++++++++++++++++++++++++
 1 file changed, 196 insertions(+)
 create mode 100644 python/gtsam/tests/test_Sim2.py

diff --git a/python/gtsam/tests/test_Sim2.py b/python/gtsam/tests/test_Sim2.py
new file mode 100644
index 0000000000..67bc770d2c
--- /dev/null
+++ b/python/gtsam/tests/test_Sim2.py
@@ -0,0 +1,196 @@
+"""
+GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
+Atlanta, Georgia 30332-0415
+All Rights Reserved
+
+See LICENSE for the license information
+
+Sim3 unit tests.
+Author: John Lambert
+"""
+# pylint: disable=no-name-in-module
+import unittest
+
+import numpy as np
+
+import gtsam
+from gtsam import Point2, Pose2, Pose2Pairs, Rot2, Similarity2
+from gtsam.utils.test_case import GtsamTestCase
+
+
+class TestSim2(GtsamTestCase):
+    """Test selected Sim2 methods."""
+
+    def test_align_poses_along_straight_line(self) -> None:
+        """Test Align Pose2Pairs method.
+
+        Scenario:
+           3 object poses
+           same scale (no gauge ambiguity)
+           world frame has poses rotated about x-axis (90 degree roll)
+           world and egovehicle frame translated by 15 meters w.r.t. each other
+        """
+        Rx90 = Rot2.fromDegrees(90)
+
+        # Create source poses (three objects o1, o2, o3 living in the egovehicle "e" frame)
+        # Suppose they are 3d cuboids detected by an onboard sensor in the egovehicle frame
+        eTo0 = Pose2(Rot2(), np.array([5, 0]))
+        eTo1 = Pose2(Rot2(), np.array([10, 0]))
+        eTo2 = Pose2(Rot2(), np.array([15, 0]))
+
+        eToi_list = [eTo0, eTo1, eTo2]
+
+        # Create destination poses
+        # (same three objects, but instead living in the world/city "w" frame)
+        wTo0 = Pose2(Rx90, np.array([-10, 0]))
+        wTo1 = Pose2(Rx90, np.array([-5, 0]))
+        wTo2 = Pose2(Rx90, np.array([0, 0]))
+
+        wToi_list = [wTo0, wTo1, wTo2]
+
+        we_pairs = Pose2Pairs(list(zip(wToi_list, eToi_list)))
+
+        # Recover the transformation wSe (i.e. world_S_egovehicle)
+        wSe = Similarity2.Align(we_pairs)
+
+        for wToi, eToi in zip(wToi_list, eToi_list):
+            self.gtsamAssertEquals(wToi, wSe.transformFrom(eToi))
+
+    def test_align_poses_along_straight_line_gauge(self):
+        """Test if Align Pose3Pairs method can account for gauge ambiguity.
+
+        Scenario:
+           3 object poses
+           with gauge ambiguity (2x scale)
+           world frame has poses rotated about z-axis (90 degree yaw)
+           world and egovehicle frame translated by 11 meters w.r.t. each other
+        """
+        Rz90 = Rot3.Rz(np.deg2rad(90))
+
+        # Create source poses (three objects o1, o2, o3 living in the egovehicle "e" frame)
+        # Suppose they are 3d cuboids detected by an onboard sensor in the egovehicle frame
+        eTo0 = Pose2(Rot2(), np.array([1, 0]))
+        eTo1 = Pose2(Rot2(), np.array([2, 0]))
+        eTo2 = Pose2(Rot2(), np.array([4, 0]))
+
+        eToi_list = [eTo0, eTo1, eTo2]
+
+        # Create destination poses
+        # (same three objects, but instead living in the world/city "w" frame)
+        wTo0 = Pose2(Rz90, np.array([0, 12]))
+        wTo1 = Pose2(Rz90, np.array([0, 14]))
+        wTo2 = Pose2(Rz90, np.array([0, 18]))
+
+        wToi_list = [wTo0, wTo1, wTo2]
+
+        we_pairs = Pose2Pairs(list(zip(wToi_list, eToi_list)))
+
+        # Recover the transformation wSe (i.e. world_S_egovehicle)
+        wSe = Similarity2.Align(we_pairs)
+
+        for wToi, eToi in zip(wToi_list, eToi_list):
+            self.gtsamAssertEquals(wToi, wSe.transformFrom(eToi))
+
+    def test_align_poses_scaled_squares(self):
+        """Test if Align Pose2Pairs method can account for gauge ambiguity.
+
+        Make sure a big and small square can be aligned.
+        The u's represent a big square (10x10), and v's represents a small square (4x4).
+
+        Scenario:
+           4 object poses
+           with gauge ambiguity (2.5x scale)
+        """
+        # 0, 90, 180, and 270 degrees yaw
+        R0 = Rot2.fromDegrees(0)
+        R90 = Rot2.fromDegrees(90)
+        R180 = Rot2.fromDegrees(180)
+        R270 = Rot2.fromDegrees(270)
+
+        aTi0 = Pose3(R0, np.array([2, 3]))
+        aTi1 = Pose3(R90, np.array([12, 3]))
+        aTi2 = Pose3(R180, np.array([12, 13]))
+        aTi3 = Pose3(R270, np.array([2, 13]))
+
+        aTi_list = [aTi0, aTi1, aTi2, aTi3]
+
+        bTi0 = Pose2(R0, np.array([4, 3]))
+        bTi1 = Pose2(R90, np.array([8, 3]))
+        bTi2 = Pose2(R180, np.array([8, 7]))
+        bTi3 = Pose2(R270, np.array([4, 7]))
+
+        bTi_list = [bTi0, bTi1, bTi2, bTi3]
+
+        ab_pairs = Pose2Pairs(list(zip(aTi_list, bTi_list)))
+
+        # Recover the transformation wSe (i.e. world_S_egovehicle)
+        aSb = Similarity2.Align(ab_pairs)
+
+        for aTi, bTi in zip(aTi_list, bTi_list):
+            self.gtsamAssertEquals(aTi, aSb.transformFrom(bTi))
+
+    def test_constructor(self) -> None:
+        """Sim(2) to perform p_b = bSa * p_a"""
+        bRa = Rot2()
+        bta = np.array([1, 2])
+        bsa = 3.0
+        bSa = Similarity2(R=bRa, t=bta, s=bsa)
+        assert isinstance(bSa, Similarity2)
+        assert np.allclose(bSa.rotation().matrix(), bRa.matrix())
+        assert np.allclose(bSa.translation(), bta)
+        assert np.allclose(bSa.scale(), bsa)
+
+    def test_is_eq(self) -> None:
+        """Ensure object equality works properly (are equal)."""
+        bSa = Similarity2(R=Rot2(), t=np.array([1, 2]), s=3.0)
+        bSa_ = Similarity2(R=Rot2(), t=np.array([1.0, 2.0]), s=3)
+        assert bSa == bSa_
+
+    def test_not_eq_translation(self) -> None:
+        """Ensure object equality works properly (not equal translation)."""
+        bSa = Similarity2(R=Rot2(), t=np.array([2, 1]), s=3.0)
+        bSa_ = Similarity2(R=Rot2(), t=np.array([1.0, 2.0]), s=3)
+        assert bSa != bSa_
+
+    def test_not_eq_rotation(self) -> None:
+        """Ensure object equality works properly (not equal rotation)."""
+        bSa = Similarity2(R=Rot2(), t=np.array([2, 1]), s=3.0)
+        bSa_ = Similarity2(R=Rot2.fromDegrees(180), t=np.array([2.0, 1.0]), s=3)
+        assert bSa != bSa_
+
+    def test_not_eq_scale(self) -> None:
+        """Ensure object equality works properly (not equal scale)."""
+        bSa = Similarity2(R=Rot2(), t=np.array([2, 1]), s=3.0)
+        bSa_ = Similarity2(R=Rot2(), t=np.array([2.0, 1.0]), s=1.0)
+        assert bSa != bSa_
+
+    def test_rotation(self) -> None:
+        """Ensure rotation component is returned properly."""
+        R = Rot2.fromDegrees(90)
+        t = np.array([1, 2])
+        bSa = Similarity2(R=R, t=t, s=3.0)
+
+        # evaluates to [[0, -1], [1, 0]]
+        expected_R = Rot2.fromDegrees(90)
+        assert np.allclose(expected_R.matrix(), bSa.rotation().matrix())
+
+    def test_translation(self) -> None:
+        """Ensure translation component is returned properly."""
+        R = Rot2.fromDegrees(90)
+        t = np.array([1, 2])
+        bSa = Similarity2(R=R, t=t, s=3.0)
+
+        expected_t = np.array([1, 2])
+        assert np.allclose(expected_t, bSa.translation())
+
+    def test_scale(self) -> None:
+        """Ensure the scale factor is returned properly."""
+        bRa = Rot2()
+        bta = np.array([1, 2])
+        bsa = 3.0
+        bSa = Similarity2(R=bRa, t=bta, s=bsa)
+        assert bSa.scale() == 3.0
+
+
+if __name__ == "__main__":
+    unittest.main()

From 8e9815b2705ed1acb945e46c943426618d92c16f Mon Sep 17 00:00:00 2001
From: John Lambert <johnlambert@gatech.edu>
Date: Sat, 6 Nov 2021 17:35:07 -0400
Subject: [PATCH 05/35] fix typo

---
 gtsam/geometry/Similarity2.cpp | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/gtsam/geometry/Similarity2.cpp b/gtsam/geometry/Similarity2.cpp
index 7970b31902..ed66352339 100644
--- a/gtsam/geometry/Similarity2.cpp
+++ b/gtsam/geometry/Similarity2.cpp
@@ -184,7 +184,7 @@ Similarity2 Similarity2::Align(const vector<Pose3Pair> &abPosePairs) {
 }
 
 std::ostream &operator<<(std::ostream &os, const Similarity2& p) {
-  os << "[" << p.rotation().xyz().transpose() << " "
+  os << "[" << p.rotation().theta() << " "
       << p.translation().transpose() << " " << p.scale() << "]\';";
   return os;
 }

From 9fd745156ed761bf6f9a034a2fc9d2967d5093f9 Mon Sep 17 00:00:00 2001
From: John Lambert <johnlambert@gatech.edu>
Date: Sat, 6 Nov 2021 17:37:58 -0400
Subject: [PATCH 06/35] add Pose2Pair typedef

---
 gtsam/geometry/Pose2.h | 4 ++++
 1 file changed, 4 insertions(+)

diff --git a/gtsam/geometry/Pose2.h b/gtsam/geometry/Pose2.h
index a54951728c..12087a34ce 100644
--- a/gtsam/geometry/Pose2.h
+++ b/gtsam/geometry/Pose2.h
@@ -322,6 +322,10 @@ inline Matrix wedge<Pose2>(const Vector& xi) {
 typedef std::pair<Point2,Point2> Point2Pair;
 GTSAM_EXPORT boost::optional<Pose2> align(const std::vector<Point2Pair>& pairs);
 
+// Convenience typedef
+using Pose2Pair = std::pair<Pose2, Pose2>;
+using Pose2Pairs = std::vector<std::pair<Pose2, Pose2> >;
+	
 template <>
 struct traits<Pose2> : public internal::LieGroup<Pose2> {};
 

From 7082b67bc3081547a0fb475e2bbdc1b9c008f44d Mon Sep 17 00:00:00 2001
From: John Lambert <johnlambert@gatech.edu>
Date: Sat, 6 Nov 2021 20:38:22 -0400
Subject: [PATCH 07/35] fix typo in types

---
 gtsam/geometry/Similarity2.cpp | 6 +++---
 1 file changed, 3 insertions(+), 3 deletions(-)

diff --git a/gtsam/geometry/Similarity2.cpp b/gtsam/geometry/Similarity2.cpp
index ed66352339..9c051a313b 100644
--- a/gtsam/geometry/Similarity2.cpp
+++ b/gtsam/geometry/Similarity2.cpp
@@ -104,7 +104,7 @@ Similarity2::Similarity2(const Matrix3& T) :
 }
 
 bool Similarity2::equals(const Similarity2& other, double tol) const {
-  return R_.equals(other.R_, tol) && traits<Point3>::Equals(t_, other.t_, tol)
+  return R_.equals(other.R_, tol) && traits<Point2>::Equals(t_, other.t_, tol)
       && s_ < (other.s_ + tol) && s_ > (other.s_ - tol);
 }
 
@@ -132,7 +132,7 @@ Similarity2 Similarity2::inverse() const {
   return Similarity2(Rt, sRt, 1.0 / s_);
 }
 
-Point3 Similarity2::transformFrom(const Point2& p) const {
+Point2 Similarity2::transformFrom(const Point2& p) const {
   const Point2 q = R_ * p + t_;
   return s_ * q;
 }
@@ -160,7 +160,7 @@ Similarity2 Similarity2::Align(const Point2Pairs &abPointPairs) {
   return align(d_abPointPairs, aRb, centroids);
 }
 
-Similarity2 Similarity2::Align(const vector<Pose3Pair> &abPosePairs) {
+Similarity2 Similarity2::Align(const vector<Pose2Pair> &abPosePairs) {
   const size_t n = abPosePairs.size();
   if (n < 2)
     throw std::runtime_error("input should have at least 2 pairs of poses");

From 4372ed82f2627b38916a86a5abf375ee13cefac9 Mon Sep 17 00:00:00 2001
From: John Lambert <johnlambert@gatech.edu>
Date: Sat, 6 Nov 2021 20:49:04 -0400
Subject: [PATCH 08/35] add means() function to Point2.h

---
 gtsam/geometry/Point2.h | 3 +++
 1 file changed, 3 insertions(+)

diff --git a/gtsam/geometry/Point2.h b/gtsam/geometry/Point2.h
index cdb9f44809..d8b6daca80 100644
--- a/gtsam/geometry/Point2.h
+++ b/gtsam/geometry/Point2.h
@@ -71,6 +71,9 @@ GTSAM_EXPORT boost::optional<Point2> circleCircleIntersection(double R_d, double
  * @return list of solutions (0,1, or 2). Identical circles will return empty list, as well.
  */
 GTSAM_EXPORT std::list<Point2> circleCircleIntersection(Point2 c1, Point2 c2, boost::optional<Point2> fh);
+  
+/// Calculate the two means of a set of Point2 pairs
+GTSAM_EXPORT Point2Pair means(const std::vector<Point2Pair> &abPointPairs);
 
 /**
  * @brief Intersect 2 circles

From 1dd20a39fc59ed248244295da148d585e69b301a Mon Sep 17 00:00:00 2001
From: John Lambert <johnlambert@gatech.edu>
Date: Sat, 6 Nov 2021 21:40:42 -0400
Subject: [PATCH 09/35] add missing `means()` function for Point2

---
 gtsam/geometry/Point2.cpp | 12 ++++++++++++
 1 file changed, 12 insertions(+)

diff --git a/gtsam/geometry/Point2.cpp b/gtsam/geometry/Point2.cpp
index d8060cfcfd..06568e37c5 100644
--- a/gtsam/geometry/Point2.cpp
+++ b/gtsam/geometry/Point2.cpp
@@ -113,6 +113,18 @@ list<Point2> circleCircleIntersection(Point2 c1, double r1, Point2 c2,
   return circleCircleIntersection(c1, c2, fh);
 }
 
+Point2Pair means(const std::vector<Point2Pair> &abPointPairs) {
+  const size_t n = abPointPairs.size();
+  if (n == 0) throw std::invalid_argument("Point2::mean input Point2Pair vector is empty");
+  Point2 aSum(0, 0, 0), bSum(0, 0, 0);
+  for (const Point2Pair &abPair : abPointPairs) {
+    aSum += abPair.first;
+    bSum += abPair.second;
+  }
+  const double f = 1.0 / n;
+  return {aSum * f, bSum * f};
+}
+  
 /* ************************************************************************* */
 ostream &operator<<(ostream &os, const gtsam::Point2Pair &p) {
   os << p.first << " <-> " << p.second;

From 2252d5ee0d666308ab5cc7dc5407bc25bfcb2cfe Mon Sep 17 00:00:00 2001
From: John Lambert <johnlambert@gatech.edu>
Date: Mon, 8 Nov 2021 13:23:57 -0500
Subject: [PATCH 10/35] fix typo in size of mean vector for Point2Pair means()

---
 gtsam/geometry/Point2.cpp | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/gtsam/geometry/Point2.cpp b/gtsam/geometry/Point2.cpp
index 06568e37c5..06c32526b0 100644
--- a/gtsam/geometry/Point2.cpp
+++ b/gtsam/geometry/Point2.cpp
@@ -116,7 +116,7 @@ list<Point2> circleCircleIntersection(Point2 c1, double r1, Point2 c2,
 Point2Pair means(const std::vector<Point2Pair> &abPointPairs) {
   const size_t n = abPointPairs.size();
   if (n == 0) throw std::invalid_argument("Point2::mean input Point2Pair vector is empty");
-  Point2 aSum(0, 0, 0), bSum(0, 0, 0);
+  Point2 aSum(0, 0), bSum(0, 0);
   for (const Point2Pair &abPair : abPointPairs) {
     aSum += abPair.first;
     bSum += abPair.second;

From dfb9497d8162206f2dd419f218e4bf6c435f04d5 Mon Sep 17 00:00:00 2001
From: John Lambert <johnlambert@gatech.edu>
Date: Tue, 7 Dec 2021 11:53:34 -0700
Subject: [PATCH 11/35] add Rot2.ClosestTo()

---
 gtsam/geometry/Rot2.h | 4 ++++
 1 file changed, 4 insertions(+)

diff --git a/gtsam/geometry/Rot2.h b/gtsam/geometry/Rot2.h
index ec30c66576..42dba3487f 100644
--- a/gtsam/geometry/Rot2.h
+++ b/gtsam/geometry/Rot2.h
@@ -14,6 +14,7 @@
  * @brief 2D rotation
  * @date Dec 9, 2009
  * @author Frank Dellaert
+ * @author John Lambert
  */
 
 #pragma once
@@ -208,6 +209,9 @@ namespace gtsam {
 
     /** return 2*2 transpose (inverse) rotation matrix   */
     Matrix2 transpose() const;
+    
+    /** Find closest valid rotation matrix, given a 2x2 matrix */
+    static Rot2 ClosestTo(const Matrix2& M);
 
   private:
     /** Serialization function */

From 37af8cfbecf7bf05446976e485f44f0396c2a819 Mon Sep 17 00:00:00 2001
From: John Lambert <johnlambert@gatech.edu>
Date: Tue, 7 Dec 2021 11:59:28 -0700
Subject: [PATCH 12/35] finish Rot2.ClosestTo()

---
 gtsam/geometry/Rot2.cpp | 8 ++++++++
 1 file changed, 8 insertions(+)

diff --git a/gtsam/geometry/Rot2.cpp b/gtsam/geometry/Rot2.cpp
index 283147e4cc..f8ec9c9e69 100644
--- a/gtsam/geometry/Rot2.cpp
+++ b/gtsam/geometry/Rot2.cpp
@@ -129,6 +129,14 @@ Rot2 Rot2::relativeBearing(const Point2& d, OptionalJacobian<1, 2> H) {
   }
 }
 
+/* ************************************************************************* */
+static Rot2 ClosestTo(const Matrix2& M) {
+   double theta_rad = atan2(M(0,0), M(1,0));
+   double c = cos(theta_rad);
+   double s = sin(theta_rad);
+   return Rot2::fromCosSin(c, s);
+}
+  
 /* ************************************************************************* */
 
 } // gtsam

From 68535708af01697ff92ebd0d01b536f4687db728 Mon Sep 17 00:00:00 2001
From: John Lambert <johnlambert@gatech.edu>
Date: Tue, 7 Dec 2021 13:31:39 -0700
Subject: [PATCH 13/35] fix typo

---
 gtsam/geometry/Rot2.cpp | 8 +++++---
 1 file changed, 5 insertions(+), 3 deletions(-)

diff --git a/gtsam/geometry/Rot2.cpp b/gtsam/geometry/Rot2.cpp
index f8ec9c9e69..9f62869e4e 100644
--- a/gtsam/geometry/Rot2.cpp
+++ b/gtsam/geometry/Rot2.cpp
@@ -131,9 +131,11 @@ Rot2 Rot2::relativeBearing(const Point2& d, OptionalJacobian<1, 2> H) {
 
 /* ************************************************************************* */
 static Rot2 ClosestTo(const Matrix2& M) {
-   double theta_rad = atan2(M(0,0), M(1,0));
-   double c = cos(theta_rad);
-   double s = sin(theta_rad);
+   double c = M(0,0);
+   double s = M(1,0);
+   double theta_rad = atan2(s, c);
+   c = cos(theta_rad);
+   s = sin(theta_rad);
    return Rot2::fromCosSin(c, s);
 }
   

From c2040fb910f7fd6e2f012084ccfa8c19facf652d Mon Sep 17 00:00:00 2001
From: John Lambert <johnlambert@gatech.edu>
Date: Tue, 7 Dec 2021 13:34:06 -0700
Subject: [PATCH 14/35] use ClosestTo() in initializer list

---
 gtsam/geometry/Similarity2.cpp | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/gtsam/geometry/Similarity2.cpp b/gtsam/geometry/Similarity2.cpp
index 9c051a313b..1ed0dd1b02 100644
--- a/gtsam/geometry/Similarity2.cpp
+++ b/gtsam/geometry/Similarity2.cpp
@@ -96,7 +96,7 @@ Similarity2::Similarity2(const Rot2& R, const Point2& t, double s) :
 }
 
 Similarity2::Similarity2(const Matrix2& R, const Vector2& t, double s) :
-    R_(R), t_(t), s_(s) {
+    R_(Rot2.ClosestTo(R)), t_(t), s_(s) {
 }
 
 Similarity2::Similarity2(const Matrix3& T) :

From 7ebbe1869eeac0393c5e992944f672d0ac093db5 Mon Sep 17 00:00:00 2001
From: John Lambert <johnlambert@hayoungs-mbp.lan>
Date: Tue, 7 Dec 2021 19:07:50 -0500
Subject: [PATCH 15/35] fix Eigen error

---
 gtsam/geometry/Similarity2.cpp | 89 +++++++++++++++++-----------------
 gtsam/geometry/Similarity2.h   | 54 ++++++++++-----------
 2 files changed, 72 insertions(+), 71 deletions(-)

diff --git a/gtsam/geometry/Similarity2.cpp b/gtsam/geometry/Similarity2.cpp
index 1ed0dd1b02..5a63e70cb5 100644
--- a/gtsam/geometry/Similarity2.cpp
+++ b/gtsam/geometry/Similarity2.cpp
@@ -17,6 +17,7 @@
 
 #include <gtsam/geometry/Similarity2.h>
 
+#include <gtsam/geometry/Rot3.h>
 #include <gtsam/geometry/Pose2.h>
 #include <gtsam/base/Manifold.h>
 #include <gtsam/slam/KarcherMeanFactor-inl.h>
@@ -95,13 +96,13 @@ Similarity2::Similarity2(const Rot2& R, const Point2& t, double s) :
     R_(R), t_(t), s_(s) {
 }
 
-Similarity2::Similarity2(const Matrix2& R, const Vector2& t, double s) :
-    R_(Rot2.ClosestTo(R)), t_(t), s_(s) {
-}
+// Similarity2::Similarity2(const Matrix2& R, const Vector2& t, double s) :
+//     R_(Rot2::ClosestTo(R)), t_(t), s_(s) {
+// }
 
-Similarity2::Similarity2(const Matrix3& T) :
-    R_(T.topLeftCorner<2, 2>()), t_(T.topRightCorner<2, 1>()), s_(1.0 / T(2, 2)) {
-}
+// Similarity2::Similarity2(const Matrix3& T) :
+//     R_(Rot2::ClosestTo(T.topLeftCorner<2, 2>())), t_(T.topRightCorner<2, 1>()), s_(1.0 / T(2, 2)) {
+// }
 
 bool Similarity2::equals(const Similarity2& other, double tol) const {
   return R_.equals(other.R_, tol) && traits<Point2>::Equals(t_, other.t_, tol)
@@ -122,9 +123,9 @@ void Similarity2::print(const std::string& s) const {
 Similarity2 Similarity2::identity() {
   return Similarity2();
 }
-Similarity2 Similarity2::operator*(const Similarity2& S) const {
-  return Similarity2(R_ * S.R_, ((1.0 / S.s_) * t_) + R_ * S.t_, s_ * S.s_);
-}
+// Similarity2 Similarity2::operator*(const Similarity2& S) const {
+//   return Similarity2(R_ * S.R_, ((1.0 / S.s_) * t_) + R_ * S.t_, s_ * S.s_);
+// }
 
 Similarity2 Similarity2::inverse() const {
   const Rot2 Rt = R_.inverse();
@@ -147,41 +148,41 @@ Point2 Similarity2::operator*(const Point2& p) const {
   return transformFrom(p);
 }
 
-Similarity2 Similarity2::Align(const Point2Pairs &abPointPairs) {
-  // Refer to Chapter 3 of
-  // http://www5.informatik.uni-erlangen.de/Forschung/Publikationen/2005/Zinsser05-PSR.pdf
-  if (abPointPairs.size() < 2)
-    throw std::runtime_error("input should have at least 2 pairs of points");
-  auto centroids = means(abPointPairs);
-  auto d_abPointPairs = subtractCentroids(abPointPairs, centroids);
-  Matrix3 H = calculateH(d_abPointPairs);
-  // ClosestTo finds rotation matrix closest to H in Frobenius sense
-  Rot2 aRb = Rot2::ClosestTo(H);
-  return align(d_abPointPairs, aRb, centroids);
-}
-
-Similarity2 Similarity2::Align(const vector<Pose2Pair> &abPosePairs) {
-  const size_t n = abPosePairs.size();
-  if (n < 2)
-    throw std::runtime_error("input should have at least 2 pairs of poses");
-
-  // calculate rotation
-  vector<Rot2> rotations;
-  Point2Pairs abPointPairs;
-  rotations.reserve(n);
-  abPointPairs.reserve(n);
-  // Below denotes the pose of the i'th object/camera/etc in frame "a" or frame "b"
-  Pose2 aTi, bTi;
-  for (const Pose2Pair &abPair : abPosePairs) {
-    std::tie(aTi, bTi) = abPair;
-    const Rot2 aRb = aTi.rotation().compose(bTi.rotation().inverse());
-    rotations.emplace_back(aRb);
-    abPointPairs.emplace_back(aTi.translation(), bTi.translation());
-  }
-  const Rot2 aRb_estimate = FindKarcherMean<Rot2>(rotations);
-
-  return alignGivenR(abPointPairs, aRb_estimate);
-}
+// Similarity2 Similarity2::Align(const Point2Pairs &abPointPairs) {
+//   // Refer to Chapter 3 of
+//   // http://www5.informatik.uni-erlangen.de/Forschung/Publikationen/2005/Zinsser05-PSR.pdf
+//   if (abPointPairs.size() < 2)
+//     throw std::runtime_error("input should have at least 2 pairs of points");
+//   auto centroids = means(abPointPairs);
+//   auto d_abPointPairs = subtractCentroids(abPointPairs, centroids);
+//   Matrix3 H = calculateH(d_abPointPairs);
+//   // ClosestTo finds rotation matrix closest to H in Frobenius sense
+//   Rot2 aRb = Rot2::ClosestTo(H);
+//   return align(d_abPointPairs, aRb, centroids);
+// }
+
+// Similarity2 Similarity2::Align(const vector<Pose2Pair> &abPosePairs) {
+//   const size_t n = abPosePairs.size();
+//   if (n < 2)
+//     throw std::runtime_error("input should have at least 2 pairs of poses");
+
+//   // calculate rotation
+//   vector<Rot2> rotations;
+//   Point2Pairs abPointPairs;
+//   rotations.reserve(n);
+//   abPointPairs.reserve(n);
+//   // Below denotes the pose of the i'th object/camera/etc in frame "a" or frame "b"
+//   Pose2 aTi, bTi;
+//   for (const Pose2Pair &abPair : abPosePairs) {
+//     std::tie(aTi, bTi) = abPair;
+//     const Rot2 aRb = aTi.rotation().compose(bTi.rotation().inverse());
+//     rotations.emplace_back(aRb);
+//     abPointPairs.emplace_back(aTi.translation(), bTi.translation());
+//   }
+//   const Rot2 aRb_estimate = FindKarcherMean<Rot2>(rotations);
+
+//   return alignGivenR(abPointPairs, aRb_estimate);
+// }
 
 std::ostream &operator<<(std::ostream &os, const Similarity2& p) {
   os << "[" << p.rotation().theta() << " "
diff --git a/gtsam/geometry/Similarity2.h b/gtsam/geometry/Similarity2.h
index 120e6690ab..85a6324c5c 100644
--- a/gtsam/geometry/Similarity2.h
+++ b/gtsam/geometry/Similarity2.h
@@ -60,11 +60,11 @@ class Similarity2: public LieGroup<Similarity2, 4> {
   /// Construct from GTSAM types
   GTSAM_EXPORT Similarity2(const Rot2& R, const Point2& t, double s);
 
-  /// Construct from Eigen types
-  GTSAM_EXPORT Similarity2(const Matrix2& R, const Vector2& t, double s);
+  // /// Construct from Eigen types
+  // GTSAM_EXPORT Similarity2(const Matrix2& R, const Vector2& t, double s);
 
-  /// Construct from matrix [R t; 0 s^-1]
-  GTSAM_EXPORT Similarity2(const Matrix3& T);
+  // /// Construct from matrix [R t; 0 s^-1]
+  // GTSAM_EXPORT Similarity2(const Matrix3& T);
 
   /// @}
   /// @name Testable
@@ -94,9 +94,9 @@ class Similarity2: public LieGroup<Similarity2, 4> {
   /// Return the inverse
   GTSAM_EXPORT Similarity2 inverse() const;
 
-  /// @}
-  /// @name Group action on Point2
-  /// @{
+  // /// @}
+  // /// @name Group action on Point2
+  // /// @{
 
   /// Action on a point p is s*(R*p+t)
   GTSAM_EXPORT Point2 transformFrom(const Point2& p) const;
@@ -114,25 +114,25 @@ class Similarity2: public LieGroup<Similarity2, 4> {
    */
   GTSAM_EXPORT Pose2 transformFrom(const Pose2& T) const;
 
-  /** syntactic sugar for transformFrom */
+  /* syntactic sugar for transformFrom */
   GTSAM_EXPORT Point2 operator*(const Point2& p) const;
 
-  /**
-   *  Create Similarity2 by aligning at least two point pairs
-   */
-  GTSAM_EXPORT static Similarity2 Align(const std::vector<Point2Pair>& abPointPairs);
+  // /**
+  //  *  Create Similarity2 by aligning at least two point pairs
+  //  */
+  // GTSAM_EXPORT static Similarity2 Align(const std::vector<Point2Pair>& abPointPairs);
   
-  /**
-   * Create the Similarity2 object that aligns at least two pose pairs.
-   * Each pair is of the form (aTi, bTi).
-   * Given a list of pairs in frame a, and a list of pairs in frame b, Align()
-   * will compute the best-fit Similarity2 aSb transformation to align them.
-   * First, the rotation aRb will be computed as the average (Karcher mean) of
-   * many estimates aRb (from each pair). Afterwards, the scale factor will be computed
-   * using the algorithm described here:
-   * http://www5.informatik.uni-erlangen.de/Forschung/Publikationen/2005/Zinsser05-PSR.pdf
-   */
-  GTSAM_EXPORT static Similarity2 Align(const std::vector<Pose2Pair>& abPosePairs);
+  // /**
+  //  * Create the Similarity2 object that aligns at least two pose pairs.
+  //  * Each pair is of the form (aTi, bTi).
+  //  * Given a list of pairs in frame a, and a list of pairs in frame b, Align()
+  //  * will compute the best-fit Similarity2 aSb transformation to align them.
+  //  * First, the rotation aRb will be computed as the average (Karcher mean) of
+  //  * many estimates aRb (from each pair). Afterwards, the scale factor will be computed
+  //  * using the algorithm described here:
+  //  * http://www5.informatik.uni-erlangen.de/Forschung/Publikationen/2005/Zinsser05-PSR.pdf
+  //  */
+  // GTSAM_EXPORT static Similarity2 Align(const std::vector<Pose2Pair>& abPosePairs);
 
   /// @}
   /// @name Lie Group
@@ -180,10 +180,10 @@ class Similarity2: public LieGroup<Similarity2, 4> {
 };
 
 
-template<>
-struct traits<Similarity2> : public internal::LieGroup<Similarity2> {};
+// template<>
+// struct traits<Similarity2> : public internal::LieGroup<Similarity2> {};
 
-template<>
-struct traits<const Similarity2> : public internal::LieGroup<Similarity2> {};
+// template<>
+// struct traits<const Similarity2> : public internal::LieGroup<Similarity2> {};
 
 } // namespace gtsam

From 784bdc64c5266990201e5d8659d05a2084fe7567 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Sun, 6 Feb 2022 00:08:31 -0500
Subject: [PATCH 16/35] minor fixes to Pose2 and Rot2

---
 gtsam/geometry/Pose2.h  |  4 ++--
 gtsam/geometry/Rot2.cpp | 16 ++++++++--------
 gtsam/geometry/Rot2.h   |  2 +-
 3 files changed, 11 insertions(+), 11 deletions(-)

diff --git a/gtsam/geometry/Pose2.h b/gtsam/geometry/Pose2.h
index 12087a34ce..eb8ddfb5b1 100644
--- a/gtsam/geometry/Pose2.h
+++ b/gtsam/geometry/Pose2.h
@@ -324,8 +324,8 @@ GTSAM_EXPORT boost::optional<Pose2> align(const std::vector<Point2Pair>& pairs);
 
 // Convenience typedef
 using Pose2Pair = std::pair<Pose2, Pose2>;
-using Pose2Pairs = std::vector<std::pair<Pose2, Pose2> >;
-	
+using Pose2Pairs = std::vector<Pose2Pair>;
+
 template <>
 struct traits<Pose2> : public internal::LieGroup<Pose2> {};
 
diff --git a/gtsam/geometry/Rot2.cpp b/gtsam/geometry/Rot2.cpp
index 9f62869e4e..d43b9b12df 100644
--- a/gtsam/geometry/Rot2.cpp
+++ b/gtsam/geometry/Rot2.cpp
@@ -130,15 +130,15 @@ Rot2 Rot2::relativeBearing(const Point2& d, OptionalJacobian<1, 2> H) {
 }
 
 /* ************************************************************************* */
-static Rot2 ClosestTo(const Matrix2& M) {
-   double c = M(0,0);
-   double s = M(1,0);
-   double theta_rad = atan2(s, c);
-   c = cos(theta_rad);
-   s = sin(theta_rad);
-   return Rot2::fromCosSin(c, s);
+Rot2 Rot2::ClosestTo(const Matrix2& M) {
+  double c = M(0, 0);
+  double s = M(1, 0);
+  double theta_rad = std::atan2(s, c);
+  c = cos(theta_rad);
+  s = sin(theta_rad);
+  return Rot2::fromCosSin(c, s);
 }
-  
+
 /* ************************************************************************* */
 
 } // gtsam
diff --git a/gtsam/geometry/Rot2.h b/gtsam/geometry/Rot2.h
index 42dba3487f..2690ca2481 100644
--- a/gtsam/geometry/Rot2.h
+++ b/gtsam/geometry/Rot2.h
@@ -209,7 +209,7 @@ namespace gtsam {
 
     /** return 2*2 transpose (inverse) rotation matrix   */
     Matrix2 transpose() const;
-    
+
     /** Find closest valid rotation matrix, given a 2x2 matrix */
     static Rot2 ClosestTo(const Matrix2& M);
 

From bf668e58697f5236ba20363b7b100714ea70820b Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Sun, 6 Feb 2022 00:08:54 -0500
Subject: [PATCH 17/35] Similarity2 fixes

---
 gtsam/geometry/Similarity2.cpp | 188 ++++++++++++++++++---------------
 gtsam/geometry/Similarity2.h   | 110 +++++++++----------
 2 files changed, 150 insertions(+), 148 deletions(-)

diff --git a/gtsam/geometry/Similarity2.cpp b/gtsam/geometry/Similarity2.cpp
index 5a63e70cb5..12529f52df 100644
--- a/gtsam/geometry/Similarity2.cpp
+++ b/gtsam/geometry/Similarity2.cpp
@@ -15,21 +15,21 @@
  * @author John Lambert
  */
 
-#include <gtsam/geometry/Similarity2.h>
-
-#include <gtsam/geometry/Rot3.h>
-#include <gtsam/geometry/Pose2.h>
 #include <gtsam/base/Manifold.h>
+#include <gtsam/geometry/Pose2.h>
+#include <gtsam/geometry/Rot3.h>
+#include <gtsam/geometry/Similarity2.h>
 #include <gtsam/slam/KarcherMeanFactor-inl.h>
 
 namespace gtsam {
 
 using std::vector;
 
-namespace {
+namespace internal {
+
 /// Subtract centroids from point pairs.
-static Point2Pairs subtractCentroids(const Point2Pairs &abPointPairs,
-                                    const Point2Pair &centroids) {
+static Point2Pairs subtractCentroids(const Point2Pairs& abPointPairs,
+                                     const Point2Pair& centroids) {
   Point2Pairs d_abPointPairs;
   for (const Point2Pair& abPair : abPointPairs) {
     Point2 da = abPair.first - centroids.first;
@@ -40,10 +40,11 @@ static Point2Pairs subtractCentroids(const Point2Pairs &abPointPairs,
 }
 
 /// Form inner products x and y and calculate scale.
-static const double calculateScale(const Point2Pairs &d_abPointPairs,
-                                   const Rot2 &aRb) {
+static double calculateScale(const Point2Pairs& d_abPointPairs,
+                             const Rot2& aRb) {
   double x = 0, y = 0;
   Point2 da, db;
+
   for (const Point2Pair& d_abPair : d_abPointPairs) {
     std::tie(da, db) = d_abPair;
     const Vector2 da_prime = aRb * db;
@@ -55,7 +56,7 @@ static const double calculateScale(const Point2Pairs &d_abPointPairs,
 }
 
 /// Form outer product H.
-static Matrix2 calculateH(const Point2Pairs &d_abPointPairs) {
+static Matrix2 calculateH(const Point2Pairs& d_abPointPairs) {
   Matrix2 H = Z_2x2;
   for (const Point2Pair& d_abPair : d_abPointPairs) {
     H += d_abPair.first * d_abPair.second.transpose();
@@ -63,10 +64,17 @@ static Matrix2 calculateH(const Point2Pairs &d_abPointPairs) {
   return H;
 }
 
-/// This method estimates the similarity transform from differences point pairs,
-// given a known or estimated rotation and point centroids.
-static Similarity2 align(const Point2Pairs &d_abPointPairs, const Rot2 &aRb,
-                         const Point2Pair &centroids) {
+/**
+ * @brief This method estimates the similarity transform from differences point
+ * pairs, given a known or estimated rotation and point centroids.
+ *
+ * @param d_abPointPairs
+ * @param aRb
+ * @param centroids
+ * @return Similarity2
+ */
+static Similarity2 align(const Point2Pairs& d_abPointPairs, const Rot2& aRb,
+                         const Point2Pair& centroids) {
   const double s = calculateScale(d_abPointPairs, aRb);
   // dividing aTb by s is required because the registration cost function
   // minimizes ||a - sRb - t||, whereas Sim(2) computes s(Rb + t)
@@ -74,39 +82,44 @@ static Similarity2 align(const Point2Pairs &d_abPointPairs, const Rot2 &aRb,
   return Similarity2(aRb, aTb, s);
 }
 
-/// This method estimates the similarity transform from point pairs, given a known or estimated rotation.
-// Refer to: http://www5.informatik.uni-erlangen.de/Forschung/Publikationen/2005/Zinsser05-PSR.pdf Chapter 3
-static Similarity2 alignGivenR(const Point2Pairs &abPointPairs,
-                               const Rot2 &aRb) {
+/**
+ * @brief This method estimates the similarity transform from point pairs,
+ * given a known or estimated rotation.
+ * Refer to:
+ * http://www5.informatik.uni-erlangen.de/Forschung/Publikationen/2005/Zinsser05-PSR.pdf
+ * Chapter 3
+ *
+ * @param abPointPairs
+ * @param aRb
+ * @return Similarity2
+ */
+static Similarity2 alignGivenR(const Point2Pairs& abPointPairs,
+                               const Rot2& aRb) {
   auto centroids = means(abPointPairs);
-  auto d_abPointPairs = subtractCentroids(abPointPairs, centroids);
-  return align(d_abPointPairs, aRb, centroids);
+  auto d_abPointPairs = internal::subtractCentroids(abPointPairs, centroids);
+  return internal::align(d_abPointPairs, aRb, centroids);
 }
-}  // namespace
+}  // namespace internal
 
-Similarity2::Similarity2() :
-    t_(0,0), s_(1) {
-}
+Similarity2::Similarity2() : t_(0, 0), s_(1) {}
 
-Similarity2::Similarity2(double s) :
-    t_(0,0), s_(s) {
-}
+Similarity2::Similarity2(double s) : t_(0, 0), s_(s) {}
 
-Similarity2::Similarity2(const Rot2& R, const Point2& t, double s) :
-    R_(R), t_(t), s_(s) {
-}
+Similarity2::Similarity2(const Rot2& R, const Point2& t, double s)
+    : R_(R), t_(t), s_(s) {}
 
-// Similarity2::Similarity2(const Matrix2& R, const Vector2& t, double s) :
-//     R_(Rot2::ClosestTo(R)), t_(t), s_(s) {
-// }
+Similarity2::Similarity2(const Matrix2& R, const Vector2& t, double s)
+    : R_(Rot2::ClosestTo(R)), t_(t), s_(s) {}
 
-// Similarity2::Similarity2(const Matrix3& T) :
-//     R_(Rot2::ClosestTo(T.topLeftCorner<2, 2>())), t_(T.topRightCorner<2, 1>()), s_(1.0 / T(2, 2)) {
-// }
+Similarity2::Similarity2(const Matrix3& T)
+    : R_(Rot2::ClosestTo(T.topLeftCorner<2, 2>())),
+      t_(T.topRightCorner<2, 1>()),
+      s_(1.0 / T(2, 2)) {}
 
 bool Similarity2::equals(const Similarity2& other, double tol) const {
-  return R_.equals(other.R_, tol) && traits<Point2>::Equals(t_, other.t_, tol)
-      && s_ < (other.s_ + tol) && s_ > (other.s_ - tol);
+  return R_.equals(other.R_, tol) &&
+         traits<Point2>::Equals(t_, other.t_, tol) && s_ < (other.s_ + tol) &&
+         s_ > (other.s_ - tol);
 }
 
 bool Similarity2::operator==(const Similarity2& other) const {
@@ -117,15 +130,15 @@ void Similarity2::print(const std::string& s) const {
   std::cout << std::endl;
   std::cout << s;
   rotation().print("\nR:\n");
-  std::cout << "t: " << translation().transpose() << " s: " << scale() << std::endl;
+  std::cout << "t: " << translation().transpose() << " s: " << scale()
+            << std::endl;
 }
 
-Similarity2 Similarity2::identity() {
-  return Similarity2();
+Similarity2 Similarity2::identity() { return Similarity2(); }
+
+Similarity2 Similarity2::operator*(const Similarity2& S) const {
+  return Similarity2(R_ * S.R_, ((1.0 / S.s_) * t_) + R_ * S.t_, s_ * S.s_);
 }
-// Similarity2 Similarity2::operator*(const Similarity2& S) const {
-//   return Similarity2(R_ * S.R_, ((1.0 / S.s_) * t_) + R_ * S.t_, s_ * S.s_);
-// }
 
 Similarity2 Similarity2::inverse() const {
   const Rot2 Rt = R_.inverse();
@@ -148,57 +161,56 @@ Point2 Similarity2::operator*(const Point2& p) const {
   return transformFrom(p);
 }
 
-// Similarity2 Similarity2::Align(const Point2Pairs &abPointPairs) {
-//   // Refer to Chapter 3 of
-//   // http://www5.informatik.uni-erlangen.de/Forschung/Publikationen/2005/Zinsser05-PSR.pdf
-//   if (abPointPairs.size() < 2)
-//     throw std::runtime_error("input should have at least 2 pairs of points");
-//   auto centroids = means(abPointPairs);
-//   auto d_abPointPairs = subtractCentroids(abPointPairs, centroids);
-//   Matrix3 H = calculateH(d_abPointPairs);
-//   // ClosestTo finds rotation matrix closest to H in Frobenius sense
-//   Rot2 aRb = Rot2::ClosestTo(H);
-//   return align(d_abPointPairs, aRb, centroids);
-// }
-
-// Similarity2 Similarity2::Align(const vector<Pose2Pair> &abPosePairs) {
-//   const size_t n = abPosePairs.size();
-//   if (n < 2)
-//     throw std::runtime_error("input should have at least 2 pairs of poses");
-
-//   // calculate rotation
-//   vector<Rot2> rotations;
-//   Point2Pairs abPointPairs;
-//   rotations.reserve(n);
-//   abPointPairs.reserve(n);
-//   // Below denotes the pose of the i'th object/camera/etc in frame "a" or frame "b"
-//   Pose2 aTi, bTi;
-//   for (const Pose2Pair &abPair : abPosePairs) {
-//     std::tie(aTi, bTi) = abPair;
-//     const Rot2 aRb = aTi.rotation().compose(bTi.rotation().inverse());
-//     rotations.emplace_back(aRb);
-//     abPointPairs.emplace_back(aTi.translation(), bTi.translation());
-//   }
-//   const Rot2 aRb_estimate = FindKarcherMean<Rot2>(rotations);
-
-//   return alignGivenR(abPointPairs, aRb_estimate);
-// }
-
-std::ostream &operator<<(std::ostream &os, const Similarity2& p) {
-  os << "[" << p.rotation().theta() << " "
-      << p.translation().transpose() << " " << p.scale() << "]\';";
+Similarity2 Similarity2::Align(const Point2Pairs& abPointPairs) {
+  // Refer to Chapter 3 of
+  // http://www5.informatik.uni-erlangen.de/Forschung/Publikationen/2005/Zinsser05-PSR.pdf
+  if (abPointPairs.size() < 2)
+    throw std::runtime_error("input should have at least 2 pairs of points");
+  auto centroids = means(abPointPairs);
+  auto d_abPointPairs = internal::subtractCentroids(abPointPairs, centroids);
+  Matrix2 H = internal::calculateH(d_abPointPairs);
+  // ClosestTo finds rotation matrix closest to H in Frobenius sense
+  Rot2 aRb = Rot2::ClosestTo(H);
+  return internal::align(d_abPointPairs, aRb, centroids);
+}
+
+Similarity2 Similarity2::Align(const Pose2Pairs& abPosePairs) {
+  const size_t n = abPosePairs.size();
+  if (n < 2)
+    throw std::runtime_error("input should have at least 2 pairs of poses");
+
+  // calculate rotation
+  vector<Rot2> rotations;
+  Point2Pairs abPointPairs;
+  rotations.reserve(n);
+  abPointPairs.reserve(n);
+  // Below denotes the pose of the i'th object/camera/etc
+  // in frame "a" or frame "b".
+  Pose2 aTi, bTi;
+  for (const Pose2Pair& abPair : abPosePairs) {
+    std::tie(aTi, bTi) = abPair;
+    const Rot2 aRb = aTi.rotation().compose(bTi.rotation().inverse());
+    rotations.emplace_back(aRb);
+    abPointPairs.emplace_back(aTi.translation(), bTi.translation());
+  }
+  const Rot2 aRb_estimate;  // = FindKarcherMean<Rot2>(rotations);
+
+  return internal::alignGivenR(abPointPairs, aRb_estimate);
+}
+
+std::ostream& operator<<(std::ostream& os, const Similarity2& p) {
+  os << "[" << p.rotation().theta() << " " << p.translation().transpose() << " "
+     << p.scale() << "]\';";
   return os;
 }
 
-const Matrix3 Similarity2::matrix() const {
+Matrix3 Similarity2::matrix() const {
   Matrix3 T;
   T.topRows<2>() << R_.matrix(), t_;
   T.bottomRows<1>() << 0, 0, 1.0 / s_;
   return T;
 }
 
-Similarity2::operator Pose2() const {
-  return Pose2(R_, s_ * t_);
-}
+Similarity2::operator Pose2() const { return Pose2(R_, s_ * t_); }
 
-} // namespace gtsam
+}  // namespace gtsam
diff --git a/gtsam/geometry/Similarity2.h b/gtsam/geometry/Similarity2.h
index 85a6324c5c..93a1227f5b 100644
--- a/gtsam/geometry/Similarity2.h
+++ b/gtsam/geometry/Similarity2.h
@@ -17,13 +17,12 @@
 
 #pragma once
 
-#include <gtsam/geometry/Rot2.h>
-#include <gtsam/geometry/Point2.h>
-#include <gtsam/geometry/Pose2.h>
 #include <gtsam/base/Lie.h>
 #include <gtsam/base/Manifold.h>
 #include <gtsam/dllexport.h>
-
+#include <gtsam/geometry/Point2.h>
+#include <gtsam/geometry/Pose2.h>
+#include <gtsam/geometry/Rot2.h>
 
 namespace gtsam {
 
@@ -33,21 +32,19 @@ class Pose2;
 /**
  * 2D similarity transform
  */
-class Similarity2: public LieGroup<Similarity2, 4> {
-
+class Similarity2 : public LieGroup<Similarity2, 4> {
   /// @name Pose Concept
   /// @{
   typedef Rot2 Rotation;
   typedef Point2 Translation;
   /// @}
 
-private:
+ private:
   Rot2 R_;
   Point2 t_;
   double s_;
 
-public:
-
+ public:
   /// @name Constructors
   /// @{
 
@@ -60,11 +57,11 @@ class Similarity2: public LieGroup<Similarity2, 4> {
   /// Construct from GTSAM types
   GTSAM_EXPORT Similarity2(const Rot2& R, const Point2& t, double s);
 
-  // /// Construct from Eigen types
-  // GTSAM_EXPORT Similarity2(const Matrix2& R, const Vector2& t, double s);
+  /// Construct from Eigen types
+  GTSAM_EXPORT Similarity2(const Matrix2& R, const Vector2& t, double s);
 
-  // /// Construct from matrix [R t; 0 s^-1]
-  // GTSAM_EXPORT Similarity2(const Matrix3& T);
+  /// Construct from matrix [R t; 0 s^-1]
+  GTSAM_EXPORT Similarity2(const Matrix3& T);
 
   /// @}
   /// @name Testable
@@ -79,7 +76,8 @@ class Similarity2: public LieGroup<Similarity2, 4> {
   /// Print with optional string
   GTSAM_EXPORT void print(const std::string& s) const;
 
-  GTSAM_EXPORT friend std::ostream &operator<<(std::ostream &os, const Similarity2& p);
+  GTSAM_EXPORT friend std::ostream& operator<<(std::ostream& os,
+                                               const Similarity2& p);
 
   /// @}
   /// @name Group
@@ -94,22 +92,22 @@ class Similarity2: public LieGroup<Similarity2, 4> {
   /// Return the inverse
   GTSAM_EXPORT Similarity2 inverse() const;
 
-  // /// @}
-  // /// @name Group action on Point2
-  // /// @{
+  /// @}
+  /// @name Group action on Point2
+  /// @{
 
   /// Action on a point p is s*(R*p+t)
   GTSAM_EXPORT Point2 transformFrom(const Point2& p) const;
 
-  /** 
+  /**
    * Action on a pose T.
-   * |Rs  ts|   |R t|   |Rs*R Rs*t+ts| 
+   * |Rs  ts|   |R t|   |Rs*R Rs*t+ts|
    * |0  1/s| * |0 1| = | 0      1/s |, the result is still a Sim2 object.
    * To retrieve a Pose2, we normalized the scale value into 1.
    * |Rs*R Rs*t+ts|   |Rs*R s(Rs*t+ts)|
    * | 0      1/s | = |  0       1    |
-   * 
-   * This group action satisfies the compatibility condition. 
+   *
+   * This group action satisfies the compatibility condition.
    * For more details, refer to: https://en.wikipedia.org/wiki/Group_action
    */
   GTSAM_EXPORT Pose2 transformFrom(const Pose2& T) const;
@@ -117,22 +115,26 @@ class Similarity2: public LieGroup<Similarity2, 4> {
   /* syntactic sugar for transformFrom */
   GTSAM_EXPORT Point2 operator*(const Point2& p) const;
 
-  // /**
-  //  *  Create Similarity2 by aligning at least two point pairs
-  //  */
-  // GTSAM_EXPORT static Similarity2 Align(const std::vector<Point2Pair>& abPointPairs);
-  
-  // /**
-  //  * Create the Similarity2 object that aligns at least two pose pairs.
-  //  * Each pair is of the form (aTi, bTi).
-  //  * Given a list of pairs in frame a, and a list of pairs in frame b, Align()
-  //  * will compute the best-fit Similarity2 aSb transformation to align them.
-  //  * First, the rotation aRb will be computed as the average (Karcher mean) of
-  //  * many estimates aRb (from each pair). Afterwards, the scale factor will be computed
-  //  * using the algorithm described here:
-  //  * http://www5.informatik.uni-erlangen.de/Forschung/Publikationen/2005/Zinsser05-PSR.pdf
-  //  */
-  // GTSAM_EXPORT static Similarity2 Align(const std::vector<Pose2Pair>& abPosePairs);
+  /**
+   *  Create Similarity2 by aligning at least two point pairs
+   */
+  GTSAM_EXPORT static Similarity2 Align(const Point2Pairs& abPointPairs);
+
+  /**
+   * Create the Similarity2 object that aligns at least two pose pairs.
+   * Each pair is of the form (aTi, bTi).
+   * Given a list of pairs in frame a, and a list of pairs in frame b,
+   Align()
+   * will compute the best-fit Similarity2 aSb transformation to align them.
+   * First, the rotation aRb will be computed as the average (Karcher mean)
+   of
+   * many estimates aRb (from each pair). Afterwards, the scale factor will
+   be computed
+   * using the algorithm described here:
+   * http://www5.informatik.uni-erlangen.de/Forschung/Publikationen/2005/Zinsser05-PSR.pdf
+   */
+  GTSAM_EXPORT static Similarity2 Align(
+      const std::vector<Pose2Pair>& abPosePairs);
 
   /// @}
   /// @name Lie Group
@@ -145,45 +147,33 @@ class Similarity2: public LieGroup<Similarity2, 4> {
   /// @{
 
   /// Calculate 4*4 matrix group equivalent
-  GTSAM_EXPORT const Matrix3 matrix() const;
+  GTSAM_EXPORT Matrix3 matrix() const;
 
   /// Return a GTSAM rotation
-  const Rot2& rotation() const {
-    return R_;
-  }
+  Rot2 rotation() const { return R_; }
 
   /// Return a GTSAM translation
-  const Point2& translation() const {
-    return t_;
-  }
+  Point2 translation() const { return t_; }
 
   /// Return the scale
-  double scale() const {
-    return s_;
-  }
+  double scale() const { return s_; }
 
   /// Convert to a rigid body pose (R, s*t)
-  /// TODO(frank): why is this here? Red flag! Definitely don't have it as a cast.
   GTSAM_EXPORT operator Pose2() const;
 
   /// Dimensionality of tangent space = 4 DOF - used to autodetect sizes
-  inline static size_t Dim() {
-    return 4;
-  }
+  inline static size_t Dim() { return 4; }
 
   /// Dimensionality of tangent space = 4 DOF
-  inline size_t dim() const {
-    return 4;
-  }
+  inline size_t dim() const { return 4; }
 
   /// @}
 };
 
+template <>
+struct traits<Similarity2> : public internal::LieGroup<Similarity2> {};
 
-// template<>
-// struct traits<Similarity2> : public internal::LieGroup<Similarity2> {};
-
-// template<>
-// struct traits<const Similarity2> : public internal::LieGroup<Similarity2> {};
+template <>
+struct traits<const Similarity2> : public internal::LieGroup<Similarity2> {};
 
-} // namespace gtsam
+}  // namespace gtsam

From a57465ee642558b35f8fefcc7a351c6937f84959 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Sun, 6 Feb 2022 00:09:06 -0500
Subject: [PATCH 18/35] Similarity3 fixes

---
 gtsam/geometry/Similarity3.cpp | 16 ++++++++--------
 gtsam/geometry/Similarity3.h   |  6 +++---
 2 files changed, 11 insertions(+), 11 deletions(-)

diff --git a/gtsam/geometry/Similarity3.cpp b/gtsam/geometry/Similarity3.cpp
index fcaf0c8740..58bace0f1b 100644
--- a/gtsam/geometry/Similarity3.cpp
+++ b/gtsam/geometry/Similarity3.cpp
@@ -26,7 +26,7 @@ namespace gtsam {
 
 using std::vector;
 
-namespace {
+namespace internal {
 /// Subtract centroids from point pairs.
 static Point3Pairs subtractCentroids(const Point3Pairs &abPointPairs,
                                     const Point3Pair &centroids) {
@@ -79,10 +79,10 @@ static Similarity3 align(const Point3Pairs &d_abPointPairs, const Rot3 &aRb,
 static Similarity3 alignGivenR(const Point3Pairs &abPointPairs,
                                const Rot3 &aRb) {
   auto centroids = means(abPointPairs);
-  auto d_abPointPairs = subtractCentroids(abPointPairs, centroids);
+  auto d_abPointPairs = internal::subtractCentroids(abPointPairs, centroids);
   return align(d_abPointPairs, aRb, centroids);
 }
-}  // namespace
+}  // namespace internal
 
 Similarity3::Similarity3() :
     t_(0,0,0), s_(1) {
@@ -163,11 +163,11 @@ Similarity3 Similarity3::Align(const Point3Pairs &abPointPairs) {
   if (abPointPairs.size() < 3)
     throw std::runtime_error("input should have at least 3 pairs of points");
   auto centroids = means(abPointPairs);
-  auto d_abPointPairs = subtractCentroids(abPointPairs, centroids);
-  Matrix3 H = calculateH(d_abPointPairs);
+  auto d_abPointPairs = internal::subtractCentroids(abPointPairs, centroids);
+  Matrix3 H = internal::calculateH(d_abPointPairs);
   // ClosestTo finds rotation matrix closest to H in Frobenius sense
   Rot3 aRb = Rot3::ClosestTo(H);
-  return align(d_abPointPairs, aRb, centroids);
+  return internal::align(d_abPointPairs, aRb, centroids);
 }
 
 Similarity3 Similarity3::Align(const vector<Pose3Pair> &abPosePairs) {
@@ -190,7 +190,7 @@ Similarity3 Similarity3::Align(const vector<Pose3Pair> &abPosePairs) {
   }
   const Rot3 aRb_estimate = FindKarcherMean<Rot3>(rotations);
 
-  return alignGivenR(abPointPairs, aRb_estimate);
+  return internal::alignGivenR(abPointPairs, aRb_estimate);
 }
 
 Matrix4 Similarity3::wedge(const Vector7 &xi) {
@@ -281,7 +281,7 @@ std::ostream &operator<<(std::ostream &os, const Similarity3& p) {
   return os;
 }
 
-const Matrix4 Similarity3::matrix() const {
+Matrix4 Similarity3::matrix() const {
   Matrix4 T;
   T.topRows<3>() << R_.matrix(), t_;
   T.bottomRows<1>() << 0, 0, 0, 1.0 / s_;
diff --git a/gtsam/geometry/Similarity3.h b/gtsam/geometry/Similarity3.h
index 0ef787b059..27fdba6d72 100644
--- a/gtsam/geometry/Similarity3.h
+++ b/gtsam/geometry/Similarity3.h
@@ -180,15 +180,15 @@ class Similarity3: public LieGroup<Similarity3, 7> {
   /// @{
 
   /// Calculate 4*4 matrix group equivalent
-  GTSAM_EXPORT const Matrix4 matrix() const;
+  GTSAM_EXPORT Matrix4 matrix() const;
 
   /// Return a GTSAM rotation
-  const Rot3& rotation() const {
+  Rot3 rotation() const {
     return R_;
   }
 
   /// Return a GTSAM translation
-  const Point3& translation() const {
+  Point3 translation() const {
     return t_;
   }
 

From 5e7598606a152142405e45fc7c00c85632af0508 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Sun, 6 Feb 2022 00:09:16 -0500
Subject: [PATCH 19/35] fix wrapper

---
 gtsam/geometry/geometry.i | 12 ++++++------
 1 file changed, 6 insertions(+), 6 deletions(-)

diff --git a/gtsam/geometry/geometry.i b/gtsam/geometry/geometry.i
index 151c421555..6ad7baeddb 100644
--- a/gtsam/geometry/geometry.i
+++ b/gtsam/geometry/geometry.i
@@ -907,9 +907,9 @@ class Similarity2 {
   static gtsam::Similarity2 Align(const gtsam::Pose2Pairs& abPosePairs);
 
   // Standard Interface
-  const Matrix matrix() const;
-  const gtsam::Rot2& rotation();
-  const gtsam::Point2& translation();
+  Matrix matrix() const;
+  gtsam::Rot2& rotation();
+  gtsam::Point2& translation();
   double scale() const;
 };
 
@@ -929,9 +929,9 @@ class Similarity3 {
   static gtsam::Similarity3 Align(const gtsam::Pose3Pairs& abPosePairs);
 
   // Standard Interface
-  const Matrix matrix() const;
-  const gtsam::Rot3& rotation();
-  const gtsam::Point3& translation();
+  Matrix matrix() const;
+  gtsam::Rot3& rotation();
+  gtsam::Point3& translation();
   double scale() const;
 };
 

From b484a214e6793655206786fa8fd1d55af43b8b9c Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Sun, 6 Feb 2022 00:09:33 -0500
Subject: [PATCH 20/35] fix tests

---
 gtsam/navigation/tests/testGPSFactor.cpp | 13 +++---
 gtsam/navigation/tests/testMagFactor.cpp | 51 ++++++++++++++----------
 2 files changed, 39 insertions(+), 25 deletions(-)

diff --git a/gtsam/navigation/tests/testGPSFactor.cpp b/gtsam/navigation/tests/testGPSFactor.cpp
index b784c0c94c..5b96b80a84 100644
--- a/gtsam/navigation/tests/testGPSFactor.cpp
+++ b/gtsam/navigation/tests/testGPSFactor.cpp
@@ -27,7 +27,6 @@
 #include <GeographicLib/Config.h>
 #include <GeographicLib/LocalCartesian.hpp>
 
-using namespace std::placeholders;
 using namespace std;
 using namespace gtsam;
 using namespace GeographicLib;
@@ -71,8 +70,10 @@ TEST( GPSFactor, Constructor ) {
   EXPECT(assert_equal(Z_3x1,factor.evaluateError(T),1e-5));
 
   // Calculate numerical derivatives
-  Matrix expectedH = numericalDerivative11<Vector,Pose3>(
-      std::bind(&GPSFactor::evaluateError, &factor, _1, boost::none), T);
+  Matrix expectedH = numericalDerivative11<Vector, Pose3>(
+      std::bind(&GPSFactor::evaluateError, &factor, std::placeholders::_1,
+                boost::none),
+      T);
 
   // Use the factor to calculate the derivative
   Matrix actualH;
@@ -100,8 +101,10 @@ TEST( GPSFactor2, Constructor ) {
   EXPECT(assert_equal(Z_3x1,factor.evaluateError(T),1e-5));
 
   // Calculate numerical derivatives
-  Matrix expectedH = numericalDerivative11<Vector,NavState>(
-      std::bind(&GPSFactor2::evaluateError, &factor, _1, boost::none), T);
+  Matrix expectedH = numericalDerivative11<Vector, NavState>(
+      std::bind(&GPSFactor2::evaluateError, &factor, std::placeholders::_1,
+                boost::none),
+      T);
 
   // Use the factor to calculate the derivative
   Matrix actualH;
diff --git a/gtsam/navigation/tests/testMagFactor.cpp b/gtsam/navigation/tests/testMagFactor.cpp
index 5107b3b6b3..2450b16c72 100644
--- a/gtsam/navigation/tests/testMagFactor.cpp
+++ b/gtsam/navigation/tests/testMagFactor.cpp
@@ -26,7 +26,6 @@
 
 #include <GeographicLib/LocalCartesian.hpp>
 
-using namespace std::placeholders;
 using namespace std;
 using namespace gtsam;
 using namespace GeographicLib;
@@ -63,8 +62,10 @@ TEST( MagFactor, unrotate ) {
   Matrix H;
   Point3 expected(22735.5, 314.502, 44202.5);
   EXPECT( assert_equal(expected, MagFactor::unrotate(theta,nM,H),1e-1));
-  EXPECT( assert_equal(numericalDerivative11<Point3,Rot2> //
-      (std::bind(&MagFactor::unrotate, _1, nM, none), theta), H, 1e-6));
+  EXPECT(assert_equal(
+      numericalDerivative11<Point3, Rot2>  //
+      (std::bind(&MagFactor::unrotate, std::placeholders::_1, nM, none), theta),
+      H, 1e-6));
 }
 
 // *************************************************************************
@@ -75,36 +76,46 @@ TEST( MagFactor, Factors ) {
   // MagFactor
   MagFactor f(1, measured, s, dir, bias, model);
   EXPECT( assert_equal(Z_3x1,f.evaluateError(theta,H1),1e-5));
-  EXPECT( assert_equal((Matrix)numericalDerivative11<Vector,Rot2> //
-      (std::bind(&MagFactor::evaluateError, &f, _1, none), theta), H1, 1e-7));
+  EXPECT(assert_equal(
+      (Matrix)numericalDerivative11<Vector, Rot2>  //
+      (std::bind(&MagFactor::evaluateError, &f, std::placeholders::_1, none),
+       theta),
+      H1, 1e-7));
 
-// MagFactor1
+  // MagFactor1
   MagFactor1 f1(1, measured, s, dir, bias, model);
   EXPECT( assert_equal(Z_3x1,f1.evaluateError(nRb,H1),1e-5));
-  EXPECT( assert_equal(numericalDerivative11<Vector,Rot3> //
-      (std::bind(&MagFactor1::evaluateError, &f1, _1, none), nRb), H1, 1e-7));
+  EXPECT(assert_equal(
+      numericalDerivative11<Vector, Rot3>  //
+      (std::bind(&MagFactor1::evaluateError, &f1, std::placeholders::_1, none),
+       nRb),
+      H1, 1e-7));
 
-// MagFactor2
+  // MagFactor2
   MagFactor2 f2(1, 2, measured, nRb, model);
   EXPECT( assert_equal(Z_3x1,f2.evaluateError(scaled,bias,H1,H2),1e-5));
-  EXPECT( assert_equal(numericalDerivative11<Vector,Point3> //
-      (std::bind(&MagFactor2::evaluateError, &f2, _1, bias, none, none), scaled),//
-      H1, 1e-7));
-  EXPECT( assert_equal(numericalDerivative11<Vector,Point3> //
-      (std::bind(&MagFactor2::evaluateError, &f2, scaled, _1, none, none), bias),//
-      H2, 1e-7));
-
-// MagFactor2
+  EXPECT(assert_equal(numericalDerivative11<Vector, Point3>  //
+                      (std::bind(&MagFactor2::evaluateError, &f2,
+                                 std::placeholders::_1, bias, none, none),
+                       scaled),  //
+                      H1, 1e-7));
+  EXPECT(assert_equal(numericalDerivative11<Vector, Point3>  //
+                      (std::bind(&MagFactor2::evaluateError, &f2, scaled,
+                                 std::placeholders::_1, none, none),
+                       bias),  //
+                      H2, 1e-7));
+
+  // MagFactor2
   MagFactor3 f3(1, 2, 3, measured, nRb, model);
   EXPECT(assert_equal(Z_3x1,f3.evaluateError(s,dir,bias,H1,H2,H3),1e-5));
   EXPECT(assert_equal((Matrix)numericalDerivative11<Vector,double> //
-      (std::bind(&MagFactor3::evaluateError, &f3, _1, dir, bias, none, none, none), s),//
+      (std::bind(&MagFactor3::evaluateError, &f3, std::placeholders::_1, dir, bias, none, none, none), s),//
       H1, 1e-7));
   EXPECT(assert_equal(numericalDerivative11<Vector,Unit3> //
-      (std::bind(&MagFactor3::evaluateError, &f3, s, _1, bias, none, none, none), dir),//
+      (std::bind(&MagFactor3::evaluateError, &f3, s, std::placeholders::_1, bias, none, none, none), dir),//
       H2, 1e-7));
   EXPECT(assert_equal(numericalDerivative11<Vector,Point3> //
-      (std::bind(&MagFactor3::evaluateError, &f3, s, dir, _1, none, none, none), bias),//
+      (std::bind(&MagFactor3::evaluateError, &f3, s, dir, std::placeholders::_1, none, none, none), bias),//
       H3, 1e-7));
 }
 

From c716f63219d5d8b0ae1fca0242ad3320cca53438 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Sun, 6 Feb 2022 11:26:29 -0500
Subject: [PATCH 21/35] wrap Pose2Pairs

---
 python/gtsam/preamble/geometry.h        | 4 ++--
 python/gtsam/specializations/geometry.h | 1 +
 2 files changed, 3 insertions(+), 2 deletions(-)

diff --git a/python/gtsam/preamble/geometry.h b/python/gtsam/preamble/geometry.h
index 35fe2a577a..bd0441d067 100644
--- a/python/gtsam/preamble/geometry.h
+++ b/python/gtsam/preamble/geometry.h
@@ -23,8 +23,8 @@ PYBIND11_MAKE_OPAQUE(
     std::vector<gtsam::Point2, Eigen::aligned_allocator<gtsam::Point2>>);
 PYBIND11_MAKE_OPAQUE(gtsam::Point2Pairs);
 PYBIND11_MAKE_OPAQUE(gtsam::Point3Pairs);
+PYBIND11_MAKE_OPAQUE(gtsam::Pose2Pairs);
 PYBIND11_MAKE_OPAQUE(gtsam::Pose3Pairs);
 PYBIND11_MAKE_OPAQUE(std::vector<gtsam::Pose3>);
-PYBIND11_MAKE_OPAQUE(
-    gtsam::CameraSet<gtsam::PinholeCamera<gtsam::Cal3Bundler>>);
+PYBIND11_MAKE_OPAQUE(gtsam::CameraSet<gtsam::PinholeCamera<gtsam::Cal3Bundler>>);
 PYBIND11_MAKE_OPAQUE(gtsam::CameraSet<gtsam::PinholeCamera<gtsam::Cal3_S2>>);
diff --git a/python/gtsam/specializations/geometry.h b/python/gtsam/specializations/geometry.h
index a492ce8eb2..5a0ea35ef8 100644
--- a/python/gtsam/specializations/geometry.h
+++ b/python/gtsam/specializations/geometry.h
@@ -16,6 +16,7 @@ py::bind_vector<
     m_, "Point2Vector");
 py::bind_vector<std::vector<gtsam::Point2Pair>>(m_, "Point2Pairs");
 py::bind_vector<std::vector<gtsam::Point3Pair>>(m_, "Point3Pairs");
+py::bind_vector<std::vector<gtsam::Pose2Pair>>(m_, "Pose2Pairs");
 py::bind_vector<std::vector<gtsam::Pose3Pair>>(m_, "Pose3Pairs");
 py::bind_vector<std::vector<gtsam::Pose3>>(m_, "Pose3Vector");
 py::bind_vector<gtsam::CameraSet<gtsam::PinholeCamera<gtsam::Cal3_S2>>>(

From 509870619b4efe14b5bb8d8f1536be1f370988a9 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Sun, 6 Feb 2022 11:26:43 -0500
Subject: [PATCH 22/35] wrap equals for Sim2 and Sim3

---
 gtsam/geometry/geometry.i | 2 ++
 1 file changed, 2 insertions(+)

diff --git a/gtsam/geometry/geometry.i b/gtsam/geometry/geometry.i
index f872283f33..350f23d18a 100644
--- a/gtsam/geometry/geometry.i
+++ b/gtsam/geometry/geometry.i
@@ -862,6 +862,7 @@ class Similarity2 {
   static gtsam::Similarity2 Align(const gtsam::Pose2Pairs& abPosePairs);
 
   // Standard Interface
+  bool equals(const gtsam::Similarity2& sim, double tol) const;
   Matrix matrix() const;
   gtsam::Rot2& rotation();
   gtsam::Point2& translation();
@@ -884,6 +885,7 @@ class Similarity3 {
   static gtsam::Similarity3 Align(const gtsam::Pose3Pairs& abPosePairs);
 
   // Standard Interface
+  bool equals(const gtsam::Similarity3& sim, double tol) const;
   Matrix matrix() const;
   gtsam::Rot3& rotation();
   gtsam::Point3& translation();

From 464af9f7111e89de58dcf30eac38f29a44cd098b Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Sun, 6 Feb 2022 11:28:24 -0500
Subject: [PATCH 23/35] Fix syntactic errors in test_Sim2.py

---
 python/gtsam/tests/test_Sim2.py | 12 ++++++------
 1 file changed, 6 insertions(+), 6 deletions(-)

diff --git a/python/gtsam/tests/test_Sim2.py b/python/gtsam/tests/test_Sim2.py
index 67bc770d2c..1d09dcc3b5 100644
--- a/python/gtsam/tests/test_Sim2.py
+++ b/python/gtsam/tests/test_Sim2.py
@@ -65,7 +65,7 @@ def test_align_poses_along_straight_line_gauge(self):
            world frame has poses rotated about z-axis (90 degree yaw)
            world and egovehicle frame translated by 11 meters w.r.t. each other
         """
-        Rz90 = Rot3.Rz(np.deg2rad(90))
+        Rz90 = Rot2.Rz(np.deg2rad(90))
 
         # Create source poses (three objects o1, o2, o3 living in the egovehicle "e" frame)
         # Suppose they are 3d cuboids detected by an onboard sensor in the egovehicle frame
@@ -107,10 +107,10 @@ def test_align_poses_scaled_squares(self):
         R180 = Rot2.fromDegrees(180)
         R270 = Rot2.fromDegrees(270)
 
-        aTi0 = Pose3(R0, np.array([2, 3]))
-        aTi1 = Pose3(R90, np.array([12, 3]))
-        aTi2 = Pose3(R180, np.array([12, 13]))
-        aTi3 = Pose3(R270, np.array([2, 13]))
+        aTi0 = Pose2(R0, np.array([2, 3]))
+        aTi1 = Pose2(R90, np.array([12, 3]))
+        aTi2 = Pose2(R180, np.array([12, 13]))
+        aTi3 = Pose2(R270, np.array([2, 13]))
 
         aTi_list = [aTi0, aTi1, aTi2, aTi3]
 
@@ -144,7 +144,7 @@ def test_is_eq(self) -> None:
         """Ensure object equality works properly (are equal)."""
         bSa = Similarity2(R=Rot2(), t=np.array([1, 2]), s=3.0)
         bSa_ = Similarity2(R=Rot2(), t=np.array([1.0, 2.0]), s=3)
-        assert bSa == bSa_
+        self.gtsamAssertEquals(bSa, bSa_)
 
     def test_not_eq_translation(self) -> None:
         """Ensure object equality works properly (not equal translation)."""

From cc8d80fb7ecab4f1043d02678f24ddfe121b78aa Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Sun, 6 Feb 2022 12:14:30 -0500
Subject: [PATCH 24/35] Remove SFINAE from KarcherMean so we can also use it
 for Rot2

---
 gtsam/slam/KarcherMeanFactor-inl.h | 3 +--
 1 file changed, 1 insertion(+), 2 deletions(-)

diff --git a/gtsam/slam/KarcherMeanFactor-inl.h b/gtsam/slam/KarcherMeanFactor-inl.h
index c81a9adc5d..00f7417056 100644
--- a/gtsam/slam/KarcherMeanFactor-inl.h
+++ b/gtsam/slam/KarcherMeanFactor-inl.h
@@ -40,8 +40,7 @@ T FindKarcherMeanImpl(const vector<T, ALLOC>& rotations) {
   return result.at<T>(kKey);
 }
 
-template <class T,
-        typename = typename std::enable_if< std::is_same<gtsam::Rot3, T>::value >::type >
+template <class T>
 T FindKarcherMean(const std::vector<T>& rotations) {
   return FindKarcherMeanImpl(rotations);
 }

From 37ae7038fa033e861f99375cea9b9b1dbbfb7727 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Sun, 6 Feb 2022 12:27:50 -0500
Subject: [PATCH 25/35] Fix tests

---
 gtsam/geometry/Similarity2.cpp  |  2 +-
 python/gtsam/tests/test_Sim2.py | 22 +++++++++++-----------
 2 files changed, 12 insertions(+), 12 deletions(-)

diff --git a/gtsam/geometry/Similarity2.cpp b/gtsam/geometry/Similarity2.cpp
index 12529f52df..6e5da635ba 100644
--- a/gtsam/geometry/Similarity2.cpp
+++ b/gtsam/geometry/Similarity2.cpp
@@ -193,7 +193,7 @@ Similarity2 Similarity2::Align(const Pose2Pairs& abPosePairs) {
     rotations.emplace_back(aRb);
     abPointPairs.emplace_back(aTi.translation(), bTi.translation());
   }
-  const Rot2 aRb_estimate;  // = FindKarcherMean<Rot2>(rotations);
+  const Rot2 aRb_estimate = FindKarcherMean<Rot2>(rotations);
 
   return internal::alignGivenR(abPointPairs, aRb_estimate);
 }
diff --git a/python/gtsam/tests/test_Sim2.py b/python/gtsam/tests/test_Sim2.py
index 1d09dcc3b5..3e39ac45c6 100644
--- a/python/gtsam/tests/test_Sim2.py
+++ b/python/gtsam/tests/test_Sim2.py
@@ -27,10 +27,10 @@ def test_align_poses_along_straight_line(self) -> None:
         Scenario:
            3 object poses
            same scale (no gauge ambiguity)
-           world frame has poses rotated about x-axis (90 degree roll)
+           world frame has poses rotated about 180 degrees.
            world and egovehicle frame translated by 15 meters w.r.t. each other
         """
-        Rx90 = Rot2.fromDegrees(90)
+        R180 = Rot2.fromDegrees(180)
 
         # Create source poses (three objects o1, o2, o3 living in the egovehicle "e" frame)
         # Suppose they are 3d cuboids detected by an onboard sensor in the egovehicle frame
@@ -41,10 +41,10 @@ def test_align_poses_along_straight_line(self) -> None:
         eToi_list = [eTo0, eTo1, eTo2]
 
         # Create destination poses
-        # (same three objects, but instead living in the world/city "w" frame)
-        wTo0 = Pose2(Rx90, np.array([-10, 0]))
-        wTo1 = Pose2(Rx90, np.array([-5, 0]))
-        wTo2 = Pose2(Rx90, np.array([0, 0]))
+        # (same three objects, but instead living in the world "w" frame)
+        wTo0 = Pose2(R180, np.array([-10, 0]))
+        wTo1 = Pose2(R180, np.array([-5, 0]))
+        wTo2 = Pose2(R180, np.array([0, 0]))
 
         wToi_list = [wTo0, wTo1, wTo2]
 
@@ -62,10 +62,10 @@ def test_align_poses_along_straight_line_gauge(self):
         Scenario:
            3 object poses
            with gauge ambiguity (2x scale)
-           world frame has poses rotated about z-axis (90 degree yaw)
+           world frame has poses rotated by 90 degrees.
            world and egovehicle frame translated by 11 meters w.r.t. each other
         """
-        Rz90 = Rot2.Rz(np.deg2rad(90))
+        R90 = Rot2.fromDegrees(90)
 
         # Create source poses (three objects o1, o2, o3 living in the egovehicle "e" frame)
         # Suppose they are 3d cuboids detected by an onboard sensor in the egovehicle frame
@@ -77,9 +77,9 @@ def test_align_poses_along_straight_line_gauge(self):
 
         # Create destination poses
         # (same three objects, but instead living in the world/city "w" frame)
-        wTo0 = Pose2(Rz90, np.array([0, 12]))
-        wTo1 = Pose2(Rz90, np.array([0, 14]))
-        wTo2 = Pose2(Rz90, np.array([0, 18]))
+        wTo0 = Pose2(R90, np.array([0, 12]))
+        wTo1 = Pose2(R90, np.array([0, 14]))
+        wTo2 = Pose2(R90, np.array([0, 18]))
 
         wToi_list = [wTo0, wTo1, wTo2]
 

From d8e56585fefe42b922b92cb9a6cb93d8558b69e4 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Fri, 29 Apr 2022 17:36:38 -0400
Subject: [PATCH 26/35] capitalize static methods

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

diff --git a/gtsam/geometry/Similarity2.cpp b/gtsam/geometry/Similarity2.cpp
index 6e5da635ba..e6fa73ef3f 100644
--- a/gtsam/geometry/Similarity2.cpp
+++ b/gtsam/geometry/Similarity2.cpp
@@ -28,7 +28,7 @@ using std::vector;
 namespace internal {
 
 /// Subtract centroids from point pairs.
-static Point2Pairs subtractCentroids(const Point2Pairs& abPointPairs,
+static Point2Pairs SubtractCentroids(const Point2Pairs& abPointPairs,
                                      const Point2Pair& centroids) {
   Point2Pairs d_abPointPairs;
   for (const Point2Pair& abPair : abPointPairs) {
@@ -40,7 +40,7 @@ static Point2Pairs subtractCentroids(const Point2Pairs& abPointPairs,
 }
 
 /// Form inner products x and y and calculate scale.
-static double calculateScale(const Point2Pairs& d_abPointPairs,
+static double CalculateScale(const Point2Pairs& d_abPointPairs,
                              const Rot2& aRb) {
   double x = 0, y = 0;
   Point2 da, db;
@@ -56,7 +56,7 @@ static double calculateScale(const Point2Pairs& d_abPointPairs,
 }
 
 /// Form outer product H.
-static Matrix2 calculateH(const Point2Pairs& d_abPointPairs) {
+static Matrix2 CalculateH(const Point2Pairs& d_abPointPairs) {
   Matrix2 H = Z_2x2;
   for (const Point2Pair& d_abPair : d_abPointPairs) {
     H += d_abPair.first * d_abPair.second.transpose();
@@ -73,9 +73,9 @@ static Matrix2 calculateH(const Point2Pairs& d_abPointPairs) {
  * @param centroids
  * @return Similarity2
  */
-static Similarity2 align(const Point2Pairs& d_abPointPairs, const Rot2& aRb,
+static Similarity2 Align(const Point2Pairs& d_abPointPairs, const Rot2& aRb,
                          const Point2Pair& centroids) {
-  const double s = calculateScale(d_abPointPairs, aRb);
+  const double s = CalculateScale(d_abPointPairs, aRb);
   // dividing aTb by s is required because the registration cost function
   // minimizes ||a - sRb - t||, whereas Sim(2) computes s(Rb + t)
   const Point2 aTb = (centroids.first - s * (aRb * centroids.second)) / s;
@@ -93,11 +93,11 @@ static Similarity2 align(const Point2Pairs& d_abPointPairs, const Rot2& aRb,
  * @param aRb
  * @return Similarity2
  */
-static Similarity2 alignGivenR(const Point2Pairs& abPointPairs,
+static Similarity2 AlignGivenR(const Point2Pairs& abPointPairs,
                                const Rot2& aRb) {
   auto centroids = means(abPointPairs);
-  auto d_abPointPairs = internal::subtractCentroids(abPointPairs, centroids);
-  return internal::align(d_abPointPairs, aRb, centroids);
+  auto d_abPointPairs = internal::SubtractCentroids(abPointPairs, centroids);
+  return internal::Align(d_abPointPairs, aRb, centroids);
 }
 }  // namespace internal
 
@@ -134,7 +134,7 @@ void Similarity2::print(const std::string& s) const {
             << std::endl;
 }
 
-Similarity2 Similarity2::identity() { return Similarity2(); }
+Similarity2 Similarity2::Identity() { return Similarity2(); }
 
 Similarity2 Similarity2::operator*(const Similarity2& S) const {
   return Similarity2(R_ * S.R_, ((1.0 / S.s_) * t_) + R_ * S.t_, s_ * S.s_);
@@ -167,11 +167,11 @@ Similarity2 Similarity2::Align(const Point2Pairs& abPointPairs) {
   if (abPointPairs.size() < 2)
     throw std::runtime_error("input should have at least 2 pairs of points");
   auto centroids = means(abPointPairs);
-  auto d_abPointPairs = internal::subtractCentroids(abPointPairs, centroids);
-  Matrix2 H = internal::calculateH(d_abPointPairs);
+  auto d_abPointPairs = internal::SubtractCentroids(abPointPairs, centroids);
+  Matrix2 H = internal::CalculateH(d_abPointPairs);
   // ClosestTo finds rotation matrix closest to H in Frobenius sense
   Rot2 aRb = Rot2::ClosestTo(H);
-  return internal::align(d_abPointPairs, aRb, centroids);
+  return internal::Align(d_abPointPairs, aRb, centroids);
 }
 
 Similarity2 Similarity2::Align(const Pose2Pairs& abPosePairs) {
@@ -195,7 +195,7 @@ Similarity2 Similarity2::Align(const Pose2Pairs& abPosePairs) {
   }
   const Rot2 aRb_estimate = FindKarcherMean<Rot2>(rotations);
 
-  return internal::alignGivenR(abPointPairs, aRb_estimate);
+  return internal::AlignGivenR(abPointPairs, aRb_estimate);
 }
 
 std::ostream& operator<<(std::ostream& os, const Similarity2& p) {
diff --git a/gtsam/geometry/Similarity2.h b/gtsam/geometry/Similarity2.h
index 93a1227f5b..511ac8a289 100644
--- a/gtsam/geometry/Similarity2.h
+++ b/gtsam/geometry/Similarity2.h
@@ -84,7 +84,7 @@ class Similarity2 : public LieGroup<Similarity2, 4> {
   /// @{
 
   /// Return an identity transform
-  GTSAM_EXPORT static Similarity2 identity();
+  GTSAM_EXPORT static Similarity2 Identity();
 
   /// Composition
   GTSAM_EXPORT Similarity2 operator*(const Similarity2& S) const;

From 5be6309a58cee80abf1823ec89827b0408c02415 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Fri, 29 Apr 2022 17:36:53 -0400
Subject: [PATCH 27/35] GTSAM_EXPORT at the class level

---
 gtsam/geometry/Similarity2.h |  42 ++++++------
 gtsam/geometry/Similarity3.h | 122 ++++++++++++++++-------------------
 2 files changed, 76 insertions(+), 88 deletions(-)

diff --git a/gtsam/geometry/Similarity2.h b/gtsam/geometry/Similarity2.h
index 511ac8a289..74ebf96409 100644
--- a/gtsam/geometry/Similarity2.h
+++ b/gtsam/geometry/Similarity2.h
@@ -32,7 +32,7 @@ class Pose2;
 /**
  * 2D similarity transform
  */
-class Similarity2 : public LieGroup<Similarity2, 4> {
+class GTSAM_EXPORT Similarity2 : public LieGroup<Similarity2, 4> {
   /// @name Pose Concept
   /// @{
   typedef Rot2 Rotation;
@@ -49,55 +49,54 @@ class Similarity2 : public LieGroup<Similarity2, 4> {
   /// @{
 
   /// Default constructor
-  GTSAM_EXPORT Similarity2();
+  Similarity2();
 
   /// Construct pure scaling
-  GTSAM_EXPORT Similarity2(double s);
+  Similarity2(double s);
 
   /// Construct from GTSAM types
-  GTSAM_EXPORT Similarity2(const Rot2& R, const Point2& t, double s);
+  Similarity2(const Rot2& R, const Point2& t, double s);
 
   /// Construct from Eigen types
-  GTSAM_EXPORT Similarity2(const Matrix2& R, const Vector2& t, double s);
+  Similarity2(const Matrix2& R, const Vector2& t, double s);
 
   /// Construct from matrix [R t; 0 s^-1]
-  GTSAM_EXPORT Similarity2(const Matrix3& T);
+  Similarity2(const Matrix3& T);
 
   /// @}
   /// @name Testable
   /// @{
 
   /// Compare with tolerance
-  GTSAM_EXPORT bool equals(const Similarity2& sim, double tol) const;
+  bool equals(const Similarity2& sim, double tol) const;
 
   /// Exact equality
-  GTSAM_EXPORT bool operator==(const Similarity2& other) const;
+  bool operator==(const Similarity2& other) const;
 
   /// Print with optional string
-  GTSAM_EXPORT void print(const std::string& s) const;
+  void print(const std::string& s) const;
 
-  GTSAM_EXPORT friend std::ostream& operator<<(std::ostream& os,
-                                               const Similarity2& p);
+  friend std::ostream& operator<<(std::ostream& os, const Similarity2& p);
 
   /// @}
   /// @name Group
   /// @{
 
   /// Return an identity transform
-  GTSAM_EXPORT static Similarity2 Identity();
+  static Similarity2 Identity();
 
   /// Composition
-  GTSAM_EXPORT Similarity2 operator*(const Similarity2& S) const;
+  Similarity2 operator*(const Similarity2& S) const;
 
   /// Return the inverse
-  GTSAM_EXPORT Similarity2 inverse() const;
+  Similarity2 inverse() const;
 
   /// @}
   /// @name Group action on Point2
   /// @{
 
   /// Action on a point p is s*(R*p+t)
-  GTSAM_EXPORT Point2 transformFrom(const Point2& p) const;
+  Point2 transformFrom(const Point2& p) const;
 
   /**
    * Action on a pose T.
@@ -110,15 +109,15 @@ class Similarity2 : public LieGroup<Similarity2, 4> {
    * This group action satisfies the compatibility condition.
    * For more details, refer to: https://en.wikipedia.org/wiki/Group_action
    */
-  GTSAM_EXPORT Pose2 transformFrom(const Pose2& T) const;
+  Pose2 transformFrom(const Pose2& T) const;
 
   /* syntactic sugar for transformFrom */
-  GTSAM_EXPORT Point2 operator*(const Point2& p) const;
+  Point2 operator*(const Point2& p) const;
 
   /**
    *  Create Similarity2 by aligning at least two point pairs
    */
-  GTSAM_EXPORT static Similarity2 Align(const Point2Pairs& abPointPairs);
+  static Similarity2 Align(const Point2Pairs& abPointPairs);
 
   /**
    * Create the Similarity2 object that aligns at least two pose pairs.
@@ -133,8 +132,7 @@ class Similarity2 : public LieGroup<Similarity2, 4> {
    * using the algorithm described here:
    * http://www5.informatik.uni-erlangen.de/Forschung/Publikationen/2005/Zinsser05-PSR.pdf
    */
-  GTSAM_EXPORT static Similarity2 Align(
-      const std::vector<Pose2Pair>& abPosePairs);
+  static Similarity2 Align(const std::vector<Pose2Pair>& abPosePairs);
 
   /// @}
   /// @name Lie Group
@@ -147,7 +145,7 @@ class Similarity2 : public LieGroup<Similarity2, 4> {
   /// @{
 
   /// Calculate 4*4 matrix group equivalent
-  GTSAM_EXPORT Matrix3 matrix() const;
+  Matrix3 matrix() const;
 
   /// Return a GTSAM rotation
   Rot2 rotation() const { return R_; }
@@ -159,7 +157,7 @@ class Similarity2 : public LieGroup<Similarity2, 4> {
   double scale() const { return s_; }
 
   /// Convert to a rigid body pose (R, s*t)
-  GTSAM_EXPORT operator Pose2() const;
+  operator Pose2() const;
 
   /// Dimensionality of tangent space = 4 DOF - used to autodetect sizes
   inline static size_t Dim() { return 4; }
diff --git a/gtsam/geometry/Similarity3.h b/gtsam/geometry/Similarity3.h
index 27fdba6d72..8799ba2447 100644
--- a/gtsam/geometry/Similarity3.h
+++ b/gtsam/geometry/Similarity3.h
@@ -18,13 +18,12 @@
 
 #pragma once
 
-#include <gtsam/geometry/Rot3.h>
-#include <gtsam/geometry/Point3.h>
-#include <gtsam/geometry/Pose3.h>
 #include <gtsam/base/Lie.h>
 #include <gtsam/base/Manifold.h>
 #include <gtsam/dllexport.h>
-
+#include <gtsam/geometry/Point3.h>
+#include <gtsam/geometry/Pose3.h>
+#include <gtsam/geometry/Rot3.h>
 
 namespace gtsam {
 
@@ -34,108 +33,106 @@ class Pose3;
 /**
  * 3D similarity transform
  */
-class Similarity3: public LieGroup<Similarity3, 7> {
-
+class GTSAM_EXPORT Similarity3 : public LieGroup<Similarity3, 7> {
   /// @name Pose Concept
   /// @{
   typedef Rot3 Rotation;
   typedef Point3 Translation;
   /// @}
 
-private:
+ private:
   Rot3 R_;
   Point3 t_;
   double s_;
 
-public:
-
+ public:
   /// @name Constructors
   /// @{
 
   /// Default constructor
-  GTSAM_EXPORT Similarity3();
+  Similarity3();
 
   /// Construct pure scaling
-  GTSAM_EXPORT Similarity3(double s);
+  Similarity3(double s);
 
   /// Construct from GTSAM types
-  GTSAM_EXPORT Similarity3(const Rot3& R, const Point3& t, double s);
+  Similarity3(const Rot3& R, const Point3& t, double s);
 
   /// Construct from Eigen types
-  GTSAM_EXPORT Similarity3(const Matrix3& R, const Vector3& t, double s);
+  Similarity3(const Matrix3& R, const Vector3& t, double s);
 
   /// Construct from matrix [R t; 0 s^-1]
-  GTSAM_EXPORT Similarity3(const Matrix4& T);
+  Similarity3(const Matrix4& T);
 
   /// @}
   /// @name Testable
   /// @{
 
   /// Compare with tolerance
-  GTSAM_EXPORT bool equals(const Similarity3& sim, double tol) const;
+  bool equals(const Similarity3& sim, double tol) const;
 
   /// Exact equality
-  GTSAM_EXPORT bool operator==(const Similarity3& other) const;
+  bool operator==(const Similarity3& other) const;
 
   /// Print with optional string
-  GTSAM_EXPORT void print(const std::string& s) const;
+  void print(const std::string& s) const;
 
-  GTSAM_EXPORT friend std::ostream &operator<<(std::ostream &os, const Similarity3& p);
+  friend std::ostream& operator<<(std::ostream& os, const Similarity3& p);
 
   /// @}
   /// @name Group
   /// @{
 
   /// Return an identity transform
-  GTSAM_EXPORT static Similarity3 identity();
+  static Similarity3 identity();
 
   /// Composition
-  GTSAM_EXPORT Similarity3 operator*(const Similarity3& S) const;
+  Similarity3 operator*(const Similarity3& S) const;
 
   /// Return the inverse
-  GTSAM_EXPORT Similarity3 inverse() const;
+  Similarity3 inverse() const;
 
   /// @}
   /// @name Group action on Point3
   /// @{
 
   /// Action on a point p is s*(R*p+t)
-  GTSAM_EXPORT Point3 transformFrom(const Point3& p, //
-      OptionalJacobian<3, 7> H1 = boost::none, //
-      OptionalJacobian<3, 3> H2 = boost::none) const;
+  Point3 transformFrom(const Point3& p,                          //
+                       OptionalJacobian<3, 7> H1 = boost::none,  //
+                       OptionalJacobian<3, 3> H2 = boost::none) const;
 
-  /** 
+  /**
    * Action on a pose T.
-   * |Rs  ts|   |R t|   |Rs*R Rs*t+ts| 
+   * |Rs  ts|   |R t|   |Rs*R Rs*t+ts|
    * |0  1/s| * |0 1| = | 0      1/s |, the result is still a Sim3 object.
    * To retrieve a Pose3, we normalized the scale value into 1.
    * |Rs*R Rs*t+ts|   |Rs*R s(Rs*t+ts)|
    * | 0      1/s | = |  0       1    |
-   * 
-   * This group action satisfies the compatibility condition. 
+   *
+   * This group action satisfies the compatibility condition.
    * For more details, refer to: https://en.wikipedia.org/wiki/Group_action
    */
-  GTSAM_EXPORT Pose3 transformFrom(const Pose3& T) const;
+  Pose3 transformFrom(const Pose3& T) const;
 
   /** syntactic sugar for transformFrom */
-  GTSAM_EXPORT Point3 operator*(const Point3& p) const;
+  Point3 operator*(const Point3& p) const;
 
   /**
    *  Create Similarity3 by aligning at least three point pairs
    */
-  GTSAM_EXPORT static Similarity3 Align(const std::vector<Point3Pair>& abPointPairs);
-  
+  static Similarity3 Align(const std::vector<Point3Pair>& abPointPairs);
+
   /**
    * Create the Similarity3 object that aligns at least two pose pairs.
    * Each pair is of the form (aTi, bTi).
    * Given a list of pairs in frame a, and a list of pairs in frame b, Align()
    * will compute the best-fit Similarity3 aSb transformation to align them.
    * First, the rotation aRb will be computed as the average (Karcher mean) of
-   * many estimates aRb (from each pair). Afterwards, the scale factor will be computed
-   * using the algorithm described here:
+   * many estimates aRb (from each pair). Afterwards, the scale factor will be
+   * computed using the algorithm described here:
    * http://www5.informatik.uni-erlangen.de/Forschung/Publikationen/2005/Zinsser05-PSR.pdf
    */
-  GTSAM_EXPORT static Similarity3 Align(const std::vector<Pose3Pair>& abPosePairs);
+  static Similarity3 Align(const std::vector<Pose3Pair>& abPosePairs);
 
   /// @}
   /// @name Lie Group
@@ -144,20 +141,22 @@ class Similarity3: public LieGroup<Similarity3, 7> {
   /** Log map at the identity
    * \f$ [R_x,R_y,R_z, t_x, t_y, t_z, \lambda] \f$
    */
-  GTSAM_EXPORT static Vector7 Logmap(const Similarity3& s, //
-      OptionalJacobian<7, 7> Hm = boost::none);
+  static Vector7 Logmap(const Similarity3& s,  //
+                        OptionalJacobian<7, 7> Hm = boost::none);
 
   /** Exponential map at the identity
    */
-  GTSAM_EXPORT static Similarity3 Expmap(const Vector7& v, //
-      OptionalJacobian<7, 7> Hm = boost::none);
+  static Similarity3 Expmap(const Vector7& v,  //
+                            OptionalJacobian<7, 7> Hm = boost::none);
 
   /// Chart at the origin
   struct ChartAtOrigin {
-    static Similarity3 Retract(const Vector7& v, ChartJacobian H = boost::none) {
+    static Similarity3 Retract(const Vector7& v,
+                               ChartJacobian H = boost::none) {
       return Similarity3::Expmap(v, H);
     }
-    static Vector7 Local(const Similarity3& other, ChartJacobian H = boost::none) {
+    static Vector7 Local(const Similarity3& other,
+                         ChartJacobian H = boost::none) {
       return Similarity3::Logmap(other, H);
     }
   };
@@ -170,67 +169,58 @@ class Similarity3: public LieGroup<Similarity3, 7> {
    * @return 4*4 element of Lie algebra that can be exponentiated
    * TODO(frank): rename to Hat, make part of traits
    */
-  GTSAM_EXPORT static Matrix4 wedge(const Vector7& xi);
+  static Matrix4 wedge(const Vector7& xi);
 
   /// Project from one tangent space to another
-  GTSAM_EXPORT  Matrix7 AdjointMap() const;
+  Matrix7 AdjointMap() const;
 
   /// @}
   /// @name Standard interface
   /// @{
 
   /// Calculate 4*4 matrix group equivalent
-  GTSAM_EXPORT Matrix4 matrix() const;
+  Matrix4 matrix() const;
 
   /// Return a GTSAM rotation
-  Rot3 rotation() const {
-    return R_;
-  }
+  Rot3 rotation() const { return R_; }
 
   /// Return a GTSAM translation
-  Point3 translation() const {
-    return t_;
-  }
+  Point3 translation() const { return t_; }
 
   /// Return the scale
-  double scale() const {
-    return s_;
-  }
+  double scale() const { return s_; }
 
   /// Convert to a rigid body pose (R, s*t)
-  /// TODO(frank): why is this here? Red flag! Definitely don't have it as a cast.
-  GTSAM_EXPORT operator Pose3() const;
+  /// TODO(frank): why is this here? Red flag! Definitely don't have it as a
+  /// cast.
+  operator Pose3() const;
 
   /// Dimensionality of tangent space = 7 DOF - used to autodetect sizes
-  inline static size_t Dim() {
-    return 7;
-  }
+  inline static size_t Dim() { return 7; }
 
   /// Dimensionality of tangent space = 7 DOF
-  inline size_t dim() const {
-    return 7;
-  }
+  inline size_t dim() const { return 7; }
 
   /// @}
   /// @name Helper functions
   /// @{
 
-private:
+ private:
   /// Calculate expmap and logmap coefficients.
   static Matrix3 GetV(Vector3 w, double lambda);
 
   /// @}
 };
 
-template<>
+template <>
 inline Matrix wedge<Similarity3>(const Vector& xi) {
   return Similarity3::wedge(xi);
 }
 
-template<>
+template <>
 struct traits<Similarity3> : public internal::LieGroup<Similarity3> {};
 
-template<>
+template <>
 struct traits<const Similarity3> : public internal::LieGroup<Similarity3> {};
 
-} // namespace gtsam
+}  // namespace gtsam

From 53b370566e26501c78df4ba44e72bd16d8d92824 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Fri, 29 Apr 2022 17:53:07 -0400
Subject: [PATCH 28/35] remove Pose2/3 casts from Sim2/3

---
 gtsam/geometry/Similarity2.cpp           |  2 --
 gtsam/geometry/Similarity2.h             |  3 ---
 gtsam/geometry/Similarity3.cpp           |  4 ----
 gtsam/geometry/Similarity3.h             |  5 -----
 gtsam/geometry/tests/testSimilarity3.cpp | 24 ++++++++++++------------
 5 files changed, 12 insertions(+), 26 deletions(-)

diff --git a/gtsam/geometry/Similarity2.cpp b/gtsam/geometry/Similarity2.cpp
index e6fa73ef3f..3c780e9075 100644
--- a/gtsam/geometry/Similarity2.cpp
+++ b/gtsam/geometry/Similarity2.cpp
@@ -211,6 +211,4 @@ Matrix3 Similarity2::matrix() const {
   return T;
 }
 
-Similarity2::operator Pose2() const { return Pose2(R_, s_ * t_); }
-
 }  // namespace gtsam
diff --git a/gtsam/geometry/Similarity2.h b/gtsam/geometry/Similarity2.h
index 74ebf96409..a09dc1858b 100644
--- a/gtsam/geometry/Similarity2.h
+++ b/gtsam/geometry/Similarity2.h
@@ -156,9 +156,6 @@ class GTSAM_EXPORT Similarity2 : public LieGroup<Similarity2, 4> {
   /// Return the scale
   double scale() const { return s_; }
 
-  /// Convert to a rigid body pose (R, s*t)
-  operator Pose2() const;
-
   /// Dimensionality of tangent space = 4 DOF - used to autodetect sizes
   inline static size_t Dim() { return 4; }
 
diff --git a/gtsam/geometry/Similarity3.cpp b/gtsam/geometry/Similarity3.cpp
index 43cfaaa96f..7fde974c55 100644
--- a/gtsam/geometry/Similarity3.cpp
+++ b/gtsam/geometry/Similarity3.cpp
@@ -290,8 +290,4 @@ Matrix4 Similarity3::matrix() const {
   return T;
 }
 
-Similarity3::operator Pose3() const {
-  return Pose3(R_, s_ * t_);
-}
-
 } // namespace gtsam
diff --git a/gtsam/geometry/Similarity3.h b/gtsam/geometry/Similarity3.h
index 8799ba2447..845d4c810d 100644
--- a/gtsam/geometry/Similarity3.h
+++ b/gtsam/geometry/Similarity3.h
@@ -190,11 +190,6 @@ class GTSAM_EXPORT Similarity3 : public LieGroup<Similarity3, 7> {
   /// Return the scale
   double scale() const { return s_; }
 
-  /// Convert to a rigid body pose (R, s*t)
-  /// TODO(frank): why is this here? Red flag! Definitely don't have it as a
-  /// cast.
-  operator Pose3() const;
-
   /// Dimensionality of tangent space = 7 DOF - used to autodetect sizes
   inline static size_t Dim() { return 7; }
 
diff --git a/gtsam/geometry/tests/testSimilarity3.cpp b/gtsam/geometry/tests/testSimilarity3.cpp
index 428422072f..7a134f6efd 100644
--- a/gtsam/geometry/tests/testSimilarity3.cpp
+++ b/gtsam/geometry/tests/testSimilarity3.cpp
@@ -458,18 +458,18 @@ TEST(Similarity3, Optimization2) {
   Values result;
   result = LevenbergMarquardtOptimizer(graph, initial).optimize();
   //result.print("Optimized Estimate\n");
-  Pose3 p1, p2, p3, p4, p5;
-  p1 = Pose3(result.at<Similarity3>(X(1)));
-  p2 = Pose3(result.at<Similarity3>(X(2)));
-  p3 = Pose3(result.at<Similarity3>(X(3)));
-  p4 = Pose3(result.at<Similarity3>(X(4)));
-  p5 = Pose3(result.at<Similarity3>(X(5)));
-
-  //p1.print("Pose1");
-  //p2.print("Pose2");
-  //p3.print("Pose3");
-  //p4.print("Pose4");
-  //p5.print("Pose5");
+  Similarity3 p1, p2, p3, p4, p5;
+  p1 = result.at<Similarity3>(X(1));
+  p2 = result.at<Similarity3>(X(2));
+  p3 = result.at<Similarity3>(X(3));
+  p4 = result.at<Similarity3>(X(4));
+  p5 = result.at<Similarity3>(X(5));
+
+  //p1.print("Similarity1");
+  //p2.print("Similarity2");
+  //p3.print("Similarity3");
+  //p4.print("Similarity4");
+  //p5.print("Similarity5");
 
   Similarity3 expected(0.7);
   EXPECT(assert_equal(expected, result.at<Similarity3>(X(5)), 0.4));

From ad2e347c120f5c11b1cc5960fede4f6f41406c2f Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Fri, 29 Apr 2022 18:08:18 -0400
Subject: [PATCH 29/35] update assertions

---
 python/gtsam/tests/test_Sim2.py | 24 +++++++++++-------------
 1 file changed, 11 insertions(+), 13 deletions(-)

diff --git a/python/gtsam/tests/test_Sim2.py b/python/gtsam/tests/test_Sim2.py
index 3e39ac45c6..44ecd5a04e 100644
--- a/python/gtsam/tests/test_Sim2.py
+++ b/python/gtsam/tests/test_Sim2.py
@@ -12,9 +12,7 @@
 import unittest
 
 import numpy as np
-
-import gtsam
-from gtsam import Point2, Pose2, Pose2Pairs, Rot2, Similarity2
+from gtsam import Pose2, Pose2Pairs, Rot2, Similarity2
 from gtsam.utils.test_case import GtsamTestCase
 
 
@@ -135,10 +133,10 @@ def test_constructor(self) -> None:
         bta = np.array([1, 2])
         bsa = 3.0
         bSa = Similarity2(R=bRa, t=bta, s=bsa)
-        assert isinstance(bSa, Similarity2)
-        assert np.allclose(bSa.rotation().matrix(), bRa.matrix())
-        assert np.allclose(bSa.translation(), bta)
-        assert np.allclose(bSa.scale(), bsa)
+        self.assertIsInstance(bSa, Similarity2)
+        np.testing.assert_allclose(bSa.rotation().matrix(), bRa.matrix())
+        np.testing.assert_allcloseallclose(bSa.translation(), bta)
+        np.testing.assert_allcloseallclose(bSa.scale(), bsa)
 
     def test_is_eq(self) -> None:
         """Ensure object equality works properly (are equal)."""
@@ -150,19 +148,19 @@ def test_not_eq_translation(self) -> None:
         """Ensure object equality works properly (not equal translation)."""
         bSa = Similarity2(R=Rot2(), t=np.array([2, 1]), s=3.0)
         bSa_ = Similarity2(R=Rot2(), t=np.array([1.0, 2.0]), s=3)
-        assert bSa != bSa_
+        self.assertNotEqual(bSa, bSa_)
 
     def test_not_eq_rotation(self) -> None:
         """Ensure object equality works properly (not equal rotation)."""
         bSa = Similarity2(R=Rot2(), t=np.array([2, 1]), s=3.0)
         bSa_ = Similarity2(R=Rot2.fromDegrees(180), t=np.array([2.0, 1.0]), s=3)
-        assert bSa != bSa_
+        self.assertNotEqual(bSa, bSa_)
 
     def test_not_eq_scale(self) -> None:
         """Ensure object equality works properly (not equal scale)."""
         bSa = Similarity2(R=Rot2(), t=np.array([2, 1]), s=3.0)
         bSa_ = Similarity2(R=Rot2(), t=np.array([2.0, 1.0]), s=1.0)
-        assert bSa != bSa_
+        self.assertNotEqual(bSa, bSa_)
 
     def test_rotation(self) -> None:
         """Ensure rotation component is returned properly."""
@@ -172,7 +170,7 @@ def test_rotation(self) -> None:
 
         # evaluates to [[0, -1], [1, 0]]
         expected_R = Rot2.fromDegrees(90)
-        assert np.allclose(expected_R.matrix(), bSa.rotation().matrix())
+        np.testing.assert_allclose(expected_R.matrix(), bSa.rotation().matrix())
 
     def test_translation(self) -> None:
         """Ensure translation component is returned properly."""
@@ -181,7 +179,7 @@ def test_translation(self) -> None:
         bSa = Similarity2(R=R, t=t, s=3.0)
 
         expected_t = np.array([1, 2])
-        assert np.allclose(expected_t, bSa.translation())
+        np.testing.assert_allclose(expected_t, bSa.translation())
 
     def test_scale(self) -> None:
         """Ensure the scale factor is returned properly."""
@@ -189,7 +187,7 @@ def test_scale(self) -> None:
         bta = np.array([1, 2])
         bsa = 3.0
         bSa = Similarity2(R=bRa, t=bta, s=bsa)
-        assert bSa.scale() == 3.0
+        self.assertEqual(bSa.scale(), 3.0)
 
 
 if __name__ == "__main__":

From e704b40ab56aebe0d5c8f9d01c0215a89a7e57a9 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Fri, 29 Apr 2022 18:19:08 -0400
Subject: [PATCH 30/35] typo fix

---
 python/gtsam/tests/test_Sim2.py | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/python/gtsam/tests/test_Sim2.py b/python/gtsam/tests/test_Sim2.py
index 44ecd5a04e..ea809b9656 100644
--- a/python/gtsam/tests/test_Sim2.py
+++ b/python/gtsam/tests/test_Sim2.py
@@ -135,8 +135,8 @@ def test_constructor(self) -> None:
         bSa = Similarity2(R=bRa, t=bta, s=bsa)
         self.assertIsInstance(bSa, Similarity2)
         np.testing.assert_allclose(bSa.rotation().matrix(), bRa.matrix())
-        np.testing.assert_allcloseallclose(bSa.translation(), bta)
-        np.testing.assert_allcloseallclose(bSa.scale(), bsa)
+        np.testing.assert_allclose(bSa.translation(), bta)
+        np.testing.assert_allclose(bSa.scale(), bsa)
 
     def test_is_eq(self) -> None:
         """Ensure object equality works properly (are equal)."""

From 9eff71ea8e75b9232692251e34fcf37eea27cf68 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Fri, 29 Apr 2022 18:42:18 -0400
Subject: [PATCH 31/35] Rot2::ClosestTo with SVD

---
 gtsam/geometry/Rot2.cpp | 13 ++++++++-----
 1 file changed, 8 insertions(+), 5 deletions(-)

diff --git a/gtsam/geometry/Rot2.cpp b/gtsam/geometry/Rot2.cpp
index d43b9b12df..9bf631e50e 100644
--- a/gtsam/geometry/Rot2.cpp
+++ b/gtsam/geometry/Rot2.cpp
@@ -131,11 +131,14 @@ Rot2 Rot2::relativeBearing(const Point2& d, OptionalJacobian<1, 2> H) {
 
 /* ************************************************************************* */
 Rot2 Rot2::ClosestTo(const Matrix2& M) {
-  double c = M(0, 0);
-  double s = M(1, 0);
-  double theta_rad = std::atan2(s, c);
-  c = cos(theta_rad);
-  s = sin(theta_rad);
+  Eigen::JacobiSVD<Matrix2> svd(M, Eigen::ComputeFullU | Eigen::ComputeFullV);
+  const Matrix2& U = svd.matrixU();
+  const Matrix2& V = svd.matrixV();
+  const double det = (U * V.transpose()).determinant();
+  Matrix2 M_prime = (U * Vector2(1, det).asDiagonal() * V.transpose());
+
+  double c = M_prime(0, 0);
+  double s = M_prime(1, 0);
   return Rot2::fromCosSin(c, s);
 }
 

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 32/35] 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 3c780e9075..258039b5b0 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 a09dc1858b..0453758e0e 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;
 
   /// @}

From b8b52cb719c1bbf8570d456af3734ea72c49c3ae Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Sat, 30 Apr 2022 08:52:19 -0400
Subject: [PATCH 33/35] AdjointMap method

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

diff --git a/gtsam/geometry/Similarity2.cpp b/gtsam/geometry/Similarity2.cpp
index 258039b5b0..4b8cfd5818 100644
--- a/gtsam/geometry/Similarity2.cpp
+++ b/gtsam/geometry/Similarity2.cpp
@@ -222,6 +222,10 @@ Similarity2 Similarity2::Expmap(const Vector4& v,  //
   return Similarity2(R, t, s);
 }
 
+Matrix4 Similarity2::AdjointMap() const {
+  throw std::runtime_error("Similarity2::AdjointMap not implemented");
+}
+
 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 0453758e0e..e72cda484c 100644
--- a/gtsam/geometry/Similarity2.h
+++ b/gtsam/geometry/Similarity2.h
@@ -161,6 +161,9 @@ class GTSAM_EXPORT Similarity2 : public LieGroup<Similarity2, 4> {
     }
   };
 
+  /// Project from one tangent space to another
+  Matrix4 AdjointMap() const;
+
   using LieGroup<Similarity2, 4>::inverse;
 
   /// @}

From a73d6e0f47567b6f79cadb32fd402243f4214ce6 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Sat, 30 Apr 2022 15:21:05 -0400
Subject: [PATCH 34/35] Sim3 cpp unit tests

---
 gtsam/geometry/tests/testSimilarity2.cpp | 66 ++++++++++++++++++++++++
 1 file changed, 66 insertions(+)
 create mode 100644 gtsam/geometry/tests/testSimilarity2.cpp

diff --git a/gtsam/geometry/tests/testSimilarity2.cpp b/gtsam/geometry/tests/testSimilarity2.cpp
new file mode 100644
index 0000000000..f60e8f55e7
--- /dev/null
+++ b/gtsam/geometry/tests/testSimilarity2.cpp
@@ -0,0 +1,66 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation,
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
+/**
+ * @file   testSimilarity2.cpp
+ * @brief  Unit tests for Similarity2 class
+ * @author Varun Agrawal
+ */
+
+#include <CppUnitLite/TestHarness.h>
+#include <gtsam/base/Testable.h>
+#include <gtsam/base/numericalDerivative.h>
+#include <gtsam/base/testLie.h>
+#include <gtsam/geometry/Similarity2.h>
+
+#include <functional>
+
+using namespace std::placeholders;
+using namespace gtsam;
+using namespace std;
+using symbol_shorthand::X;
+
+GTSAM_CONCEPT_TESTABLE_INST(Similarity2)
+
+static const Point2 P(0.2, 0.7);
+static const Rot2 R = Rot2::fromAngle(0.3);
+
+const double degree = M_PI / 180;
+
+//******************************************************************************
+TEST(Similarity2, Concepts) {
+  BOOST_CONCEPT_ASSERT((IsGroup<Similarity2>));
+  BOOST_CONCEPT_ASSERT((IsManifold<Similarity2>));
+  BOOST_CONCEPT_ASSERT((IsLieGroup<Similarity2>));
+}
+
+//******************************************************************************
+TEST(Similarity2, Constructors) {
+  Similarity2 sim2_Construct1;
+  Similarity2 sim2_Construct2(s);
+  Similarity2 sim2_Construct3(R, P, s);
+  Similarity2 sim2_Construct4(R.matrix(), P, s);
+}
+
+//******************************************************************************
+TEST(Similarity2, Getters) {
+  Similarity2 sim2_default;
+  EXPECT(assert_equal(Rot2(), sim2_default.rotation()));
+  EXPECT(assert_equal(Point2(0, 0), sim2_default.translation()));
+  EXPECT_DOUBLES_EQUAL(1.0, sim2_default.scale(), 1e-9);
+}
+
+//******************************************************************************
+int main() {
+  TestResult tr;
+  return TestRegistry::runAllTests(tr);
+}
+//******************************************************************************

From 12dd2ecc47d2ee967b0bd16296dc33f36239cb50 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Sat, 30 Apr 2022 16:41:14 -0400
Subject: [PATCH 35/35] identity needs to be lowercase and fix tests

---
 gtsam/geometry/Similarity2.cpp           | 2 +-
 gtsam/geometry/Similarity2.h             | 2 +-
 gtsam/geometry/tests/testSimilarity2.cpp | 2 +-
 3 files changed, 3 insertions(+), 3 deletions(-)

diff --git a/gtsam/geometry/Similarity2.cpp b/gtsam/geometry/Similarity2.cpp
index 4b8cfd5818..4ed3351f8b 100644
--- a/gtsam/geometry/Similarity2.cpp
+++ b/gtsam/geometry/Similarity2.cpp
@@ -134,7 +134,7 @@ void Similarity2::print(const std::string& s) const {
             << std::endl;
 }
 
-Similarity2 Similarity2::Identity() { return Similarity2(); }
+Similarity2 Similarity2::identity() { return Similarity2(); }
 
 Similarity2 Similarity2::operator*(const Similarity2& S) const {
   return Similarity2(R_ * S.R_, ((1.0 / S.s_) * t_) + R_ * S.t_, s_ * S.s_);
diff --git a/gtsam/geometry/Similarity2.h b/gtsam/geometry/Similarity2.h
index e72cda484c..05f10d1493 100644
--- a/gtsam/geometry/Similarity2.h
+++ b/gtsam/geometry/Similarity2.h
@@ -83,7 +83,7 @@ class GTSAM_EXPORT Similarity2 : public LieGroup<Similarity2, 4> {
   /// @{
 
   /// Return an identity transform
-  static Similarity2 Identity();
+  static Similarity2 identity();
 
   /// Composition
   Similarity2 operator*(const Similarity2& S) const;
diff --git a/gtsam/geometry/tests/testSimilarity2.cpp b/gtsam/geometry/tests/testSimilarity2.cpp
index f60e8f55e7..dd4fd0efd1 100644
--- a/gtsam/geometry/tests/testSimilarity2.cpp
+++ b/gtsam/geometry/tests/testSimilarity2.cpp
@@ -26,12 +26,12 @@
 using namespace std::placeholders;
 using namespace gtsam;
 using namespace std;
-using symbol_shorthand::X;
 
 GTSAM_CONCEPT_TESTABLE_INST(Similarity2)
 
 static const Point2 P(0.2, 0.7);
 static const Rot2 R = Rot2::fromAngle(0.3);
+static const double s = 4;
 
 const double degree = M_PI / 180;