diff --git a/gtsam/geometry/Pose2.h b/gtsam/geometry/Pose2.h
index 12087a34c..eb8ddfb5b 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 9f62869e4..d43b9b12d 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 42dba3487..2690ca248 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);
 
diff --git a/gtsam/geometry/Similarity2.cpp b/gtsam/geometry/Similarity2.cpp
index 5a63e70cb..6e5da635b 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 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 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");
+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);
+  // 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);
-// }
+  return internal::alignGivenR(abPointPairs, aRb_estimate);
+}
 
-std::ostream &operator<<(std::ostream &os, const Similarity2& p) {
-  os << "[" << p.rotation().theta() << " "
-      << p.translation().transpose() << " " << p.scale() << "]\';";
+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 85a6324c5..93a1227f5 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 @@ public:
   /// 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 @@ public:
   /// 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 @@ public:
   /// 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 @@ public:
   /* 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 @@ public:
   /// @{
 
   /// 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
diff --git a/gtsam/geometry/Similarity3.cpp b/gtsam/geometry/Similarity3.cpp
index e8d6e7510..43cfaaa96 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) {
@@ -81,10 +81,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) {
@@ -165,11 +165,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) {
@@ -192,7 +192,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) {
@@ -283,7 +283,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 0ef787b05..27fdba6d7 100644
--- a/gtsam/geometry/Similarity3.h
+++ b/gtsam/geometry/Similarity3.h
@@ -180,15 +180,15 @@ public:
   /// @{
 
   /// 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_;
   }
 
diff --git a/gtsam/geometry/geometry.i b/gtsam/geometry/geometry.i
index e77458696..350f23d18 100644
--- a/gtsam/geometry/geometry.i
+++ b/gtsam/geometry/geometry.i
@@ -862,9 +862,10 @@ class Similarity2 {
   static gtsam::Similarity2 Align(const gtsam::Pose2Pairs& abPosePairs);
 
   // Standard Interface
-  const Matrix matrix() const;
-  const gtsam::Rot2& rotation();
-  const gtsam::Point2& translation();
+  bool equals(const gtsam::Similarity2& sim, double tol) const;
+  Matrix matrix() const;
+  gtsam::Rot2& rotation();
+  gtsam::Point2& translation();
   double scale() const;
 };
 
@@ -884,9 +885,10 @@ class Similarity3 {
   static gtsam::Similarity3 Align(const gtsam::Pose3Pairs& abPosePairs);
 
   // Standard Interface
-  const Matrix matrix() const;
-  const gtsam::Rot3& rotation();
-  const gtsam::Point3& translation();
+  bool equals(const gtsam::Similarity3& sim, double tol) const;
+  Matrix matrix() const;
+  gtsam::Rot3& rotation();
+  gtsam::Point3& translation();
   double scale() const;
 };
 
diff --git a/gtsam/navigation/tests/testGPSFactor.cpp b/gtsam/navigation/tests/testGPSFactor.cpp
index c94e1d3d5..5607add16 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,7 +70,7 @@ TEST( GPSFactor, Constructor ) {
   EXPECT(assert_equal(Z_3x1,factor.evaluateError(T),1e-5));
 
   // Calculate numerical derivatives
-  Matrix expectedH = numericalDerivative11<Vector,Pose3>(
+  Matrix expectedH = numericalDerivative11<Vector, Pose3>(
       std::bind(&GPSFactor::evaluateError, &factor, std::placeholders::_1, boost::none), T);
 
   // Use the factor to calculate the derivative
@@ -100,7 +99,7 @@ TEST( GPSFactor2, Constructor ) {
   EXPECT(assert_equal(Z_3x1,factor.evaluateError(T),1e-5));
 
   // Calculate numerical derivatives
-  Matrix expectedH = numericalDerivative11<Vector,NavState>(
+  Matrix expectedH = numericalDerivative11<Vector, NavState>(
       std::bind(&GPSFactor2::evaluateError, &factor, std::placeholders::_1, boost::none), T);
 
   // Use the factor to calculate the derivative
diff --git a/gtsam/navigation/tests/testMagFactor.cpp b/gtsam/navigation/tests/testMagFactor.cpp
index 85447facd..ac0bbbc20 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,7 +62,7 @@ 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> //
+  EXPECT(assert_equal(numericalDerivative11<Point3, Rot2> //
       (std::bind(&MagFactor::unrotate, std::placeholders::_1, nM, none), theta), H, 1e-6));
 }
 
@@ -74,27 +73,27 @@ 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> //
+  EXPECT(assert_equal(Z_3x1,f.evaluateError(theta,H1),1e-5));
+  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> //
+  EXPECT(assert_equal(Z_3x1,f1.evaluateError(nRb,H1),1e-5));
+  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> //
+  EXPECT(assert_equal(Z_3x1,f2.evaluateError(scaled,bias,H1,H2),1e-5));
+  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> //
+  EXPECT(assert_equal(numericalDerivative11<Vector, Point3> //
       (std::bind(&MagFactor2::evaluateError, &f2, scaled, std::placeholders::_1, none, none), bias),//
       H2, 1e-7));
 
-// MagFactor2
+  // MagFactor3
   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> //
diff --git a/gtsam/slam/KarcherMeanFactor-inl.h b/gtsam/slam/KarcherMeanFactor-inl.h
index c81a9adc5..00f741705 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);
 }
diff --git a/python/gtsam/preamble/geometry.h b/python/gtsam/preamble/geometry.h
index 35fe2a577..bd0441d06 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 a492ce8eb..5a0ea35ef 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>>>(
diff --git a/python/gtsam/tests/test_Sim2.py b/python/gtsam/tests/test_Sim2.py
index 67bc770d2..3e39ac45c 100644
--- a/python/gtsam/tests/test_Sim2.py
+++ b/python/gtsam/tests/test_Sim2.py
@@ -27,10 +27,10 @@ class TestSim2(GtsamTestCase):
         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 @@ class TestSim2(GtsamTestCase):
         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 @@ class TestSim2(GtsamTestCase):
         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 = Rot3.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 @@ class TestSim2(GtsamTestCase):
 
         # 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]
 
@@ -107,10 +107,10 @@ class TestSim2(GtsamTestCase):
         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 @@ class TestSim2(GtsamTestCase):
         """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)."""