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/78] 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 000000000..120e6690a
--- /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/78] 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 000000000..7970b3190
--- /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/78] 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 9baa49e8e..151c42155 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/78] 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 000000000..67bc770d2
--- /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/78] 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 7970b3190..ed6635233 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/78] 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 a54951728..12087a34c 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/78] 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 ed6635233..9c051a313 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/78] 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 cdb9f4480..d8b6daca8 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/78] 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 d8060cfcf..06568e37c 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/78] 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 06568e37c..06c32526b 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/78] 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 ec30c6657..42dba3487 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/78] 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 283147e4c..f8ec9c9e6 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/78] 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 f8ec9c9e6..9f62869e4 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/78] 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 9c051a313..1ed0dd1b0 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/78] fix Eigen error

---
 gtsam/geometry/Similarity2.cpp | 83 +++++++++++++++++-----------------
 gtsam/geometry/Similarity2.h   | 54 +++++++++++-----------
 2 files changed, 69 insertions(+), 68 deletions(-)

diff --git a/gtsam/geometry/Similarity2.cpp b/gtsam/geometry/Similarity2.cpp
index 1ed0dd1b0..5a63e70cb 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 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");
+// 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);
+//   // 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 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 120e6690a..85a6324c5 100644
--- a/gtsam/geometry/Similarity2.h
+++ b/gtsam/geometry/Similarity2.h
@@ -60,11 +60,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
@@ -94,9 +94,9 @@ 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;
@@ -114,25 +114,25 @@ public:
    */
   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 @@ public:
 };
 
 
-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/78] 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 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);
 

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/78] Similarity2 fixes

---
 gtsam/geometry/Similarity2.cpp | 180 ++++++++++++++++++---------------
 gtsam/geometry/Similarity2.h   | 110 +++++++++-----------
 2 files changed, 146 insertions(+), 144 deletions(-)

diff --git a/gtsam/geometry/Similarity2.cpp b/gtsam/geometry/Similarity2.cpp
index 5a63e70cb..12529f52d 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

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/78] 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 fcaf0c874..58bace0f1 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 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_;
   }
 

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/78] 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 151c42155..6ad7baedd 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/78] fix tests

---
 gtsam/navigation/tests/testGPSFactor.cpp | 13 ++++---
 gtsam/navigation/tests/testMagFactor.cpp | 49 +++++++++++++++---------
 2 files changed, 38 insertions(+), 24 deletions(-)

diff --git a/gtsam/navigation/tests/testGPSFactor.cpp b/gtsam/navigation/tests/testGPSFactor.cpp
index b784c0c94..5b96b80a8 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 5107b3b6b..2450b16c7 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));
+  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
+  // 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/78] 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 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>>>(

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/78] 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 f872283f3..350f23d18 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/78] 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 67bc770d2..1d09dcc3b 100644
--- a/python/gtsam/tests/test_Sim2.py
+++ b/python/gtsam/tests/test_Sim2.py
@@ -65,7 +65,7 @@ class TestSim2(GtsamTestCase):
            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 @@ 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)."""

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/78] 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 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);
 }

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/78] 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 12529f52d..6e5da635b 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 1d09dcc3b..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 = 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 @@ 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]
 

From fb3f42f7f67cfc7420b1f89bc361037fd7f57bd6 Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@gmail.com>
Date: Sun, 27 Mar 2022 11:24:45 -0400
Subject: [PATCH 26/78] Bump version

---
 CMakeLists.txt | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/CMakeLists.txt b/CMakeLists.txt
index b86598663..cfb251663 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -10,7 +10,7 @@ endif()
 set (GTSAM_VERSION_MAJOR 4)
 set (GTSAM_VERSION_MINOR 2)
 set (GTSAM_VERSION_PATCH 0)
-set (GTSAM_PRERELEASE_VERSION "a5")
+set (GTSAM_PRERELEASE_VERSION "a6")
 math (EXPR GTSAM_VERSION_NUMERIC "10000 * ${GTSAM_VERSION_MAJOR} + 100 * ${GTSAM_VERSION_MINOR} + ${GTSAM_VERSION_PATCH}")
 
 if (${GTSAM_VERSION_PATCH} EQUAL 0)

From d5d5ecc3b3fdd954253578e8fdadfb5b5930c36d Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Thu, 31 Mar 2022 06:10:40 -0400
Subject: [PATCH 27/78] refactor DecisionTree to make a distinction between
 leaves and assignments

---
 gtsam/discrete/DecisionTree-inl.h | 106 +++++++++++++++++-------------
 gtsam/discrete/DecisionTree.h     |  37 ++++++-----
 2 files changed, 80 insertions(+), 63 deletions(-)

diff --git a/gtsam/discrete/DecisionTree-inl.h b/gtsam/discrete/DecisionTree-inl.h
index 826e54b95..48da55ce1 100644
--- a/gtsam/discrete/DecisionTree-inl.h
+++ b/gtsam/discrete/DecisionTree-inl.h
@@ -59,7 +59,7 @@ namespace gtsam {
     /** constant stored in this leaf */
     Y constant_;
 
-    /** The number of assignments contained within this leaf
+    /** The number of assignments contained within this leaf.
      * Particularly useful when leaves have been pruned.
      */
     size_t nrAssignments_;
@@ -68,7 +68,7 @@ namespace gtsam {
     Leaf(const Y& constant, size_t nrAssignments = 1)
         : constant_(constant), nrAssignments_(nrAssignments) {}
 
-    /** return the constant */
+    /// Return the constant
     const Y& constant() const {
       return constant_;
     }
@@ -81,19 +81,19 @@ namespace gtsam {
       return constant_ == q.constant_;
     }
 
-    /// polymorphic equality: is q is a leaf, could be
+    /// polymorphic equality: is q a leaf and is it the same as this leaf?
     bool sameLeaf(const Node& q) const override {
       return (q.isLeaf() && q.sameLeaf(*this));
     }
 
-    /** equality up to tolerance */
+    /// equality up to tolerance
     bool equals(const Node& q, const CompareFunc& compare) const override {
       const Leaf* other = dynamic_cast<const Leaf*>(&q);
       if (!other) return false;
       return compare(this->constant_, other->constant_);
     }
 
-    /** print */
+    /// print
     void print(const std::string& s, const LabelFormatter& labelFormatter,
                const ValueFormatter& valueFormatter) const override {
       std::cout << s << " Leaf " << valueFormatter(constant_) << std::endl;
@@ -122,8 +122,8 @@ namespace gtsam {
 
     /// Apply unary operator with assignment
     NodePtr apply(const UnaryAssignment& op,
-                  const Assignment<L>& choices) const override {
-      NodePtr f(new Leaf(op(choices, constant_), nrAssignments_));
+                  const Assignment<L>& assignment) const override {
+      NodePtr f(new Leaf(op(assignment, constant_), nrAssignments_));
       return f;
     }
 
@@ -168,7 +168,10 @@ namespace gtsam {
     std::vector<NodePtr> branches_;
 
    private:
-    /** incremental allSame */
+    /**
+     * Incremental allSame.
+     * Records if all the branches are the same leaf.
+     */
     size_t allSame_;
 
     using ChoicePtr = boost::shared_ptr<const Choice>;
@@ -181,7 +184,7 @@ namespace gtsam {
 #endif
     }
 
-    /** If all branches of a choice node f are the same, just return a branch */
+    /// If all branches of a choice node f are the same, just return a branch.
     static NodePtr Unique(const ChoicePtr& f) {
 #ifndef DT_NO_PRUNING
       if (f->allSame_) {
@@ -205,15 +208,13 @@ namespace gtsam {
 
     bool isLeaf() const override { return false; }
 
-    /** Constructor, given choice label and mandatory expected branch count */
+    /// Constructor, given choice label and mandatory expected branch count.
     Choice(const L& label, size_t count) :
       label_(label), allSame_(true) {
       branches_.reserve(count);
     }
 
-    /**
-     * Construct from applying binary op to two Choice nodes
-     */
+    /// Construct from applying binary op to two Choice nodes.
     Choice(const Choice& f, const Choice& g, const Binary& op) :
       allSame_(true) {
       // Choose what to do based on label
@@ -241,6 +242,7 @@ namespace gtsam {
       }
     }
 
+    /// Return the label of this choice node.
     const L& label() const {
       return label_;
     }
@@ -262,7 +264,7 @@ namespace gtsam {
       branches_.push_back(node);
     }
 
-    /** print (as a tree) */
+    /// print (as a tree).
     void print(const std::string& s, const LabelFormatter& labelFormatter,
                const ValueFormatter& valueFormatter) const override {
       std::cout << s << " Choice(";
@@ -308,7 +310,7 @@ namespace gtsam {
       return (q.isLeaf() && q.sameLeaf(*this));
     }
 
-    /** equality */
+    /// equality
     bool equals(const Node& q, const CompareFunc& compare) const override {
       const Choice* other = dynamic_cast<const Choice*>(&q);
       if (!other) return false;
@@ -321,7 +323,7 @@ namespace gtsam {
       return true;
     }
 
-    /** evaluate */
+    /// evaluate
     const Y& operator()(const Assignment<L>& x) const override {
 #ifndef NDEBUG
       typename Assignment<L>::const_iterator it = x.find(label_);
@@ -336,13 +338,13 @@ namespace gtsam {
       return (*child)(x);
     }
 
-    /**
-     * Construct from applying unary op to a Choice node
-     */
+    /// Construct from applying unary op to a Choice node.
     Choice(const L& label, const Choice& f, const Unary& op) :
       label_(label), allSame_(true) {
       branches_.reserve(f.branches_.size());  // reserve space
-      for (const NodePtr& branch : f.branches_) push_back(branch->apply(op));
+      for (const NodePtr& branch : f.branches_) {
+        push_back(branch->apply(op));
+      }
     }
 
     /**
@@ -353,28 +355,28 @@ namespace gtsam {
      * @param f The original choice node to apply the op on.
      * @param op Function to apply on the choice node. Takes Assignment and
      * value as arguments.
-     * @param choices The Assignment that will go to op.
+     * @param assignment The Assignment that will go to op.
      */
     Choice(const L& label, const Choice& f, const UnaryAssignment& op,
-           const Assignment<L>& choices)
+           const Assignment<L>& assignment)
         : label_(label), allSame_(true) {
       branches_.reserve(f.branches_.size());  // reserve space
 
-      Assignment<L> choices_ = choices;
+      Assignment<L> assignment_ = assignment;
 
       for (size_t i = 0; i < f.branches_.size(); i++) {
-        choices_[label_] = i;  // Set assignment for label to i
+        assignment_[label_] = i;  // Set assignment for label to i
 
         const NodePtr branch = f.branches_[i];
-        push_back(branch->apply(op, choices_));
+        push_back(branch->apply(op, assignment_));
 
-        // Remove the choice so we are backtracking
-        auto choice_it = choices_.find(label_);
-        choices_.erase(choice_it);
+        // Remove the assignment so we are backtracking
+        auto assignment_it = assignment_.find(label_);
+        assignment_.erase(assignment_it);
       }
     }
 
-    /** apply unary operator */
+    /// apply unary operator.
     NodePtr apply(const Unary& op) const override {
       auto r = boost::make_shared<Choice>(label_, *this, op);
       return Unique(r);
@@ -382,8 +384,8 @@ namespace gtsam {
 
     /// Apply unary operator with assignment
     NodePtr apply(const UnaryAssignment& op,
-                  const Assignment<L>& choices) const override {
-      auto r = boost::make_shared<Choice>(label_, *this, op, choices);
+                  const Assignment<L>& assignment) const override {
+      auto r = boost::make_shared<Choice>(label_, *this, op, assignment);
       return Unique(r);
     }
 
@@ -678,7 +680,14 @@ namespace gtsam {
   }
 
   /****************************************************************************/
-  // Functor performing depth-first visit without Assignment<L> argument.
+  /**
+   * Functor performing depth-first visit without Assignment<L> argument.
+   *
+   * NOTE: We differentiate between leaves and assignments. Concretely, a 3
+   * binary variable tree will have 2^3=8 assignments, but based on pruning, it
+   * can have <8 leaves. For example, if a tree has all assignment values as 1,
+   * then pruning will cause the tree to have only 1 leaf yet 8 assignments.
+   */
   template <typename L, typename Y>
   struct Visit {
     using F = std::function<void(const Y&)>;
@@ -707,33 +716,36 @@ namespace gtsam {
   }
 
   /****************************************************************************/
-  // Functor performing depth-first visit with Assignment<L> argument.
+  /**
+   * Functor performing depth-first visit with Assignment<L> argument.
+   *
+   * NOTE: Follows the same pruning semantics as `visit`.
+   */
   template <typename L, typename Y>
   struct VisitWith {
-    using Choices = Assignment<L>;
-    using F = std::function<void(const Choices&, const Y&)>;
+    using F = std::function<void(const Assignment<L>&, const Y&)>;
     explicit VisitWith(F f) : f(f) {}  ///< Construct from folding function.
-    Choices choices;  ///< Assignment, mutating through recursion.
-    F f;              ///< folding function object.
+    Assignment<L> assignment;  ///< Assignment, mutating through recursion.
+    F f;                       ///< folding function object.
 
     /// Do a depth-first visit on the tree rooted at node.
     void operator()(const typename DecisionTree<L, Y>::NodePtr& node) {
       using Leaf = typename DecisionTree<L, Y>::Leaf;
       if (auto leaf = boost::dynamic_pointer_cast<const Leaf>(node))
-        return f(choices, leaf->constant());
+        return f(assignment, leaf->constant());
 
       using Choice = typename DecisionTree<L, Y>::Choice;
       auto choice = boost::dynamic_pointer_cast<const Choice>(node);
       if (!choice)
         throw std::invalid_argument("DecisionTree::VisitWith: Invalid NodePtr");
       for (size_t i = 0; i < choice->nrChoices(); i++) {
-        choices[choice->label()] = i;  // Set assignment for label to i
+        assignment[choice->label()] = i;  // Set assignment for label to i
 
         (*this)(choice->branches()[i]);  // recurse!
 
         // Remove the choice so we are backtracking
-        auto choice_it = choices.find(choice->label());
-        choices.erase(choice_it);
+        auto choice_it = assignment.find(choice->label());
+        assignment.erase(choice_it);
       }
     }
   };
@@ -763,12 +775,14 @@ namespace gtsam {
   }
 
   /****************************************************************************/
-  // labels is just done with a visit
+  // Get (partial) labels by performing a visit.
   template <typename L, typename Y>
   std::set<L> DecisionTree<L, Y>::labels() const {
     std::set<L> unique;
-    auto f = [&](const Assignment<L>& choices, const Y&) {
-      for (auto&& kv : choices) unique.insert(kv.first);
+    auto f = [&](const Assignment<L>& assignment, const Y&) {
+      for (auto&& kv : assignment) {
+        unique.insert(kv.first);
+      }
     };
     visitWith(f);
     return unique;
@@ -817,8 +831,8 @@ namespace gtsam {
       throw std::runtime_error(
           "DecisionTree::apply(unary op) undefined for empty tree.");
     }
-    Assignment<L> choices;
-    return DecisionTree(root_->apply(op, choices));
+    Assignment<L> assignment;
+    return DecisionTree(root_->apply(op, assignment));
   }
 
   /****************************************************************************/
diff --git a/gtsam/discrete/DecisionTree.h b/gtsam/discrete/DecisionTree.h
index c0a2a7a1c..9520d43bc 100644
--- a/gtsam/discrete/DecisionTree.h
+++ b/gtsam/discrete/DecisionTree.h
@@ -105,7 +105,7 @@ namespace gtsam {
       virtual const Y& operator()(const Assignment<L>& x) const = 0;
       virtual Ptr apply(const Unary& op) const = 0;
       virtual Ptr apply(const UnaryAssignment& op,
-                        const Assignment<L>& choices) const = 0;
+                        const Assignment<L>& assignment) const = 0;
       virtual Ptr apply_f_op_g(const Node&, const Binary&) const = 0;
       virtual Ptr apply_g_op_fL(const Leaf&, const Binary&) const = 0;
       virtual Ptr apply_g_op_fC(const Choice&, const Binary&) const = 0;
@@ -153,7 +153,7 @@ namespace gtsam {
     /** Create a constant */
     explicit DecisionTree(const Y& y);
 
-    /** Create a new leaf function splitting on a variable */
+    /// Create tree with 2 assignments `y1`, `y2`, splitting on variable `label`
     DecisionTree(const L& label, const Y& y1, const Y& y2);
 
     /** Allow Label+Cardinality for convenience */
@@ -219,9 +219,8 @@ namespace gtsam {
     /// @name Standard Interface
     /// @{
 
-    /** Make virtual */
-    virtual ~DecisionTree() {
-    }
+    /// Make virtual
+    virtual ~DecisionTree() {}
 
     /// Check if tree is empty.
     bool empty() const { return !root_; }
@@ -234,11 +233,13 @@ namespace gtsam {
 
     /**
      * @brief Visit all leaves in depth-first fashion.
-     * 
-     * @param f side-effect taking a value.
-     * 
-     * @note Due to pruning, leaves might not exhaust choices.
-     * 
+     *
+     * @param f (side-effect) Function taking a value.
+     *
+     * @note Due to pruning, the number of leaves may not be the same as the
+     * number of assignments. E.g. if we have a tree on 2 binary variables with
+     * all values being 1, then there are 2^2=4 assignments, but only 1 leaf.
+     *
      * Example:
      *   int sum = 0;
      *   auto visitor = [&](int y) { sum += y; };
@@ -249,14 +250,16 @@ namespace gtsam {
 
     /**
      * @brief Visit all leaves in depth-first fashion.
-     * 
-     * @param f side-effect taking an assignment and a value.
-     * 
-     * @note Due to pruning, leaves might not exhaust choices.
-     * 
+     *
+     * @param f (side-effect) Function taking an assignment and a value.
+     *
+     * @note Due to pruning, the number of leaves may not be the same as the
+     * number of assignments. E.g. if we have a tree on 2 binary variables with
+     * all values being 1, then there are 2^2=4 assignments, but only 1 leaf.
+     *
      * Example:
      *   int sum = 0;
-     *   auto visitor = [&](const Assignment<L>& choices, int y) { sum += y; };
+     *   auto visitor = [&](const Assignment<L>& assignment, int y) { sum += y; };
      *   tree.visitWith(visitor);
      */
     template <typename Func>
@@ -275,7 +278,7 @@ namespace gtsam {
      * 
      * @note X is always passed by value.
      * @note Due to pruning, leaves might not exhaust choices.
-     * 
+     *
      * Example:
      *   auto add = [](const double& y, double x) { return y + x; };
      *   double sum = tree.fold(add, 0.0);

From e81e04acf57747a9442d297292049189716d9a2c Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Thu, 31 Mar 2022 06:35:59 -0400
Subject: [PATCH 28/78] convert DT_NO_PRUNING flag to GTSAM_DT_NO_PRUNING in
 case we wish to expose it via cmake

---
 gtsam/discrete/DecisionTree-inl.h                  | 2 +-
 gtsam/discrete/tests/testAlgebraicDecisionTree.cpp | 2 +-
 gtsam/discrete/tests/testDecisionTree.cpp          | 2 +-
 3 files changed, 3 insertions(+), 3 deletions(-)

diff --git a/gtsam/discrete/DecisionTree-inl.h b/gtsam/discrete/DecisionTree-inl.h
index 48da55ce1..ed345461c 100644
--- a/gtsam/discrete/DecisionTree-inl.h
+++ b/gtsam/discrete/DecisionTree-inl.h
@@ -186,7 +186,7 @@ namespace gtsam {
 
     /// If all branches of a choice node f are the same, just return a branch.
     static NodePtr Unique(const ChoicePtr& f) {
-#ifndef DT_NO_PRUNING
+#ifndef GTSAM_DT_NO_PRUNING
       if (f->allSame_) {
         assert(f->branches().size() > 0);
         NodePtr f0 = f->branches_[0];
diff --git a/gtsam/discrete/tests/testAlgebraicDecisionTree.cpp b/gtsam/discrete/tests/testAlgebraicDecisionTree.cpp
index c800321d6..6a3fb2388 100644
--- a/gtsam/discrete/tests/testAlgebraicDecisionTree.cpp
+++ b/gtsam/discrete/tests/testAlgebraicDecisionTree.cpp
@@ -20,7 +20,7 @@
 #include <gtsam/discrete/DiscreteKey.h>  // make sure we have traits
 #include <gtsam/discrete/DiscreteValues.h>
 // headers first to make sure no missing headers
-//#define DT_NO_PRUNING
+//#define GTSAM_DT_NO_PRUNING
 #include <gtsam/discrete/AlgebraicDecisionTree.h>
 #include <gtsam/discrete/DecisionTree-inl.h>  // for convert only
 #define DISABLE_TIMING
diff --git a/gtsam/discrete/tests/testDecisionTree.cpp b/gtsam/discrete/tests/testDecisionTree.cpp
index 14cf307a5..5ccbcf916 100644
--- a/gtsam/discrete/tests/testDecisionTree.cpp
+++ b/gtsam/discrete/tests/testDecisionTree.cpp
@@ -18,7 +18,7 @@
  */
 
 // #define DT_DEBUG_MEMORY
-// #define DT_NO_PRUNING
+// #define GTSAM_DT_NO_PRUNING
 #define DISABLE_DOT
 #include <gtsam/discrete/DecisionTree-inl.h>
 

From 039ecfc3c3bf67232ffe4da524e02ba65018fe4c Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Thu, 31 Mar 2022 10:03:23 -0400
Subject: [PATCH 29/78] add new visitLeaf method that provides the leaf as the
 function argument

---
 gtsam/discrete/DecisionTree-inl.h | 36 +++++++++++++++++++++++++++++++
 gtsam/discrete/DecisionTree.h     | 17 +++++++++++++++
 2 files changed, 53 insertions(+)

diff --git a/gtsam/discrete/DecisionTree-inl.h b/gtsam/discrete/DecisionTree-inl.h
index ed345461c..b3ad66721 100644
--- a/gtsam/discrete/DecisionTree-inl.h
+++ b/gtsam/discrete/DecisionTree-inl.h
@@ -715,6 +715,42 @@ namespace gtsam {
     visit(root_);
   }
 
+  /****************************************************************************/
+  /**
+   * Functor performing depth-first visit with Leaf argument.
+   *
+   * NOTE: We differentiate between leaves and assignments. Concretely, a 3
+   * binary variable tree will have 2^3=8 assignments, but based on pruning, it
+   * can have <8 leaves. For example, if a tree has all assignment values as 1,
+   * then pruning will cause the tree to have only 1 leaf yet 8 assignments.
+   */
+  template <typename L, typename Y>
+  struct VisitLeaf {
+    using F = std::function<void(const typename DecisionTree<L, Y>::Leaf&)>;
+    explicit VisitLeaf(F f) : f(f) {}  ///< Construct from folding function.
+    F f;                           ///< folding function object.
+
+    /// Do a depth-first visit on the tree rooted at node.
+    void operator()(const typename DecisionTree<L, Y>::NodePtr& node) const {
+      using Leaf = typename DecisionTree<L, Y>::Leaf;
+      if (auto leaf = boost::dynamic_pointer_cast<const Leaf>(node))
+        return f(*leaf);
+
+      using Choice = typename DecisionTree<L, Y>::Choice;
+      auto choice = boost::dynamic_pointer_cast<const Choice>(node);
+      if (!choice)
+        throw std::invalid_argument("DecisionTree::VisitLeaf: Invalid NodePtr");
+      for (auto&& branch : choice->branches()) (*this)(branch);  // recurse!
+    }
+  };
+
+  template <typename L, typename Y>
+  template <typename Func>
+  void DecisionTree<L, Y>::visitLeaf(Func f) const {
+    VisitLeaf<L, Y> visit(f);
+    visit(root_);
+  }
+
   /****************************************************************************/
   /**
    * Functor performing depth-first visit with Assignment<L> argument.
diff --git a/gtsam/discrete/DecisionTree.h b/gtsam/discrete/DecisionTree.h
index 9520d43bc..1f45d320b 100644
--- a/gtsam/discrete/DecisionTree.h
+++ b/gtsam/discrete/DecisionTree.h
@@ -248,6 +248,23 @@ namespace gtsam {
     template <typename Func>
     void visit(Func f) const;
 
+    /**
+     * @brief Visit all leaves in depth-first fashion.
+     *
+     * @param f (side-effect) Function taking the leaf node pointer.
+     *
+     * @note Due to pruning, the number of leaves may not be the same as the
+     * number of assignments. E.g. if we have a tree on 2 binary variables with
+     * all values being 1, then there are 2^2=4 assignments, but only 1 leaf.
+     *
+     * Example:
+     *   int sum = 0;
+     *   auto visitor = [&](int y) { sum += y; };
+     *   tree.visitWith(visitor);
+     */
+    template <typename Func>
+    void visitLeaf(Func f) const;
+
     /**
      * @brief Visit all leaves in depth-first fashion.
      *

From dac84e99321f992620a1004e9f873b4c14c85024 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Thu, 31 Mar 2022 10:04:00 -0400
Subject: [PATCH 30/78] update prune to new max number of assignments scheme

---
 gtsam/discrete/DecisionTreeFactor.cpp | 10 +++++++---
 gtsam/discrete/DecisionTreeFactor.h   |  7 ++++---
 2 files changed, 11 insertions(+), 6 deletions(-)

diff --git a/gtsam/discrete/DecisionTreeFactor.cpp b/gtsam/discrete/DecisionTreeFactor.cpp
index acd4d4af2..4e16fc689 100644
--- a/gtsam/discrete/DecisionTreeFactor.cpp
+++ b/gtsam/discrete/DecisionTreeFactor.cpp
@@ -287,12 +287,16 @@ namespace gtsam {
         cardinalities_(keys.cardinalities()) {}
 
   /* ************************************************************************ */
-  DecisionTreeFactor DecisionTreeFactor::prune(size_t maxNrLeaves) const {
-    const size_t N = maxNrLeaves;
+  DecisionTreeFactor DecisionTreeFactor::prune(size_t maxNrAssignments) const {
+    const size_t N = maxNrAssignments;
 
     // Get the probabilities in the decision tree so we can threshold.
     std::vector<double> probabilities;
-    this->visit([&](const double& prob) { probabilities.emplace_back(prob); });
+    this->visitLeaf([&](const Leaf& leaf) {
+      size_t nrAssignments = leaf.nrAssignments();
+      double prob = leaf.constant();
+      probabilities.insert(probabilities.end(), nrAssignments, prob);
+    });
 
     // The number of probabilities can be lower than max_leaves
     if (probabilities.size() <= N) {
diff --git a/gtsam/discrete/DecisionTreeFactor.h b/gtsam/discrete/DecisionTreeFactor.h
index 1f3d69292..286571ffc 100644
--- a/gtsam/discrete/DecisionTreeFactor.h
+++ b/gtsam/discrete/DecisionTreeFactor.h
@@ -175,12 +175,13 @@ namespace gtsam {
      *
      * Pruning will set the leaves to be "pruned" to 0 indicating a 0
      * probability.
-     * A leaf is pruned if it is not in the top `maxNrLeaves` values.
+     * An assignment is pruned if it is not in the top `maxNrAssignments`
+     * values.
      *
-     * @param maxNrLeaves The maximum number of leaves to keep.
+     * @param maxNrAssignments The maximum number of assignments to keep.
      * @return DecisionTreeFactor
      */
-    DecisionTreeFactor prune(size_t maxNrLeaves) const;
+    DecisionTreeFactor prune(size_t maxNrAssignments) const;
 
     /// @}
     /// @name Wrapper support

From 2da2bcbf9c0c4ad9d29090dff48d72434495cf84 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Thu, 31 Mar 2022 10:08:02 -0400
Subject: [PATCH 31/78] update docs and test

---
 gtsam/discrete/DecisionTreeFactor.h           | 13 +++++++---
 .../discrete/tests/testDecisionTreeFactor.cpp | 25 ++++++++++++++-----
 2 files changed, 29 insertions(+), 9 deletions(-)

diff --git a/gtsam/discrete/DecisionTreeFactor.h b/gtsam/discrete/DecisionTreeFactor.h
index 286571ffc..86fa44649 100644
--- a/gtsam/discrete/DecisionTreeFactor.h
+++ b/gtsam/discrete/DecisionTreeFactor.h
@@ -174,9 +174,16 @@ namespace gtsam {
      * @brief Prune the decision tree of discrete variables.
      *
      * Pruning will set the leaves to be "pruned" to 0 indicating a 0
-     * probability.
-     * An assignment is pruned if it is not in the top `maxNrAssignments`
-     * values.
+     * probability. An assignment is pruned if it is not in the top
+     * `maxNrAssignments` values.
+     *
+     * A violation can occur if there are more
+     * duplicate values than `maxNrAssignments`. A violation here is the need to
+     * un-prune the decision tree (e.g. all assignment values are 1.0). We could
+     * have another case where some subset of duplicates exist (e.g. for a tree
+     * with 8 assignments we have 1, 1, 1, 1, 0.8, 0.7, 0.6, 0.5), but this is
+     * not a violation since the for `maxNrAssignments=5` the top values are (1,
+     * 0.8).
      *
      * @param maxNrAssignments The maximum number of assignments to keep.
      * @return DecisionTreeFactor
diff --git a/gtsam/discrete/tests/testDecisionTreeFactor.cpp b/gtsam/discrete/tests/testDecisionTreeFactor.cpp
index 83b586bbb..84e45a0f5 100644
--- a/gtsam/discrete/tests/testDecisionTreeFactor.cpp
+++ b/gtsam/discrete/tests/testDecisionTreeFactor.cpp
@@ -113,18 +113,32 @@ TEST(DecisionTreeFactor, Prune) {
   DecisionTreeFactor f(A & B & C, "1 5 3 7 2 6 4 8");
 
   // Only keep the leaves with the top 5 values.
-  size_t maxNrLeaves = 5;
-  auto pruned5 = f.prune(maxNrLeaves);
+  size_t maxNrAssignments = 5;
+  auto pruned5 = f.prune(maxNrAssignments);
 
   // Pruned leaves should be 0
   DecisionTreeFactor expected(A & B & C, "0 5 0 7 0 6 4 8");
   EXPECT(assert_equal(expected, pruned5));
 
   // Check for more extreme pruning where we only keep the top 2 leaves
-  maxNrLeaves = 2;
-  auto pruned2 = f.prune(maxNrLeaves);
+  maxNrAssignments = 2;
+  auto pruned2 = f.prune(maxNrAssignments);
   DecisionTreeFactor expected2(A & B & C, "0 0 0 7 0 0 0 8");
   EXPECT(assert_equal(expected2, pruned2));
+
+  DiscreteKey D(4, 2);
+  DecisionTreeFactor factor(
+      D & C & B & A,
+      "0.0 0.0 0.0 0.60658897 0.61241912 0.61241969 0.61247685 0.61247742 0.0 "
+      "0.0 0.0 0.99995287 1.0 1.0 1.0 1.0");
+
+  DecisionTreeFactor expected3(
+      D & C & B & A,
+      "0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 "
+      "0.999952870000 1.0 1.0 1.0 1.0");
+  maxNrAssignments = 5;
+  auto pruned3 = factor.prune(maxNrAssignments);
+  EXPECT(assert_equal(expected3, pruned3));
 }
 
 /* ************************************************************************* */
@@ -133,7 +147,7 @@ TEST(DecisionTreeFactor, DotWithNames) {
   DecisionTreeFactor f(A & B, "1 2  3 4  5 6");
   auto formatter = [](Key key) { return key == 12 ? "A" : "B"; };
 
-  for (bool showZero:{true, false}) {  
+  for (bool showZero:{true, false}) {
     string actual = f.dot(formatter, showZero);
     // pretty weak test, as ids are pointers and not stable across platforms.
     string expected = "digraph G {";
@@ -215,4 +229,3 @@ int main() {
   return TestRegistry::runAllTests(tr);
 }
 /* ************************************************************************* */
-

From 29fe4364e748e92a689297159468723d99353a20 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Thu, 7 Apr 2022 16:41:24 -0400
Subject: [PATCH 32/78] increase precision of ADT printing

---
 gtsam/discrete/AlgebraicDecisionTree.h | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/gtsam/discrete/AlgebraicDecisionTree.h b/gtsam/discrete/AlgebraicDecisionTree.h
index a2ceac834..9769715a1 100644
--- a/gtsam/discrete/AlgebraicDecisionTree.h
+++ b/gtsam/discrete/AlgebraicDecisionTree.h
@@ -160,7 +160,7 @@ namespace gtsam {
               const typename Base::LabelFormatter& labelFormatter =
                   &DefaultFormatter) const {
       auto valueFormatter = [](const double& v) {
-        return (boost::format("%4.4g") % v).str();
+        return (boost::format("%4.8g") % v).str();
       };
       Base::print(s, labelFormatter, valueFormatter);
     }

From 49b40d3942eeef93e8a94ae61180bdd757d585a3 Mon Sep 17 00:00:00 2001
From: Fan Jiang <ProfFan@users.noreply.github.com>
Date: Tue, 12 Apr 2022 13:56:36 -0400
Subject: [PATCH 33/78] Fix slow dynamic cast

---
 gtsam/nonlinear/Values-inl.h | 16 +++++++++-------
 1 file changed, 9 insertions(+), 7 deletions(-)

diff --git a/gtsam/nonlinear/Values-inl.h b/gtsam/nonlinear/Values-inl.h
index dfcb7e174..078ffc502 100644
--- a/gtsam/nonlinear/Values-inl.h
+++ b/gtsam/nonlinear/Values-inl.h
@@ -294,7 +294,7 @@ namespace gtsam {
    // Handle dynamic matrices
    template <int M, int N>
    struct handle_matrix<Eigen::Matrix<double, M, N>, true> {
-     Eigen::Matrix<double, M, N> operator()(Key j, const Value* const pointer) {
+     inline Eigen::Matrix<double, M, N> operator()(Key j, const Value* const pointer) {
        try {
          // value returns a const Matrix&, and the return makes a copy !!!!!
          return dynamic_cast<const GenericValue<Eigen::Matrix<double, M, N>>&>(*pointer).value();
@@ -308,16 +308,18 @@ namespace gtsam {
    // Handle fixed matrices
    template <int M, int N>
    struct handle_matrix<Eigen::Matrix<double, M, N>, false> {
-     Eigen::Matrix<double, M, N> operator()(Key j, const Value* const pointer) {
-       try {
+     inline Eigen::Matrix<double, M, N> operator()(Key j, const Value* const pointer) {
+       auto ptr = dynamic_cast<const GenericValue<Eigen::Matrix<double, M, N>>*>(pointer);
+       if (ptr) {
          // value returns a const MatrixMN&, and the return makes a copy !!!!!
          return dynamic_cast<const GenericValue<Eigen::Matrix<double, M, N>>&>(*pointer).value();
-       } catch (std::bad_cast&) {
+       } else {
          Matrix A;
-         try {
+         auto ptr = dynamic_cast<const GenericValue<Eigen::MatrixXd>*>(pointer);
+         if (ptr) {
            // Check if a dynamic matrix was stored
-           A = handle_matrix<Eigen::MatrixXd, true>()(j, pointer);  // will throw if not....
-         } catch (const ValuesIncorrectType&) {
+           A = ptr->value();  // will throw if not....
+         } else {
            // Or a dynamic vector
            A = handle_matrix<Eigen::VectorXd, true>()(j, pointer);  // will throw if not....
          }

From 8e6a583e7ea6811690902da6bb523b7306c8dc77 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Tue, 12 Apr 2022 14:04:42 -0400
Subject: [PATCH 34/78] update for better docstrings

---
 gtsam/discrete/DecisionTree-inl.h | 28 ++++++++++++++++++++++------
 1 file changed, 22 insertions(+), 6 deletions(-)

diff --git a/gtsam/discrete/DecisionTree-inl.h b/gtsam/discrete/DecisionTree-inl.h
index b3ad66721..99f29b8e5 100644
--- a/gtsam/discrete/DecisionTree-inl.h
+++ b/gtsam/discrete/DecisionTree-inl.h
@@ -681,12 +681,14 @@ namespace gtsam {
 
   /****************************************************************************/
   /**
-   * Functor performing depth-first visit without Assignment<L> argument.
+   * Functor performing depth-first visit to each leaf with the leaf value as
+   * the argument.
    *
    * NOTE: We differentiate between leaves and assignments. Concretely, a 3
    * binary variable tree will have 2^3=8 assignments, but based on pruning, it
-   * can have <8 leaves. For example, if a tree has all assignment values as 1,
-   * then pruning will cause the tree to have only 1 leaf yet 8 assignments.
+   * can have less than 8 leaves. For example, if a tree has all assignment
+   * values as 1, then pruning will cause the tree to have only 1 leaf yet 8
+   * assignments.
    */
   template <typename L, typename Y>
   struct Visit {
@@ -717,7 +719,8 @@ namespace gtsam {
 
   /****************************************************************************/
   /**
-   * Functor performing depth-first visit with Leaf argument.
+   * Functor performing depth-first visit to each leaf with the Leaf object
+   * passed as an argument.
    *
    * NOTE: We differentiate between leaves and assignments. Concretely, a 3
    * binary variable tree will have 2^3=8 assignments, but based on pruning, it
@@ -753,7 +756,8 @@ namespace gtsam {
 
   /****************************************************************************/
   /**
-   * Functor performing depth-first visit with Assignment<L> argument.
+   * Functor performing depth-first visit to each leaf with the leaf's
+   * `Assignment<L>` and value passed as arguments.
    *
    * NOTE: Follows the same pruning semantics as `visit`.
    */
@@ -811,7 +815,19 @@ namespace gtsam {
   }
 
   /****************************************************************************/
-  // Get (partial) labels by performing a visit.
+  /**
+   * Get (partial) labels by performing a visit.
+   *
+   * This method performs a depth-first search to go to every leaf and records
+   * the keys assignment which leads to that leaf. Since the tree can be pruned,
+   * there might be a leaf at a lower depth which results in a partial
+   * assignment (i.e. not all keys are specified).
+   *
+   * E.g. given a tree with 3 keys, there may be a branch where the 3rd key has
+   * the same values for all the leaves. This leads to the branch being pruned
+   * so we get a leaf which is arrived at by just the first 2 keys and their
+   * assignments.
+   */
   template <typename L, typename Y>
   std::set<L> DecisionTree<L, Y>::labels() const {
     std::set<L> unique;

From ed34ee324587d5f8cf64bf83a44e5988671124d9 Mon Sep 17 00:00:00 2001
From: Fan Jiang <ProfFan@users.noreply.github.com>
Date: Tue, 12 Apr 2022 15:09:52 -0400
Subject: [PATCH 35/78] Update the other codepath to also use ptr cast

---
 gtsam/nonlinear/Values-inl.h | 13 +++++++------
 1 file changed, 7 insertions(+), 6 deletions(-)

diff --git a/gtsam/nonlinear/Values-inl.h b/gtsam/nonlinear/Values-inl.h
index 078ffc502..0436ade4a 100644
--- a/gtsam/nonlinear/Values-inl.h
+++ b/gtsam/nonlinear/Values-inl.h
@@ -295,10 +295,11 @@ namespace gtsam {
    template <int M, int N>
    struct handle_matrix<Eigen::Matrix<double, M, N>, true> {
      inline Eigen::Matrix<double, M, N> operator()(Key j, const Value* const pointer) {
-       try {
+       auto ptr = dynamic_cast<const GenericValue<Eigen::Matrix<double, M, N>>*>(pointer);
+       if (ptr) {
          // value returns a const Matrix&, and the return makes a copy !!!!!
-         return dynamic_cast<const GenericValue<Eigen::Matrix<double, M, N>>&>(*pointer).value();
-       } catch (std::bad_cast&) {
+         return ptr->value();
+       } else {
          // If a fixed matrix was stored, we end up here as well.
          throw ValuesIncorrectType(j, typeid(*pointer), typeid(Eigen::Matrix<double, M, N>));
        }
@@ -312,13 +313,13 @@ namespace gtsam {
        auto ptr = dynamic_cast<const GenericValue<Eigen::Matrix<double, M, N>>*>(pointer);
        if (ptr) {
          // value returns a const MatrixMN&, and the return makes a copy !!!!!
-         return dynamic_cast<const GenericValue<Eigen::Matrix<double, M, N>>&>(*pointer).value();
+         return ptr->value();
        } else {
          Matrix A;
+         // Check if a dynamic matrix was stored
          auto ptr = dynamic_cast<const GenericValue<Eigen::MatrixXd>*>(pointer);
          if (ptr) {
-           // Check if a dynamic matrix was stored
-           A = ptr->value();  // will throw if not....
+           A = ptr->value();
          } else {
            // Or a dynamic vector
            A = handle_matrix<Eigen::VectorXd, true>()(j, pointer);  // will throw if not....

From 1b63fb51eb98f10a83d525e8755cf2e9db4bc525 Mon Sep 17 00:00:00 2001
From: Fan Jiang <prof.fan@foxmail.com>
Date: Thu, 14 Apr 2022 13:05:15 -0400
Subject: [PATCH 36/78] Address comments by Frank

---
 gtsam/nonlinear/Values-inl.h | 15 ++++++++-------
 gtsam/nonlinear/nonlinear.i  |  4 ++++
 2 files changed, 12 insertions(+), 7 deletions(-)

diff --git a/gtsam/nonlinear/Values-inl.h b/gtsam/nonlinear/Values-inl.h
index 0436ade4a..0370c5cee 100644
--- a/gtsam/nonlinear/Values-inl.h
+++ b/gtsam/nonlinear/Values-inl.h
@@ -279,10 +279,11 @@ namespace gtsam {
    template <typename ValueType>
    struct handle {
      ValueType operator()(Key j, const Value* const pointer) {
-       try {
+       auto ptr = dynamic_cast<const GenericValue<ValueType>*>(pointer);
+       if (ptr) {
          // value returns a const ValueType&, and the return makes a copy !!!!!
-         return dynamic_cast<const GenericValue<ValueType>&>(*pointer).value();
-       } catch (std::bad_cast&) {
+         return ptr->value();
+       } else {
          throw ValuesIncorrectType(j, typeid(*pointer), typeid(ValueType));
        }
      }
@@ -367,10 +368,10 @@ namespace gtsam {
 
     if(item != values_.end()) {
       // dynamic cast the type and throw exception if incorrect
-      const Value& value = *item->second;
-      try {
-        return dynamic_cast<const GenericValue<ValueType>&>(value).value();
-      } catch (std::bad_cast &) {
+      auto ptr = dynamic_cast<const GenericValue<ValueType>*>(item->second);
+      if (ptr) {
+        return ptr->value();
+      } else {
         // NOTE(abe): clang warns about potential side effects if done in typeid
         const Value* value = item->second;
         throw ValuesIncorrectType(j, typeid(*value), typeid(ValueType));
diff --git a/gtsam/nonlinear/nonlinear.i b/gtsam/nonlinear/nonlinear.i
index 326b84d16..3fff71978 100644
--- a/gtsam/nonlinear/nonlinear.i
+++ b/gtsam/nonlinear/nonlinear.i
@@ -245,6 +245,10 @@ class Values {
   void insert(size_t j, const gtsam::ParameterMatrix<14>& X);
   void insert(size_t j, const gtsam::ParameterMatrix<15>& X);
 
+  template <T = {gtsam::Point2,
+                 gtsam::Point3}>
+  void insert(size_t j, const T& val);
+
   void update(size_t j, const gtsam::Point2& point2);
   void update(size_t j, const gtsam::Point3& point3);
   void update(size_t j, const gtsam::Rot2& rot2);

From 41c8a6710795a02e6c2a2eb98036c84b2dc1e55a Mon Sep 17 00:00:00 2001
From: Fan Jiang <prof.fan@foxmail.com>
Date: Thu, 14 Apr 2022 15:24:29 -0400
Subject: [PATCH 37/78] Fix robust deserialization

---
 gtsam/linear/LossFunctions.h | 2 ++
 1 file changed, 2 insertions(+)

diff --git a/gtsam/linear/LossFunctions.h b/gtsam/linear/LossFunctions.h
index c3d7d64db..1699b833f 100644
--- a/gtsam/linear/LossFunctions.h
+++ b/gtsam/linear/LossFunctions.h
@@ -223,6 +223,7 @@ class GTSAM_EXPORT Cauchy : public Base {
   void serialize(ARCHIVE &ar, const unsigned int /*version*/) {
     ar &BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
     ar &BOOST_SERIALIZATION_NVP(k_);
+    ar &BOOST_SERIALIZATION_NVP(ksquared_);
   }
 };
 
@@ -273,6 +274,7 @@ class GTSAM_EXPORT Welsch : public Base {
   void serialize(ARCHIVE &ar, const unsigned int /*version*/) {
     ar &BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
     ar &BOOST_SERIALIZATION_NVP(c_);
+    ar &BOOST_SERIALIZATION_NVP(csquared_);
   }
 };
 

From 939416c3d0005843cefbbfec8ce308e8f374d21e Mon Sep 17 00:00:00 2001
From: Fan Jiang <prof.fan@foxmail.com>
Date: Fri, 15 Apr 2022 13:57:01 -0400
Subject: [PATCH 38/78] Add camera Jacobian

---
 gtsam/geometry/PinholePose.h | 13 +++++++++++++
 gtsam/geometry/geometry.i    |  2 ++
 2 files changed, 15 insertions(+)

diff --git a/gtsam/geometry/PinholePose.h b/gtsam/geometry/PinholePose.h
index 7b92be5d5..4bfbc8b2d 100644
--- a/gtsam/geometry/PinholePose.h
+++ b/gtsam/geometry/PinholePose.h
@@ -121,6 +121,19 @@ public:
     return _project(pw, Dpose, Dpoint, Dcal);
   }
 
+  /// project, but for Python use
+  Point2 project(const Point3& pw, Eigen::Ref<Matrix> Dpose, Eigen::Ref<Matrix> Dpoint, Eigen::Ref<Matrix> Dcal) const {
+    Eigen::Matrix<double, 2, 6> Dpose_;
+    Eigen::Matrix<double, 2, 3> Dpoint_;
+    Eigen::Matrix<double, 2, DimK> Dcal_;
+
+    auto ret = _project(pw, Dpose_, Dpoint_, Dcal_);
+    Dpose = Dpose_;
+    Dpoint = Dpoint_;
+    Dcal = Dcal_;
+    return ret;
+  }
+
   /// project a 3D point from world coordinates into the image
   Point2 reprojectionError(const Point3& pw, const Point2& measured, OptionalJacobian<2, 6> Dpose = boost::none,
       OptionalJacobian<2, 3> Dpoint = boost::none,
diff --git a/gtsam/geometry/geometry.i b/gtsam/geometry/geometry.i
index 858270c00..46efebde3 100644
--- a/gtsam/geometry/geometry.i
+++ b/gtsam/geometry/geometry.i
@@ -824,6 +824,7 @@ template <CALIBRATION>
 class PinholeCamera {
   // Standard Constructors and Named Constructors
   PinholeCamera();
+  PinholeCamera(const gtsam::PinholeCamera<CALIBRATION> other);
   PinholeCamera(const gtsam::Pose3& pose);
   PinholeCamera(const gtsam::Pose3& pose, const CALIBRATION& K);
   static This Level(const CALIBRATION& K, const gtsam::Pose2& pose,
@@ -850,6 +851,7 @@ class PinholeCamera {
   static gtsam::Point2 Project(const gtsam::Point3& cameraPoint);
   pair<gtsam::Point2, bool> projectSafe(const gtsam::Point3& pw) const;
   gtsam::Point2 project(const gtsam::Point3& point);
+  gtsam::Point2 project(const gtsam::Point3& point, Eigen::Ref<Eigen::MatrixXd> Dpose, Eigen::Ref<Eigen::MatrixXd> Dpoint, Eigen::Ref<Eigen::MatrixXd> Dcal);
   gtsam::Point3 backproject(const gtsam::Point2& p, double depth) const;
   double range(const gtsam::Point3& point);
   double range(const gtsam::Pose3& pose);

From f3c9b3967b572bbd1b117fce674e19257effe151 Mon Sep 17 00:00:00 2001
From: Fan Jiang <prof.fan@foxmail.com>
Date: Sat, 16 Apr 2022 13:51:18 -0400
Subject: [PATCH 39/78] Use implicit conversion

---
 gtsam/base/OptionalJacobian.h | 15 +++++++++++++++
 gtsam/geometry/PinholePose.h  | 13 -------------
 2 files changed, 15 insertions(+), 13 deletions(-)

diff --git a/gtsam/base/OptionalJacobian.h b/gtsam/base/OptionalJacobian.h
index 07801df7a..8a8891446 100644
--- a/gtsam/base/OptionalJacobian.h
+++ b/gtsam/base/OptionalJacobian.h
@@ -20,6 +20,7 @@
 #pragma once
 #include <gtsam/config.h>      // Configuration from CMake
 #include <Eigen/Dense>
+#include <stdexcept>
 
 #ifndef OPTIONALJACOBIAN_NOBOOST
 #include <boost/optional.hpp>
@@ -96,6 +97,20 @@ public:
     usurp(dynamic->data());
   }
 
+  /**
+   * @brief Constructor from an Eigen::Ref *value*. Will not usurp if dimension is wrong
+   * @note This is important so we don't overwrite someone else's memory!
+   */
+  OptionalJacobian(Eigen::Ref<Eigen::MatrixXd> dynamic_ref) :
+      map_(nullptr) {
+    if (dynamic_ref.rows() == Rows && dynamic_ref.cols() == Cols && !dynamic_ref.IsRowMajor) {
+      usurp(dynamic_ref.data());
+    } else {
+      // It's never a good idea to throw in the constructor
+      throw std::invalid_argument("OptionalJacobian called with wrong dimensions or storage order.\n");
+    }
+  }
+
 #ifndef OPTIONALJACOBIAN_NOBOOST
 
   /// Constructor with boost::none just makes empty
diff --git a/gtsam/geometry/PinholePose.h b/gtsam/geometry/PinholePose.h
index 4bfbc8b2d..7b92be5d5 100644
--- a/gtsam/geometry/PinholePose.h
+++ b/gtsam/geometry/PinholePose.h
@@ -121,19 +121,6 @@ public:
     return _project(pw, Dpose, Dpoint, Dcal);
   }
 
-  /// project, but for Python use
-  Point2 project(const Point3& pw, Eigen::Ref<Matrix> Dpose, Eigen::Ref<Matrix> Dpoint, Eigen::Ref<Matrix> Dcal) const {
-    Eigen::Matrix<double, 2, 6> Dpose_;
-    Eigen::Matrix<double, 2, 3> Dpoint_;
-    Eigen::Matrix<double, 2, DimK> Dcal_;
-
-    auto ret = _project(pw, Dpose_, Dpoint_, Dcal_);
-    Dpose = Dpose_;
-    Dpoint = Dpoint_;
-    Dcal = Dcal_;
-    return ret;
-  }
-
   /// project a 3D point from world coordinates into the image
   Point2 reprojectionError(const Point3& pw, const Point2& measured, OptionalJacobian<2, 6> Dpose = boost::none,
       OptionalJacobian<2, 3> Dpoint = boost::none,

From 923c57e68ba7619d174166b865a1223c53691c4a Mon Sep 17 00:00:00 2001
From: Fan Jiang <prof.fan@foxmail.com>
Date: Sun, 17 Apr 2022 16:03:00 -0400
Subject: [PATCH 40/78] Address comments

---
 gtsam/base/OptionalJacobian.h            |  8 +++-
 gtsam/geometry/geometry.i                | 20 +++++++-
 python/gtsam/tests/test_PinholeCamera.py | 58 ++++++++++++++++++++++++
 3 files changed, 83 insertions(+), 3 deletions(-)
 create mode 100644 python/gtsam/tests/test_PinholeCamera.py

diff --git a/gtsam/base/OptionalJacobian.h b/gtsam/base/OptionalJacobian.h
index 8a8891446..a990ffc0e 100644
--- a/gtsam/base/OptionalJacobian.h
+++ b/gtsam/base/OptionalJacobian.h
@@ -21,6 +21,7 @@
 #include <gtsam/config.h>      // Configuration from CMake
 #include <Eigen/Dense>
 #include <stdexcept>
+#include <string>
 
 #ifndef OPTIONALJACOBIAN_NOBOOST
 #include <boost/optional.hpp>
@@ -106,8 +107,11 @@ public:
     if (dynamic_ref.rows() == Rows && dynamic_ref.cols() == Cols && !dynamic_ref.IsRowMajor) {
       usurp(dynamic_ref.data());
     } else {
-      // It's never a good idea to throw in the constructor
-      throw std::invalid_argument("OptionalJacobian called with wrong dimensions or storage order.\n");
+      throw std::invalid_argument(
+          std::string("OptionalJacobian called with wrong dimensions or "
+                      "storage order.\n"
+                      "Expected: ") +
+          "(" + std::to_string(Rows) + ", " + std::to_string(Cols) + ")");
     }
   }
 
diff --git a/gtsam/geometry/geometry.i b/gtsam/geometry/geometry.i
index 46efebde3..fd96bcc02 100644
--- a/gtsam/geometry/geometry.i
+++ b/gtsam/geometry/geometry.i
@@ -623,7 +623,13 @@ virtual class Cal3DS2_Base {
 
   // Action on Point2
   gtsam::Point2 uncalibrate(const gtsam::Point2& p) const;
+  gtsam::Point2 uncalibrate(const gtsam::Point2& p,
+                            Eigen::Ref<Eigen::MatrixXd> Dcal,
+                            Eigen::Ref<Eigen::MatrixXd> Dp) const;
   gtsam::Point2 calibrate(const gtsam::Point2& p) const;
+  gtsam::Point2 calibrate(const gtsam::Point2& p,
+                          Eigen::Ref<Eigen::MatrixXd> Dcal,
+                          Eigen::Ref<Eigen::MatrixXd> Dp) const;
 
   // enabling serialization functionality
   void serialize() const;
@@ -851,10 +857,22 @@ class PinholeCamera {
   static gtsam::Point2 Project(const gtsam::Point3& cameraPoint);
   pair<gtsam::Point2, bool> projectSafe(const gtsam::Point3& pw) const;
   gtsam::Point2 project(const gtsam::Point3& point);
-  gtsam::Point2 project(const gtsam::Point3& point, Eigen::Ref<Eigen::MatrixXd> Dpose, Eigen::Ref<Eigen::MatrixXd> Dpoint, Eigen::Ref<Eigen::MatrixXd> Dcal);
+  gtsam::Point2 project(const gtsam::Point3& point,
+                        Eigen::Ref<Eigen::MatrixXd> Dpose,
+                        Eigen::Ref<Eigen::MatrixXd> Dpoint,
+                        Eigen::Ref<Eigen::MatrixXd> Dcal);
   gtsam::Point3 backproject(const gtsam::Point2& p, double depth) const;
+  gtsam::Point3 backproject(const gtsam::Point2& p, double depth,
+                            Eigen::Ref<Eigen::MatrixXd> Dresult_dpose,
+                            Eigen::Ref<Eigen::MatrixXd> Dresult_dp,
+                            Eigen::Ref<Eigen::MatrixXd> Dresult_ddepth,
+                            Eigen::Ref<Eigen::MatrixXd> Dresult_dcal);
   double range(const gtsam::Point3& point);
+  double range(const gtsam::Point3& point, Eigen::Ref<Eigen::MatrixXd> Dcamera,
+               Eigen::Ref<Eigen::MatrixXd> Dpoint);
   double range(const gtsam::Pose3& pose);
+  double range(const gtsam::Pose3& pose, Eigen::Ref<Eigen::MatrixXd> Dcamera,
+               Eigen::Ref<Eigen::MatrixXd> Dpose);
 
   // enabling serialization functionality
   void serialize() const;
diff --git a/python/gtsam/tests/test_PinholeCamera.py b/python/gtsam/tests/test_PinholeCamera.py
new file mode 100644
index 000000000..ceed8638d
--- /dev/null
+++ b/python/gtsam/tests/test_PinholeCamera.py
@@ -0,0 +1,58 @@
+"""
+GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
+Atlanta, Georgia 30332-0415
+All Rights Reserved
+
+See LICENSE for the license information
+
+PinholeCamera unit tests.
+Author: Fan Jiang
+"""
+import unittest
+from math import pi
+
+import numpy as np
+
+import gtsam
+from gtsam.utils.test_case import GtsamTestCase
+
+
+class TestPinholeCamera(GtsamTestCase):
+    def test_jacobian(self):
+        cam1 = gtsam.PinholeCameraCal3Bundler()
+        cam1.deserialize(
+            '22 serialization::archive 19 1 0\n0 0 0 0 0 1 0\n1 1 0\n2 9.99727427959442139e-01 6.30191620439291000e-03 2.24814359098672867e-02 5.97546668723225594e-03 -9.99876141548156738e-01 1.45585928112268448e-02 2.25703977048397064e-02 -1.44202867522835732e-02 -9.99641239643096924e-01 0 0 -5.81446531765312802e-02 -3.64078342172925590e-02 -5.63949743097517997e-01 1 0\n3 0 0 5.18692016601562500e+02 5.18692016601562500e+02 0.00000000000000000e+00 0.00000000000000000e+00 0.00000000000000000e+00 -1.14570140838623047e-01 -3.44798192381858826e-02 1.00000000000000008e-05\n'
+        )
+
+        # order is important because Eigen is column major!
+        Dpose = np.zeros((2, 6), order='F')
+        Dpoint = np.zeros((2, 3), order='F')
+        Dcal = np.zeros((2, 3), order='F')
+        cam1.project(np.array([0.43350768, 0.0305741, -1.93050155]), Dpose,
+                     Dpoint, Dcal)
+
+        self.gtsamAssertEquals(
+            Dpoint,
+            np.array([[358.22984814, -0.58837638, 128.85360812],
+                      [3.58714683, -371.03278204, -16.89571148]]))
+
+        Dpose_expected = np.array([[
+                -3.97279895, -553.22184659, -16.40213844, -361.03694491,
+                -0.98773192, 120.76242884
+            ],
+            [
+                512.13105406, 3.97279895, -171.21912901,
+                -0.98773192, -371.25308927, -11.56857932
+            ]])
+
+        self.gtsamAssertEquals(
+            Dpose,
+            Dpose_expected)
+
+        self.gtsamAssertEquals(
+            Dcal,
+            np.array([[0.33009787, 19.60464375, 2.214697],
+                      [-0.03162211, -1.87804997, -0.21215951]]))
+
+if __name__ == "__main__":
+    unittest.main()

From 807d63c7bfe6a5d59833b4b4740c2050c8f18863 Mon Sep 17 00:00:00 2001
From: Fan Jiang <prof.fan@foxmail.com>
Date: Sun, 17 Apr 2022 16:49:52 -0400
Subject: [PATCH 41/78] Use templated Eigen::Ref wrapper

---
 gtsam/base/OptionalJacobian.h | 3 ++-
 1 file changed, 2 insertions(+), 1 deletion(-)

diff --git a/gtsam/base/OptionalJacobian.h b/gtsam/base/OptionalJacobian.h
index a990ffc0e..c9a960a89 100644
--- a/gtsam/base/OptionalJacobian.h
+++ b/gtsam/base/OptionalJacobian.h
@@ -102,7 +102,8 @@ public:
    * @brief Constructor from an Eigen::Ref *value*. Will not usurp if dimension is wrong
    * @note This is important so we don't overwrite someone else's memory!
    */
-  OptionalJacobian(Eigen::Ref<Eigen::MatrixXd> dynamic_ref) :
+  template<class MATRIX>
+  OptionalJacobian(Eigen::Ref<MATRIX> dynamic_ref) :
       map_(nullptr) {
     if (dynamic_ref.rows() == Rows && dynamic_ref.cols() == Cols && !dynamic_ref.IsRowMajor) {
       usurp(dynamic_ref.data());

From e6420cfcb7727c69f9461a03c5361fe1ee35ea4f Mon Sep 17 00:00:00 2001
From: Fan Jiang <prof.fan@foxmail.com>
Date: Sun, 17 Apr 2022 16:50:07 -0400
Subject: [PATCH 42/78] Added Jacobians for all calibrations

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

diff --git a/gtsam/geometry/geometry.i b/gtsam/geometry/geometry.i
index fd96bcc02..75c0847a4 100644
--- a/gtsam/geometry/geometry.i
+++ b/gtsam/geometry/geometry.i
@@ -584,7 +584,13 @@ class Cal3_S2 {
 
   // Action on Point2
   gtsam::Point2 calibrate(const gtsam::Point2& p) const;
+  gtsam::Point2 calibrate(const gtsam::Point2& p,
+                          Eigen::Ref<Eigen::MatrixXd> Dcal,
+                          Eigen::Ref<Eigen::MatrixXd> Dp) const;
   gtsam::Point2 uncalibrate(const gtsam::Point2& p) const;
+  gtsam::Point2 uncalibrate(const gtsam::Point2& p,
+                            Eigen::Ref<Eigen::MatrixXd> Dcal,
+                            Eigen::Ref<Eigen::MatrixXd> Dp) const;
 
   // Standard Interface
   double fx() const;
@@ -686,7 +692,13 @@ virtual class Cal3Unified : gtsam::Cal3DS2_Base {
   // Note: the signature of this functions differ from the functions
   // with equal name in the base class.
   gtsam::Point2 calibrate(const gtsam::Point2& p) const;
+  gtsam::Point2 calibrate(const gtsam::Point2& p,
+                          Eigen::Ref<Eigen::MatrixXd> Dcal,
+                          Eigen::Ref<Eigen::MatrixXd> Dp) const;
   gtsam::Point2 uncalibrate(const gtsam::Point2& p) const;
+  gtsam::Point2 uncalibrate(const gtsam::Point2& p,
+                            Eigen::Ref<Eigen::MatrixXd> Dcal,
+                            Eigen::Ref<Eigen::MatrixXd> Dp) const;
 
   // enabling serialization functionality
   void serialize() const;
@@ -712,7 +724,13 @@ class Cal3Fisheye {
 
   // Action on Point2
   gtsam::Point2 calibrate(const gtsam::Point2& p) const;
+  gtsam::Point2 calibrate(const gtsam::Point2& p,
+                          Eigen::Ref<Eigen::MatrixXd> Dcal,
+                          Eigen::Ref<Eigen::MatrixXd> Dp) const;
   gtsam::Point2 uncalibrate(const gtsam::Point2& p) const;
+  gtsam::Point2 uncalibrate(const gtsam::Point2& p,
+                            Eigen::Ref<Eigen::MatrixXd> Dcal,
+                            Eigen::Ref<Eigen::MatrixXd> Dp) const;
 
   // Standard Interface
   double fx() const;
@@ -775,7 +793,13 @@ class Cal3Bundler {
 
   // Action on Point2
   gtsam::Point2 calibrate(const gtsam::Point2& p) const;
+  gtsam::Point2 calibrate(const gtsam::Point2& p,
+                          Eigen::Ref<Eigen::MatrixXd> Dcal,
+                          Eigen::Ref<Eigen::MatrixXd> Dp) const;
   gtsam::Point2 uncalibrate(const gtsam::Point2& p) const;
+  gtsam::Point2 uncalibrate(const gtsam::Point2& p,
+                            Eigen::Ref<Eigen::MatrixXd> Dcal,
+                            Eigen::Ref<Eigen::MatrixXd> Dp) const;
 
   // Standard Interface
   double fx() const;

From 693f05b04a50e3cfaf1a13365e05dc2a297e58a4 Mon Sep 17 00:00:00 2001
From: Fan Jiang <prof.fan@foxmail.com>
Date: Sun, 17 Apr 2022 16:50:23 -0400
Subject: [PATCH 43/78] Use rations numbers in test

---
 python/gtsam/tests/test_PinholeCamera.py | 35 ++++++++----------------
 1 file changed, 12 insertions(+), 23 deletions(-)

diff --git a/python/gtsam/tests/test_PinholeCamera.py b/python/gtsam/tests/test_PinholeCamera.py
index ceed8638d..a5282d5b0 100644
--- a/python/gtsam/tests/test_PinholeCamera.py
+++ b/python/gtsam/tests/test_PinholeCamera.py
@@ -18,41 +18,30 @@ from gtsam.utils.test_case import GtsamTestCase
 
 
 class TestPinholeCamera(GtsamTestCase):
+    """
+    Tests if we can correctly get the camera Jacobians in Python
+    """
     def test_jacobian(self):
         cam1 = gtsam.PinholeCameraCal3Bundler()
-        cam1.deserialize(
-            '22 serialization::archive 19 1 0\n0 0 0 0 0 1 0\n1 1 0\n2 9.99727427959442139e-01 6.30191620439291000e-03 2.24814359098672867e-02 5.97546668723225594e-03 -9.99876141548156738e-01 1.45585928112268448e-02 2.25703977048397064e-02 -1.44202867522835732e-02 -9.99641239643096924e-01 0 0 -5.81446531765312802e-02 -3.64078342172925590e-02 -5.63949743097517997e-01 1 0\n3 0 0 5.18692016601562500e+02 5.18692016601562500e+02 0.00000000000000000e+00 0.00000000000000000e+00 0.00000000000000000e+00 -1.14570140838623047e-01 -3.44798192381858826e-02 1.00000000000000008e-05\n'
-        )
 
         # order is important because Eigen is column major!
         Dpose = np.zeros((2, 6), order='F')
         Dpoint = np.zeros((2, 3), order='F')
         Dcal = np.zeros((2, 3), order='F')
-        cam1.project(np.array([0.43350768, 0.0305741, -1.93050155]), Dpose,
-                     Dpoint, Dcal)
+        cam1.project(np.array([1, 1, 1]), Dpose, Dpoint, Dcal)
 
-        self.gtsamAssertEquals(
-            Dpoint,
-            np.array([[358.22984814, -0.58837638, 128.85360812],
-                      [3.58714683, -371.03278204, -16.89571148]]))
-
-        Dpose_expected = np.array([[
-                -3.97279895, -553.22184659, -16.40213844, -361.03694491,
-                -0.98773192, 120.76242884
-            ],
-            [
-                512.13105406, 3.97279895, -171.21912901,
-                -0.98773192, -371.25308927, -11.56857932
-            ]])
+        self.gtsamAssertEquals(Dpoint, np.array([[1, 0, -1], [0, 1, -1]]))
 
+        print(repr(Dpose), repr(Dcal))
         self.gtsamAssertEquals(
             Dpose,
-            Dpose_expected)
+            np.array([
+                [1., -2., 1., -1., 0., 1.],  #
+                [2., -1., -1., 0., -1., 1.]
+            ]))
+
+        self.gtsamAssertEquals(Dcal, np.array([[1., 2., 4.], [1., 2., 4.]]))
 
-        self.gtsamAssertEquals(
-            Dcal,
-            np.array([[0.33009787, 19.60464375, 2.214697],
-                      [-0.03162211, -1.87804997, -0.21215951]]))
 
 if __name__ == "__main__":
     unittest.main()

From 4c8260bc0e76bd16072539943dc67b8f8fd49059 Mon Sep 17 00:00:00 2001
From: Fan Jiang <prof.fan@foxmail.com>
Date: Sun, 17 Apr 2022 17:04:07 -0400
Subject: [PATCH 44/78] Add stereo jacobians

---
 gtsam/geometry/geometry.i | 24 +++++++++++++++++++++++-
 1 file changed, 23 insertions(+), 1 deletion(-)

diff --git a/gtsam/geometry/geometry.i b/gtsam/geometry/geometry.i
index 75c0847a4..7818058a3 100644
--- a/gtsam/geometry/geometry.i
+++ b/gtsam/geometry/geometry.i
@@ -837,12 +837,25 @@ class CalibratedCamera {
 
   // Action on Point3
   gtsam::Point2 project(const gtsam::Point3& point) const;
+  gtsam::Point2 project(const gtsam::Point3& point,
+                        Eigen::Ref<Eigen::MatrixXd> Dcamera,
+                        Eigen::Ref<Eigen::MatrixXd> Dpoint);
+  gtsam::Point3 backproject(const gtsam::Point2& p, double depth) const;
+  gtsam::Point3 backproject(const gtsam::Point2& p, double depth,
+                            Eigen::Ref<Eigen::MatrixXd> Dresult_dpose,
+                            Eigen::Ref<Eigen::MatrixXd> Dresult_dp,
+                            Eigen::Ref<Eigen::MatrixXd> Dresult_ddepth);
+
   static gtsam::Point2 Project(const gtsam::Point3& cameraPoint);
 
   // Standard Interface
   gtsam::Pose3 pose() const;
   double range(const gtsam::Point3& point) const;
+  double range(const gtsam::Point3& point, Eigen::Ref<Eigen::MatrixXd> Dcamera,
+               Eigen::Ref<Eigen::MatrixXd> Dpoint);
   double range(const gtsam::Pose3& pose) const;
+  double range(const gtsam::Pose3& point, Eigen::Ref<Eigen::MatrixXd> Dcamera,
+               Eigen::Ref<Eigen::MatrixXd> Dpose);
   double range(const gtsam::CalibratedCamera& camera) const;
 
   // enabling serialization functionality
@@ -965,9 +978,18 @@ class StereoCamera {
   static size_t Dim();
 
   // Transformations and measurement functions
-  gtsam::StereoPoint2 project(const gtsam::Point3& point);
+  gtsam::StereoPoint2 project(const gtsam::Point3& point) const;
   gtsam::Point3 backproject(const gtsam::StereoPoint2& p) const;
 
+  // project with Jacobian
+  gtsam::StereoPoint2 project2(const gtsam::Point3& point,
+                              Eigen::Ref<Eigen::MatrixXd> H1,
+                              Eigen::Ref<Eigen::MatrixXd> H2) const;
+
+  gtsam::Point3 backproject2(const gtsam::StereoPoint2& p,
+                             Eigen::Ref<Eigen::MatrixXd> H1,
+                             Eigen::Ref<Eigen::MatrixXd> H2) const;
+
   // enabling serialization functionality
   void serialize() const;
 };

From f212ac363abe391c9b67b269032e3c60fae86bf9 Mon Sep 17 00:00:00 2001
From: Fan Jiang <prof.fan@foxmail.com>
Date: Sun, 17 Apr 2022 17:04:19 -0400
Subject: [PATCH 45/78] Add Cal3Unified unit test

---
 python/gtsam/tests/test_Cal3Unified.py | 11 +++++++++++
 1 file changed, 11 insertions(+)

diff --git a/python/gtsam/tests/test_Cal3Unified.py b/python/gtsam/tests/test_Cal3Unified.py
index bafbacfa4..630109d66 100644
--- a/python/gtsam/tests/test_Cal3Unified.py
+++ b/python/gtsam/tests/test_Cal3Unified.py
@@ -139,6 +139,17 @@ class TestCal3Unified(GtsamTestCase):
         self.gtsamAssertEquals(z, np.zeros(2))
         self.gtsamAssertEquals(H @ H.T, 4*np.eye(2))
 
+        Dcal = np.zeros((2, 10), order='F')
+        Dp = np.zeros((2, 2), order='F')
+        camera.calibrate(img_point, Dcal, Dp)
+        
+        self.gtsamAssertEquals(Dcal, np.array(
+            [[ 0.,  0.,  0., -1.,  0.,  0.,  0.,  0.,  0.,  0.],
+            [ 0.,  0.,  0.,  0., -1.,  0.,  0.,  0.,  0.,  0.]]))
+        self.gtsamAssertEquals(Dp, np.array(
+            [[ 1., -0.],
+            [-0.,  1.]]))
+
     @unittest.skip("triangulatePoint3 currently seems to require perspective projections.")
     def test_triangulation(self):
         """Estimate spatial point from image measurements"""

From da09271d97f8d193f4934787c66d5bb3c3f888b4 Mon Sep 17 00:00:00 2001
From: Fan Jiang <prof.fan@foxmail.com>
Date: Sun, 17 Apr 2022 17:04:40 -0400
Subject: [PATCH 46/78] Remove print

---
 python/gtsam/tests/test_PinholeCamera.py | 1 -
 1 file changed, 1 deletion(-)

diff --git a/python/gtsam/tests/test_PinholeCamera.py b/python/gtsam/tests/test_PinholeCamera.py
index a5282d5b0..392d48d3f 100644
--- a/python/gtsam/tests/test_PinholeCamera.py
+++ b/python/gtsam/tests/test_PinholeCamera.py
@@ -32,7 +32,6 @@ class TestPinholeCamera(GtsamTestCase):
 
         self.gtsamAssertEquals(Dpoint, np.array([[1, 0, -1], [0, 1, -1]]))
 
-        print(repr(Dpose), repr(Dcal))
         self.gtsamAssertEquals(
             Dpose,
             np.array([

From f12db7ab0ebd043b4386d5fa27d89921d310e54c Mon Sep 17 00:00:00 2001
From: Jose Luis Blanco Claraco <joseluisblancoc@gmail.com>
Date: Tue, 19 Apr 2022 18:35:03 +0200
Subject: [PATCH 47/78] noise models: Add getters for all model parameters

---
 gtsam/linear/LossFunctions.h | 10 ++++++++++
 1 file changed, 10 insertions(+)

diff --git a/gtsam/linear/LossFunctions.h b/gtsam/linear/LossFunctions.h
index 1699b833f..cdd6a26d3 100644
--- a/gtsam/linear/LossFunctions.h
+++ b/gtsam/linear/LossFunctions.h
@@ -70,6 +70,8 @@ class GTSAM_EXPORT Base {
   Base(const ReweightScheme reweight = Block) : reweight_(reweight) {}
   virtual ~Base() {}
 
+  ReweightScheme reweightScheme() const { return reweight_; }
+
   /*
    * This method is responsible for returning the total penalty for a given
    * amount of error. For example, this method is responsible for implementing
@@ -160,6 +162,7 @@ class GTSAM_EXPORT Fair : public Base {
   void print(const std::string &s) const override;
   bool equals(const Base &expected, double tol = 1e-8) const override;
   static shared_ptr Create(double c, const ReweightScheme reweight = Block);
+  double modelParameter() const { return c_; }
 
  private:
   /** Serialization function */
@@ -185,6 +188,7 @@ class GTSAM_EXPORT Huber : public Base {
   void print(const std::string &s) const override;
   bool equals(const Base &expected, double tol = 1e-8) const override;
   static shared_ptr Create(double k, const ReweightScheme reweight = Block);
+  double modelParameter() const { return k_; }
 
  private:
   /** Serialization function */
@@ -215,6 +219,7 @@ class GTSAM_EXPORT Cauchy : public Base {
   void print(const std::string &s) const override;
   bool equals(const Base &expected, double tol = 1e-8) const override;
   static shared_ptr Create(double k, const ReweightScheme reweight = Block);
+  double modelParameter() const { return k_; }
 
  private:
   /** Serialization function */
@@ -241,6 +246,7 @@ class GTSAM_EXPORT Tukey : public Base {
   void print(const std::string &s) const override;
   bool equals(const Base &expected, double tol = 1e-8) const override;
   static shared_ptr Create(double k, const ReweightScheme reweight = Block);
+  double modelParameter() const { return c_; }
 
  private:
   /** Serialization function */
@@ -266,6 +272,7 @@ class GTSAM_EXPORT Welsch : public Base {
   void print(const std::string &s) const override;
   bool equals(const Base &expected, double tol = 1e-8) const override;
   static shared_ptr Create(double k, const ReweightScheme reweight = Block);
+  double modelParameter() const { return c_; }
 
  private:
   /** Serialization function */
@@ -295,6 +302,7 @@ class GTSAM_EXPORT GemanMcClure : public Base {
   void print(const std::string &s) const override;
   bool equals(const Base &expected, double tol = 1e-8) const override;
   static shared_ptr Create(double k, const ReweightScheme reweight = Block);
+  double modelParameter() const { return c_; }
 
  protected:
   double c_;
@@ -325,6 +333,7 @@ class GTSAM_EXPORT DCS : public Base {
   void print(const std::string &s) const override;
   bool equals(const Base &expected, double tol = 1e-8) const override;
   static shared_ptr Create(double k, const ReweightScheme reweight = Block);
+  double modelParameter() const { return c_; }
 
  protected:
   double c_;
@@ -358,6 +367,7 @@ class GTSAM_EXPORT L2WithDeadZone : public Base {
   void print(const std::string &s) const override;
   bool equals(const Base &expected, double tol = 1e-8) const override;
   static shared_ptr Create(double k, const ReweightScheme reweight = Block);
+  double modelParameter() const { return k_; }
 
  private:
   /** Serialization function */

From 48310cf8d4e7cd74ce91f992d4c1abe44920a4be Mon Sep 17 00:00:00 2001
From: Gerry Chen <gerry.chen2015@gmail.com>
Date: Wed, 20 Apr 2022 16:33:50 -0400
Subject: [PATCH 48/78] fix bug that python utils files don't regenerate upon
 edit

---
 python/CMakeLists.txt | 8 ++++++--
 1 file changed, 6 insertions(+), 2 deletions(-)

diff --git a/python/CMakeLists.txt b/python/CMakeLists.txt
index f5869b145..d3cbff32d 100644
--- a/python/CMakeLists.txt
+++ b/python/CMakeLists.txt
@@ -98,11 +98,15 @@ set(GTSAM_MODULE_PATH ${GTSAM_PYTHON_BUILD_DIRECTORY}/gtsam)
 copy_directory("${CMAKE_CURRENT_SOURCE_DIR}/gtsam"
         "${GTSAM_MODULE_PATH}")
 
-# Hack to get python test files copied every time they are modified
+# Hack to get python test and util files copied every time they are modified
 file(GLOB GTSAM_PYTHON_TEST_FILES "${CMAKE_CURRENT_SOURCE_DIR}/gtsam/tests/*.py")
 foreach(test_file ${GTSAM_PYTHON_TEST_FILES})
         configure_file(${test_file} "${GTSAM_MODULE_PATH}/tests/${test_file}" COPYONLY)
 endforeach()
+file(GLOB GTSAM_PYTHON_UTIL_FILES "${CMAKE_CURRENT_SOURCE_DIR}/gtsam/utils/*.py")
+foreach(util_file ${GTSAM_PYTHON_UTIL_FILES})
+        configure_file(${util_file} "${GTSAM_MODULE_PATH}/utils/${test_file}" COPYONLY)
+endforeach()
 
 # Common directory for data/datasets stored with the package.
 # This will store the data in the Python site package directly.
@@ -160,7 +164,7 @@ if(GTSAM_UNSTABLE_BUILD_PYTHON)
 
     # Hack to get python test files copied every time they are modified
     file(GLOB GTSAM_UNSTABLE_PYTHON_TEST_FILES "${CMAKE_CURRENT_SOURCE_DIR}/gtsam_unstable/tests/*.py")
-    foreach(test_file ${GTSAM_PYTHON_TEST_FILES})
+    foreach(test_file ${GTSAM_UNSTABLE_PYTHON_TEST_FILES})
         configure_file(${test_file} "${GTSAM_UNSTABLE_MODULE_PATH}/tests/${test_file}" COPYONLY)
     endforeach()
 

From 017e3cdb179a784bc28452e1ded40d0442bfd9a4 Mon Sep 17 00:00:00 2001
From: Jose Luis Blanco Claraco <joseluisblancoc@gmail.com>
Date: Fri, 22 Apr 2022 01:01:48 +0200
Subject: [PATCH 49/78] document loss functions

Signed-off-by: Jose Luis Blanco Claraco <joseluisblancoc@gmail.com>
---
 gtsam/linear/LossFunctions.h | 141 ++++++++++++++++++++++++++---------
 1 file changed, 106 insertions(+), 35 deletions(-)

diff --git a/gtsam/linear/LossFunctions.h b/gtsam/linear/LossFunctions.h
index cdd6a26d3..d9cfc1f3c 100644
--- a/gtsam/linear/LossFunctions.h
+++ b/gtsam/linear/LossFunctions.h
@@ -54,25 +54,31 @@ namespace noiseModel {
 // clang-format on
 namespace mEstimator {
 
-//---------------------------------------------------------------------------------------
-
+/**
+ * Pure virtual class for all robust error function classes.
+ *
+ * It provides the machinery for block vs scalar reweighting strategies, in
+ * addition to defining the interface of derived classes.
+ */
 class GTSAM_EXPORT Base {
  public:
+  /** the rows can be weighted independently according to the error
+   * or uniformly with the norm of the right hand side */
   enum ReweightScheme { Scalar, Block };
   typedef boost::shared_ptr<Base> shared_ptr;
 
  protected:
-  /** the rows can be weighted independently according to the error
-   * or uniformly with the norm of the right hand side */
+  /// Strategy for reweighting \sa ReweightScheme
   ReweightScheme reweight_;
 
  public:
   Base(const ReweightScheme reweight = Block) : reweight_(reweight) {}
   virtual ~Base() {}
 
+  /// Returns the reweight scheme, as explained in ReweightScheme
   ReweightScheme reweightScheme() const { return reweight_; }
 
-  /*
+  /**
    * This method is responsible for returning the total penalty for a given
    * amount of error. For example, this method is responsible for implementing
    * the quadratic function for an L2 penalty, the absolute value function for
@@ -82,16 +88,20 @@ class GTSAM_EXPORT Base {
    * error vector, then it prevents implementations of asymmeric loss
    * functions. It would be better for this function to accept the vector and
    * internally call the norm if necessary.
+   *
+   * This returns \rho(x) in \ref mEstimator
    */
-  virtual double loss(double distance) const { return 0; };
+  virtual double loss(double distance) const { return 0; }
 
-  /*
+  /**
    * This method is responsible for returning the weight function for a given
    * amount of error. The weight function is related to the analytic derivative
    * of the loss function. See
    *  https://members.loria.fr/MOBerger/Enseignement/Master2/Documents/ZhangIVC-97-01.pdf
    * for details. This method is required when optimizing cost functions with
    * robust penalties using iteratively re-weighted least squares.
+   *
+   * This returns w(x) in \ref mEstimator
    */
   virtual double weight(double distance) const = 0;
 
@@ -126,7 +136,15 @@ class GTSAM_EXPORT Base {
   }
 };
 
-/// Null class should behave as Gaussian
+/** "Null" robust loss function, equivalent to a Gaussian pdf noise model, or
+ *  plain least-squares (non-robust).
+ *
+ *  This model has no additional parameters.
+ *
+ * - Loss       \rho(x)          = 0.5 x²
+ * - Derivative \phi(x)          = x
+ * - Weight     w(x) = \phi(x)/x = 1
+ */
 class GTSAM_EXPORT Null : public Base {
  public:
   typedef boost::shared_ptr<Null> shared_ptr;
@@ -148,7 +166,14 @@ class GTSAM_EXPORT Null : public Base {
   }
 };
 
-/// Fair implements the "Fair" robust error model (Zhang97ivc)
+/** Implementation of the "Fair" robust error model (Zhang97ivc)
+ *
+ *  This model has a scalar parameter "c".
+ *
+ * - Loss       \rho(x) = c² (|x|/c - log(1+|x|/c))
+ * - Derivative \phi(x) = x/(1+|x|/c)
+ * - Weight     w(x) = \phi(x)/x = 1/(1+|x|/c)
+ */
 class GTSAM_EXPORT Fair : public Base {
  protected:
   double c_;
@@ -174,7 +199,14 @@ class GTSAM_EXPORT Fair : public Base {
   }
 };
 
-/// Huber implements the "Huber" robust error model (Zhang97ivc)
+/** The "Huber" robust error model (Zhang97ivc).
+ *
+ *  This model has a scalar parameter "k".
+ *
+ * - Loss       \rho(x)          = 0.5 x²  if |x|<k, 0.5 k² + k|x-k|  otherwise
+ * - Derivative \phi(x)          = x       if |x|<k, k sgn(x)         otherwise
+ * - Weight     w(x) = \phi(x)/x = 1       if |x|<k, k/|x|            otherwise
+ */
 class GTSAM_EXPORT Huber : public Base {
  protected:
   double k_;
@@ -200,12 +232,19 @@ class GTSAM_EXPORT Huber : public Base {
   }
 };
 
-/// Cauchy implements the "Cauchy" robust error model (Lee2013IROS). Contributed
-/// by:
-///   Dipl.-Inform. Jan Oberlaender (M.Sc.), FZI Research Center for
-///   Information Technology, Karlsruhe, Germany.
-///   oberlaender@fzi.de
-/// Thanks Jan!
+/** Implementation of the "Cauchy" robust error model (Lee2013IROS).
+ * Contributed by:
+ *  Dipl.-Inform. Jan Oberlaender (M.Sc.), FZI Research Center for
+ *  Information Technology, Karlsruhe, Germany.
+ *  oberlaender@fzi.de
+ *  Thanks Jan!
+ *
+ *  This model has a scalar parameter "k".
+ *
+ * - Loss       \rho(x) = 0.5 k² log(1+x²/k²)
+ * - Derivative \phi(x) = (k²x)/(x²+k²)
+ * - Weight     w(x) = \phi(x)/x = k²/(x²+k²)
+ */
 class GTSAM_EXPORT Cauchy : public Base {
  protected:
   double k_, ksquared_;
@@ -232,7 +271,14 @@ class GTSAM_EXPORT Cauchy : public Base {
   }
 };
 
-/// Tukey implements the "Tukey" robust error model (Zhang97ivc)
+/** Implementation of the "Tukey" robust error model (Zhang97ivc).
+ *
+ *  This model has a scalar parameter "c".
+ *
+ * - Loss       \rho(x) = c² (1 - (1-x²/c²)³)/6  if |x|<c,  c²/6   otherwise
+ * - Derivative \phi(x) = x(1-x²/c²)² if |x|<c,  0   otherwise
+ * - Weight     w(x) = \phi(x)/x = (1-x²/c²)² if |x|<c,  0   otherwise
+ */
 class GTSAM_EXPORT Tukey : public Base {
  protected:
   double c_, csquared_;
@@ -258,7 +304,14 @@ class GTSAM_EXPORT Tukey : public Base {
   }
 };
 
-/// Welsch implements the "Welsch" robust error model (Zhang97ivc)
+/** Implementation of the "Welsch" robust error model (Zhang97ivc).
+ *
+ *  This model has a scalar parameter "c".
+ *
+ * - Loss       \rho(x) = -0.5 c² (exp(-x²/c²) - 1)
+ * - Derivative \phi(x) = x exp(-x²/c²)
+ * - Weight     w(x) = \phi(x)/x = exp(-x²/c²)
+ */
 class GTSAM_EXPORT Welsch : public Base {
  protected:
   double c_, csquared_;
@@ -285,12 +338,16 @@ class GTSAM_EXPORT Welsch : public Base {
   }
 };
 
-/// GemanMcClure implements the "Geman-McClure" robust error model
-/// (Zhang97ivc).
-///
-/// Note that Geman-McClure weight function uses the parameter c == 1.0,
-/// but here it's allowed to use different values, so we actually have
-/// the generalized Geman-McClure from (Agarwal15phd).
+/** Implementation of the "Geman-McClure" robust error model (Zhang97ivc).
+ *
+ * Note that Geman-McClure weight function uses the parameter c == 1.0,
+ * but here it's allowed to use different values, so we actually have
+ * the generalized Geman-McClure from (Agarwal15phd).
+ *
+ * - Loss       \rho(x) = 0.5 (c²x²)/(c²+x²)
+ * - Derivative \phi(x) = xc⁴/(c²+x²)²
+ * - Weight     w(x) = \phi(x)/x = c⁴/(c²+x²)²
+ */
 class GTSAM_EXPORT GemanMcClure : public Base {
  public:
   typedef boost::shared_ptr<GemanMcClure> shared_ptr;
@@ -317,11 +374,18 @@ class GTSAM_EXPORT GemanMcClure : public Base {
   }
 };
 
-/// DCS implements the Dynamic Covariance Scaling robust error model
-/// from the paper Robust Map Optimization (Agarwal13icra).
-///
-/// Under the special condition of the parameter c == 1.0 and not
-/// forcing the output weight s <= 1.0, DCS is similar to Geman-McClure.
+/** DCS implements the Dynamic Covariance Scaling robust error model
+ *  from the paper Robust Map Optimization (Agarwal13icra).
+ *
+ *  Under the special condition of the parameter c == 1.0 and not
+ *  forcing the output weight s <= 1.0, DCS is similar to Geman-McClure.
+ *
+ *  This model has a scalar parameter "c" (with "units" of squared error).
+ *
+ * - Loss       \rho(x) = (c²x² + cx⁴)/(x²+c)²   (for any "x")
+ * - Derivative \phi(x) = 2c²x/(x²+c)²
+ * - Weight     w(x) = \phi(x)/x = 2c²/(x²+c)²  if x²>c,   1  otherwise
+ */
 class GTSAM_EXPORT DCS : public Base {
  public:
   typedef boost::shared_ptr<DCS> shared_ptr;
@@ -348,12 +412,19 @@ class GTSAM_EXPORT DCS : public Base {
   }
 };
 
-/// L2WithDeadZone implements a standard L2 penalty, but with a dead zone of
-/// width 2*k, centered at the origin. The resulting penalty within the dead
-/// zone is always zero, and grows quadratically outside the dead zone. In this
-/// sense, the L2WithDeadZone penalty is "robust to inliers", rather than being
-/// robust to outliers. This penalty can be used to create barrier functions in
-/// a general way.
+/** L2WithDeadZone implements a standard L2 penalty, but with a dead zone of
+ *  width 2*k, centered at the origin. The resulting penalty within the dead
+ *  zone is always zero, and grows quadratically outside the dead zone. In this
+ *  sense, the L2WithDeadZone penalty is "robust to inliers", rather than being
+ *  robust to outliers. This penalty can be used to create barrier functions in
+ *  a general way.
+ *
+ *  This model has a scalar parameter "k".
+ *
+ * - Loss       \rho(x) = 0 if |x|<k,    0.5(k-|x|)² otherwise
+ * - Derivative \phi(x) = 0 if |x|<k, (-k+x) if x>k,  (k+x) if x<-k
+ * - Weight     w(x) = \phi(x)/x = 0 if |x|<k, (-k+x)/x if x>k,  (k+x)/x if x<-k
+ */
 class GTSAM_EXPORT L2WithDeadZone : public Base {
  protected:
   double k_;

From 42b795c769e66dacc24cbe8b13309cdfb3506fc2 Mon Sep 17 00:00:00 2001
From: Jose Luis Blanco Claraco <joseluisblancoc@gmail.com>
Date: Fri, 22 Apr 2022 01:02:03 +0200
Subject: [PATCH 50/78] fix derivative

Signed-off-by: Jose Luis Blanco Claraco <joseluisblancoc@gmail.com>
---
 gtsam/linear/LossFunctions.cpp | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/gtsam/linear/LossFunctions.cpp b/gtsam/linear/LossFunctions.cpp
index 7307c4a68..6573beac9 100644
--- a/gtsam/linear/LossFunctions.cpp
+++ b/gtsam/linear/LossFunctions.cpp
@@ -350,8 +350,8 @@ double DCS::weight(double distance) const {
   const double e2 = distance*distance;
   if (e2 > c_)
   {
-    const double w = 2.0*c_/(c_ + e2);
-    return w*w;
+    const double w = c_/(c_ + e2);
+    return 2.0*w*w;
   }
 
   return 1.0;

From a9ba1da877051632154bf6b6090f91ce402ac6ce Mon Sep 17 00:00:00 2001
From: Jose Luis Blanco Claraco <joseluisblancoc@gmail.com>
Date: Fri, 22 Apr 2022 12:37:48 +0200
Subject: [PATCH 51/78] Revert "fix derivative"

This reverts commit 42b795c769e66dacc24cbe8b13309cdfb3506fc2.
---
 gtsam/linear/LossFunctions.cpp | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/gtsam/linear/LossFunctions.cpp b/gtsam/linear/LossFunctions.cpp
index 6573beac9..7307c4a68 100644
--- a/gtsam/linear/LossFunctions.cpp
+++ b/gtsam/linear/LossFunctions.cpp
@@ -350,8 +350,8 @@ double DCS::weight(double distance) const {
   const double e2 = distance*distance;
   if (e2 > c_)
   {
-    const double w = c_/(c_ + e2);
-    return 2.0*w*w;
+    const double w = 2.0*c_/(c_ + e2);
+    return w*w;
   }
 
   return 1.0;

From 84dc91fbb9ccc97eeedf91b999c909ec62a3e492 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Fri, 29 Apr 2022 16:52:06 -0400
Subject: [PATCH 52/78] fix type warnings

---
 gtsam/geometry/Pose2.cpp | 2 +-
 gtsam/geometry/Pose3.cpp | 2 +-
 2 files changed, 2 insertions(+), 2 deletions(-)

diff --git a/gtsam/geometry/Pose2.cpp b/gtsam/geometry/Pose2.cpp
index 96bdce69b..f54d6b034 100644
--- a/gtsam/geometry/Pose2.cpp
+++ b/gtsam/geometry/Pose2.cpp
@@ -365,7 +365,7 @@ boost::optional<Pose2> Pose2::Align(const Matrix& a, const Matrix& b) {
       "Pose2:Align expects 2*N matrices of equal shape.");
   }
   Point2Pairs ab_pairs;
-  for (size_t j=0; j < a.cols(); j++) {
+  for (size_t j=0; j < size_t(a.cols()); j++) {
     ab_pairs.emplace_back(a.col(j), b.col(j));
   }
   return Pose2::Align(ab_pairs);
diff --git a/gtsam/geometry/Pose3.cpp b/gtsam/geometry/Pose3.cpp
index b88d812a4..c79ff07c3 100644
--- a/gtsam/geometry/Pose3.cpp
+++ b/gtsam/geometry/Pose3.cpp
@@ -473,7 +473,7 @@ boost::optional<Pose3> Pose3::Align(const Matrix& a, const Matrix& b) {
       "Pose3:Align expects 3*N matrices of equal shape.");
   }
   Point3Pairs abPointPairs;
-  for (size_t j=0; j < a.cols(); j++) {
+  for (size_t j=0; j < size_t(a.cols()); j++) {
     abPointPairs.emplace_back(a.col(j), b.col(j));
   }
   return Pose3::Align(abPointPairs);

From a0799f7e88b0c09e1f6d85f5880685b81469118d Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Fri, 29 Apr 2022 17:03:12 -0400
Subject: [PATCH 53/78] update IMU factor docs to clarify that the measurements
 are in sensor frame

---
 gtsam/navigation/AHRSFactor.h        | 6 +++++-
 gtsam/navigation/CombinedImuFactor.h | 7 +++++--
 gtsam/navigation/ImuFactor.h         | 6 +++++-
 3 files changed, 15 insertions(+), 4 deletions(-)

diff --git a/gtsam/navigation/AHRSFactor.h b/gtsam/navigation/AHRSFactor.h
index 10c33d101..c7d82975a 100644
--- a/gtsam/navigation/AHRSFactor.h
+++ b/gtsam/navigation/AHRSFactor.h
@@ -90,7 +90,11 @@ class GTSAM_EXPORT PreintegratedAhrsMeasurements : public PreintegratedRotation
 
   /**
    * Add a single Gyroscope measurement to the preintegration.
-   * @param measureOmedga Measured angular velocity (in body frame)
+   * Measurements are taken to be in the sensor
+   * frame and conversion to the body frame is handled by `body_P_sensor` in
+   * `PreintegratedRotationParams` (if provided).
+   *
+   * @param measuredOmega Measured angular velocity (as given by the sensor)
    * @param deltaT Time step
    */
   void integrateMeasurement(const Vector3& measuredOmega, double deltaT);
diff --git a/gtsam/navigation/CombinedImuFactor.h b/gtsam/navigation/CombinedImuFactor.h
index 068a17ca4..213f5f223 100644
--- a/gtsam/navigation/CombinedImuFactor.h
+++ b/gtsam/navigation/CombinedImuFactor.h
@@ -208,8 +208,11 @@ public:
 
   /**
    * Add a single IMU measurement to the preintegration.
-   * @param measuredAcc Measured acceleration (in body frame, as given by the
-   * sensor)
+   * Both accelerometer and gyroscope measurements are taken to be in the sensor
+   * frame and conversion to the body frame is handled by `body_P_sensor` in
+   * `PreintegrationParams`.
+   *
+   * @param measuredAcc Measured acceleration (as given by the sensor)
    * @param measuredOmega Measured angular velocity (as given by the sensor)
    * @param dt Time interval between two consecutive IMU measurements
    */
diff --git a/gtsam/navigation/ImuFactor.h b/gtsam/navigation/ImuFactor.h
index 35207e452..740827162 100644
--- a/gtsam/navigation/ImuFactor.h
+++ b/gtsam/navigation/ImuFactor.h
@@ -121,7 +121,11 @@ public:
 
   /**
    * Add a single IMU measurement to the preintegration.
-   * @param measuredAcc Measured acceleration (in body frame, as given by the sensor)
+   * Both accelerometer and gyroscope measurements are taken to be in the sensor
+   * frame and conversion to the body frame is handled by `body_P_sensor` in
+   * `PreintegrationParams`.
+   *
+   * @param measuredAcc Measured acceleration (as given by the sensor)
    * @param measuredOmega Measured angular velocity (as given by the sensor)
    * @param dt Time interval between this and the last IMU measurement
    */

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 54/78] 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 6e5da635b..e6fa73ef3 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 93a1227f5..511ac8a28 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 55/78] 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 511ac8a28..74ebf9640 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 27fdba6d7..8799ba244 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 @@ public:
   /** 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 @@ public:
    * @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 56/78] 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 | 22 +++++++++++-----------
 5 files changed, 11 insertions(+), 25 deletions(-)

diff --git a/gtsam/geometry/Similarity2.cpp b/gtsam/geometry/Similarity2.cpp
index e6fa73ef3..3c780e907 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 74ebf9640..a09dc1858 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 43cfaaa96..7fde974c5 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 8799ba244..845d4c810 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 428422072..7a134f6ef 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)));
+  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("Pose1");
-  //p2.print("Pose2");
-  //p3.print("Pose3");
-  //p4.print("Pose4");
-  //p5.print("Pose5");
+  //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 57/78] 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 3e39ac45c..44ecd5a04 100644
--- a/python/gtsam/tests/test_Sim2.py
+++ b/python/gtsam/tests/test_Sim2.py
@@ -12,9 +12,7 @@ Author: John Lambert
 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 @@ class TestSim2(GtsamTestCase):
         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 @@ class TestSim2(GtsamTestCase):
         """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 @@ class TestSim2(GtsamTestCase):
 
         # 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 @@ class TestSim2(GtsamTestCase):
         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 @@ class TestSim2(GtsamTestCase):
         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 58/78] 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 44ecd5a04..ea809b965 100644
--- a/python/gtsam/tests/test_Sim2.py
+++ b/python/gtsam/tests/test_Sim2.py
@@ -135,8 +135,8 @@ class TestSim2(GtsamTestCase):
         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 59/78] 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 d43b9b12d..9bf631e50 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 60/78] add Lie group operations

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

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

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 61/78] 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 258039b5b..4b8cfd581 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 0453758e0..e72cda484 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 62/78] 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 000000000..f60e8f55e
--- /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 63/78] 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 4b8cfd581..4ed3351f8 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 e72cda484..05f10d149 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 f60e8f55e..dd4fd0efd 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;
 

From 287279fc4295e3d30bfe3775060e973cc9d9d985 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Sat, 30 Apr 2022 17:05:41 -0400
Subject: [PATCH 64/78] use Jose's suggestion

---
 gtsam/geometry/Pose2.cpp | 2 +-
 gtsam/geometry/Pose3.cpp | 2 +-
 2 files changed, 2 insertions(+), 2 deletions(-)

diff --git a/gtsam/geometry/Pose2.cpp b/gtsam/geometry/Pose2.cpp
index f54d6b034..b37674b92 100644
--- a/gtsam/geometry/Pose2.cpp
+++ b/gtsam/geometry/Pose2.cpp
@@ -365,7 +365,7 @@ boost::optional<Pose2> Pose2::Align(const Matrix& a, const Matrix& b) {
       "Pose2:Align expects 2*N matrices of equal shape.");
   }
   Point2Pairs ab_pairs;
-  for (size_t j=0; j < size_t(a.cols()); j++) {
+  for (Eigen::Index j = 0; j < a.cols(); j++) {
     ab_pairs.emplace_back(a.col(j), b.col(j));
   }
   return Pose2::Align(ab_pairs);
diff --git a/gtsam/geometry/Pose3.cpp b/gtsam/geometry/Pose3.cpp
index c79ff07c3..2da51a625 100644
--- a/gtsam/geometry/Pose3.cpp
+++ b/gtsam/geometry/Pose3.cpp
@@ -473,7 +473,7 @@ boost::optional<Pose3> Pose3::Align(const Matrix& a, const Matrix& b) {
       "Pose3:Align expects 3*N matrices of equal shape.");
   }
   Point3Pairs abPointPairs;
-  for (size_t j=0; j < size_t(a.cols()); j++) {
+  for (Eigen::Index j = 0; j < a.cols(); j++) {
     abPointPairs.emplace_back(a.col(j), b.col(j));
   }
   return Pose3::Align(abPointPairs);

From 099ba8a8ca8f8dfc05fd871402d1f081bb47c3c0 Mon Sep 17 00:00:00 2001
From: Fan Jiang <prof.fan@foxmail.com>
Date: Tue, 3 May 2022 13:52:09 -0400
Subject: [PATCH 65/78] Add block Jacobi preconditioner wrap

---
 gtsam/linear/linear.i | 4 ++++
 1 file changed, 4 insertions(+)

diff --git a/gtsam/linear/linear.i b/gtsam/linear/linear.i
index f1bc92f69..943b661d8 100644
--- a/gtsam/linear/linear.i
+++ b/gtsam/linear/linear.i
@@ -671,6 +671,10 @@ virtual class DummyPreconditionerParameters : gtsam::PreconditionerParameters {
   DummyPreconditionerParameters();
 };
 
+virtual class BlockJacobiPreconditionerParameters : gtsam::PreconditionerParameters {
+  BlockJacobiPreconditionerParameters();
+};
+
 #include <gtsam/linear/PCGSolver.h>
 virtual class PCGSolverParameters : gtsam::ConjugateGradientParameters {
   PCGSolverParameters();

From 3571420010a332743293a8903daeba0afc927d90 Mon Sep 17 00:00:00 2001
From: Fan Jiang <prof.fan@foxmail.com>
Date: Tue, 3 May 2022 13:54:25 -0400
Subject: [PATCH 66/78] Added PinholePose in wrap and FromPose3 in
 EssentialMatrix

---
 gtsam/geometry/geometry.i   | 71 ++++++++++++++++++++++++++++++++++++-
 gtsam/nonlinear/nonlinear.i | 16 +++++++++
 2 files changed, 86 insertions(+), 1 deletion(-)

diff --git a/gtsam/geometry/geometry.i b/gtsam/geometry/geometry.i
index 415aa0dc4..63fd7056a 100644
--- a/gtsam/geometry/geometry.i
+++ b/gtsam/geometry/geometry.i
@@ -547,6 +547,12 @@ class EssentialMatrix {
   // Standard Constructors
   EssentialMatrix(const gtsam::Rot3& aRb, const gtsam::Unit3& aTb);
 
+  // Constructors from Pose3
+  gtsam::EssentialMatrix FromPose3(const gtsam::Pose3& _1P2_);
+
+  gtsam::EssentialMatrix FromPose3(const gtsam::Pose3& _1P2_,
+                            Eigen::Ref<Eigen::MatrixXd> H);
+
   // Testable
   void print(string s = "") const;
   bool equals(const gtsam::EssentialMatrix& pose, double tol) const;
@@ -904,6 +910,12 @@ class PinholeCamera {
                             Eigen::Ref<Eigen::MatrixXd> Dresult_dp,
                             Eigen::Ref<Eigen::MatrixXd> Dresult_ddepth,
                             Eigen::Ref<Eigen::MatrixXd> Dresult_dcal);
+
+  gtsam::Point2 reprojectionError(const gtsam::Point3& pw, const gtsam::Point2& measured,
+                                  Eigen::Ref<Eigen::MatrixXd> Dpose,
+                                  Eigen::Ref<Eigen::MatrixXd> Dpoint,
+                                  Eigen::Ref<Eigen::MatrixXd> Dcal);
+
   double range(const gtsam::Point3& point);
   double range(const gtsam::Point3& point, Eigen::Ref<Eigen::MatrixXd> Dcamera,
                Eigen::Ref<Eigen::MatrixXd> Dpoint);
@@ -914,7 +926,58 @@ class PinholeCamera {
   // enabling serialization functionality
   void serialize() const;
 };
-  
+
+#include <gtsam/geometry/PinholePose.h>
+template <CALIBRATION>
+class PinholePose {
+  // Standard Constructors and Named Constructors
+  PinholePose();
+  PinholePose(const gtsam::PinholePose<CALIBRATION> other);
+  PinholePose(const gtsam::Pose3& pose);
+  PinholePose(const gtsam::Pose3& pose, const CALIBRATION* K);
+  static This Level(const gtsam::Pose2& pose, double height);
+  static This Lookat(const gtsam::Point3& eye, const gtsam::Point3& target,
+                     const gtsam::Point3& upVector, const CALIBRATION* K);
+
+  // Testable
+  void print(string s = "PinholePose") const;
+  bool equals(const This& camera, double tol) const;
+
+  // Standard Interface
+  gtsam::Pose3 pose() const;
+  CALIBRATION calibration() const;
+
+  // Manifold
+  This retract(Vector d) const;
+  Vector localCoordinates(const This& T2) const;
+  size_t dim() const;
+  static size_t Dim();
+
+  // Transformations and measurement functions
+  static gtsam::Point2 Project(const gtsam::Point3& cameraPoint);
+  pair<gtsam::Point2, bool> projectSafe(const gtsam::Point3& pw) const;
+  gtsam::Point2 project(const gtsam::Point3& point);
+  gtsam::Point2 project(const gtsam::Point3& point,
+                        Eigen::Ref<Eigen::MatrixXd> Dpose,
+                        Eigen::Ref<Eigen::MatrixXd> Dpoint,
+                        Eigen::Ref<Eigen::MatrixXd> Dcal);
+  gtsam::Point3 backproject(const gtsam::Point2& p, double depth) const;
+  gtsam::Point3 backproject(const gtsam::Point2& p, double depth,
+                            Eigen::Ref<Eigen::MatrixXd> Dresult_dpose,
+                            Eigen::Ref<Eigen::MatrixXd> Dresult_dp,
+                            Eigen::Ref<Eigen::MatrixXd> Dresult_ddepth,
+                            Eigen::Ref<Eigen::MatrixXd> Dresult_dcal);
+  double range(const gtsam::Point3& point);
+  double range(const gtsam::Point3& point, Eigen::Ref<Eigen::MatrixXd> Dcamera,
+               Eigen::Ref<Eigen::MatrixXd> Dpoint);
+  double range(const gtsam::Pose3& pose);
+  double range(const gtsam::Pose3& pose, Eigen::Ref<Eigen::MatrixXd> Dcamera,
+               Eigen::Ref<Eigen::MatrixXd> Dpose);
+
+  // enabling serialization functionality
+  void serialize() const;
+};
+
 #include <gtsam/geometry/Similarity2.h>
 class Similarity2 {
   // Standard Constructors
@@ -971,6 +1034,12 @@ typedef gtsam::PinholeCamera<gtsam::Cal3Unified> PinholeCameraCal3Unified;
 typedef gtsam::PinholeCamera<gtsam::Cal3Bundler> PinholeCameraCal3Bundler;
 typedef gtsam::PinholeCamera<gtsam::Cal3Fisheye> PinholeCameraCal3Fisheye;
 
+typedef gtsam::PinholePose<gtsam::Cal3_S2> PinholePoseCal3_S2;
+typedef gtsam::PinholePose<gtsam::Cal3DS2> PinholePoseCal3DS2;
+typedef gtsam::PinholePose<gtsam::Cal3Unified> PinholePoseCal3Unified;
+typedef gtsam::PinholePose<gtsam::Cal3Bundler> PinholePoseCal3Bundler;
+typedef gtsam::PinholePose<gtsam::Cal3Fisheye> PinholePoseCal3Fisheye;
+
 template <T>
 class CameraSet {
   CameraSet();
diff --git a/gtsam/nonlinear/nonlinear.i b/gtsam/nonlinear/nonlinear.i
index 3fff71978..30181e08d 100644
--- a/gtsam/nonlinear/nonlinear.i
+++ b/gtsam/nonlinear/nonlinear.i
@@ -226,6 +226,10 @@ class Values {
   void insert(size_t j, const gtsam::PinholeCamera<gtsam::Cal3Bundler>& camera);
   void insert(size_t j, const gtsam::PinholeCamera<gtsam::Cal3Fisheye>& camera);
   void insert(size_t j, const gtsam::PinholeCamera<gtsam::Cal3Unified>& camera);
+  void insert(size_t j, const gtsam::PinholePose<gtsam::Cal3_S2>& camera);
+  void insert(size_t j, const gtsam::PinholePose<gtsam::Cal3Bundler>& camera);
+  void insert(size_t j, const gtsam::PinholePose<gtsam::Cal3Fisheye>& camera);
+  void insert(size_t j, const gtsam::PinholePose<gtsam::Cal3Unified>& camera);
   void insert(size_t j, const gtsam::imuBias::ConstantBias& constant_bias);
   void insert(size_t j, const gtsam::NavState& nav_state);
   void insert(size_t j, double c);
@@ -269,6 +273,10 @@ class Values {
   void update(size_t j, const gtsam::PinholeCamera<gtsam::Cal3Bundler>& camera);
   void update(size_t j, const gtsam::PinholeCamera<gtsam::Cal3Fisheye>& camera);
   void update(size_t j, const gtsam::PinholeCamera<gtsam::Cal3Unified>& camera);
+  void update(size_t j, const gtsam::PinholePose<gtsam::Cal3_S2>& camera);
+  void update(size_t j, const gtsam::PinholePose<gtsam::Cal3Bundler>& camera);
+  void update(size_t j, const gtsam::PinholePose<gtsam::Cal3Fisheye>& camera);
+  void update(size_t j, const gtsam::PinholePose<gtsam::Cal3Unified>& camera);
   void update(size_t j, const gtsam::imuBias::ConstantBias& constant_bias);
   void update(size_t j, const gtsam::NavState& nav_state);
   void update(size_t j, Vector vector);
@@ -310,6 +318,10 @@ class Values {
   void insert_or_assign(size_t j, const gtsam::PinholeCamera<gtsam::Cal3Bundler>& camera);
   void insert_or_assign(size_t j, const gtsam::PinholeCamera<gtsam::Cal3Fisheye>& camera);
   void insert_or_assign(size_t j, const gtsam::PinholeCamera<gtsam::Cal3Unified>& camera);
+  void insert_or_assign(size_t j, const gtsam::PinholePose<gtsam::Cal3_S2>& camera);
+  void insert_or_assign(size_t j, const gtsam::PinholePose<gtsam::Cal3Bundler>& camera);
+  void insert_or_assign(size_t j, const gtsam::PinholePose<gtsam::Cal3Fisheye>& camera);
+  void insert_or_assign(size_t j, const gtsam::PinholePose<gtsam::Cal3Unified>& camera);
   void insert_or_assign(size_t j, const gtsam::imuBias::ConstantBias& constant_bias);
   void insert_or_assign(size_t j, const gtsam::NavState& nav_state);
   void insert_or_assign(size_t j, Vector vector);
@@ -351,6 +363,10 @@ class Values {
                  gtsam::PinholeCamera<gtsam::Cal3Bundler>,
                  gtsam::PinholeCamera<gtsam::Cal3Fisheye>,
                  gtsam::PinholeCamera<gtsam::Cal3Unified>,
+                 gtsam::PinholePose<gtsam::Cal3_S2>,
+                 gtsam::PinholePose<gtsam::Cal3Bundler>,
+                 gtsam::PinholePose<gtsam::Cal3Fisheye>,
+                 gtsam::PinholePose<gtsam::Cal3Unified>,
                  gtsam::imuBias::ConstantBias,
                  gtsam::NavState,
                  Vector,

From d4eadbaf20099791e9fbae43075652535be31370 Mon Sep 17 00:00:00 2001
From: Fan Jiang <prof.fan@foxmail.com>
Date: Tue, 3 May 2022 13:54:45 -0400
Subject: [PATCH 67/78] Added wrapping for Shonan constructor

---
 gtsam/sfm/sfm.i | 9 +++++++++
 1 file changed, 9 insertions(+)

diff --git a/gtsam/sfm/sfm.i b/gtsam/sfm/sfm.i
index bf9a73ac5..901270d9c 100644
--- a/gtsam/sfm/sfm.i
+++ b/gtsam/sfm/sfm.i
@@ -9,6 +9,8 @@ class SfmTrack {
   SfmTrack();
   SfmTrack(const gtsam::Point3& pt);
   const Point3& point3() const;
+  
+  Point3 p;
 
   double r;
   double g;
@@ -34,6 +36,9 @@ class SfmData {
   static gtsam::SfmData FromBundlerFile(string filename);
   static gtsam::SfmData FromBalFile(string filename);
 
+  std::vector<gtsam::SfmTrack> tracks;
+  std::vector<gtsam::PinholeCamera<gtsam::Cal3Bundler>> cameras;
+
   void addTrack(const gtsam::SfmTrack& t);
   void addCamera(const gtsam::SfmCamera& cam);
   size_t numberTracks() const;
@@ -184,6 +189,10 @@ class ShonanAveraging2 {
 };
 
 class ShonanAveraging3 {
+  ShonanAveraging3(
+      const std::vector<gtsam::BinaryMeasurement<gtsam::Rot3>>& measurements,
+      const gtsam::ShonanAveragingParameters3& parameters =
+          gtsam::ShonanAveragingParameters3());
   ShonanAveraging3(string g2oFile);
   ShonanAveraging3(string g2oFile,
                    const gtsam::ShonanAveragingParameters3& parameters);

From acaf6a273ac8d91d411004f514bf758f7619023d Mon Sep 17 00:00:00 2001
From: Fan Jiang <prof.fan@foxmail.com>
Date: Tue, 3 May 2022 13:55:08 -0400
Subject: [PATCH 68/78] Wrap GeneralSFMFactor for PinholePose

---
 gtsam/slam/slam.i | 16 ++++++++++++++++
 1 file changed, 16 insertions(+)

diff --git a/gtsam/slam/slam.i b/gtsam/slam/slam.i
index 4e943253e..8e1e06d5b 100644
--- a/gtsam/slam/slam.i
+++ b/gtsam/slam/slam.i
@@ -90,6 +90,22 @@ typedef gtsam::GeneralSFMFactor<gtsam::PinholeCamera<gtsam::Cal3Unified>,
                                 gtsam::Point3>
     GeneralSFMFactorCal3Unified;
 
+typedef gtsam::GeneralSFMFactor<gtsam::PinholePose<gtsam::Cal3_S2>,
+                                gtsam::Point3>
+    GeneralSFMFactorPoseCal3_S2;
+typedef gtsam::GeneralSFMFactor<gtsam::PinholePose<gtsam::Cal3DS2>,
+                                gtsam::Point3>
+    GeneralSFMFactorPoseCal3DS2;
+typedef gtsam::GeneralSFMFactor<gtsam::PinholePose<gtsam::Cal3Bundler>,
+                                gtsam::Point3>
+    GeneralSFMFactorPoseCal3Bundler;
+typedef gtsam::GeneralSFMFactor<gtsam::PinholePose<gtsam::Cal3Fisheye>,
+                                gtsam::Point3>
+    GeneralSFMFactorPoseCal3Fisheye;
+typedef gtsam::GeneralSFMFactor<gtsam::PinholePose<gtsam::Cal3Unified>,
+                                gtsam::Point3>
+    GeneralSFMFactorPoseCal3Unified;
+
 template <CALIBRATION = {gtsam::Cal3_S2, gtsam::Cal3DS2, gtsam::Cal3Bundler,
                          gtsam::Cal3Fisheye, gtsam::Cal3Unified}>
 virtual class GeneralSFMFactor2 : gtsam::NoiseModelFactor {

From fcec6839d3562c4b34b62243f5ff13b126bad3fc Mon Sep 17 00:00:00 2001
From: Fan Jiang <prof.fan@foxmail.com>
Date: Tue, 3 May 2022 13:56:41 -0400
Subject: [PATCH 69/78] added SfmTracks and SfmCameras so we can easily access
 them in Python

---
 python/gtsam/preamble/sfm.h        | 11 ++++++++++-
 python/gtsam/specializations/sfm.h | 13 +++++++++++++
 2 files changed, 23 insertions(+), 1 deletion(-)

diff --git a/python/gtsam/preamble/sfm.h b/python/gtsam/preamble/sfm.h
index a34e73058..778375b6b 100644
--- a/python/gtsam/preamble/sfm.h
+++ b/python/gtsam/preamble/sfm.h
@@ -9,4 +9,13 @@
  * automatic STL binding, such that the raw objects can be accessed in Python.
  * Without this they will be automatically converted to a Python object, and all
  * mutations on Python side will not be reflected on C++.
- */
\ No newline at end of file
+ */
+
+#include <pybind11/stl.h>
+#include <pybind11/stl_bind.h>
+
+PYBIND11_MAKE_OPAQUE(
+    std::vector<gtsam::SfmTrack>);
+
+PYBIND11_MAKE_OPAQUE(
+    std::vector<gtsam::SfmCamera>);
\ No newline at end of file
diff --git a/python/gtsam/specializations/sfm.h b/python/gtsam/specializations/sfm.h
index 6de15217f..a7a1e822f 100644
--- a/python/gtsam/specializations/sfm.h
+++ b/python/gtsam/specializations/sfm.h
@@ -14,3 +14,16 @@
 py::bind_vector<std::vector<gtsam::BinaryMeasurement<gtsam::Unit3> > >(
     m_, "BinaryMeasurementsUnit3");
 py::bind_map<gtsam::KeyPairDoubleMap>(m_, "KeyPairDoubleMap");
+
+py::bind_vector<
+    std::vector<gtsam::SfmTrack> >(
+    m_, "SfmTracks");
+
+py::bind_vector<
+    std::vector<gtsam::SfmCamera> >(
+    m_, "SfmCameras");
+
+py::bind_vector<
+    std::vector<std::pair<size_t, gtsam::Point2>>>(
+        m_, "SfmMeasurementVector"
+    );

From 86b9761b5b4dcc4f6a361df376645c7ddf6f5cfc Mon Sep 17 00:00:00 2001
From: Fan Jiang <prof.fan@foxmail.com>
Date: Tue, 3 May 2022 13:57:48 -0400
Subject: [PATCH 70/78] Also copy the preambles and specializations

---
 python/CMakeLists.txt | 8 ++++++++
 1 file changed, 8 insertions(+)

diff --git a/python/CMakeLists.txt b/python/CMakeLists.txt
index d3cbff32d..0d143354d 100644
--- a/python/CMakeLists.txt
+++ b/python/CMakeLists.txt
@@ -107,6 +107,14 @@ file(GLOB GTSAM_PYTHON_UTIL_FILES "${CMAKE_CURRENT_SOURCE_DIR}/gtsam/utils/*.py"
 foreach(util_file ${GTSAM_PYTHON_UTIL_FILES})
         configure_file(${util_file} "${GTSAM_MODULE_PATH}/utils/${test_file}" COPYONLY)
 endforeach()
+file(GLOB GTSAM_PYTHON_PREAMBLE_FILES "${CMAKE_CURRENT_SOURCE_DIR}/gtsam/preamble/*.h")
+foreach(util_file ${GTSAM_PYTHON_PREAMBLE_FILES})
+        configure_file(${util_file} "${GTSAM_MODULE_PATH}/preamble/${test_file}" COPYONLY)
+endforeach()
+file(GLOB GTSAM_PYTHON_SPECIALIZATION_FILES "${CMAKE_CURRENT_SOURCE_DIR}/gtsam/specializations/*.h")
+foreach(util_file ${GTSAM_PYTHON_SPECIALIZATION_FILES})
+        configure_file(${util_file} "${GTSAM_MODULE_PATH}/specializations/${test_file}" COPYONLY)
+endforeach()
 
 # Common directory for data/datasets stored with the package.
 # This will store the data in the Python site package directly.

From 21b8365d7bda979b21bbcf949f7ea78682e05030 Mon Sep 17 00:00:00 2001
From: Fan Jiang <prof.fan@foxmail.com>
Date: Tue, 3 May 2022 16:02:00 -0400
Subject: [PATCH 71/78] Address comments

---
 gtsam/geometry/geometry.i | 32 ++++++++++++++++----------------
 gtsam/sfm/SfmData.h       |  8 ++++++--
 gtsam/sfm/sfm.i           |  8 ++++----
 3 files changed, 26 insertions(+), 22 deletions(-)

diff --git a/gtsam/geometry/geometry.i b/gtsam/geometry/geometry.i
index 63fd7056a..0dc23c160 100644
--- a/gtsam/geometry/geometry.i
+++ b/gtsam/geometry/geometry.i
@@ -927,6 +927,16 @@ class PinholeCamera {
   void serialize() const;
 };
 
+// Forward declaration of PinholeCameraCalX is defined here.
+#include <gtsam/geometry/SimpleCamera.h>
+// Some typedefs for common camera types
+// PinholeCameraCal3_S2 is the same as SimpleCamera above
+typedef gtsam::PinholeCamera<gtsam::Cal3_S2> PinholeCameraCal3_S2;
+typedef gtsam::PinholeCamera<gtsam::Cal3DS2> PinholeCameraCal3DS2;
+typedef gtsam::PinholeCamera<gtsam::Cal3Unified> PinholeCameraCal3Unified;
+typedef gtsam::PinholeCamera<gtsam::Cal3Bundler> PinholeCameraCal3Bundler;
+typedef gtsam::PinholeCamera<gtsam::Cal3Fisheye> PinholeCameraCal3Fisheye;
+
 #include <gtsam/geometry/PinholePose.h>
 template <CALIBRATION>
 class PinholePose {
@@ -978,6 +988,12 @@ class PinholePose {
   void serialize() const;
 };
 
+typedef gtsam::PinholePose<gtsam::Cal3_S2> PinholePoseCal3_S2;
+typedef gtsam::PinholePose<gtsam::Cal3DS2> PinholePoseCal3DS2;
+typedef gtsam::PinholePose<gtsam::Cal3Unified> PinholePoseCal3Unified;
+typedef gtsam::PinholePose<gtsam::Cal3Bundler> PinholePoseCal3Bundler;
+typedef gtsam::PinholePose<gtsam::Cal3Fisheye> PinholePoseCal3Fisheye;
+
 #include <gtsam/geometry/Similarity2.h>
 class Similarity2 {
   // Standard Constructors
@@ -1024,22 +1040,6 @@ class Similarity3 {
   double scale() const;
 };
 
-// Forward declaration of PinholeCameraCalX is defined here.
-#include <gtsam/geometry/SimpleCamera.h>
-// Some typedefs for common camera types
-// PinholeCameraCal3_S2 is the same as SimpleCamera above
-typedef gtsam::PinholeCamera<gtsam::Cal3_S2> PinholeCameraCal3_S2;
-typedef gtsam::PinholeCamera<gtsam::Cal3DS2> PinholeCameraCal3DS2;
-typedef gtsam::PinholeCamera<gtsam::Cal3Unified> PinholeCameraCal3Unified;
-typedef gtsam::PinholeCamera<gtsam::Cal3Bundler> PinholeCameraCal3Bundler;
-typedef gtsam::PinholeCamera<gtsam::Cal3Fisheye> PinholeCameraCal3Fisheye;
-
-typedef gtsam::PinholePose<gtsam::Cal3_S2> PinholePoseCal3_S2;
-typedef gtsam::PinholePose<gtsam::Cal3DS2> PinholePoseCal3DS2;
-typedef gtsam::PinholePose<gtsam::Cal3Unified> PinholePoseCal3Unified;
-typedef gtsam::PinholePose<gtsam::Cal3Bundler> PinholePoseCal3Bundler;
-typedef gtsam::PinholePose<gtsam::Cal3Fisheye> PinholePoseCal3Fisheye;
-
 template <T>
 class CameraSet {
   CameraSet();
diff --git a/gtsam/sfm/SfmData.h b/gtsam/sfm/SfmData.h
index afce12205..430e107ad 100644
--- a/gtsam/sfm/SfmData.h
+++ b/gtsam/sfm/SfmData.h
@@ -77,10 +77,14 @@ struct GTSAM_EXPORT SfmData {
   size_t numberCameras() const { return cameras.size(); }
 
   /// The track formed by series of landmark measurements
-  SfmTrack track(size_t idx) const { return tracks[idx]; }
+  const SfmTrack& track(size_t idx) const { return tracks[idx]; }
 
   /// The camera pose at frame index `idx`
-  SfmCamera camera(size_t idx) const { return cameras[idx]; }
+  const SfmCamera& camera(size_t idx) const { return cameras[idx]; }
+
+  /// Getters
+  const std::vector<SfmCamera>& cameraList() const { return cameras; }
+  const std::vector<SfmTrack>& trackList() const { return tracks; }
 
   /**
    * @brief Create projection factors using keys i and P(j)
diff --git a/gtsam/sfm/sfm.i b/gtsam/sfm/sfm.i
index 901270d9c..0a61874c1 100644
--- a/gtsam/sfm/sfm.i
+++ b/gtsam/sfm/sfm.i
@@ -36,15 +36,15 @@ class SfmData {
   static gtsam::SfmData FromBundlerFile(string filename);
   static gtsam::SfmData FromBalFile(string filename);
 
-  std::vector<gtsam::SfmTrack> tracks;
-  std::vector<gtsam::PinholeCamera<gtsam::Cal3Bundler>> cameras;
+  std::vector<gtsam::SfmTrack>& trackList() const;
+  std::vector<gtsam::PinholeCamera<gtsam::Cal3Bundler>>& cameraList() const;
 
   void addTrack(const gtsam::SfmTrack& t);
   void addCamera(const gtsam::SfmCamera& cam);
   size_t numberTracks() const;
   size_t numberCameras() const;
-  gtsam::SfmTrack track(size_t idx) const;
-  gtsam::PinholeCamera<gtsam::Cal3Bundler> camera(size_t idx) const;
+  gtsam::SfmTrack& track(size_t idx) const;
+  gtsam::PinholeCamera<gtsam::Cal3Bundler>& camera(size_t idx) const;
 
   gtsam::NonlinearFactorGraph generalSfmFactors(
       const gtsam::SharedNoiseModel& model =

From 28b216b72fc0e1cc95a13e2998f2972c377783d0 Mon Sep 17 00:00:00 2001
From: Fan Jiang <prof.fan@foxmail.com>
Date: Tue, 3 May 2022 16:24:09 -0400
Subject: [PATCH 72/78] Correct the binding for BinaryMeasurement

---
 gtsam/slam/dataset.h               | 1 +
 python/CMakeLists.txt              | 2 ++
 python/gtsam/preamble/sfm.h        | 6 +++++-
 python/gtsam/specializations/sfm.h | 2 ++
 4 files changed, 10 insertions(+), 1 deletion(-)

diff --git a/gtsam/slam/dataset.h b/gtsam/slam/dataset.h
index dc450a9f7..7c4d7fea7 100644
--- a/gtsam/slam/dataset.h
+++ b/gtsam/slam/dataset.h
@@ -223,6 +223,7 @@ parse3DFactors(const std::string &filename,
                size_t maxIndex = 0);
 
 using BinaryMeasurementsUnit3 = std::vector<BinaryMeasurement<Unit3>>;
+using BinaryMeasurementsRot3 = std::vector<BinaryMeasurement<Rot3>>;
 
 #ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42
 inline boost::optional<IndexedPose> GTSAM_DEPRECATED
diff --git a/python/CMakeLists.txt b/python/CMakeLists.txt
index 0d143354d..0c8dc314b 100644
--- a/python/CMakeLists.txt
+++ b/python/CMakeLists.txt
@@ -48,6 +48,7 @@ set(ignore
     gtsam::Rot3Vector
     gtsam::KeyVector
     gtsam::BinaryMeasurementsUnit3
+    gtsam::BinaryMeasurementsRot3
     gtsam::DiscreteKey
     gtsam::KeyPairDoubleMap)
 
@@ -137,6 +138,7 @@ if(GTSAM_UNSTABLE_BUILD_PYTHON)
             gtsam::KeyVector
             gtsam::FixedLagSmootherKeyTimestampMapValue
             gtsam::BinaryMeasurementsUnit3
+            gtsam::BinaryMeasurementsRot3
             gtsam::CameraSetCal3_S2
             gtsam::CameraSetCal3Bundler
             gtsam::CameraSetCal3Unified
diff --git a/python/gtsam/preamble/sfm.h b/python/gtsam/preamble/sfm.h
index 778375b6b..c74f37109 100644
--- a/python/gtsam/preamble/sfm.h
+++ b/python/gtsam/preamble/sfm.h
@@ -18,4 +18,8 @@ PYBIND11_MAKE_OPAQUE(
     std::vector<gtsam::SfmTrack>);
 
 PYBIND11_MAKE_OPAQUE(
-    std::vector<gtsam::SfmCamera>);
\ No newline at end of file
+    std::vector<gtsam::SfmCamera>);
+PYBIND11_MAKE_OPAQUE(
+    std::vector<gtsam::BinaryMeasurement<gtsam::Unit3>>);
+PYBIND11_MAKE_OPAQUE(
+    std::vector<gtsam::BinaryMeasurement<gtsam::Rot3>>);
diff --git a/python/gtsam/specializations/sfm.h b/python/gtsam/specializations/sfm.h
index a7a1e822f..90a2b8417 100644
--- a/python/gtsam/specializations/sfm.h
+++ b/python/gtsam/specializations/sfm.h
@@ -13,6 +13,8 @@
 
 py::bind_vector<std::vector<gtsam::BinaryMeasurement<gtsam::Unit3> > >(
     m_, "BinaryMeasurementsUnit3");
+py::bind_vector<std::vector<gtsam::BinaryMeasurement<gtsam::Rot3> > >(
+    m_, "BinaryMeasurementsRot3");
 py::bind_map<gtsam::KeyPairDoubleMap>(m_, "KeyPairDoubleMap");
 
 py::bind_vector<

From 4d275a45e64dc3c39b975e582bcdf05b8c248aa3 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Thu, 5 May 2022 11:23:51 -0400
Subject: [PATCH 73/78] remove key formatting from keys when defining vertices
 and edges to avoid syntax errors

---
 gtsam/inference/DotWriter.cpp | 8 ++++----
 1 file changed, 4 insertions(+), 4 deletions(-)

diff --git a/gtsam/inference/DotWriter.cpp b/gtsam/inference/DotWriter.cpp
index ad5330575..eac0c90f9 100644
--- a/gtsam/inference/DotWriter.cpp
+++ b/gtsam/inference/DotWriter.cpp
@@ -43,7 +43,7 @@ void DotWriter::drawVariable(Key key, const KeyFormatter& keyFormatter,
                              const boost::optional<Vector2>& position,
                              ostream* os) const {
   // Label the node with the label from the KeyFormatter
-  *os << "  var" << keyFormatter(key) << "[label=\"" << keyFormatter(key)
+  *os << "  var" << key << "[label=\"" << keyFormatter(key)
       << "\"";
   if (position) {
     *os << ", pos=\"" << position->x() << "," << position->y() << "!\"";
@@ -65,13 +65,13 @@ void DotWriter::DrawFactor(size_t i, const boost::optional<Vector2>& position,
 
 static void ConnectVariables(Key key1, Key key2,
                              const KeyFormatter& keyFormatter, ostream* os) {
-  *os << "  var" << keyFormatter(key1) << "--"
-      << "var" << keyFormatter(key2) << ";\n";
+  *os << "  var" << key1 << "--"
+      << "var" << key2 << ";\n";
 }
 
 static void ConnectVariableFactor(Key key, const KeyFormatter& keyFormatter,
                                   size_t i, ostream* os) {
-  *os << "  var" << keyFormatter(key) << "--"
+  *os << "  var" << key << "--"
       << "factor" << i << ";\n";
 }
 

From 51d1c27f2d8c829dfb4711739f2a0fd4d1cd3311 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Thu, 5 May 2022 11:24:26 -0400
Subject: [PATCH 74/78] add prior factor tests and remove TODO

---
 gtsam/nonlinear/Marginals.h          |  2 +-
 gtsam/nonlinear/PriorFactor.h        |  1 -
 gtsam/slam/tests/testPriorFactor.cpp | 40 +++++++++++++++++++++++++---
 3 files changed, 37 insertions(+), 6 deletions(-)

diff --git a/gtsam/nonlinear/Marginals.h b/gtsam/nonlinear/Marginals.h
index 028545d01..3c5aa9cab 100644
--- a/gtsam/nonlinear/Marginals.h
+++ b/gtsam/nonlinear/Marginals.h
@@ -121,7 +121,7 @@ public:
 
   /** Optimize the bayes tree */
   VectorValues optimize() const;
-            
+
 protected:
   
   /** Compute the Bayes Tree as a helper function to the constructor */
diff --git a/gtsam/nonlinear/PriorFactor.h b/gtsam/nonlinear/PriorFactor.h
index c745f7bd9..a490162ac 100644
--- a/gtsam/nonlinear/PriorFactor.h
+++ b/gtsam/nonlinear/PriorFactor.h
@@ -94,7 +94,6 @@ namespace gtsam {
     Vector evaluateError(const T& x, boost::optional<Matrix&> H = boost::none) const override {
       if (H) (*H) = Matrix::Identity(traits<T>::GetDimension(x),traits<T>::GetDimension(x));
       // manifold equivalent of z-x -> Local(x,z)
-      // TODO(ASL) Add Jacobians.
       return -traits<T>::Local(x, prior_);
     }
 
diff --git a/gtsam/slam/tests/testPriorFactor.cpp b/gtsam/slam/tests/testPriorFactor.cpp
index 2dc083cb2..d1a60e346 100644
--- a/gtsam/slam/tests/testPriorFactor.cpp
+++ b/gtsam/slam/tests/testPriorFactor.cpp
@@ -5,12 +5,16 @@
  * @date   Nov 4, 2014
  */
 
-#include <gtsam/base/Vector.h>
-#include <gtsam/nonlinear/PriorFactor.h>
 #include <CppUnitLite/TestHarness.h>
+#include <gtsam/base/Vector.h>
+#include <gtsam/navigation/ImuBias.h>
+#include <gtsam/nonlinear/PriorFactor.h>
+#include <gtsam/nonlinear/factorTesting.h>
 
 using namespace std;
+using namespace std::placeholders;
 using namespace gtsam;
+using namespace imuBias;
 
 /* ************************************************************************* */
 
@@ -23,16 +27,44 @@ TEST(PriorFactor, ConstructorScalar) {
 // Constructor vector3
 TEST(PriorFactor, ConstructorVector3) {
   SharedNoiseModel model = noiseModel::Isotropic::Sigma(3, 1.0);
-  PriorFactor<Vector3> factor(1, Vector3(1,2,3), model);
+  PriorFactor<Vector3> factor(1, Vector3(1, 2, 3), model);
 }
 
 // Constructor dynamic sized vector
 TEST(PriorFactor, ConstructorDynamicSizeVector) {
-  Vector v(5); v << 1, 2, 3, 4, 5;
+  Vector v(5);
+  v << 1, 2, 3, 4, 5;
   SharedNoiseModel model = noiseModel::Isotropic::Sigma(5, 1.0);
   PriorFactor<Vector> factor(1, v, model);
 }
 
+Vector callEvaluateError(const PriorFactor<ConstantBias>& factor,
+                         const ConstantBias& bias) {
+  return factor.evaluateError(bias);
+}
+
+// Test for imuBias::ConstantBias
+TEST(PriorFactor, ConstantBias) {
+  Vector3 biasAcc(1, 2, 3);
+  Vector3 biasGyro(0.1, 0.2, 0.3);
+  ConstantBias bias(biasAcc, biasGyro);
+
+  PriorFactor<ConstantBias> factor(1, bias,
+                                   noiseModel::Isotropic::Sigma(6, 0.1));
+  Values values;
+  values.insert(1, bias);
+
+  EXPECT_DOUBLES_EQUAL(0.0, factor.error(values), 1e-8);
+  EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-7, 1e-5);
+
+  ConstantBias incorrectBias(
+      (Vector6() << 1.1, 2.1, 3.1, 0.2, 0.3, 0.4).finished());
+  values.clear();
+  values.insert(1, incorrectBias);
+  EXPECT_DOUBLES_EQUAL(3.0, factor.error(values), 1e-8);
+  EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-7, 1e-5);
+}
+
 /* ************************************************************************* */
 int main() {
   TestResult tr;

From 0cfeb8621c5605b0d32fb469f4aa8d1600c4c8fd Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Thu, 5 May 2022 14:35:10 -0400
Subject: [PATCH 75/78] fix tests and remove additional keyformatter calls

---
 .../tests/testDiscreteFactorGraph.cpp         | 14 +++----
 gtsam/inference/BayesNet-inst.h               |  5 ++-
 gtsam/symbolic/tests/testSymbolicBayesNet.cpp | 18 ++++-----
 tests/testNonlinearFactorGraph.cpp            | 40 +++++++++----------
 4 files changed, 39 insertions(+), 38 deletions(-)

diff --git a/gtsam/discrete/tests/testDiscreteFactorGraph.cpp b/gtsam/discrete/tests/testDiscreteFactorGraph.cpp
index 0a7d869ec..3d9621aff 100644
--- a/gtsam/discrete/tests/testDiscreteFactorGraph.cpp
+++ b/gtsam/discrete/tests/testDiscreteFactorGraph.cpp
@@ -415,16 +415,16 @@ TEST(DiscreteFactorGraph, DotWithNames) {
       "graph {\n"
       "  size=\"5,5\";\n"
       "\n"
-      "  varC[label=\"C\"];\n"
-      "  varA[label=\"A\"];\n"
-      "  varB[label=\"B\"];\n"
+      "  var0[label=\"C\"];\n"
+      "  var1[label=\"A\"];\n"
+      "  var2[label=\"B\"];\n"
       "\n"
       "  factor0[label=\"\", shape=point];\n"
-      "  varC--factor0;\n"
-      "  varA--factor0;\n"
+      "  var0--factor0;\n"
+      "  var1--factor0;\n"
       "  factor1[label=\"\", shape=point];\n"
-      "  varC--factor1;\n"
-      "  varB--factor1;\n"
+      "  var0--factor1;\n"
+      "  var2--factor1;\n"
       "}\n";
   EXPECT(actual == expected);
 }
diff --git a/gtsam/inference/BayesNet-inst.h b/gtsam/inference/BayesNet-inst.h
index afde5498d..e792b5c03 100644
--- a/gtsam/inference/BayesNet-inst.h
+++ b/gtsam/inference/BayesNet-inst.h
@@ -53,8 +53,9 @@ void BayesNet<CONDITIONAL>::dot(std::ostream& os,
     auto frontals = conditional->frontals();
     const Key me = frontals.front();
     auto parents = conditional->parents();
-    for (const Key& p : parents)
-      os << "  var" << keyFormatter(p) << "->var" << keyFormatter(me) << "\n";
+    for (const Key& p : parents) {
+      os << "  var" << p << "->var" << me << "\n";
+    }
   }
 
   os << "}";
diff --git a/gtsam/symbolic/tests/testSymbolicBayesNet.cpp b/gtsam/symbolic/tests/testSymbolicBayesNet.cpp
index 2e13be10e..7795d5b89 100644
--- a/gtsam/symbolic/tests/testSymbolicBayesNet.cpp
+++ b/gtsam/symbolic/tests/testSymbolicBayesNet.cpp
@@ -104,16 +104,16 @@ TEST(SymbolicBayesNet, Dot) {
          "digraph {\n"
          "  size=\"5,5\";\n"
          "\n"
-         "  vara1[label=\"a1\", pos=\"1,2!\", shape=box];\n"
-         "  vara2[label=\"a2\", pos=\"2,2!\", shape=box];\n"
-         "  varx1[label=\"x1\", pos=\"1,1!\"];\n"
-         "  varx2[label=\"x2\", pos=\"2,1!\"];\n"
-         "  varx3[label=\"x3\", pos=\"3,1!\"];\n"
+         "  var6989586621679009793[label=\"a1\", pos=\"1,2!\", shape=box];\n"
+         "  var6989586621679009794[label=\"a2\", pos=\"2,2!\", shape=box];\n"
+         "  var8646911284551352321[label=\"x1\", pos=\"1,1!\"];\n"
+         "  var8646911284551352322[label=\"x2\", pos=\"2,1!\"];\n"
+         "  var8646911284551352323[label=\"x3\", pos=\"3,1!\"];\n"
          "\n"
-         "  varx1->varx2\n"
-         "  vara1->varx2\n"
-         "  varx2->varx3\n"
-         "  vara2->varx3\n"
+         "  var8646911284551352321->var8646911284551352322\n"
+         "  var6989586621679009793->var8646911284551352322\n"
+         "  var8646911284551352322->var8646911284551352323\n"
+         "  var6989586621679009794->var8646911284551352323\n"
          "}");
 }
 
diff --git a/tests/testNonlinearFactorGraph.cpp b/tests/testNonlinearFactorGraph.cpp
index 05a6e7f45..e1a88d616 100644
--- a/tests/testNonlinearFactorGraph.cpp
+++ b/tests/testNonlinearFactorGraph.cpp
@@ -335,21 +335,21 @@ TEST(NonlinearFactorGraph, dot) {
       "graph {\n"
       "  size=\"5,5\";\n"
       "\n"
-      "  varl1[label=\"l1\"];\n"
-      "  varx1[label=\"x1\"];\n"
-      "  varx2[label=\"x2\"];\n"
+      "  var7782220156096217089[label=\"l1\"];\n"
+      "  var8646911284551352321[label=\"x1\"];\n"
+      "  var8646911284551352322[label=\"x2\"];\n"
       "\n"
       "  factor0[label=\"\", shape=point];\n"
-      "  varx1--factor0;\n"
+      "  var8646911284551352321--factor0;\n"
       "  factor1[label=\"\", shape=point];\n"
-      "  varx1--factor1;\n"
-      "  varx2--factor1;\n"
+      "  var8646911284551352321--factor1;\n"
+      "  var8646911284551352322--factor1;\n"
       "  factor2[label=\"\", shape=point];\n"
-      "  varx1--factor2;\n"
-      "  varl1--factor2;\n"
+      "  var8646911284551352321--factor2;\n"
+      "  var7782220156096217089--factor2;\n"
       "  factor3[label=\"\", shape=point];\n"
-      "  varx2--factor3;\n"
-      "  varl1--factor3;\n"
+      "  var8646911284551352322--factor3;\n"
+      "  var7782220156096217089--factor3;\n"
       "}\n";
 
   const NonlinearFactorGraph fg = createNonlinearFactorGraph();
@@ -363,21 +363,21 @@ TEST(NonlinearFactorGraph, dot_extra) {
       "graph {\n"
       "  size=\"5,5\";\n"
       "\n"
-      "  varl1[label=\"l1\", pos=\"0,0!\"];\n"
-      "  varx1[label=\"x1\", pos=\"1,0!\"];\n"
-      "  varx2[label=\"x2\", pos=\"1,1.5!\"];\n"
+      "  var7782220156096217089[label=\"l1\", pos=\"0,0!\"];\n"
+      "  var8646911284551352321[label=\"x1\", pos=\"1,0!\"];\n"
+      "  var8646911284551352322[label=\"x2\", pos=\"1,1.5!\"];\n"
       "\n"
       "  factor0[label=\"\", shape=point];\n"
-      "  varx1--factor0;\n"
+      "  var8646911284551352321--factor0;\n"
       "  factor1[label=\"\", shape=point];\n"
-      "  varx1--factor1;\n"
-      "  varx2--factor1;\n"
+      "  var8646911284551352321--factor1;\n"
+      "  var8646911284551352322--factor1;\n"
       "  factor2[label=\"\", shape=point];\n"
-      "  varx1--factor2;\n"
-      "  varl1--factor2;\n"
+      "  var8646911284551352321--factor2;\n"
+      "  var7782220156096217089--factor2;\n"
       "  factor3[label=\"\", shape=point];\n"
-      "  varx2--factor3;\n"
-      "  varl1--factor3;\n"
+      "  var8646911284551352322--factor3;\n"
+      "  var7782220156096217089--factor3;\n"
       "}\n";
 
   const NonlinearFactorGraph fg = createNonlinearFactorGraph();

From 1d77ba55e400560e59dce39961d56ea59ccd240b Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Thu, 5 May 2022 16:49:03 -0400
Subject: [PATCH 76/78] fix python test

---
 python/gtsam/tests/test_DiscreteBayesNet.py | 10 +++++-----
 1 file changed, 5 insertions(+), 5 deletions(-)

diff --git a/python/gtsam/tests/test_DiscreteBayesNet.py b/python/gtsam/tests/test_DiscreteBayesNet.py
index 74191dcc7..10c5db612 100644
--- a/python/gtsam/tests/test_DiscreteBayesNet.py
+++ b/python/gtsam/tests/test_DiscreteBayesNet.py
@@ -11,12 +11,12 @@ Author: Frank Dellaert
 
 # pylint: disable=no-name-in-module, invalid-name
 
-import unittest
 import textwrap
+import unittest
 
 import gtsam
-from gtsam import (DiscreteBayesNet, DiscreteConditional, DiscreteFactorGraph,
-                   DiscreteKeys, DiscreteDistribution, DiscreteValues, Ordering)
+from gtsam import (DiscreteBayesNet, DiscreteConditional, DiscreteDistribution,
+                   DiscreteFactorGraph, DiscreteKeys, DiscreteValues, Ordering)
 from gtsam.utils.test_case import GtsamTestCase
 
 # Some keys:
@@ -152,10 +152,10 @@ class TestDiscreteBayesNet(GtsamTestCase):
               var4[label="4"];
               var5[label="5"];
               var6[label="6"];
-              vara0[label="a0", pos="0,2!"];
+              var6989586621679009792[label="a0", pos="0,2!"];
 
               var4->var6
-              vara0->var3
+              var6989586621679009792->var3
               var3->var5
               var6->var5
             }"""

From e2a26346528f997e036bc52d00ffe91dbf9d3d9e Mon Sep 17 00:00:00 2001
From: Fan Jiang <prof.fan@foxmail.com>
Date: Sat, 7 May 2022 15:53:36 -0700
Subject: [PATCH 77/78] Finally debugged the root cause!

---
 python/gtsam/preamble/sfm.h | 3 ++-
 1 file changed, 2 insertions(+), 1 deletion(-)

diff --git a/python/gtsam/preamble/sfm.h b/python/gtsam/preamble/sfm.h
index c74f37109..8ff0ea82e 100644
--- a/python/gtsam/preamble/sfm.h
+++ b/python/gtsam/preamble/sfm.h
@@ -11,7 +11,8 @@
  * mutations on Python side will not be reflected on C++.
  */
 
-#include <pybind11/stl.h>
+// Including <stl.h> can cause some serious hard-to-debug bugs!!!
+// #include <pybind11/stl.h>
 #include <pybind11/stl_bind.h>
 
 PYBIND11_MAKE_OPAQUE(

From 02dce469d185f105f46eb42c147712ece2ff2f3d Mon Sep 17 00:00:00 2001
From: Fan Jiang <prof.fan@foxmail.com>
Date: Sat, 7 May 2022 15:55:06 -0700
Subject: [PATCH 78/78] Add fallback entry for MATLAB

---
 gtsam/sfm/sfm.i | 7 +++++++
 1 file changed, 7 insertions(+)

diff --git a/gtsam/sfm/sfm.i b/gtsam/sfm/sfm.i
index 0a61874c1..19e21b10c 100644
--- a/gtsam/sfm/sfm.i
+++ b/gtsam/sfm/sfm.i
@@ -96,6 +96,13 @@ class BinaryMeasurementsUnit3 {
   void push_back(const gtsam::BinaryMeasurement<gtsam::Unit3>& measurement);
 };
 
+class BinaryMeasurementsRot3 {
+  BinaryMeasurementsRot3();
+  size_t size() const;
+  gtsam::BinaryMeasurement<gtsam::Rot3> at(size_t idx) const;
+  void push_back(const gtsam::BinaryMeasurement<gtsam::Rot3>& measurement);
+};
+
 #include <gtsam/sfm/ShonanAveraging.h>
 
 // TODO(frank): copy/pasta below until we have integer template paremeters in