From 04fb3390bebfddfe13dd758417779ca6ff1105d4 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Tue, 1 Dec 2020 16:02:25 -0500
Subject: [PATCH 01/18] Base class for all calibration models

---
 gtsam/geometry/Cal3.cpp |  72 ++++++++++++++++++++++
 gtsam/geometry/Cal3.h   | 131 +++++++++++++++++++++++++++++++++++++++-
 2 files changed, 201 insertions(+), 2 deletions(-)
 create mode 100644 gtsam/geometry/Cal3.cpp

diff --git a/gtsam/geometry/Cal3.cpp b/gtsam/geometry/Cal3.cpp
new file mode 100644
index 000000000..240d01e12
--- /dev/null
+++ b/gtsam/geometry/Cal3.cpp
@@ -0,0 +1,72 @@
+/* ----------------------------------------------------------------------------
+
+ * 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   Cal3.cpp
+ * @brief  Common code for all calibration models.
+ * @author Frank Dellaert
+ */
+
+#include <gtsam/geometry/Cal3.h>
+
+#include <cmath>
+#include <fstream>
+#include <iostream>
+
+namespace gtsam {
+using namespace std;
+
+/* ************************************************************************* */
+Cal3::Cal3(double fov, int w, int h)
+    : s_(0), u0_((double)w / 2.0), v0_((double)h / 2.0) {
+  double a = fov * M_PI / 360.0;  // fov/2 in radians
+  fx_ =
+      (double)w / (2.0 * tan(a));  //    old formula: fx_ = (double) w * tan(a);
+  fy_ = fx_;
+}
+
+/* ************************************************************************* */
+Cal3::Cal3(const std::string& path)
+    : fx_(320), fy_(320), s_(0), u0_(320), v0_(140) {
+  char buffer[200];
+  buffer[0] = 0;
+  sprintf(buffer, "%s/calibration_info.txt", path.c_str());
+  std::ifstream infile(buffer, std::ios::in);
+
+  if (infile)
+    infile >> fx_ >> fy_ >> s_ >> u0_ >> v0_;
+  else {
+    throw std::runtime_error("Cal3: Unable to load the calibration");
+  }
+
+  infile.close();
+}
+
+/* ************************************************************************* */
+ostream& operator<<(ostream& os, const Cal3& cal) {
+  os << "{fx: " << cal.fx() << ", fy: " << cal.fy() << ", s:" << cal.skew()
+     << ", px:" << cal.px() << ", py:" << cal.py() << "}";
+  return os;
+}
+
+/* ************************************************************************* */
+void Cal3::print(const std::string& s) const { gtsam::print((Matrix)K(), s); }
+
+/* ************************************************************************* */
+bool Cal3::equals(const Cal3& K, double tol) const {
+  return (std::abs(fx_ - K.fx_) < tol) && (std::abs(fy_ - K.fy_) < tol) &&
+         (std::abs(s_ - K.s_) < tol) && (std::abs(u0_ - K.u0_) < tol) &&
+         (std::abs(v0_ - K.v0_) < tol);
+}
+
+/* ************************************************************************* */
+
+}  // namespace gtsam
diff --git a/gtsam/geometry/Cal3.h b/gtsam/geometry/Cal3.h
index d9e12f7d2..13cd4de69 100644
--- a/gtsam/geometry/Cal3.h
+++ b/gtsam/geometry/Cal3.h
@@ -30,7 +30,7 @@ namespace gtsam {
  * Jacobians of `calibrate` using `uncalibrate`.
  * This is useful when there are iterative operations in the `calibrate`
  * function which make computing jacobians difficult.
- * 
+ *
  * Given f(pi, pn) = uncalibrate(pn) - pi, and g(pi) = calibrate, we can
  * easily compute the Jacobians:
  * df/pi = -I (pn and pi are independent args)
@@ -61,6 +61,133 @@ void calibrateJacobians(const Cal& calibration, const Point2& pn,
   }
 }
 
-//TODO(Varun) Make common base class for all calibration models.
+/**
+ * @brief Common base class for all calibration models.
+ * @addtogroup geometry
+ * \nosubgrouping
+ */
+class GTSAM_EXPORT Cal3 {
+ protected:
+  double fx_, fy_, s_, u0_, v0_;
+
+ public:
+  enum { dimension = 5 };
+  typedef boost::shared_ptr<Cal3>
+      shared_ptr;  ///< shared pointer to calibration object
+
+  /// @name Standard Constructors
+  /// @{
+
+  /// Create a default calibration that leaves coordinates unchanged
+  Cal3() : fx_(1), fy_(1), s_(0), u0_(0), v0_(0) {}
+
+  /// constructor from doubles
+  Cal3(double fx, double fy, double s, double u0, double v0)
+      : fx_(fx), fy_(fy), s_(s), u0_(u0), v0_(v0) {}
+
+  /// constructor from vector
+  Cal3(const Vector& d)
+      : fx_(d(0)), fy_(d(1)), s_(d(2)), u0_(d(3)), v0_(d(4)) {}
+
+  /**
+   * Easy constructor, takes fov in degrees, asssumes zero skew, unit aspect
+   * @param fov field of view in degrees
+   * @param w image width
+   * @param h image height
+   */
+  Cal3(double fov, int w, int h);
+
+  /// @}
+  /// @name Advanced Constructors
+  /// @{
+
+  /// load calibration from location (default name is calibration_info.txt)
+  Cal3(const std::string& path);
+
+  /// @}
+  /// @name Testable
+  /// @{
+
+  /// Output stream operator
+  GTSAM_EXPORT friend std::ostream& operator<<(std::ostream& os,
+                                               const Cal3& cal);
+
+  /// print with optional string
+  virtual void print(const std::string& s = "Cal3") const;
+
+  /// Check if equal up to specified tolerance
+  bool equals(const Cal3& K, double tol = 10e-9) const;
+
+  /// @}
+  /// @name Standard Interface
+  /// @{
+
+  /// focal length x
+  inline double fx() const { return fx_; }
+
+  /// focal length y
+  inline double fy() const { return fy_; }
+
+  /// aspect ratio
+  inline double aspectRatio() const { return fx_ / fy_; }
+
+  /// skew
+  inline double skew() const { return s_; }
+
+  /// image center in x
+  inline double px() const { return u0_; }
+
+  /// image center in y
+  inline double py() const { return v0_; }
+
+  /// return the principal point
+  Point2 principalPoint() const { return Point2(u0_, v0_); }
+
+  /// vectorized form (column-wise)
+  Vector5 vector() const {
+    Vector5 v;
+    v << fx_, fy_, s_, u0_, v0_;
+    return v;
+  }
+
+  /// return calibration matrix K
+  Matrix3 K() const {
+    Matrix3 K;
+    K << fx_, s_, u0_, 0.0, fy_, v0_, 0.0, 0.0, 1.0;
+    return K;
+  }
+
+#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V41
+  /** @deprecated The following function has been deprecated, use K above */
+  Matrix3 matrix() const { return K(); }
+#endif
+
+  /// return inverted calibration matrix inv(K)
+  Matrix3 matrix_inverse() const {
+    const double fxy = fx_ * fy_, sv0 = s_ * v0_, fyu0 = fy_ * u0_;
+    Matrix3 K_inverse;
+    K_inverse << 1.0 / fx_, -s_ / fxy, (sv0 - fyu0) / fxy, 0.0, 1.0 / fy_,
+        -v0_ / fy_, 0.0, 0.0, 1.0;
+    return K_inverse;
+  }
+
+  /// @}
+  /// @name Advanced Interface
+  /// @{
+
+ private:
+  /// Serialization function
+  friend class boost::serialization::access;
+  template <class Archive>
+  void serialize(Archive& ar, const unsigned int /*version*/) {
+    ar& BOOST_SERIALIZATION_NVP(fx_);
+    ar& BOOST_SERIALIZATION_NVP(fy_);
+    ar& BOOST_SERIALIZATION_NVP(s_);
+    ar& BOOST_SERIALIZATION_NVP(u0_);
+    ar& BOOST_SERIALIZATION_NVP(v0_);
+  }
+
+  /// @}
+};
 
 }  // \ namespace gtsam

From ad66a5927dc2512b14e1f0cfa9931c9e0b39ca9d Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Tue, 1 Dec 2020 16:03:47 -0500
Subject: [PATCH 02/18] Refactor Cal3_S2 and Cal3_S2Stereo classes

---
 gtsam/geometry/Cal3_S2.cpp           |  87 ++++---------
 gtsam/geometry/Cal3_S2.h             | 188 ++++++++-------------------
 gtsam/geometry/Cal3_S2Stereo.cpp     |  13 +-
 gtsam/geometry/Cal3_S2Stereo.h       |  59 +++------
 gtsam/geometry/tests/testCal3_S2.cpp |   1 -
 5 files changed, 102 insertions(+), 246 deletions(-)

diff --git a/gtsam/geometry/Cal3_S2.cpp b/gtsam/geometry/Cal3_S2.cpp
index b3d1be4b6..12635abdd 100644
--- a/gtsam/geometry/Cal3_S2.cpp
+++ b/gtsam/geometry/Cal3_S2.cpp
@@ -24,93 +24,52 @@
 namespace gtsam {
 using namespace std;
 
-/* ************************************************************************* */
-Cal3_S2::Cal3_S2(double fov, int w, int h) :
-    s_(0), u0_((double) w / 2.0), v0_((double) h / 2.0) {
-  double a = fov * M_PI / 360.0; // fov/2 in radians
-  fx_ = (double) w / (2.0 * tan(a)); //    old formula: fx_ = (double) w * tan(a);
-  fy_ = fx_;
-}
-
-/* ************************************************************************* */
-Cal3_S2::Cal3_S2(const std::string &path) :
-    fx_(320), fy_(320), s_(0), u0_(320), v0_(140) {
-
-  char buffer[200];
-  buffer[0] = 0;
-  sprintf(buffer, "%s/calibration_info.txt", path.c_str());
-  std::ifstream infile(buffer, std::ios::in);
-
-  if (infile)
-    infile >> fx_ >> fy_ >> s_ >> u0_ >> v0_;
-  else {
-    throw std::runtime_error("Cal3_S2: Unable to load the calibration");
-  }
-
-  infile.close();
-}
-
 /* ************************************************************************* */
 ostream& operator<<(ostream& os, const Cal3_S2& cal) {
-  os << "{fx: " << cal.fx() << ", fy: " << cal.fy() << ", s:" << cal.skew() << ", px:" << cal.px()
-     << ", py:" << cal.py() << "}";
+  os << "{fx: " << cal.fx() << ", fy: " << cal.fy() << ", s:" << cal.skew()
+     << ", px:" << cal.px() << ", py:" << cal.py() << "}";
   return os;
 }
 
 /* ************************************************************************* */
 void Cal3_S2::print(const std::string& s) const {
-  gtsam::print((Matrix)matrix(), s);
+  gtsam::print((Matrix)K(), s);
 }
 
 /* ************************************************************************* */
 bool Cal3_S2::equals(const Cal3_S2& K, double tol) const {
-  if (std::abs(fx_ - K.fx_) > tol)
-    return false;
-  if (std::abs(fy_ - K.fy_) > tol)
-    return false;
-  if (std::abs(s_ - K.s_) > tol)
-    return false;
-  if (std::abs(u0_ - K.u0_) > tol)
-    return false;
-  if (std::abs(v0_ - K.v0_) > tol)
-    return false;
-  return true;
+  return Cal3::equals(K, tol);
 }
 
 /* ************************************************************************* */
 Point2 Cal3_S2::uncalibrate(const Point2& p, OptionalJacobian<2, 5> Dcal,
-    OptionalJacobian<2, 2> Dp) const {
+                            OptionalJacobian<2, 2> Dp) const {
   const double x = p.x(), y = p.y();
-  if (Dcal)
-    *Dcal << x, 0.0, y, 1.0, 0.0, 0.0, y, 0.0, 0.0, 1.0;
-  if (Dp)
-    *Dp << fx_, s_, 0.0, fy_;
+  if (Dcal) *Dcal << x, 0.0, y, 1.0, 0.0, 0.0, y, 0.0, 0.0, 1.0;
+  if (Dp) *Dp << fx_, s_, 0.0, fy_;
   return Point2(fx_ * x + s_ * y + u0_, fy_ * y + v0_);
 }
 
 /* ************************************************************************* */
-Point2 Cal3_S2::calibrate(const Point2& p, OptionalJacobian<2,5> Dcal,
-                           OptionalJacobian<2,2> Dp) const {
-    const double u = p.x(), v = p.y();
-    double delta_u = u - u0_, delta_v = v - v0_;
-    double inv_fx = 1/ fx_, inv_fy = 1/fy_;
-    double inv_fy_delta_v = inv_fy * delta_v, inv_fx_s_inv_fy = inv_fx * s_ * inv_fy;
-    Point2 point(inv_fx * (delta_u - s_ * inv_fy_delta_v),
-                  inv_fy_delta_v);
-    if(Dcal)
-       *Dcal << - inv_fx * point.x(), inv_fx * s_ * inv_fy * inv_fy_delta_v,  -inv_fx * point.y(),
-            -inv_fx,  inv_fx_s_inv_fy,
-            0, -inv_fy * point.y(), 0,  0, -inv_fy;
-    if(Dp)
-        *Dp << inv_fx, -inv_fx_s_inv_fy, 0, inv_fy;
-    return point;
+Point2 Cal3_S2::calibrate(const Point2& p, OptionalJacobian<2, 5> Dcal,
+                          OptionalJacobian<2, 2> Dp) const {
+  const double u = p.x(), v = p.y();
+  double delta_u = u - u0_, delta_v = v - v0_;
+  double inv_fx = 1 / fx_, inv_fy = 1 / fy_;
+  double inv_fy_delta_v = inv_fy * delta_v,
+         inv_fx_s_inv_fy = inv_fx * s_ * inv_fy;
+  Point2 point(inv_fx * (delta_u - s_ * inv_fy_delta_v), inv_fy_delta_v);
+  if (Dcal)
+    *Dcal << -inv_fx * point.x(), inv_fx * s_ * inv_fy * inv_fy_delta_v,
+        -inv_fx * point.y(), -inv_fx, inv_fx_s_inv_fy, 0, -inv_fy * point.y(),
+        0, 0, -inv_fy;
+  if (Dp) *Dp << inv_fx, -inv_fx_s_inv_fy, 0, inv_fy;
+  return point;
 }
 
 /* ************************************************************************* */
-Vector3 Cal3_S2::calibrate(const Vector3& p) const {
-  return matrix_inverse() * p;
-}
+Vector3 Cal3_S2::calibrate(const Vector3& p) const { return matrix_inverse() * p; }
 
 /* ************************************************************************* */
 
-} // namespace gtsam
+}  // namespace gtsam
diff --git a/gtsam/geometry/Cal3_S2.h b/gtsam/geometry/Cal3_S2.h
index f2848d0a3..aef27323e 100644
--- a/gtsam/geometry/Cal3_S2.h
+++ b/gtsam/geometry/Cal3_S2.h
@@ -21,6 +21,7 @@
 
 #pragma once
 
+#include <gtsam/geometry/Cal3.h>
 #include <gtsam/geometry/Point2.h>
 
 namespace gtsam {
@@ -30,31 +31,24 @@ namespace gtsam {
  * @addtogroup geometry
  * \nosubgrouping
  */
-class GTSAM_EXPORT Cal3_S2 {
-private:
-  double fx_, fy_, s_, u0_, v0_;
-
-public:
+class GTSAM_EXPORT Cal3_S2 : public Cal3 {
+ public:
   enum { dimension = 5 };
-  typedef boost::shared_ptr<Cal3_S2> shared_ptr; ///< shared pointer to calibration object
+  typedef boost::shared_ptr<Cal3_S2>
+      shared_ptr;  ///< shared pointer to calibration object
 
   /// @name Standard Constructors
   /// @{
 
   /// Create a default calibration that leaves coordinates unchanged
-  Cal3_S2() :
-      fx_(1), fy_(1), s_(0), u0_(0), v0_(0) {
-  }
+  Cal3_S2() : Cal3() {}
 
   /// constructor from doubles
-  Cal3_S2(double fx, double fy, double s, double u0, double v0) :
-      fx_(fx), fy_(fy), s_(s), u0_(u0), v0_(v0) {
-  }
+  Cal3_S2(double fx, double fy, double s, double u0, double v0)
+      : Cal3(fx, fy, s, u0, v0) {}
 
   /// constructor from vector
-  Cal3_S2(const Vector &d) :
-      fx_(d(0)), fy_(d(1)), s_(d(2)), u0_(d(3)), v0_(d(4)) {
-  }
+  Cal3_S2(const Vector& d) : Cal3(d) {}
 
   /**
    * Easy constructor, takes fov in degrees, asssumes zero skew, unit aspect
@@ -62,132 +56,59 @@ public:
    * @param w image width
    * @param h image height
    */
-  Cal3_S2(double fov, int w, int h);
+  Cal3_S2(double fov, int w, int h) : Cal3(fov, w, h) {}
 
-  /// @}
-  /// @name Advanced Constructors
-  /// @{
+  /**
+   * Convert intrinsic coordinates xy to image coordinates uv, fixed derivaitves
+   * @param p point in intrinsic coordinates
+   * @param Dcal optional 2*5 Jacobian wrpt Cal3 parameters
+   * @param Dp optional 2*2 Jacobian wrpt intrinsic coordinates
+   * @return point in image coordinates
+   */
+  Point2 uncalibrate(const Point2& p, OptionalJacobian<2, 5> Dcal = boost::none,
+                     OptionalJacobian<2, 2> Dp = boost::none) const;
 
-  /// load calibration from location (default name is calibration_info.txt)
-  Cal3_S2(const std::string &path);
+  /**
+   * Convert image coordinates uv to intrinsic coordinates xy
+   * @param p point in image coordinates
+   * @param Dcal optional 2*5 Jacobian wrpt Cal3 parameters
+   * @param Dp optional 2*2 Jacobian wrpt intrinsic coordinates
+   * @return point in intrinsic coordinates
+   */
+  Point2 calibrate(const Point2& p, OptionalJacobian<2, 5> Dcal = boost::none,
+                   OptionalJacobian<2, 2> Dp = boost::none) const;
+
+  /**
+   * Convert homogeneous image coordinates to intrinsic coordinates
+   * @param p point in image coordinates
+   * @return point in intrinsic coordinates
+   */
+  Vector3 calibrate(const Vector3& p) const;
 
   /// @}
   /// @name Testable
   /// @{
 
   /// Output stream operator
-  GTSAM_EXPORT friend std::ostream &operator<<(std::ostream &os, const Cal3_S2& cal);
+  GTSAM_EXPORT friend std::ostream& operator<<(std::ostream& os,
+                                               const Cal3_S2& cal);
 
   /// print with optional string
-  void print(const std::string& s = "Cal3_S2") const;
+  void print(const std::string& s = "Cal3_S2") const override;
 
   /// Check if equal up to specified tolerance
   bool equals(const Cal3_S2& K, double tol = 10e-9) const;
 
-  /// @}
-  /// @name Standard Interface
-  /// @{
-
-  /// focal length x
-  inline double fx() const {
-    return fx_;
-  }
-
-  /// focal length y
-  inline double fy() const {
-    return fy_;
-  }
-
-  /// aspect ratio
-  inline double aspectRatio() const {
-    return fx_/fy_;
-  }
-
-  /// skew
-  inline double skew() const {
-    return s_;
-  }
-
-  /// image center in x
-  inline double px() const {
-    return u0_;
-  }
-
-  /// image center in y
-  inline double py() const {
-    return v0_;
-  }
-
-  /// return the principal point
-  Point2 principalPoint() const {
-    return Point2(u0_, v0_);
-  }
-
-  /// vectorized form (column-wise)
-  Vector5 vector() const {
-    Vector5 v;
-    v << fx_, fy_, s_, u0_, v0_;
-    return v;
-  }
-
-  /// return calibration matrix K
-  Matrix3 K() const {
-    Matrix3 K;
-    K <<  fx_, s_, u0_, 0.0, fy_, v0_, 0.0, 0.0, 1.0;
-    return K;
-  }
-
-  /** @deprecated The following function has been deprecated, use K above */
-  Matrix3 matrix() const {
-    return K();
-  }
-
-  /// return inverted calibration matrix inv(K)
-  Matrix3 matrix_inverse() const {
-    const double fxy = fx_ * fy_, sv0 = s_ * v0_, fyu0 = fy_ * u0_;
-    Matrix3 K_inverse;
-    K_inverse << 1.0 / fx_, -s_ / fxy, (sv0 - fyu0) / fxy, 0.0,
-        1.0 / fy_, -v0_ / fy_, 0.0, 0.0, 1.0;
-    return K_inverse;
-  }
-
-  /**
-   * convert intrinsic coordinates xy to image coordinates uv, fixed derivaitves
-   * @param p point in intrinsic coordinates
-   * @param Dcal optional 2*5 Jacobian wrpt Cal3_S2 parameters
-   * @param Dp optional 2*2 Jacobian wrpt intrinsic coordinates
-   * @return point in image coordinates
-   */
-  Point2 uncalibrate(const Point2& p, OptionalJacobian<2,5> Dcal = boost::none,
-      OptionalJacobian<2,2> Dp = boost::none) const;
-
-  /**
-   * convert image coordinates uv to intrinsic coordinates xy
-   * @param p point in image coordinates
-   * @param Dcal optional 2*5 Jacobian wrpt Cal3_S2 parameters
-   * @param Dp optional 2*2 Jacobian wrpt intrinsic coordinates
-   * @return point in intrinsic coordinates
-   */
-  Point2 calibrate(const Point2& p, OptionalJacobian<2,5> Dcal = boost::none,
-                   OptionalJacobian<2,2> Dp = boost::none) const;
-
-  /**
-   * convert homogeneous image coordinates to intrinsic coordinates
-   * @param p point in image coordinates
-   * @return point in intrinsic coordinates
-   */
-  Vector3 calibrate(const Vector3& p) const;
-
   /// "Between", subtracts calibrations. between(p,q) == compose(inverse(p),q)
   inline Cal3_S2 between(const Cal3_S2& q,
-      OptionalJacobian<5,5> H1=boost::none,
-      OptionalJacobian<5,5> H2=boost::none) const {
-    if(H1) *H1 = -I_5x5;
-    if(H2) *H2 =  I_5x5;
-    return Cal3_S2(q.fx_-fx_, q.fy_-fy_, q.s_-s_, q.u0_-u0_, q.v0_-v0_);
+                         OptionalJacobian<5, 5> H1 = boost::none,
+                         OptionalJacobian<5, 5> H2 = boost::none) const {
+    if (H1) *H1 = -I_5x5;
+    if (H2) *H2 = I_5x5;
+    return Cal3_S2(q.fx_ - fx_, q.fy_ - fy_, q.s_ - s_, q.u0_ - u0_,
+                   q.v0_ - v0_);
   }
 
-
   /// @}
   /// @name Manifold
   /// @{
@@ -212,27 +133,22 @@ public:
   /// @name Advanced Interface
   /// @{
 
-private:
-
+ private:
   /// Serialization function
   friend class boost::serialization::access;
-  template<class Archive>
-  void serialize(Archive & ar, const unsigned int /*version*/) {
-    ar & BOOST_SERIALIZATION_NVP(fx_);
-    ar & BOOST_SERIALIZATION_NVP(fy_);
-    ar & BOOST_SERIALIZATION_NVP(s_);
-    ar & BOOST_SERIALIZATION_NVP(u0_);
-    ar & BOOST_SERIALIZATION_NVP(v0_);
+  template <class Archive>
+  void serialize(Archive& ar, const unsigned int /*version*/) {
+    ar& boost::serialization::make_nvp(
+        "Cal3_S2", boost::serialization::base_object<Cal3>(*this));
   }
 
   /// @}
-
 };
 
-template<>
+template <>
 struct traits<Cal3_S2> : public internal::Manifold<Cal3_S2> {};
 
-template<>
+template <>
 struct traits<const Cal3_S2> : public internal::Manifold<Cal3_S2> {};
 
-} // \ namespace gtsam
+}  // \ namespace gtsam
diff --git a/gtsam/geometry/Cal3_S2Stereo.cpp b/gtsam/geometry/Cal3_S2Stereo.cpp
index 9b5aea4ed..b4e70202e 100644
--- a/gtsam/geometry/Cal3_S2Stereo.cpp
+++ b/gtsam/geometry/Cal3_S2Stereo.cpp
@@ -24,16 +24,17 @@ using namespace std;
 
 /* ************************************************************************* */
 void Cal3_S2Stereo::print(const std::string& s) const {
-     K_.print(s+"K: ");
-     std::cout << s << "Baseline: " << b_ << std::endl;
-   }
+  std::cout << s << (s != "" ? " " : "");
+  print("K: ");
+  std::cout << "Baseline: " << b_ << std::endl;
+}
 
 /* ************************************************************************* */
 bool Cal3_S2Stereo::equals(const Cal3_S2Stereo& other, double tol) const {
-  if (std::abs(b_ - other.b_) > tol) return false;
-  return K_.equals(other.K_,tol);
+  const Cal3_S2* base = dynamic_cast<const Cal3_S2*>(&other);
+  return Cal3_S2::equals(*base, tol) && (std::abs(b_ - other.baseline()) < tol);
 }
 
 /* ************************************************************************* */
 
-} // namespace gtsam
+}  // namespace gtsam
diff --git a/gtsam/geometry/Cal3_S2Stereo.h b/gtsam/geometry/Cal3_S2Stereo.h
index a6eb41b60..258cd0434 100644
--- a/gtsam/geometry/Cal3_S2Stereo.h
+++ b/gtsam/geometry/Cal3_S2Stereo.h
@@ -27,43 +27,40 @@ namespace gtsam {
    * @addtogroup geometry
    * \nosubgrouping
    */
-  class GTSAM_EXPORT Cal3_S2Stereo {
+  class GTSAM_EXPORT Cal3_S2Stereo : public Cal3_S2 {
   private:
-
-    Cal3_S2 K_;
     double b_;
 
   public:
 
     enum { dimension = 6 };
-    typedef boost::shared_ptr<Cal3_S2Stereo> shared_ptr;  ///< shared pointer to stereo calibration object
+    ///< shared pointer to stereo calibration object
+    typedef boost::shared_ptr<Cal3_S2Stereo> shared_ptr;
 
     /// @name Standard Constructors
     /// @
 
     /// default calibration leaves coordinates unchanged
-    Cal3_S2Stereo() :
-      K_(1, 1, 0, 0, 0), b_(1.0) {
-    }
+    Cal3_S2Stereo() : Cal3_S2(1, 1, 0, 0, 0), b_(1.0) {}
 
     /// constructor from doubles
-    Cal3_S2Stereo(double fx, double fy, double s, double u0, double v0, double b) :
-      K_(fx, fy, s, u0, v0), b_(b) {
-    }
+    Cal3_S2Stereo(double fx, double fy, double s, double u0, double v0,
+                  double b)
+        : Cal3_S2(fx, fy, s, u0, v0), b_(b) {}
 
     /// constructor from vector
-    Cal3_S2Stereo(const Vector &d): K_(d(0), d(1), d(2), d(3), d(4)), b_(d(5)){}
+    Cal3_S2Stereo(const Vector& d)
+        : Cal3_S2(d(0), d(1), d(2), d(3), d(4)), b_(d(5)) {}
 
     /// easy constructor; field-of-view in degrees, assumes zero skew
-    Cal3_S2Stereo(double fov, int w, int h, double b) :
-      K_(fov, w, h), b_(b) {
-    }
+    Cal3_S2Stereo(double fov, int w, int h, double b)
+        : Cal3_S2(fov, w, h), b_(b) {}
 
     /// @}
     /// @name Testable
     /// @{
 
-    void print(const std::string& s = "") const;
+    void print(const std::string& s = "") const override;
 
     /// Check if equal up to specified tolerance
     bool equals(const Cal3_S2Stereo& other, double tol = 10e-9) const;
@@ -73,28 +70,10 @@ namespace gtsam {
     /// @{
 
     /// return calibration, same for left and right
-    const Cal3_S2& calibration() const { return K_;}
+    const Cal3_S2& calibration() const { return *this; }
 
     /// return calibration matrix K, same for left and right
-    Matrix matrix() const { return K_.matrix();}
-
-    /// focal length x
-    inline double fx() const { return K_.fx();}
-
-    /// focal length x
-    inline double fy() const { return K_.fy();}
-
-    /// skew
-    inline double skew() const { return K_.skew();}
-
-    /// image center in x
-    inline double px() const { return K_.px();}
-
-    /// image center in y
-    inline double py() const { return K_.py();}
-
-    /// return the principal point
-    Point2 principalPoint() const { return K_.principalPoint();}
+    Matrix3 K() const { return K(); }
 
     /// return baseline
     inline double baseline() const { return b_; }
@@ -102,7 +81,7 @@ namespace gtsam {
     /// vectorized form (column-wise)
     Vector6 vector() const {
       Vector6 v;
-      v << K_.vector(), b_;
+      v << vector(), b_;
       return v;
     }
 
@@ -118,7 +97,8 @@ namespace gtsam {
 
     /// Given 6-dim tangent vector, create new calibration
     inline Cal3_S2Stereo retract(const Vector& d) const {
-      return Cal3_S2Stereo(K_.fx() + d(0), K_.fy() + d(1), K_.skew() + d(2), K_.px() + d(3), K_.py() + d(4), b_ + d(5));
+      return Cal3_S2Stereo(fx() + d(0), fy() + d(1), skew() + d(2), px() + d(3),
+                           py() + d(4), b_ + d(5));
     }
 
     /// Unretraction for the calibration
@@ -137,8 +117,9 @@ namespace gtsam {
     template<class Archive>
     void serialize(Archive & ar, const unsigned int /*version*/)
     {
-      ar & BOOST_SERIALIZATION_NVP(K_);
-      ar & BOOST_SERIALIZATION_NVP(b_);
+      ar& boost::serialization::make_nvp(
+          "Cal3_S2", boost::serialization::base_object<Cal3_S2>(*this));
+      ar& BOOST_SERIALIZATION_NVP(b_);
     }
     /// @}
 
diff --git a/gtsam/geometry/tests/testCal3_S2.cpp b/gtsam/geometry/tests/testCal3_S2.cpp
index 55ea32e32..f7c8fb6b6 100644
--- a/gtsam/geometry/tests/testCal3_S2.cpp
+++ b/gtsam/geometry/tests/testCal3_S2.cpp
@@ -125,7 +125,6 @@ TEST(Cal3_S2, between) {
   EXPECT(assert_equal(Cal3_S2(0,1,2,3,4), k1.between(k2, H1, H2)));
   EXPECT(assert_equal(-I_5x5, H1));
   EXPECT(assert_equal(I_5x5, H2));
-
 }
 
 /* ************************************************************************* */

From 42b053740232b5b64347fecdf89441c0a73f28f9 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Tue, 1 Dec 2020 17:23:10 -0500
Subject: [PATCH 03/18] Refactor all Cal3D based models

---
 gtsam/geometry/Cal3DS2.cpp      |   8 +--
 gtsam/geometry/Cal3DS2.h        |   1 +
 gtsam/geometry/Cal3DS2_Base.cpp |  28 ++------
 gtsam/geometry/Cal3DS2_Base.h   | 119 +++++++++++---------------------
 gtsam/geometry/Cal3Unified.cpp  |  15 ++--
 5 files changed, 54 insertions(+), 117 deletions(-)

diff --git a/gtsam/geometry/Cal3DS2.cpp b/gtsam/geometry/Cal3DS2.cpp
index 070d16c6c..b4595a4dc 100644
--- a/gtsam/geometry/Cal3DS2.cpp
+++ b/gtsam/geometry/Cal3DS2.cpp
@@ -13,6 +13,7 @@
  * @file Cal3DS2.cpp
  * @date Feb 28, 2010
  * @author ydjian
+ * @author Varun Agrawal
  */
 
 #include <gtsam/base/Vector.h>
@@ -30,11 +31,8 @@ void Cal3DS2::print(const std::string& s_) const {
 
 /* ************************************************************************* */
 bool Cal3DS2::equals(const Cal3DS2& K, double tol) const {
-  if (std::abs(fx_ - K.fx_) > tol || std::abs(fy_ - K.fy_) > tol || std::abs(s_ - K.s_) > tol ||
-      std::abs(u0_ - K.u0_) > tol || std::abs(v0_ - K.v0_) > tol || std::abs(k1_ - K.k1_) > tol ||
-      std::abs(k2_ - K.k2_) > tol || std::abs(p1_ - K.p1_) > tol || std::abs(p2_ - K.p2_) > tol)
-    return false;
-  return true;
+  const Cal3DS2_Base* base = dynamic_cast<const Cal3DS2_Base*>(&K);
+  return Cal3DS2_Base::equals(*base, tol);
 }
 
 /* ************************************************************************* */
diff --git a/gtsam/geometry/Cal3DS2.h b/gtsam/geometry/Cal3DS2.h
index e66c3d124..fe08fc5fb 100644
--- a/gtsam/geometry/Cal3DS2.h
+++ b/gtsam/geometry/Cal3DS2.h
@@ -14,6 +14,7 @@
  * @brief Calibration of a camera with radial distortion, calculations in base class Cal3DS2_Base
  * @date Feb 28, 2010
  * @author ydjian
+ * @autho Varun Agrawal
  */
 
 #pragma once
diff --git a/gtsam/geometry/Cal3DS2_Base.cpp b/gtsam/geometry/Cal3DS2_Base.cpp
index d175259f2..016c9dfa2 100644
--- a/gtsam/geometry/Cal3DS2_Base.cpp
+++ b/gtsam/geometry/Cal3DS2_Base.cpp
@@ -24,25 +24,6 @@
 
 namespace gtsam {
 
-/* ************************************************************************* */
-Cal3DS2_Base::Cal3DS2_Base(const Vector& v)
-    : fx_(v(0)),
-      fy_(v(1)),
-      s_(v(2)),
-      u0_(v(3)),
-      v0_(v(4)),
-      k1_(v(5)),
-      k2_(v(6)),
-      p1_(v(7)),
-      p2_(v(8)) {}
-
-/* ************************************************************************* */
-Matrix3 Cal3DS2_Base::K() const {
-    Matrix3 K;
-    K << fx_, s_, u0_, 0.0, fy_, v0_, 0.0, 0.0, 1.0;
-    return K;
-}
-
 /* ************************************************************************* */
 Vector9 Cal3DS2_Base::vector() const {
   Vector9 v;
@@ -58,11 +39,10 @@ void Cal3DS2_Base::print(const std::string& s_) const {
 
 /* ************************************************************************* */
 bool Cal3DS2_Base::equals(const Cal3DS2_Base& K, double tol) const {
-  if (std::abs(fx_ - K.fx_) > tol || std::abs(fy_ - K.fy_) > tol || std::abs(s_ - K.s_) > tol ||
-      std::abs(u0_ - K.u0_) > tol || std::abs(v0_ - K.v0_) > tol || std::abs(k1_ - K.k1_) > tol ||
-      std::abs(k2_ - K.k2_) > tol || std::abs(p1_ - K.p1_) > tol || std::abs(p2_ - K.p2_) > tol)
-    return false;
-  return true;
+  const Cal3* base = dynamic_cast<const Cal3*>(&K);
+  return Cal3::equals(*base, tol) && std::fabs(k1_ - K.k1_) < tol &&
+         std::fabs(k2_ - K.k2_) < tol && std::fabs(p1_ - K.p1_) < tol &&
+         std::fabs(p2_ - K.p2_) < tol;
 }
 
 /* ************************************************************************* */
diff --git a/gtsam/geometry/Cal3DS2_Base.h b/gtsam/geometry/Cal3DS2_Base.h
index dbd6478e1..536fb1161 100644
--- a/gtsam/geometry/Cal3DS2_Base.h
+++ b/gtsam/geometry/Cal3DS2_Base.h
@@ -34,50 +34,34 @@ namespace gtsam {
  * but using only k1,k2,p1, and p2 coefficients.
  * K = [ fx s u0 ; 0 fy v0 ; 0 0 1 ]
  * rr = Pn.x^2 + Pn.y^2
- * \hat{Pn} = (1 + k1*rr + k2*rr^2 ) Pn + [ 2*p1 Pn.x Pn.y + p2 (rr + 2 Pn.x^2) ;
+ * \hat{Pn} = (1 + k1*rr + k2*rr^2 ) Pn + [ 2*p1 Pn.x Pn.y + p2 (rr + 2 Pn.x^2)
+ * ;
  *                      p1 (rr + 2 Pn.y^2) + 2*p2 Pn.x Pn.y  ]
  * pi = K*Pn
  */
-class GTSAM_EXPORT Cal3DS2_Base {
-
-protected:
- double fx_, fy_, s_, u0_, v0_;  // focal length, skew and principal point
- double k1_, k2_;                // radial 2nd-order and 4th-order
- double p1_, p2_;                // tangential distortion
- double tol_ = 1e-5;             // tolerance value when calibrating
-
-public:
+class GTSAM_EXPORT Cal3DS2_Base : public Cal3 {
+ protected:
+  double k1_, k2_;     // radial 2nd-order and 4th-order
+  double p1_, p2_;     // tangential distortion
+  double tol_ = 1e-5;  // tolerance value when calibrating
 
+ public:
   enum { dimension = 9 };
 
   /// @name Standard Constructors
   /// @{
 
- /// Default Constructor with only unit focal length
- Cal3DS2_Base()
-     : fx_(1),
-       fy_(1),
-       s_(0),
-       u0_(0),
-       v0_(0),
-       k1_(0),
-       k2_(0),
-       p1_(0),
-       p2_(0),
-       tol_(1e-5) {}
+  /// Default Constructor with only unit focal length
+  Cal3DS2_Base() : Cal3(), k1_(0), k2_(0), p1_(0), p2_(0), tol_(1e-5) {}
 
- Cal3DS2_Base(double fx, double fy, double s, double u0, double v0, double k1,
-              double k2, double p1 = 0.0, double p2 = 0.0, double tol = 1e-5)
-     : fx_(fx),
-       fy_(fy),
-       s_(s),
-       u0_(u0),
-       v0_(v0),
-       k1_(k1),
-       k2_(k2),
-       p1_(p1),
-       p2_(p2),
-       tol_(tol) {}
+  Cal3DS2_Base(double fx, double fy, double s, double u0, double v0, double k1,
+               double k2, double p1 = 0.0, double p2 = 0.0, double tol = 1e-5)
+      : Cal3(fx, fy, s, u0, v0),
+        k1_(k1),
+        k2_(k2),
+        p1_(p1),
+        p2_(p2),
+        tol_(tol) {}
 
   virtual ~Cal3DS2_Base() {}
 
@@ -85,14 +69,19 @@ public:
   /// @name Advanced Constructors
   /// @{
 
-  Cal3DS2_Base(const Vector &v) ;
+  Cal3DS2_Base(const Vector& v)
+      : Cal3(v(0), v(1), v(2), v(3), v(4)),
+        k1_(v(5)),
+        k2_(v(6)),
+        p1_(v(7)),
+        p2_(v(8)) {}
 
   /// @}
   /// @name Testable
   /// @{
 
   /// print with optional string
-  virtual void print(const std::string& s = "") const;
+  void print(const std::string& s = "") const override;
 
   /// assert equality up to a tolerance
   bool equals(const Cal3DS2_Base& K, double tol = 1e-8) const;
@@ -101,35 +90,17 @@ public:
   /// @name Standard Interface
   /// @{
 
-  /// focal length x
-  inline double fx() const { return fx_;}
-
-  /// focal length x
-  inline double fy() const { return fy_;}
-
-  /// skew
-  inline double skew() const { return s_;}
-
-  /// image center in x
-  inline double px() const { return u0_;}
-
-  /// image center in y
-  inline double py() const { return v0_;}
-
   /// First distortion coefficient
-  inline double k1() const { return k1_;}
+  inline double k1() const { return k1_; }
 
   /// Second distortion coefficient
-  inline double k2() const { return k2_;}
+  inline double k2() const { return k2_; }
 
   /// First tangential distortion coefficient
-  inline double p1() const { return p1_;}
+  inline double p1() const { return p1_; }
 
   /// Second tangential distortion coefficient
-  inline double p2() const { return p2_;}
-
-  /// return calibration matrix -- not really applicable
-  Matrix3 K() const;
+  inline double p2() const { return p2_; }
 
   /// return distortion parameter vector
   Vector4 k() const { return Vector4(k1_, k2_, p1_, p2_); }
@@ -152,10 +123,10 @@ public:
                    OptionalJacobian<2, 2> Dp = boost::none) const;
 
   /// Derivative of uncalibrate wrpt intrinsic coordinates
-  Matrix2 D2d_intrinsic(const Point2& p) const ;
+  Matrix2 D2d_intrinsic(const Point2& p) const;
 
   /// Derivative of uncalibrate wrpt the calibration parameters
-  Matrix29 D2d_calibration(const Point2& p) const ;
+  Matrix29 D2d_calibration(const Point2& p) const;
 
   /// @}
   /// @name Clone
@@ -168,31 +139,23 @@ public:
 
   /// @}
 
-private:
-
+ private:
   /// @name Advanced Interface
   /// @{
 
   /** Serialization function */
   friend class boost::serialization::access;
-  template<class Archive>
-  void serialize(Archive & ar, const unsigned int /*version*/)
-  {
-    ar & BOOST_SERIALIZATION_NVP(fx_);
-    ar & BOOST_SERIALIZATION_NVP(fy_);
-    ar & BOOST_SERIALIZATION_NVP(s_);
-    ar & BOOST_SERIALIZATION_NVP(u0_);
-    ar & BOOST_SERIALIZATION_NVP(v0_);
-    ar & BOOST_SERIALIZATION_NVP(k1_);
-    ar & BOOST_SERIALIZATION_NVP(k2_);
-    ar & BOOST_SERIALIZATION_NVP(p1_);
-    ar & BOOST_SERIALIZATION_NVP(p2_);
-    ar & BOOST_SERIALIZATION_NVP(tol_);
+  template <class Archive>
+  void serialize(Archive& ar, const unsigned int /*version*/) {
+    ar& boost::serialization::make_nvp(
+        "Cal3DS2_Base", boost::serialization::base_object<Cal3>(*this));
+    ar& BOOST_SERIALIZATION_NVP(k1_);
+    ar& BOOST_SERIALIZATION_NVP(k2_);
+    ar& BOOST_SERIALIZATION_NVP(p1_);
+    ar& BOOST_SERIALIZATION_NVP(p2_);
+    ar& BOOST_SERIALIZATION_NVP(tol_);
   }
 
   /// @}
-
 };
-
 }
-
diff --git a/gtsam/geometry/Cal3Unified.cpp b/gtsam/geometry/Cal3Unified.cpp
index f4ce0ed75..dc963a46e 100644
--- a/gtsam/geometry/Cal3Unified.cpp
+++ b/gtsam/geometry/Cal3Unified.cpp
@@ -26,8 +26,8 @@
 namespace gtsam {
 
 /* ************************************************************************* */
-Cal3Unified::Cal3Unified(const Vector &v):
-    Base(v[0], v[1], v[2], v[3], v[4], v[5], v[6], v[7], v[8]), xi_(v[9]) {}
+Cal3Unified::Cal3Unified(const Vector& v)
+    : Base(v(0), v(1), v(2), v(3), v(4), v(5), v(6), v(7), v(8)), xi_(v(9)) {}
 
 /* ************************************************************************* */
 Vector10 Cal3Unified::vector() const {
@@ -44,12 +44,8 @@ void Cal3Unified::print(const std::string& s) const {
 
 /* ************************************************************************* */
 bool Cal3Unified::equals(const Cal3Unified& K, double tol) const {
-  if (std::abs(fx_ - K.fx_) > tol || std::abs(fy_ - K.fy_) > tol || std::abs(s_ - K.s_) > tol ||
-      std::abs(u0_ - K.u0_) > tol || std::abs(v0_ - K.v0_) > tol || std::abs(k1_ - K.k1_) > tol ||
-      std::abs(k2_ - K.k2_) > tol || std::abs(p1_ - K.p1_) > tol || std::abs(p2_ - K.p2_) > tol ||
-      std::abs(xi_ - K.xi_) > tol)
-    return false;
-  return true;
+  const Cal3DS2_Base* base = dynamic_cast<const Cal3DS2_Base*>(&K);
+  return Cal3DS2_Base::equals(*base, tol) && std::fabs(xi_ - K.xi_) < tol;
 }
 
 /* ************************************************************************* */
@@ -144,7 +140,6 @@ Vector10 Cal3Unified::localCoordinates(const Cal3Unified& T2) const {
   return T2.vector() - vector();
 }
 
-}
 /* ************************************************************************* */
 
-
+}  // \ namespace gtsam

From 17e9b73fb620b70a854c6747004da3f9aa9d4013 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Tue, 1 Dec 2020 17:23:42 -0500
Subject: [PATCH 04/18] Refactor Bundler and Fisheye models

---
 gtsam/geometry/Cal3Bundler.cpp | 30 ++----------
 gtsam/geometry/Cal3Bundler.h   | 66 ++++++++++++++-------------
 gtsam/geometry/Cal3Fisheye.cpp | 42 +++--------------
 gtsam/geometry/Cal3Fisheye.h   | 83 ++++++++++++++--------------------
 4 files changed, 79 insertions(+), 142 deletions(-)

diff --git a/gtsam/geometry/Cal3Bundler.cpp b/gtsam/geometry/Cal3Bundler.cpp
index a73bfec52..8c524e1cc 100644
--- a/gtsam/geometry/Cal3Bundler.cpp
+++ b/gtsam/geometry/Cal3Bundler.cpp
@@ -23,16 +23,6 @@
 
 namespace gtsam {
 
-/* ************************************************************************* */
-Cal3Bundler::Cal3Bundler() :
-    f_(1), k1_(0), k2_(0), u0_(0), v0_(0), tol_(1e-5) {
-}
-
-/* ************************************************************************* */
-Cal3Bundler::Cal3Bundler(double f, double k1, double k2, double u0, double v0,
-                         double tol)
-    : f_(f), k1_(k1), k2_(k2), u0_(u0), v0_(v0), tol_(tol) {}
-
 /* ************************************************************************* */
 Matrix3 Cal3Bundler::K() const {
   Matrix3 K;
@@ -59,11 +49,9 @@ void Cal3Bundler::print(const std::string& s) const {
 
 /* ************************************************************************* */
 bool Cal3Bundler::equals(const Cal3Bundler& K, double tol) const {
-  if (std::abs(f_ - K.f_) > tol || std::abs(k1_ - K.k1_) > tol
-      || std::abs(k2_ - K.k2_) > tol || std::abs(u0_ - K.u0_) > tol
-      || std::abs(v0_ - K.v0_) > tol)
-    return false;
-  return true;
+  return (std::fabs(f_ - K.f_) < tol && std::fabs(k1_ - K.k1_) < tol &&
+          std::fabs(k2_ - K.k2_) < tol && std::fabs(u0_ - K.u0_) < tol &&
+          std::fabs(v0_ - K.v0_) < tol);
 }
 
 /* ************************************************************************* */
@@ -150,14 +138,4 @@ Matrix25 Cal3Bundler::D2d_intrinsic_calibration(const Point2& p) const {
   return H;
 }
 
-/* ************************************************************************* */
-Cal3Bundler Cal3Bundler::retract(const Vector& d) const {
-  return Cal3Bundler(f_ + d(0), k1_ + d(1), k2_ + d(2), u0_, v0_);
-}
-
-/* ************************************************************************* */
-Vector3 Cal3Bundler::localCoordinates(const Cal3Bundler& T2) const {
-  return T2.vector() - vector();
-}
-
-}
+}  // \ namespace gtsam
diff --git a/gtsam/geometry/Cal3Bundler.h b/gtsam/geometry/Cal3Bundler.h
index 8c836b504..dc99f5259 100644
--- a/gtsam/geometry/Cal3Bundler.h
+++ b/gtsam/geometry/Cal3Bundler.h
@@ -28,15 +28,17 @@ namespace gtsam {
  * @addtogroup geometry
  * \nosubgrouping
  */
-class GTSAM_EXPORT Cal3Bundler {
+class GTSAM_EXPORT Cal3Bundler : public Cal3 {
 
-private:
+ private:
   double f_; ///< focal length
   double k1_, k2_; ///< radial distortion
-  double u0_, v0_; ///< image center, not a parameter to be optimized but a constant
   double tol_; ///< tolerance value when calibrating
 
-public:
+  // NOTE: image center parameters (u0, v0) are not optimized
+  // but are constants.
+
+ public:
 
   enum { dimension = 3 };
 
@@ -44,7 +46,7 @@ public:
   /// @{
 
   /// Default constructor
-  Cal3Bundler();
+  Cal3Bundler() : Cal3(), f_(1), k1_(0), k2_(0), tol_(1e-5) {}
 
   /**
    *  Constructor
@@ -56,7 +58,8 @@ public:
    *  @param tol optional calibration tolerance value
    */
   Cal3Bundler(double f, double k1, double k2, double u0 = 0, double v0 = 0,
-              double tol = 1e-5);
+              double tol = 1e-5)
+      : Cal3(f, f, 0, u0, v0), f_(f), k1_(k1), k2_(k2), tol_(tol) {}
 
   virtual ~Cal3Bundler() {}
 
@@ -65,7 +68,7 @@ public:
   /// @{
 
   /// print with optional string
-  void print(const std::string& s = "") const;
+  void print(const std::string& s = "") const override;
 
   /// assert equality up to a tolerance
   bool equals(const Cal3Bundler& K, double tol = 10e-9) const;
@@ -74,11 +77,6 @@ public:
   /// @name Standard Interface
   /// @{
 
-  Matrix3 K() const; ///< Standard 3*3 calibration matrix
-  Vector4 k() const; ///< Radial distortion parameters (4 of them, 2 0)
-
-  Vector3 vector() const;
-
   /// focal length x
   inline double fx() const {
     return f_;
@@ -109,6 +107,11 @@ public:
     return v0_;
   }
 
+  Matrix3 K() const; ///< Standard 3*3 calibration matrix
+  Vector4 k() const; ///< Radial distortion parameters (4 of them, 2 0)
+
+  Vector3 vector() const;
+
 #ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V41
   /// get parameter u0
   inline double u0() const {
@@ -121,12 +124,11 @@ public:
   }
 #endif
 
-
   /**
    * @brief: convert intrinsic coordinates xy to image coordinates uv
    * Version of uncalibrate with derivatives
    * @param p point in intrinsic coordinates
-   * @param Dcal optional 2*3 Jacobian wrpt CalBundler parameters
+   * @param Dcal optional 2*3 Jacobian wrpt Cal3Bundler parameters
    * @param Dp optional 2*2 Jacobian wrpt intrinsic coordinates
    * @return point in image coordinates
    */
@@ -158,23 +160,23 @@ public:
   /// @name Manifold
   /// @{
 
+  /// return DOF, dimensionality of tangent space
+  virtual size_t dim() const { return dimension; }
+
+  /// return DOF, dimensionality of tangent space
+  static size_t Dim() { return dimension; }
+
   /// Update calibration with tangent space delta
-  Cal3Bundler retract(const Vector& d) const;
+  inline Cal3Bundler retract(const Vector& d) const {
+    return Cal3Bundler(f_ + d(0), k1_ + d(1), k2_ + d(2), u0_, v0_);
+  }
 
   /// Calculate local coordinates to another calibration
-  Vector3 localCoordinates(const Cal3Bundler& T2) const;
-
-  /// dimensionality
-  virtual size_t dim() const {
-    return 3;
+  Vector3 localCoordinates(const Cal3Bundler& T2) const {
+    return T2.vector() - vector();
   }
 
-  /// dimensionality
-  static size_t Dim() {
-    return 3;
-  }
-
-private:
+ private:
 
   /// @}
   /// @name Advanced Interface
@@ -184,12 +186,12 @@ private:
   friend class boost::serialization::access;
   template<class Archive>
   void serialize(Archive & ar, const unsigned int /*version*/) {
-    ar & BOOST_SERIALIZATION_NVP(f_);
-    ar & BOOST_SERIALIZATION_NVP(k1_);
-    ar & BOOST_SERIALIZATION_NVP(k2_);
-    ar & BOOST_SERIALIZATION_NVP(u0_);
-    ar & BOOST_SERIALIZATION_NVP(v0_);
-    ar & BOOST_SERIALIZATION_NVP(tol_);
+    ar& boost::serialization::make_nvp(
+        "Cal3Bundler", boost::serialization::base_object<Cal3>(*this));
+    ar& BOOST_SERIALIZATION_NVP(f_);
+    ar& BOOST_SERIALIZATION_NVP(k1_);
+    ar& BOOST_SERIALIZATION_NVP(k2_);
+    ar& BOOST_SERIALIZATION_NVP(tol_);
   }
 
   /// @}
diff --git a/gtsam/geometry/Cal3Fisheye.cpp b/gtsam/geometry/Cal3Fisheye.cpp
index 1ed1826ad..55020f581 100644
--- a/gtsam/geometry/Cal3Fisheye.cpp
+++ b/gtsam/geometry/Cal3Fisheye.cpp
@@ -13,6 +13,7 @@
  * @file Cal3Fisheye.cpp
  * @date Apr 8, 2020
  * @author ghaggin
+ * @author Varun Agrawal
  */
 
 #include <gtsam/base/Matrix.h>
@@ -23,18 +24,6 @@
 
 namespace gtsam {
 
-/* ************************************************************************* */
-Cal3Fisheye::Cal3Fisheye(const Vector9& v)
-    : fx_(v[0]),
-      fy_(v[1]),
-      s_(v[2]),
-      u0_(v[3]),
-      v0_(v[4]),
-      k1_(v[5]),
-      k2_(v[6]),
-      k3_(v[7]),
-      k4_(v[8]) {}
-
 /* ************************************************************************* */
 Vector9 Cal3Fisheye::vector() const {
   Vector9 v;
@@ -42,13 +31,6 @@ Vector9 Cal3Fisheye::vector() const {
   return v;
 }
 
-/* ************************************************************************* */
-Matrix3 Cal3Fisheye::K() const {
-  Matrix3 K;
-  K << fx_, s_, u0_, 0.0, fy_, v0_, 0.0, 0.0, 1.0;
-  return K;
-}
-
 /* ************************************************************************* */
 double Cal3Fisheye::Scaling(double r) {
   static constexpr double threshold = 1e-8;
@@ -165,24 +147,12 @@ void Cal3Fisheye::print(const std::string& s_) const {
 
 /* ************************************************************************* */
 bool Cal3Fisheye::equals(const Cal3Fisheye& K, double tol) const {
-  if (std::abs(fx_ - K.fx_) > tol || std::abs(fy_ - K.fy_) > tol ||
-      std::abs(s_ - K.s_) > tol || std::abs(u0_ - K.u0_) > tol ||
-      std::abs(v0_ - K.v0_) > tol || std::abs(k1_ - K.k1_) > tol ||
-      std::abs(k2_ - K.k2_) > tol || std::abs(k3_ - K.k3_) > tol ||
-      std::abs(k4_ - K.k4_) > tol)
-    return false;
-  return true;
+  const Cal3* base = dynamic_cast<const Cal3*>(&K);
+  return Cal3::equals(*base, tol) && std::fabs(k1_ - K.k1_) < tol &&
+         std::fabs(k2_ - K.k2_) < tol && std::fabs(k3_ - K.k3_) < tol &&
+         std::fabs(k4_ - K.k4_) < tol;
 }
 
 /* ************************************************************************* */
-Cal3Fisheye Cal3Fisheye::retract(const Vector& d) const {
-  return Cal3Fisheye(vector() + d);
-}
 
-/* ************************************************************************* */
-Vector Cal3Fisheye::localCoordinates(const Cal3Fisheye& T2) const {
-  return T2.vector() - vector();
-}
-
-}  // namespace gtsam
-/* ************************************************************************* */
+}  // \ namespace gtsam
diff --git a/gtsam/geometry/Cal3Fisheye.h b/gtsam/geometry/Cal3Fisheye.h
index 77e122f21..0d8921bd9 100644
--- a/gtsam/geometry/Cal3Fisheye.h
+++ b/gtsam/geometry/Cal3Fisheye.h
@@ -46,11 +46,10 @@ namespace gtsam {
  *   K = [fx s u0; 0 fy v0 ;0 0 1]
  *   [u; v; 1] = K*[x_d; y_d; 1]
  */
-class GTSAM_EXPORT Cal3Fisheye {
+class GTSAM_EXPORT Cal3Fisheye : public Cal3 {
  private:
-  double fx_, fy_, s_, u0_, v0_;  // focal length, skew and principal point
-  double k1_, k2_, k3_, k4_;      // fisheye distortion coefficients
-  double tol_ = 1e-5;             // tolerance value when calibrating
+  double k1_, k2_, k3_, k4_;  ///< fisheye distortion coefficients
+  double tol_ = 1e-5;         ///< tolerance value when calibrating
 
  public:
   enum { dimension = 9 };
@@ -61,17 +60,12 @@ class GTSAM_EXPORT Cal3Fisheye {
   /// @{
 
   /// Default Constructor with only unit focal length
-  Cal3Fisheye()
-      : fx_(1), fy_(1), s_(0), u0_(0), v0_(0), k1_(0), k2_(0), k3_(0), k4_(0), tol_(1e-5) {}
+  Cal3Fisheye() : Cal3(), k1_(0), k2_(0), k3_(0), k4_(0), tol_(1e-5) {}
 
   Cal3Fisheye(const double fx, const double fy, const double s, const double u0,
               const double v0, const double k1, const double k2,
               const double k3, const double k4, double tol = 1e-5)
-      : fx_(fx),
-        fy_(fy),
-        s_(s),
-        u0_(u0),
-        v0_(v0),
+      : Cal3(fx, fy, s, u0, v0),
         k1_(k1),
         k2_(k2),
         k3_(k3),
@@ -84,27 +78,17 @@ class GTSAM_EXPORT Cal3Fisheye {
   /// @name Advanced Constructors
   /// @{
 
-  explicit Cal3Fisheye(const Vector9& v);
+  explicit Cal3Fisheye(const Vector9& v)
+      : Cal3(v(0), v(1), v(2), v(3), v(4)),
+        k1_(v(5)),
+        k2_(v(6)),
+        k3_(v(7)),
+        k4_(v(8)) {}
 
   /// @}
   /// @name Standard Interface
   /// @{
 
-  /// focal length x
-  inline double fx() const { return fx_; }
-
-  /// focal length x
-  inline double fy() const { return fy_; }
-
-  /// skew
-  inline double skew() const { return s_; }
-
-  /// image center in x
-  inline double px() const { return u0_; }
-
-  /// image center in y
-  inline double py() const { return v0_; }
-
   /// First distortion coefficient
   inline double k1() const { return k1_; }
 
@@ -117,9 +101,6 @@ class GTSAM_EXPORT Cal3Fisheye {
   /// Second tangential distortion coefficient
   inline double k4() const { return k4_; }
 
-  /// return calibration matrix
-  Matrix3 K() const;
-
   /// return distortion parameter vector
   Vector4 k() const { return Vector4(k1_, k2_, k3_, k4_); }
 
@@ -133,16 +114,21 @@ class GTSAM_EXPORT Cal3Fisheye {
    * @brief convert intrinsic coordinates [x_i; y_i] to (distorted) image
    * coordinates [u; v]
    * @param p point in intrinsic coordinates
-   * @param Dcal optional 2*9 Jacobian wrpt intrinsic parameters (fx, fy, ...,
-   * k4)
+   * @param Dcal optional 2*9 Jacobian wrpt intrinsic parameters
    * @param Dp optional 2*2 Jacobian wrpt intrinsic coordinates (xi, yi)
    * @return point in (distorted) image coordinates
    */
   Point2 uncalibrate(const Point2& p, OptionalJacobian<2, 9> Dcal = boost::none,
                      OptionalJacobian<2, 2> Dp = boost::none) const;
 
-  /// Convert (distorted) image coordinates [u;v] to intrinsic coordinates [x_i,
-  /// y_i]
+  /**
+   * Convert (distorted) image coordinates [u;v] to intrinsic coordinates [x_i,
+   * y_i]
+   * @param p point in image coordinates
+   * @param Dcal optional 2*9 Jacobian wrpt intrinsic parameters
+   * @param Dp optional 2*2 Jacobian wrpt intrinsic coordinates (xi, yi)
+   * @return point in intrinsic coordinates
+   */
   Point2 calibrate(const Point2& p, OptionalJacobian<2, 9> Dcal = boost::none,
                    OptionalJacobian<2, 2> Dp = boost::none) const;
 
@@ -151,7 +137,7 @@ class GTSAM_EXPORT Cal3Fisheye {
   /// @{
 
   /// print with optional string
-  virtual void print(const std::string& s = "") const;
+  virtual void print(const std::string& s = "") const override;
 
   /// assert equality up to a tolerance
   bool equals(const Cal3Fisheye& K, double tol = 10e-9) const;
@@ -160,17 +146,21 @@ class GTSAM_EXPORT Cal3Fisheye {
   /// @name Manifold
   /// @{
 
+  /// Return dimensions of calibration manifold object
+  virtual size_t dim() const { return dimension; }
+
+  /// Return dimensions of calibration manifold object
+  static size_t Dim() { return dimension; }
+
   /// Given delta vector, update calibration
-  Cal3Fisheye retract(const Vector& d) const;
+  inline Cal3Fisheye retract(const Vector& d) const {
+    return Cal3Fisheye(vector() + d);
+  }
 
   /// Given a different calibration, calculate update to obtain it
-  Vector localCoordinates(const Cal3Fisheye& T2) const;
-
-  /// Return dimensions of calibration manifold object
-  virtual size_t dim() const { return 9; }
-
-  /// Return dimensions of calibration manifold object
-  static size_t Dim() { return 9; }
+  Vector localCoordinates(const Cal3Fisheye& T2) const {
+    return T2.vector() - vector();
+  }
 
   /// @}
   /// @name Clone
@@ -191,11 +181,8 @@ class GTSAM_EXPORT Cal3Fisheye {
   friend class boost::serialization::access;
   template <class Archive>
   void serialize(Archive& ar, const unsigned int /*version*/) {
-    ar& BOOST_SERIALIZATION_NVP(fx_);
-    ar& BOOST_SERIALIZATION_NVP(fy_);
-    ar& BOOST_SERIALIZATION_NVP(s_);
-    ar& BOOST_SERIALIZATION_NVP(u0_);
-    ar& BOOST_SERIALIZATION_NVP(v0_);
+    ar& boost::serialization::make_nvp(
+        "Cal3Fisheye", boost::serialization::base_object<Cal3>(*this));
     ar& BOOST_SERIALIZATION_NVP(k1_);
     ar& BOOST_SERIALIZATION_NVP(k2_);
     ar& BOOST_SERIALIZATION_NVP(k3_);

From 8daa6a77295988802caea09db4b8ff9d2236a007 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Tue, 1 Dec 2020 17:24:04 -0500
Subject: [PATCH 05/18] Minor updates

---
 gtsam/geometry/Cal3.cpp          | 8 ++++----
 gtsam/geometry/Cal3.h            | 4 ++--
 gtsam/geometry/Cal3DS2.h         | 2 +-
 gtsam/geometry/Cal3DS2_Base.h    | 6 +++---
 gtsam/geometry/Cal3Unified.h     | 2 +-
 gtsam/geometry/Cal3_S2Stereo.cpp | 2 +-
 6 files changed, 12 insertions(+), 12 deletions(-)

diff --git a/gtsam/geometry/Cal3.cpp b/gtsam/geometry/Cal3.cpp
index 240d01e12..31e9bf834 100644
--- a/gtsam/geometry/Cal3.cpp
+++ b/gtsam/geometry/Cal3.cpp
@@ -62,11 +62,11 @@ void Cal3::print(const std::string& s) const { gtsam::print((Matrix)K(), s); }
 
 /* ************************************************************************* */
 bool Cal3::equals(const Cal3& K, double tol) const {
-  return (std::abs(fx_ - K.fx_) < tol) && (std::abs(fy_ - K.fy_) < tol) &&
-         (std::abs(s_ - K.s_) < tol) && (std::abs(u0_ - K.u0_) < tol) &&
-         (std::abs(v0_ - K.v0_) < tol);
+  return (std::fabs(fx_ - K.fx_) < tol) && (std::fabs(fy_ - K.fy_) < tol) &&
+         (std::fabs(s_ - K.s_) < tol) && (std::fabs(u0_ - K.u0_) < tol) &&
+         (std::fabs(v0_ - K.v0_) < tol);
 }
 
 /* ************************************************************************* */
 
-}  // namespace gtsam
+}  // \ namespace gtsam
diff --git a/gtsam/geometry/Cal3.h b/gtsam/geometry/Cal3.h
index 13cd4de69..7384fe958 100644
--- a/gtsam/geometry/Cal3.h
+++ b/gtsam/geometry/Cal3.h
@@ -68,7 +68,7 @@ void calibrateJacobians(const Cal& calibration, const Point2& pn,
  */
 class GTSAM_EXPORT Cal3 {
  protected:
-  double fx_, fy_, s_, u0_, v0_;
+  double fx_, fy_, s_, u0_, v0_;  ///< focal length, skew and principal point
 
  public:
   enum { dimension = 5 };
@@ -113,7 +113,7 @@ class GTSAM_EXPORT Cal3 {
                                                const Cal3& cal);
 
   /// print with optional string
-  virtual void print(const std::string& s = "Cal3") const;
+  virtual void print(const std::string& s = "") const;
 
   /// Check if equal up to specified tolerance
   bool equals(const Cal3& K, double tol = 10e-9) const;
diff --git a/gtsam/geometry/Cal3DS2.h b/gtsam/geometry/Cal3DS2.h
index fe08fc5fb..58ccae04f 100644
--- a/gtsam/geometry/Cal3DS2.h
+++ b/gtsam/geometry/Cal3DS2.h
@@ -32,7 +32,7 @@ namespace gtsam {
  */
 class GTSAM_EXPORT Cal3DS2 : public Cal3DS2_Base {
 
-  typedef Cal3DS2_Base Base;
+  using Base = Cal3DS2_Base;
 
 public:
 
diff --git a/gtsam/geometry/Cal3DS2_Base.h b/gtsam/geometry/Cal3DS2_Base.h
index 536fb1161..0c6a4a3cd 100644
--- a/gtsam/geometry/Cal3DS2_Base.h
+++ b/gtsam/geometry/Cal3DS2_Base.h
@@ -41,9 +41,9 @@ namespace gtsam {
  */
 class GTSAM_EXPORT Cal3DS2_Base : public Cal3 {
  protected:
-  double k1_, k2_;     // radial 2nd-order and 4th-order
-  double p1_, p2_;     // tangential distortion
-  double tol_ = 1e-5;  // tolerance value when calibrating
+  double k1_, k2_;     ///< radial 2nd-order and 4th-order
+  double p1_, p2_;     ///< tangential distortion
+  double tol_ = 1e-5;  ///< tolerance value when calibrating
 
  public:
   enum { dimension = 9 };
diff --git a/gtsam/geometry/Cal3Unified.h b/gtsam/geometry/Cal3Unified.h
index 6fc37b0d1..7a0931ff5 100644
--- a/gtsam/geometry/Cal3Unified.h
+++ b/gtsam/geometry/Cal3Unified.h
@@ -47,7 +47,7 @@ class GTSAM_EXPORT Cal3Unified : public Cal3DS2_Base {
 
 private:
 
-  double xi_;  // mirror parameter
+  double xi_;  ///< mirror parameter
 
 public:
 
diff --git a/gtsam/geometry/Cal3_S2Stereo.cpp b/gtsam/geometry/Cal3_S2Stereo.cpp
index b4e70202e..43f0fb12f 100644
--- a/gtsam/geometry/Cal3_S2Stereo.cpp
+++ b/gtsam/geometry/Cal3_S2Stereo.cpp
@@ -32,7 +32,7 @@ void Cal3_S2Stereo::print(const std::string& s) const {
 /* ************************************************************************* */
 bool Cal3_S2Stereo::equals(const Cal3_S2Stereo& other, double tol) const {
   const Cal3_S2* base = dynamic_cast<const Cal3_S2*>(&other);
-  return Cal3_S2::equals(*base, tol) && (std::abs(b_ - other.baseline()) < tol);
+  return Cal3_S2::equals(*base, tol) && (std::fabs(b_ - other.baseline()) < tol);
 }
 
 /* ************************************************************************* */

From f9041846cedac352c4f913eac9c5559675c901c9 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Tue, 1 Dec 2020 17:37:06 -0500
Subject: [PATCH 06/18] Remove deprecated calibration method from wrapper

---
 gtsam/gtsam.i | 1 -
 1 file changed, 1 deletion(-)

diff --git a/gtsam/gtsam.i b/gtsam/gtsam.i
index 2e1920641..e405e533c 100644
--- a/gtsam/gtsam.i
+++ b/gtsam/gtsam.i
@@ -852,7 +852,6 @@ class Cal3_S2 {
   gtsam::Point2 principalPoint() const;
   Vector vector() const;
   Matrix K() const;
-  Matrix matrix() const;
   Matrix matrix_inverse() const;
 
   // enabling serialization functionality

From 9bc63fa5a943253363091349cc9d04209ccff601 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Tue, 1 Dec 2020 17:46:03 -0500
Subject: [PATCH 07/18] replace typedef with using

---
 gtsam/geometry/Cal3.h          | 4 ++--
 gtsam/geometry/Cal3Fisheye.h   | 4 ++--
 gtsam/geometry/Cal3Unified.h   | 4 ++--
 gtsam/geometry/Cal3_S2.h       | 5 +++--
 gtsam/geometry/Cal3_S2Stereo.h | 9 +++++----
 5 files changed, 14 insertions(+), 12 deletions(-)

diff --git a/gtsam/geometry/Cal3.h b/gtsam/geometry/Cal3.h
index 7384fe958..5ce7be8ca 100644
--- a/gtsam/geometry/Cal3.h
+++ b/gtsam/geometry/Cal3.h
@@ -72,8 +72,8 @@ class GTSAM_EXPORT Cal3 {
 
  public:
   enum { dimension = 5 };
-  typedef boost::shared_ptr<Cal3>
-      shared_ptr;  ///< shared pointer to calibration object
+  ///< shared pointer to calibration object
+  using shared_ptr = boost::shared_ptr<Cal3>;
 
   /// @name Standard Constructors
   /// @{
diff --git a/gtsam/geometry/Cal3Fisheye.h b/gtsam/geometry/Cal3Fisheye.h
index 0d8921bd9..3eaf9f46d 100644
--- a/gtsam/geometry/Cal3Fisheye.h
+++ b/gtsam/geometry/Cal3Fisheye.h
@@ -53,8 +53,8 @@ class GTSAM_EXPORT Cal3Fisheye : public Cal3 {
 
  public:
   enum { dimension = 9 };
-  typedef boost::shared_ptr<Cal3Fisheye>
-      shared_ptr;  ///< shared pointer to fisheye calibration object
+  ///< shared pointer to fisheye calibration object
+  using shared_ptr = boost::shared_ptr<Cal3Fisheye>;
 
   /// @name Standard Constructors
   /// @{
diff --git a/gtsam/geometry/Cal3Unified.h b/gtsam/geometry/Cal3Unified.h
index 7a0931ff5..673d9e2c9 100644
--- a/gtsam/geometry/Cal3Unified.h
+++ b/gtsam/geometry/Cal3Unified.h
@@ -42,8 +42,8 @@ namespace gtsam {
  */
 class GTSAM_EXPORT Cal3Unified : public Cal3DS2_Base {
 
-  typedef Cal3Unified This;
-  typedef Cal3DS2_Base Base;
+  using This = Cal3Unified;
+  using Base = Cal3DS2_Base;
 
 private:
 
diff --git a/gtsam/geometry/Cal3_S2.h b/gtsam/geometry/Cal3_S2.h
index aef27323e..0a6b0e164 100644
--- a/gtsam/geometry/Cal3_S2.h
+++ b/gtsam/geometry/Cal3_S2.h
@@ -34,8 +34,9 @@ namespace gtsam {
 class GTSAM_EXPORT Cal3_S2 : public Cal3 {
  public:
   enum { dimension = 5 };
-  typedef boost::shared_ptr<Cal3_S2>
-      shared_ptr;  ///< shared pointer to calibration object
+
+  ///< shared pointer to calibration object
+  using shared_ptr = boost::shared_ptr<Cal3_S2>;
 
   /// @name Standard Constructors
   /// @{
diff --git a/gtsam/geometry/Cal3_S2Stereo.h b/gtsam/geometry/Cal3_S2Stereo.h
index 258cd0434..585807543 100644
--- a/gtsam/geometry/Cal3_S2Stereo.h
+++ b/gtsam/geometry/Cal3_S2Stereo.h
@@ -28,14 +28,15 @@ namespace gtsam {
    * \nosubgrouping
    */
   class GTSAM_EXPORT Cal3_S2Stereo : public Cal3_S2 {
-  private:
+   private:
     double b_;
 
-  public:
+   public:
 
     enum { dimension = 6 };
+
     ///< shared pointer to stereo calibration object
-    typedef boost::shared_ptr<Cal3_S2Stereo> shared_ptr;
+    using shared_ptr = boost::shared_ptr<Cal3_S2Stereo>;
 
     /// @name Standard Constructors
     /// @
@@ -111,7 +112,7 @@ namespace gtsam {
     /// @name Advanced Interface
     /// @{
 
-  private:
+   private:
     /** Serialization function */
     friend class boost::serialization::access;
     template<class Archive>

From 04597feaa4268325295679a85953c03b37b04063 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Wed, 2 Dec 2020 05:45:20 -0500
Subject: [PATCH 08/18] modernized default constructors

---
 gtsam/geometry/Cal3.h          | 10 ++++++----
 gtsam/geometry/Cal3Bundler.h   | 10 +++++-----
 gtsam/geometry/Cal3DS2.h       | 10 +++++-----
 gtsam/geometry/Cal3DS2_Base.h  | 10 +++++-----
 gtsam/geometry/Cal3Fisheye.h   |  7 ++++---
 gtsam/geometry/Cal3Unified.cpp |  6 +-----
 gtsam/geometry/Cal3Unified.h   | 26 +++++++++++++-------------
 gtsam/geometry/Cal3_S2.h       |  4 ++--
 gtsam/geometry/Cal3_S2Stereo.h |  7 ++++---
 9 files changed, 45 insertions(+), 45 deletions(-)

diff --git a/gtsam/geometry/Cal3.h b/gtsam/geometry/Cal3.h
index 5ce7be8ca..6d3973080 100644
--- a/gtsam/geometry/Cal3.h
+++ b/gtsam/geometry/Cal3.h
@@ -68,7 +68,9 @@ void calibrateJacobians(const Cal& calibration, const Point2& pn,
  */
 class GTSAM_EXPORT Cal3 {
  protected:
-  double fx_, fy_, s_, u0_, v0_;  ///< focal length, skew and principal point
+  double fx_ = 1.0f, fy_ = 1.0f;  ///< focal length
+  double s_ = 0.0f;               ///< skew
+  double u0_ = 0.0f, v0_ = 0.0f;  ///< principal point
 
  public:
   enum { dimension = 5 };
@@ -79,14 +81,14 @@ class GTSAM_EXPORT Cal3 {
   /// @{
 
   /// Create a default calibration that leaves coordinates unchanged
-  Cal3() : fx_(1), fy_(1), s_(0), u0_(0), v0_(0) {}
+  Cal3() = default;
 
   /// constructor from doubles
   Cal3(double fx, double fy, double s, double u0, double v0)
       : fx_(fx), fy_(fy), s_(s), u0_(u0), v0_(v0) {}
 
   /// constructor from vector
-  Cal3(const Vector& d)
+  Cal3(const Vector5& d)
       : fx_(d(0)), fy_(d(1)), s_(d(2)), u0_(d(3)), v0_(d(4)) {}
 
   /**
@@ -162,7 +164,7 @@ class GTSAM_EXPORT Cal3 {
   Matrix3 matrix() const { return K(); }
 #endif
 
-  /// return inverted calibration matrix inv(K)
+  /// Return inverted calibration matrix inv(K)
   Matrix3 matrix_inverse() const {
     const double fxy = fx_ * fy_, sv0 = s_ * v0_, fyu0 = fy_ * u0_;
     Matrix3 K_inverse;
diff --git a/gtsam/geometry/Cal3Bundler.h b/gtsam/geometry/Cal3Bundler.h
index dc99f5259..d128c329b 100644
--- a/gtsam/geometry/Cal3Bundler.h
+++ b/gtsam/geometry/Cal3Bundler.h
@@ -31,12 +31,12 @@ namespace gtsam {
 class GTSAM_EXPORT Cal3Bundler : public Cal3 {
 
  private:
-  double f_; ///< focal length
-  double k1_, k2_; ///< radial distortion
-  double tol_; ///< tolerance value when calibrating
+  double f_ = 1.0f;               ///< focal length
+  double k1_ = 0.0f, k2_ = 0.0f;  ///< radial distortion
+  double tol_ = 1e-5;             ///< tolerance value when calibrating
 
   // NOTE: image center parameters (u0, v0) are not optimized
-  // but are constants.
+  // but are treated as constants.
 
  public:
 
@@ -46,7 +46,7 @@ class GTSAM_EXPORT Cal3Bundler : public Cal3 {
   /// @{
 
   /// Default constructor
-  Cal3Bundler() : Cal3(), f_(1), k1_(0), k2_(0), tol_(1e-5) {}
+  Cal3Bundler() = default;
 
   /**
    *  Constructor
diff --git a/gtsam/geometry/Cal3DS2.h b/gtsam/geometry/Cal3DS2.h
index 58ccae04f..03a4d8d46 100644
--- a/gtsam/geometry/Cal3DS2.h
+++ b/gtsam/geometry/Cal3DS2.h
@@ -42,11 +42,11 @@ public:
   /// @{
 
   /// Default Constructor with only unit focal length
-  Cal3DS2() : Base() {}
+  Cal3DS2() = default;
 
-  Cal3DS2(double fx, double fy, double s, double u0, double v0,
-      double k1, double k2, double p1 = 0.0, double p2 = 0.0, double tol = 1e-5) :
-        Base(fx, fy, s, u0, v0, k1, k2, p1, p2, tol) {}
+  Cal3DS2(double fx, double fy, double s, double u0, double v0, double k1,
+          double k2, double p1 = 0.0, double p2 = 0.0, double tol = 1e-5)
+      : Base(fx, fy, s, u0, v0, k1, k2, p1, p2, tol) {}
 
   virtual ~Cal3DS2() {}
 
@@ -54,7 +54,7 @@ public:
   /// @name Advanced Constructors
   /// @{
 
-  Cal3DS2(const Vector &v) : Base(v) {}
+  Cal3DS2(const Vector9 &v) : Base(v) {}
 
   /// @}
   /// @name Testable
diff --git a/gtsam/geometry/Cal3DS2_Base.h b/gtsam/geometry/Cal3DS2_Base.h
index 0c6a4a3cd..e0de6af61 100644
--- a/gtsam/geometry/Cal3DS2_Base.h
+++ b/gtsam/geometry/Cal3DS2_Base.h
@@ -41,9 +41,9 @@ namespace gtsam {
  */
 class GTSAM_EXPORT Cal3DS2_Base : public Cal3 {
  protected:
-  double k1_, k2_;     ///< radial 2nd-order and 4th-order
-  double p1_, p2_;     ///< tangential distortion
-  double tol_ = 1e-5;  ///< tolerance value when calibrating
+  double k1_ = 0.0f, k2_ = 0.0f;  ///< radial 2nd-order and 4th-order
+  double p1_ = 0.0f, p2_ = 0.0f;  ///< tangential distortion
+  double tol_ = 1e-5;             ///< tolerance value when calibrating
 
  public:
   enum { dimension = 9 };
@@ -52,7 +52,7 @@ class GTSAM_EXPORT Cal3DS2_Base : public Cal3 {
   /// @{
 
   /// Default Constructor with only unit focal length
-  Cal3DS2_Base() : Cal3(), k1_(0), k2_(0), p1_(0), p2_(0), tol_(1e-5) {}
+  Cal3DS2_Base() = default;
 
   Cal3DS2_Base(double fx, double fy, double s, double u0, double v0, double k1,
                double k2, double p1 = 0.0, double p2 = 0.0, double tol = 1e-5)
@@ -69,7 +69,7 @@ class GTSAM_EXPORT Cal3DS2_Base : public Cal3 {
   /// @name Advanced Constructors
   /// @{
 
-  Cal3DS2_Base(const Vector& v)
+  Cal3DS2_Base(const Vector9& v)
       : Cal3(v(0), v(1), v(2), v(3), v(4)),
         k1_(v(5)),
         k2_(v(6)),
diff --git a/gtsam/geometry/Cal3Fisheye.h b/gtsam/geometry/Cal3Fisheye.h
index 3eaf9f46d..f41a9b0d7 100644
--- a/gtsam/geometry/Cal3Fisheye.h
+++ b/gtsam/geometry/Cal3Fisheye.h
@@ -48,8 +48,9 @@ namespace gtsam {
  */
 class GTSAM_EXPORT Cal3Fisheye : public Cal3 {
  private:
-  double k1_, k2_, k3_, k4_;  ///< fisheye distortion coefficients
-  double tol_ = 1e-5;         ///< tolerance value when calibrating
+  double k1_ = 0.0f, k2_ = 0.0f;  ///< fisheye distortion coefficients
+  double k3_ = 0.0f, k4_ = 0.0f;  ///< fisheye distortion coefficients
+  double tol_ = 1e-5;             ///< tolerance value when calibrating
 
  public:
   enum { dimension = 9 };
@@ -60,7 +61,7 @@ class GTSAM_EXPORT Cal3Fisheye : public Cal3 {
   /// @{
 
   /// Default Constructor with only unit focal length
-  Cal3Fisheye() : Cal3(), k1_(0), k2_(0), k3_(0), k4_(0), tol_(1e-5) {}
+  Cal3Fisheye() = default;
 
   Cal3Fisheye(const double fx, const double fy, const double s, const double u0,
               const double v0, const double k1, const double k2,
diff --git a/gtsam/geometry/Cal3Unified.cpp b/gtsam/geometry/Cal3Unified.cpp
index dc963a46e..d260dd725 100644
--- a/gtsam/geometry/Cal3Unified.cpp
+++ b/gtsam/geometry/Cal3Unified.cpp
@@ -25,10 +25,6 @@
 
 namespace gtsam {
 
-/* ************************************************************************* */
-Cal3Unified::Cal3Unified(const Vector& v)
-    : Base(v(0), v(1), v(2), v(3), v(4), v(5), v(6), v(7), v(8)), xi_(v(9)) {}
-
 /* ************************************************************************* */
 Vector10 Cal3Unified::vector() const {
   Vector10 v;
@@ -136,7 +132,7 @@ Cal3Unified Cal3Unified::retract(const Vector& d) const {
 }
 
 /* ************************************************************************* */
-Vector10 Cal3Unified::localCoordinates(const Cal3Unified& T2) const {
+Vector Cal3Unified::localCoordinates(const Cal3Unified& T2) const {
   return T2.vector() - vector();
 }
 
diff --git a/gtsam/geometry/Cal3Unified.h b/gtsam/geometry/Cal3Unified.h
index 673d9e2c9..e1368db4c 100644
--- a/gtsam/geometry/Cal3Unified.h
+++ b/gtsam/geometry/Cal3Unified.h
@@ -45,11 +45,10 @@ class GTSAM_EXPORT Cal3Unified : public Cal3DS2_Base {
   using This = Cal3Unified;
   using Base = Cal3DS2_Base;
 
-private:
+ private:
+  double xi_ = 0.0f;  ///< mirror parameter
 
-  double xi_;  ///< mirror parameter
-
-public:
+ public:
 
   enum { dimension = 10 };
 
@@ -57,11 +56,11 @@ public:
   /// @{
 
   /// Default Constructor with only unit focal length
-  Cal3Unified() : Base(), xi_(0) {}
+  Cal3Unified() = default;
 
-  Cal3Unified(double fx, double fy, double s, double u0, double v0,
-      double k1, double k2, double p1 = 0.0, double p2 = 0.0, double xi = 0.0) :
-        Base(fx, fy, s, u0, v0, k1, k2, p1, p2), xi_(xi) {}
+  Cal3Unified(double fx, double fy, double s, double u0, double v0, double k1,
+              double k2, double p1 = 0.0, double p2 = 0.0, double xi = 0.0)
+      : Base(fx, fy, s, u0, v0, k1, k2, p1, p2), xi_(xi) {}
 
   virtual ~Cal3Unified() {}
 
@@ -69,7 +68,8 @@ public:
   /// @name Advanced Constructors
   /// @{
 
-  Cal3Unified(const Vector &v) ;
+  Cal3Unified(const Vector10& v)
+      : Base(v(0), v(1), v(2), v(3), v(4), v(5), v(6), v(7), v(8)), xi_(v(9)) {}
 
   /// @}
   /// @name Testable
@@ -88,6 +88,9 @@ public:
   /// mirror parameter
   inline double xi() const { return xi_;}
 
+  /// Return all parameters as a vector
+  Vector10 vector() const ;
+
   /**
    * convert intrinsic coordinates xy to image coordinates uv
    * @param p point in intrinsic coordinates
@@ -117,7 +120,7 @@ public:
   Cal3Unified retract(const Vector& d) const ;
 
   /// Given a different calibration, calculate update to obtain it
-  Vector10 localCoordinates(const Cal3Unified& T2) const ;
+  Vector localCoordinates(const Cal3Unified& T2) const ;
 
   /// Return dimensions of calibration manifold object
   virtual size_t dim() const { return dimension ; }
@@ -125,9 +128,6 @@ public:
   /// Return dimensions of calibration manifold object
   static size_t Dim() { return dimension; }
 
-  /// Return all parameters as a vector
-  Vector10 vector() const ;
-
   /// @}
 
 private:
diff --git a/gtsam/geometry/Cal3_S2.h b/gtsam/geometry/Cal3_S2.h
index 0a6b0e164..37edc46c4 100644
--- a/gtsam/geometry/Cal3_S2.h
+++ b/gtsam/geometry/Cal3_S2.h
@@ -42,14 +42,14 @@ class GTSAM_EXPORT Cal3_S2 : public Cal3 {
   /// @{
 
   /// Create a default calibration that leaves coordinates unchanged
-  Cal3_S2() : Cal3() {}
+  Cal3_S2() = default;
 
   /// constructor from doubles
   Cal3_S2(double fx, double fy, double s, double u0, double v0)
       : Cal3(fx, fy, s, u0, v0) {}
 
   /// constructor from vector
-  Cal3_S2(const Vector& d) : Cal3(d) {}
+  Cal3_S2(const Vector5& d) : Cal3(d) {}
 
   /**
    * Easy constructor, takes fov in degrees, asssumes zero skew, unit aspect
diff --git a/gtsam/geometry/Cal3_S2Stereo.h b/gtsam/geometry/Cal3_S2Stereo.h
index 585807543..0c68ef6b4 100644
--- a/gtsam/geometry/Cal3_S2Stereo.h
+++ b/gtsam/geometry/Cal3_S2Stereo.h
@@ -29,7 +29,7 @@ namespace gtsam {
    */
   class GTSAM_EXPORT Cal3_S2Stereo : public Cal3_S2 {
    private:
-    double b_;
+    double b_ = 1.0f;  ///< Stereo baseline.
 
    public:
 
@@ -42,7 +42,7 @@ namespace gtsam {
     /// @
 
     /// default calibration leaves coordinates unchanged
-    Cal3_S2Stereo() : Cal3_S2(1, 1, 0, 0, 0), b_(1.0) {}
+    Cal3_S2Stereo() = default;
 
     /// constructor from doubles
     Cal3_S2Stereo(double fx, double fy, double s, double u0, double v0,
@@ -50,7 +50,7 @@ namespace gtsam {
         : Cal3_S2(fx, fy, s, u0, v0), b_(b) {}
 
     /// constructor from vector
-    Cal3_S2Stereo(const Vector& d)
+    Cal3_S2Stereo(const Vector6& d)
         : Cal3_S2(d(0), d(1), d(2), d(3), d(4)), b_(d(5)) {}
 
     /// easy constructor; field-of-view in degrees, assumes zero skew
@@ -61,6 +61,7 @@ namespace gtsam {
     /// @name Testable
     /// @{
 
+    /// print with optional string
     void print(const std::string& s = "") const override;
 
     /// Check if equal up to specified tolerance

From 355d2cff0619c3a07f08213c0815e2adc63f5f5f Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Wed, 2 Dec 2020 06:11:02 -0500
Subject: [PATCH 09/18] Cal3 code improvements

---
 gtsam/geometry/Cal3.cpp | 25 +++++++++++--------------
 1 file changed, 11 insertions(+), 14 deletions(-)

diff --git a/gtsam/geometry/Cal3.cpp b/gtsam/geometry/Cal3.cpp
index 31e9bf834..6183c6e5e 100644
--- a/gtsam/geometry/Cal3.cpp
+++ b/gtsam/geometry/Cal3.cpp
@@ -22,28 +22,25 @@
 #include <iostream>
 
 namespace gtsam {
-using namespace std;
 
 /* ************************************************************************* */
 Cal3::Cal3(double fov, int w, int h)
     : s_(0), u0_((double)w / 2.0), v0_((double)h / 2.0) {
   double a = fov * M_PI / 360.0;  // fov/2 in radians
-  fx_ =
-      (double)w / (2.0 * tan(a));  //    old formula: fx_ = (double) w * tan(a);
+  // old formula: fx_ = (double) w * tan(a);
+  fx_ = double(w) / (2.0 * tan(a));
   fy_ = fx_;
 }
 
 /* ************************************************************************* */
 Cal3::Cal3(const std::string& path)
     : fx_(320), fy_(320), s_(0), u0_(320), v0_(140) {
-  char buffer[200];
-  buffer[0] = 0;
-  sprintf(buffer, "%s/calibration_info.txt", path.c_str());
+  const auto buffer = path + std::string("/calibration_info.txt");
   std::ifstream infile(buffer, std::ios::in);
 
-  if (infile)
+  if (infile) {
     infile >> fx_ >> fy_ >> s_ >> u0_ >> v0_;
-  else {
+  } else {
     throw std::runtime_error("Cal3: Unable to load the calibration");
   }
 
@@ -51,9 +48,9 @@ Cal3::Cal3(const std::string& path)
 }
 
 /* ************************************************************************* */
-ostream& operator<<(ostream& os, const Cal3& cal) {
-  os << "{fx: " << cal.fx() << ", fy: " << cal.fy() << ", s:" << cal.skew()
-     << ", px:" << cal.px() << ", py:" << cal.py() << "}";
+std::ostream& operator<<(std::ostream& os, const Cal3& cal) {
+  os << "{ fx: " << cal.fx() << ", fy: " << cal.fy() << ", s: " << cal.skew()
+     << ", px: " << cal.px() << ", py: " << cal.py() << " }";
   return os;
 }
 
@@ -62,9 +59,9 @@ void Cal3::print(const std::string& s) const { gtsam::print((Matrix)K(), s); }
 
 /* ************************************************************************* */
 bool Cal3::equals(const Cal3& K, double tol) const {
-  return (std::fabs(fx_ - K.fx_) < tol) && (std::fabs(fy_ - K.fy_) < tol) &&
-         (std::fabs(s_ - K.s_) < tol) && (std::fabs(u0_ - K.u0_) < tol) &&
-         (std::fabs(v0_ - K.v0_) < tol);
+  return (std::fabs(fx_ - K.fx_) < tol && std::fabs(fy_ - K.fy_) < tol &&
+          std::fabs(s_ - K.s_) < tol && std::fabs(u0_ - K.u0_) < tol &&
+          std::fabs(v0_ - K.v0_) < tol);
 }
 
 /* ************************************************************************* */

From 25b6146633967855166a1518ea93be01048de5e5 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Wed, 2 Dec 2020 06:11:50 -0500
Subject: [PATCH 10/18] matrix_inverse() -> inverse()

---
 gtsam/geometry/Cal3.h      | 2 +-
 gtsam/geometry/Cal3_S2.cpp | 9 ++++-----
 gtsam/gtsam.i              | 2 +-
 3 files changed, 6 insertions(+), 7 deletions(-)

diff --git a/gtsam/geometry/Cal3.h b/gtsam/geometry/Cal3.h
index 6d3973080..b1a61c592 100644
--- a/gtsam/geometry/Cal3.h
+++ b/gtsam/geometry/Cal3.h
@@ -165,7 +165,7 @@ class GTSAM_EXPORT Cal3 {
 #endif
 
   /// Return inverted calibration matrix inv(K)
-  Matrix3 matrix_inverse() const {
+  Matrix3 inverse() const {
     const double fxy = fx_ * fy_, sv0 = s_ * v0_, fyu0 = fy_ * u0_;
     Matrix3 K_inverse;
     K_inverse << 1.0 / fx_, -s_ / fxy, (sv0 - fyu0) / fxy, 0.0, 1.0 / fy_,
diff --git a/gtsam/geometry/Cal3_S2.cpp b/gtsam/geometry/Cal3_S2.cpp
index 12635abdd..2d830a4a3 100644
--- a/gtsam/geometry/Cal3_S2.cpp
+++ b/gtsam/geometry/Cal3_S2.cpp
@@ -22,12 +22,11 @@
 #include <iostream>
 
 namespace gtsam {
-using namespace std;
 
 /* ************************************************************************* */
-ostream& operator<<(ostream& os, const Cal3_S2& cal) {
-  os << "{fx: " << cal.fx() << ", fy: " << cal.fy() << ", s:" << cal.skew()
-     << ", px:" << cal.px() << ", py:" << cal.py() << "}";
+std::ostream& operator<<(std::ostream& os, const Cal3_S2& cal) {
+  os << "{ fx: " << cal.fx() << ", fy: " << cal.fy() << ", s: " << cal.skew()
+     << ", px: " << cal.px() << ", py: " << cal.py() << " }";
   return os;
 }
 
@@ -68,7 +67,7 @@ Point2 Cal3_S2::calibrate(const Point2& p, OptionalJacobian<2, 5> Dcal,
 }
 
 /* ************************************************************************* */
-Vector3 Cal3_S2::calibrate(const Vector3& p) const { return matrix_inverse() * p; }
+Vector3 Cal3_S2::calibrate(const Vector3& p) const { return inverse() * p; }
 
 /* ************************************************************************* */
 
diff --git a/gtsam/gtsam.i b/gtsam/gtsam.i
index e405e533c..493c1d7db 100644
--- a/gtsam/gtsam.i
+++ b/gtsam/gtsam.i
@@ -852,7 +852,7 @@ class Cal3_S2 {
   gtsam::Point2 principalPoint() const;
   Vector vector() const;
   Matrix K() const;
-  Matrix matrix_inverse() const;
+  Matrix inverse() const;
 
   // enabling serialization functionality
   void serialize() const;

From f7d9710543e1a0d7f899159916d260afd92ecfd5 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Wed, 2 Dec 2020 06:13:35 -0500
Subject: [PATCH 11/18] remove using-namespace and fix print test

---
 gtsam/geometry/Cal3_S2Stereo.cpp     | 11 +++++++++--
 gtsam/geometry/tests/testCal3_S2.cpp |  4 ++--
 2 files changed, 11 insertions(+), 4 deletions(-)

diff --git a/gtsam/geometry/Cal3_S2Stereo.cpp b/gtsam/geometry/Cal3_S2Stereo.cpp
index 43f0fb12f..3bf7c3468 100644
--- a/gtsam/geometry/Cal3_S2Stereo.cpp
+++ b/gtsam/geometry/Cal3_S2Stereo.cpp
@@ -20,7 +20,14 @@
 #include <iostream>
 
 namespace gtsam {
-using namespace std;
+
+/* ************************************************************************* */
+std::ostream& operator<<(std::ostream& os, const Cal3_S2Stereo& cal) {
+  os << "{ fx: " << cal.fx() << ", fy: " << cal.fy() << ", s: " << cal.skew()
+     << ", px: " << cal.px() << ", py: " << cal.py()
+     << ", b: " << cal.baseline() << " }";
+  return os;
+}
 
 /* ************************************************************************* */
 void Cal3_S2Stereo::print(const std::string& s) const {
@@ -32,7 +39,7 @@ void Cal3_S2Stereo::print(const std::string& s) const {
 /* ************************************************************************* */
 bool Cal3_S2Stereo::equals(const Cal3_S2Stereo& other, double tol) const {
   const Cal3_S2* base = dynamic_cast<const Cal3_S2*>(&other);
-  return Cal3_S2::equals(*base, tol) && (std::fabs(b_ - other.baseline()) < tol);
+  return (Cal3_S2::equals(*base, tol) && std::fabs(b_ - other.baseline()) < tol);
 }
 
 /* ************************************************************************* */
diff --git a/gtsam/geometry/tests/testCal3_S2.cpp b/gtsam/geometry/tests/testCal3_S2.cpp
index f7c8fb6b6..64e807aaf 100644
--- a/gtsam/geometry/tests/testCal3_S2.cpp
+++ b/gtsam/geometry/tests/testCal3_S2.cpp
@@ -131,8 +131,8 @@ TEST(Cal3_S2, between) {
 TEST(Cal3_S2, Print) {
   Cal3_S2 cal(5, 5, 5, 5, 5);
   std::stringstream os;
-  os << "{fx: " << cal.fx() << ", fy: " << cal.fy() << ", s:" << cal.skew() << ", px:" << cal.px()
-     << ", py:" << cal.py() << "}";
+  os << "{ fx: " << cal.fx() << ", fy: " << cal.fy() << ", s: " << cal.skew()
+     << ", px: " << cal.px() << ", py: " << cal.py() << " }";
 
   EXPECT(assert_stdout_equal(os.str(), cal));
 }

From 916771c02caa14feed8fdc863c41d97c9d1a07b4 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Wed, 2 Dec 2020 08:04:36 -0500
Subject: [PATCH 12/18] Added tests for printing, plus small formatting

---
 gtsam/geometry/tests/testCal3Bundler.cpp   |  29 +++--
 gtsam/geometry/tests/testCal3DFisheye.cpp  |  14 ++-
 gtsam/geometry/tests/testCal3DS2.cpp       |  26 ++--
 gtsam/geometry/tests/testCal3Unified.cpp   |  35 ++++--
 gtsam/geometry/tests/testCal3_S2.cpp       |  20 +--
 gtsam/geometry/tests/testCal3_S2Stereo.cpp | 136 +++++++++++++++++++++
 6 files changed, 222 insertions(+), 38 deletions(-)
 create mode 100644 gtsam/geometry/tests/testCal3_S2Stereo.cpp

diff --git a/gtsam/geometry/tests/testCal3Bundler.cpp b/gtsam/geometry/tests/testCal3Bundler.cpp
index 448600266..75aa50a25 100644
--- a/gtsam/geometry/tests/testCal3Bundler.cpp
+++ b/gtsam/geometry/tests/testCal3Bundler.cpp
@@ -11,11 +11,12 @@
 
 /**
  * @file  testCal3Bundler.cpp
- * @brief Unit tests for transform derivatives
+ * @brief Unit tests for Bundler calibration model.
  */
 
 #include <CppUnitLite/TestHarness.h>
 #include <gtsam/base/Testable.h>
+#include <gtsam/base/TestableAssertions.h>
 #include <gtsam/base/numericalDerivative.h>
 #include <gtsam/geometry/Cal3Bundler.h>
 
@@ -28,7 +29,7 @@ static Cal3Bundler K(500, 1e-3, 1e-3, 1000, 2000);
 static Point2 p(2,3);
 
 /* ************************************************************************* */
-TEST( Cal3Bundler, vector)
+TEST(Cal3Bundler, vector)
 {
   Cal3Bundler K;
   Vector expected(3);
@@ -37,7 +38,7 @@ TEST( Cal3Bundler, vector)
 }
 
 /* ************************************************************************* */
-TEST( Cal3Bundler, uncalibrate)
+TEST(Cal3Bundler, uncalibrate)
 {
   Vector v = K.vector() ;
   double r = p.x()*p.x() + p.y()*p.y() ;
@@ -47,7 +48,7 @@ TEST( Cal3Bundler, uncalibrate)
   CHECK(assert_equal(expected,actual));
 }
 
-TEST( Cal3Bundler, calibrate )
+TEST(Cal3Bundler, calibrate )
 {
   Point2 pn(0.5, 0.5);
   Point2 pi = K.uncalibrate(pn);
@@ -61,7 +62,7 @@ Point2 uncalibrate_(const Cal3Bundler& k, const Point2& pt) { return k.uncalibra
 Point2 calibrate_(const Cal3Bundler& k, const Point2& pt) { return k.calibrate(pt); }
 
 /* ************************************************************************* */
-TEST( Cal3Bundler, Duncalibrate)
+TEST(Cal3Bundler, Duncalibrate)
 {
   Matrix Dcal, Dp;
   Point2 actual = K.uncalibrate(p, Dcal, Dp);
@@ -74,7 +75,7 @@ TEST( Cal3Bundler, Duncalibrate)
 }
 
 /* ************************************************************************* */
-TEST( Cal3Bundler, Dcalibrate)
+TEST(Cal3Bundler, Dcalibrate)
 {
   Matrix Dcal, Dp;
   Point2 pn(0.5, 0.5);
@@ -88,22 +89,32 @@ TEST( Cal3Bundler, Dcalibrate)
 }
 
 /* ************************************************************************* */
-TEST( Cal3Bundler, assert_equal)
+TEST(Cal3Bundler, assert_equal)
 {
   CHECK(assert_equal(K,K,1e-7));
 }
 
 /* ************************************************************************* */
-TEST( Cal3Bundler, retract)
+TEST(Cal3Bundler, retract)
 {
   Cal3Bundler expected(510, 2e-3, 2e-3, 1000, 2000);
-  Vector d(3);
+  Vector3 d;
   d << 10, 1e-3, 1e-3;
   Cal3Bundler actual = K.retract(d);
   CHECK(assert_equal(expected,actual,1e-7));
   CHECK(assert_equal(d,K.localCoordinates(actual),1e-7));
 }
 
+/* ************************************************************************* */
+TEST(Cal3_S2, Print) {
+  Cal3Bundler cal(1, 2, 3, 4, 5);
+  std::stringstream os;
+  os << "f: " << cal.fx() << ", k1: " << cal.k1() << ", k2: " << cal.k2()
+     << ", px: " << cal.px() << ", py: " << cal.py();
+
+  EXPECT(assert_stdout_equal(os.str(), cal));
+}
+
 /* ************************************************************************* */
 int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
 /* ************************************************************************* */
diff --git a/gtsam/geometry/tests/testCal3DFisheye.cpp b/gtsam/geometry/tests/testCal3DFisheye.cpp
index 6bfbe3e46..85e661728 100644
--- a/gtsam/geometry/tests/testCal3DFisheye.cpp
+++ b/gtsam/geometry/tests/testCal3DFisheye.cpp
@@ -10,12 +10,13 @@
  * -------------------------------------------------------------------------- */
 
 /**
- * @file  testCal3DFisheye.cpp
+ * @file  testCal3Fisheye.cpp
  * @brief Unit tests for fisheye calibration class
  * @author ghaggin
  */
 
 #include <gtsam/base/Testable.h>
+#include <gtsam/base/TestableAssertions.h>
 #include <gtsam/base/numericalDerivative.h>
 #include <gtsam/geometry/Cal3Fisheye.h>
 #include <gtsam/geometry/Point3.h>
@@ -198,6 +199,17 @@ TEST(Cal3Fisheye, Dcalibrate)
   CHECK(assert_equal(numerical2,Dp,1e-5));
 }
 
+/* ************************************************************************* */
+TEST(Cal3Fisheye, Print) {
+  Cal3Fisheye cal(1, 2, 3, 4, 5, 6, 7, 8, 9);
+  std::stringstream os;
+  os << "fx: " << cal.fx() << ", fy: " << cal.fy() << ", s: " << cal.skew()
+     << ", px: " << cal.px() << ", py: " << cal.py() << ", k1: " << cal.k1()
+     << ", k2: " << cal.k2() << ", k3: " << cal.k3() << ", k4: " << cal.k4();
+
+  EXPECT(assert_stdout_equal(os.str(), cal));
+}
+
 /* ************************************************************************* */
 int main() {
   TestResult tr;
diff --git a/gtsam/geometry/tests/testCal3DS2.cpp b/gtsam/geometry/tests/testCal3DS2.cpp
index beed09883..b382e85f3 100644
--- a/gtsam/geometry/tests/testCal3DS2.cpp
+++ b/gtsam/geometry/tests/testCal3DS2.cpp
@@ -11,12 +11,13 @@
 
 /**
  * @file  testCal3DS2.cpp
- * @brief Unit tests for transform derivatives
+ * @brief Unit tests for Cal3DS2 calibration model.
  */
 
 
 #include <CppUnitLite/TestHarness.h>
 #include <gtsam/base/Testable.h>
+#include <gtsam/base/TestableAssertions.h>
 #include <gtsam/base/numericalDerivative.h>
 #include <gtsam/geometry/Cal3DS2.h>
 
@@ -29,7 +30,7 @@ static Cal3DS2 K(500, 100, 0.1, 320, 240, 1e-3, 2.0*1e-3, 3.0*1e-3, 4.0*1e-3);
 static Point2 p(2,3);
 
 /* ************************************************************************* */
-TEST( Cal3DS2, uncalibrate)
+TEST(Cal3DS2, uncalibrate)
 {
   Vector k = K.k() ;
   double r = p.x()*p.x() + p.y()*p.y() ;
@@ -43,7 +44,7 @@ TEST( Cal3DS2, uncalibrate)
   CHECK(assert_equal(q,p_i));
 }
 
-TEST( Cal3DS2, calibrate )
+TEST(Cal3DS2, calibrate )
 {
   Point2 pn(0.5, 0.5);
   Point2 pi = K.uncalibrate(pn);
@@ -54,7 +55,7 @@ TEST( Cal3DS2, calibrate )
 Point2 uncalibrate_(const Cal3DS2& k, const Point2& pt) { return k.uncalibrate(pt); }
 
 /* ************************************************************************* */
-TEST( Cal3DS2, Duncalibrate1)
+TEST(Cal3DS2, Duncalibrate1)
 {
   Matrix computed;
   K.uncalibrate(p, computed, boost::none);
@@ -65,7 +66,7 @@ TEST( Cal3DS2, Duncalibrate1)
 }
 
 /* ************************************************************************* */
-TEST( Cal3DS2, Duncalibrate2)
+TEST(Cal3DS2, Duncalibrate2)
 {
   Matrix computed; K.uncalibrate(p, boost::none, computed);
   Matrix numerical = numericalDerivative22(uncalibrate_, K, p, 1e-7);
@@ -79,7 +80,7 @@ Point2 calibrate_(const Cal3DS2& k, const Point2& pt) {
 }
 
 /* ************************************************************************* */
-TEST( Cal3DS2, Dcalibrate)
+TEST(Cal3DS2, Dcalibrate)
 {
   Point2 pn(0.5, 0.5);
   Point2 pi = K.uncalibrate(pn);
@@ -95,7 +96,7 @@ TEST( Cal3DS2, Dcalibrate)
 TEST(Cal3DS2, assert_equal) { CHECK(assert_equal(K, K, 1e-5)); }
 
 /* ************************************************************************* */
-TEST( Cal3DS2, retract)
+TEST(Cal3DS2, retract)
 {
   Cal3DS2 expected(500 + 1, 100 + 2, 0.1 + 3, 320 + 4, 240 + 5, 1e-3 + 6,
       2.0 * 1e-3 + 7, 3.0 * 1e-3 + 8, 4.0 * 1e-3 + 9);
@@ -106,6 +107,17 @@ TEST( Cal3DS2, retract)
   CHECK(assert_equal(d,K.localCoordinates(actual),1e-7));
 }
 
+/* ************************************************************************* */
+TEST(Cal3DS2, Print) {
+  Cal3DS2 cal(1, 2, 3, 4, 5, 6, 7, 8, 9);
+  std::stringstream os;
+  os << "fx: " << cal.fx() << ", fy: " << cal.fy() << ", s: " << cal.skew()
+     << ", px: " << cal.px() << ", py: " << cal.py() << ", k1: " << cal.k1()
+     << ", k2: " << cal.k2() << ", p1: " << cal.p1() << ", p2: " << cal.p2();
+
+  EXPECT(assert_stdout_equal(os.str(), cal));
+}
+
 /* ************************************************************************* */
 int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
 /* ************************************************************************* */
diff --git a/gtsam/geometry/tests/testCal3Unified.cpp b/gtsam/geometry/tests/testCal3Unified.cpp
index 8abb6fe04..41a1d3ad9 100644
--- a/gtsam/geometry/tests/testCal3Unified.cpp
+++ b/gtsam/geometry/tests/testCal3Unified.cpp
@@ -10,12 +10,13 @@
  * -------------------------------------------------------------------------- */
 
 /**
- * @file  testCal3Unify.cpp
- * @brief Unit tests for transform derivatives
+ * @file  testCal3Unified.cpp
+ * @brief Unit tests for Cal3Unified calibration model.
  */
 
 #include <CppUnitLite/TestHarness.h>
 #include <gtsam/base/Testable.h>
+#include <gtsam/base/TestableAssertions.h>
 #include <gtsam/base/numericalDerivative.h>
 #include <gtsam/geometry/Cal3Unified.h>
 
@@ -39,7 +40,7 @@ static Cal3Unified K(100, 105, 0.0, 320, 240, 1e-3, 2.0*1e-3, 3.0*1e-3, 4.0*1e-3
 static Point2 p(0.5, 0.7);
 
 /* ************************************************************************* */
-TEST( Cal3Unified, uncalibrate)
+TEST(Cal3Unified, uncalibrate)
 {
   Point2 p_i(364.7791831734982, 305.6677211952602) ;
   Point2 q = K.uncalibrate(p);
@@ -47,7 +48,7 @@ TEST( Cal3Unified, uncalibrate)
 }
 
 /* ************************************************************************* */
-TEST( Cal3Unified, spaceNplane)
+TEST(Cal3Unified, spaceNplane)
 {
   Point2 q = K.spaceToNPlane(p);
   CHECK(assert_equal(Point2(0.441731600049497, 0.618424240069295), q));
@@ -55,7 +56,7 @@ TEST( Cal3Unified, spaceNplane)
 }
 
 /* ************************************************************************* */
-TEST( Cal3Unified, calibrate)
+TEST(Cal3Unified, calibrate)
 {
   Point2 pi = K.uncalibrate(p);
   Point2 pn_hat = K.calibrate(pi);
@@ -65,7 +66,7 @@ TEST( Cal3Unified, calibrate)
 Point2 uncalibrate_(const Cal3Unified& k, const Point2& pt) { return k.uncalibrate(pt); }
 
 /* ************************************************************************* */
-TEST( Cal3Unified, Duncalibrate1)
+TEST(Cal3Unified, Duncalibrate1)
 {
   Matrix computed;
   K.uncalibrate(p, computed, boost::none);
@@ -74,7 +75,7 @@ TEST( Cal3Unified, Duncalibrate1)
 }
 
 /* ************************************************************************* */
-TEST( Cal3Unified, Duncalibrate2)
+TEST(Cal3Unified, Duncalibrate2)
 {
   Matrix computed;
   K.uncalibrate(p, boost::none, computed);
@@ -87,7 +88,7 @@ Point2 calibrate_(const Cal3Unified& k, const Point2& pt) {
 }
 
 /* ************************************************************************* */
-TEST( Cal3Unified, Dcalibrate)
+TEST(Cal3Unified, Dcalibrate)
 {
   Point2 pi = K.uncalibrate(p);
   Matrix Dcal, Dp;
@@ -99,13 +100,13 @@ TEST( Cal3Unified, Dcalibrate)
 }
 
 /* ************************************************************************* */
-TEST( Cal3Unified, assert_equal)
+TEST(Cal3Unified, assert_equal)
 {
   CHECK(assert_equal(K,K,1e-9));
 }
 
 /* ************************************************************************* */
-TEST( Cal3Unified, retract)
+TEST(Cal3Unified, retract)
 {
   Cal3Unified expected(100 + 2, 105 + 3, 0.0 + 4, 320 + 5, 240 + 6,
       1e-3 + 7, 2.0*1e-3 + 8, 3.0*1e-3 + 9, 4.0*1e-3 + 10, 0.1 + 1);
@@ -117,7 +118,7 @@ TEST( Cal3Unified, retract)
 }
 
 /* ************************************************************************* */
-TEST( Cal3Unified, DerivedValue)
+TEST(Cal3Unified, DerivedValue)
 {
   Values values;
   Cal3Unified cal(1, 2, 3, 4, 5, 6, 7, 8, 9, 10);
@@ -129,6 +130,18 @@ TEST( Cal3Unified, DerivedValue)
   CHECK(assert_equal(cal,calafter,1e-9));
 }
 
+/* ************************************************************************* */
+TEST(Cal3Unified, Print) {
+  Cal3Unified cal(0, 1, 2, 3, 4, 5, 6, 7, 8, 9);
+  std::stringstream os;
+  os << "fx: " << cal.fx() << ", fy: " << cal.fy() << ", s: " << cal.skew()
+     << ", px: " << cal.px() << ", py: " << cal.py() << ", k1: " << cal.k1()
+     << ", k2: " << cal.k2() << ", p1: " << cal.p1() << ", p2: " << cal.p2()
+     << ", xi: " << cal.xi();
+
+  EXPECT(assert_stdout_equal(os.str(), cal));
+}
+
 /* ************************************************************************* */
 int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
 /* ************************************************************************* */
diff --git a/gtsam/geometry/tests/testCal3_S2.cpp b/gtsam/geometry/tests/testCal3_S2.cpp
index 64e807aaf..3317addff 100644
--- a/gtsam/geometry/tests/testCal3_S2.cpp
+++ b/gtsam/geometry/tests/testCal3_S2.cpp
@@ -11,7 +11,7 @@
 
 /**
  * @file  testCal3_S2.cpp
- * @brief Unit tests for transform derivatives
+ * @brief Unit tests for basic Cal3_S2 calibration model.
  */
 
 #include <CppUnitLite/TestHarness.h>
@@ -31,7 +31,7 @@ static Point2 p_uv(1320.3, 1740);
 static Point2 p_xy(2, 3);
 
 /* ************************************************************************* */
-TEST( Cal3_S2, easy_constructor)
+TEST(Cal3_S2, easy_constructor)
 {
   Cal3_S2 expected(554.256, 554.256, 0, 640 / 2, 480 / 2);
 
@@ -43,7 +43,7 @@ TEST( Cal3_S2, easy_constructor)
 }
 
 /* ************************************************************************* */
-TEST( Cal3_S2, calibrate)
+TEST(Cal3_S2, calibrate)
 {
   Point2 intrinsic(2,3);
   Point2 expectedimage(1320.3, 1740);
@@ -53,7 +53,7 @@ TEST( Cal3_S2, calibrate)
 }
 
 /* ************************************************************************* */
-TEST( Cal3_S2, calibrate_homogeneous) {
+TEST(Cal3_S2, calibrate_homogeneous) {
   Vector3 intrinsic(2, 3, 1);
   Vector3 image(1320.3, 1740, 1);
   CHECK(assert_equal((Vector)intrinsic,(Vector)K.calibrate(image)));
@@ -61,7 +61,7 @@ TEST( Cal3_S2, calibrate_homogeneous) {
 
 /* ************************************************************************* */
 Point2 uncalibrate_(const Cal3_S2& k, const Point2& pt) { return k.uncalibrate(pt); }
-TEST( Cal3_S2, Duncalibrate1)
+TEST(Cal3_S2, Duncalibrate1)
 {
   Matrix25 computed; K.uncalibrate(p, computed, boost::none);
   Matrix numerical = numericalDerivative21(uncalibrate_, K, p);
@@ -69,7 +69,7 @@ TEST( Cal3_S2, Duncalibrate1)
 }
 
 /* ************************************************************************* */
-TEST( Cal3_S2, Duncalibrate2)
+TEST(Cal3_S2, Duncalibrate2)
 {
   Matrix computed; K.uncalibrate(p, boost::none, computed);
   Matrix numerical = numericalDerivative22(uncalibrate_, K, p);
@@ -98,7 +98,7 @@ TEST(Cal3_S2, Dcalibrate2)
 }
 
 /* ************************************************************************* */
-TEST( Cal3_S2, assert_equal)
+TEST(Cal3_S2, assert_equal)
 {
   CHECK(assert_equal(K,K,1e-9));
 
@@ -107,7 +107,7 @@ TEST( Cal3_S2, assert_equal)
 }
 
 /* ************************************************************************* */
-TEST( Cal3_S2, retract)
+TEST(Cal3_S2, retract)
 {
   Cal3_S2 expected(500+1, 500+2, 0.1+3, 640 / 2+4, 480 / 2+5);
   Vector d(5);
@@ -131,8 +131,8 @@ TEST(Cal3_S2, between) {
 TEST(Cal3_S2, Print) {
   Cal3_S2 cal(5, 5, 5, 5, 5);
   std::stringstream os;
-  os << "{ fx: " << cal.fx() << ", fy: " << cal.fy() << ", s: " << cal.skew()
-     << ", px: " << cal.px() << ", py: " << cal.py() << " }";
+  os << "fx: " << cal.fx() << ", fy: " << cal.fy() << ", s: " << cal.skew()
+     << ", px: " << cal.px() << ", py: " << cal.py();
 
   EXPECT(assert_stdout_equal(os.str(), cal));
 }
diff --git a/gtsam/geometry/tests/testCal3_S2Stereo.cpp b/gtsam/geometry/tests/testCal3_S2Stereo.cpp
new file mode 100644
index 000000000..9c93b7496
--- /dev/null
+++ b/gtsam/geometry/tests/testCal3_S2Stereo.cpp
@@ -0,0 +1,136 @@
+/* ----------------------------------------------------------------------------
+
+ * 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  testCal3_S2Stereo.cpp
+ * @brief Unit tests for stereo-rig calibration model.
+ */
+
+#include <CppUnitLite/TestHarness.h>
+#include <gtsam/base/Testable.h>
+#include <gtsam/base/TestableAssertions.h>
+#include <gtsam/base/numericalDerivative.h>
+#include <gtsam/geometry/Cal3_S2Stereo.h>
+
+using namespace gtsam;
+
+GTSAM_CONCEPT_TESTABLE_INST(Cal3_S2Stereo)
+GTSAM_CONCEPT_MANIFOLD_INST(Cal3_S2Stereo)
+
+static Cal3_S2Stereo K(500, 500, 0.1, 640 / 2, 480 / 2, 1);
+static Point2 p(1, -2);
+static Point2 p_uv(1320.3, 1740);
+static Point2 p_xy(2, 3);
+
+/* ************************************************************************* */
+TEST(Cal3_S2Stereo, easy_constructor) {
+  Cal3_S2Stereo expected(554.256, 554.256, 0, 640 / 2, 480 / 2, 3);
+
+  double fov = 60;  // degrees
+  size_t w = 640, h = 480;
+  Cal3_S2Stereo actual(fov, w, h, 3);
+
+  CHECK(assert_equal(expected, actual, 1e-3));
+}
+
+/* ************************************************************************* */
+TEST(Cal3_S2Stereo, calibrate) {
+  Point2 intrinsic(2, 3);
+  Point2 expectedimage(1320.3, 1740);
+  Point2 imagecoordinates = K.uncalibrate(intrinsic);
+  CHECK(assert_equal(expectedimage, imagecoordinates));
+  CHECK(assert_equal(intrinsic, K.calibrate(imagecoordinates)));
+}
+
+/* ************************************************************************* */
+TEST(Cal3_S2Stereo, calibrate_homogeneous) {
+  Vector3 intrinsic(2, 3, 1);
+  Vector3 image(1320.3, 1740, 1);
+  CHECK(assert_equal((Vector)intrinsic, (Vector)K.calibrate(image)));
+}
+
+//TODO(Varun) Add calibrate and uncalibrate methods
+// /* ************************************************************************* */
+// Point2 uncalibrate_(const Cal3_S2Stereo& k, const Point2& pt) {
+//   return k.uncalibrate(pt);
+// }
+
+// TEST(Cal3_S2Stereo, Duncalibrate1) {
+//   Matrix26 computed;
+//   K.uncalibrate(p, computed, boost::none);
+//   Matrix numerical = numericalDerivative21(uncalibrate_, K, p);
+//   CHECK(assert_equal(numerical, computed, 1e-8));
+// }
+
+// /* ************************************************************************* */
+// TEST(Cal3_S2Stereo, Duncalibrate2) {
+//   Matrix computed;
+//   K.uncalibrate(p, boost::none, computed);
+//   Matrix numerical = numericalDerivative22(uncalibrate_, K, p);
+//   CHECK(assert_equal(numerical, computed, 1e-9));
+// }
+
+// Point2 calibrate_(const Cal3_S2Stereo& k, const Point2& pt) {
+//   return k.calibrate(pt);
+// }
+// /* ************************************************************************* */
+// TEST(Cal3_S2Stereo, Dcalibrate1) {
+//   Matrix computed;
+//   Point2 expected = K.calibrate(p_uv, computed, boost::none);
+//   Matrix numerical = numericalDerivative21(calibrate_, K, p_uv);
+//   CHECK(assert_equal(expected, p_xy, 1e-8));
+//   CHECK(assert_equal(numerical, computed, 1e-8));
+// }
+
+// /* ************************************************************************* */
+// TEST(Cal3_S2Stereo, Dcalibrate2) {
+//   Matrix computed;
+//   Point2 expected = K.calibrate(p_uv, boost::none, computed);
+//   Matrix numerical = numericalDerivative22(calibrate_, K, p_uv);
+//   CHECK(assert_equal(expected, p_xy, 1e-8));
+//   CHECK(assert_equal(numerical, computed, 1e-8));
+// }
+
+/* ************************************************************************* */
+TEST(Cal3_S2Stereo, assert_equal) {
+  CHECK(assert_equal(K, K, 1e-9));
+
+  Cal3_S2Stereo K1(500, 500, 0.1, 640 / 2, 480 / 2, 1);
+  CHECK(assert_equal(K, K1, 1e-9));
+}
+
+/* ************************************************************************* */
+TEST(Cal3_S2Stereo, retract) {
+  Cal3_S2Stereo expected(500 + 1, 500 + 2, 0.1 + 3, 640 / 2 + 4, 480 / 2 + 5,
+                         7);
+  Vector6 d;
+  d << 1, 2, 3, 4, 5, 6;
+  Cal3_S2Stereo actual = K.retract(d);
+  CHECK(assert_equal(expected, actual, 1e-7));
+  CHECK(assert_equal(d, K.localCoordinates(actual), 1e-7));
+}
+
+/* ************************************************************************* */
+TEST(Cal3_S2Stereo, Print) {
+  Cal3_S2Stereo cal(5, 5, 5, 5, 5, 2);
+  std::stringstream os;
+  os << "fx: " << cal.fx() << ", fy: " << cal.fy() << ", s: " << cal.skew()
+     << ", px: " << cal.px() << ", py: " << cal.py()
+     << ", b: " << cal.baseline();
+  EXPECT(assert_stdout_equal(os.str(), cal));
+}
+
+/* ************************************************************************* */
+int main() {
+  TestResult tr;
+  return TestRegistry::runAllTests(tr);
+}
+/* ************************************************************************* */

From e488dc8e9c5be5f04ec597c69b363848478939b9 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Wed, 2 Dec 2020 08:19:19 -0500
Subject: [PATCH 13/18] stream printing for all calibration models

---
 gtsam/geometry/Cal3.cpp          | 4 ++--
 gtsam/geometry/Cal3Bundler.cpp   | 7 +++++++
 gtsam/geometry/Cal3Bundler.h     | 4 ++++
 gtsam/geometry/Cal3DS2.cpp       | 6 ++++++
 gtsam/geometry/Cal3DS2.h         | 4 ++++
 gtsam/geometry/Cal3DS2_Base.cpp  | 8 ++++++++
 gtsam/geometry/Cal3DS2_Base.h    | 4 ++++
 gtsam/geometry/Cal3Fisheye.cpp   | 8 ++++++++
 gtsam/geometry/Cal3Fisheye.h     | 4 ++++
 gtsam/geometry/Cal3Unified.cpp   | 7 +++++++
 gtsam/geometry/Cal3Unified.h     | 4 ++++
 gtsam/geometry/Cal3_S2.cpp       | 4 ++--
 gtsam/geometry/Cal3_S2Stereo.cpp | 7 +++----
 gtsam/geometry/Cal3_S2Stereo.h   | 7 +++++--
 14 files changed, 68 insertions(+), 10 deletions(-)

diff --git a/gtsam/geometry/Cal3.cpp b/gtsam/geometry/Cal3.cpp
index 6183c6e5e..ec39d7eda 100644
--- a/gtsam/geometry/Cal3.cpp
+++ b/gtsam/geometry/Cal3.cpp
@@ -49,8 +49,8 @@ Cal3::Cal3(const std::string& path)
 
 /* ************************************************************************* */
 std::ostream& operator<<(std::ostream& os, const Cal3& cal) {
-  os << "{ fx: " << cal.fx() << ", fy: " << cal.fy() << ", s: " << cal.skew()
-     << ", px: " << cal.px() << ", py: " << cal.py() << " }";
+  os << "fx: " << cal.fx() << ", fy: " << cal.fy() << ", s: " << cal.skew()
+     << ", px: " << cal.px() << ", py: " << cal.py();
   return os;
 }
 
diff --git a/gtsam/geometry/Cal3Bundler.cpp b/gtsam/geometry/Cal3Bundler.cpp
index 8c524e1cc..af2f881d8 100644
--- a/gtsam/geometry/Cal3Bundler.cpp
+++ b/gtsam/geometry/Cal3Bundler.cpp
@@ -42,6 +42,13 @@ Vector3 Cal3Bundler::vector() const {
   return Vector3(f_, k1_, k2_);
 }
 
+/* ************************************************************************* */
+std::ostream& operator<<(std::ostream& os, const Cal3Bundler& cal) {
+  os << "f: " << cal.fx() << ", k1: " << cal.k1() << ", k2: " << cal.k2()
+     << ", px: " << cal.px() << ", py: " << cal.py();
+  return os;
+}
+
 /* ************************************************************************* */
 void Cal3Bundler::print(const std::string& s) const {
   gtsam::print((Vector)(Vector(5) << f_, k1_, k2_, u0_, v0_).finished(), s + ".K");
diff --git a/gtsam/geometry/Cal3Bundler.h b/gtsam/geometry/Cal3Bundler.h
index d128c329b..76703f96f 100644
--- a/gtsam/geometry/Cal3Bundler.h
+++ b/gtsam/geometry/Cal3Bundler.h
@@ -67,6 +67,10 @@ class GTSAM_EXPORT Cal3Bundler : public Cal3 {
   /// @name Testable
   /// @{
 
+  /// Output stream operator
+  GTSAM_EXPORT friend std::ostream& operator<<(std::ostream& os,
+                                               const Cal3Bundler& cal);
+
   /// print with optional string
   void print(const std::string& s = "") const override;
 
diff --git a/gtsam/geometry/Cal3DS2.cpp b/gtsam/geometry/Cal3DS2.cpp
index b4595a4dc..71aa2738d 100644
--- a/gtsam/geometry/Cal3DS2.cpp
+++ b/gtsam/geometry/Cal3DS2.cpp
@@ -24,6 +24,12 @@
 
 namespace gtsam {
 
+/* ************************************************************************* */
+std::ostream& operator<<(std::ostream& os, const Cal3DS2& cal) {
+  os << (Cal3DS2_Base&)cal;
+  return os;
+}
+
 /* ************************************************************************* */
 void Cal3DS2::print(const std::string& s_) const {
   Base::print(s_);
diff --git a/gtsam/geometry/Cal3DS2.h b/gtsam/geometry/Cal3DS2.h
index 03a4d8d46..d0bed8652 100644
--- a/gtsam/geometry/Cal3DS2.h
+++ b/gtsam/geometry/Cal3DS2.h
@@ -60,6 +60,10 @@ public:
   /// @name Testable
   /// @{
 
+  /// Output stream operator
+  GTSAM_EXPORT friend std::ostream& operator<<(std::ostream& os,
+                                               const Cal3DS2& cal);
+
   /// print with optional string
   void print(const std::string& s = "") const override;
 
diff --git a/gtsam/geometry/Cal3DS2_Base.cpp b/gtsam/geometry/Cal3DS2_Base.cpp
index 016c9dfa2..2741d0736 100644
--- a/gtsam/geometry/Cal3DS2_Base.cpp
+++ b/gtsam/geometry/Cal3DS2_Base.cpp
@@ -31,6 +31,14 @@ Vector9 Cal3DS2_Base::vector() const {
   return v;
 }
 
+/* ************************************************************************* */
+std::ostream& operator<<(std::ostream& os, const Cal3DS2_Base& cal) {
+  os << (Cal3&)cal;
+  os << ", k1: " << cal.k1() << ", k2: " << cal.k2() << ", p1: " << cal.p1()
+     << ", p2: " << cal.p2();
+  return os;
+}
+
 /* ************************************************************************* */
 void Cal3DS2_Base::print(const std::string& s_) const {
   gtsam::print((Matrix)K(), s_ + ".K");
diff --git a/gtsam/geometry/Cal3DS2_Base.h b/gtsam/geometry/Cal3DS2_Base.h
index e0de6af61..713fa4e0e 100644
--- a/gtsam/geometry/Cal3DS2_Base.h
+++ b/gtsam/geometry/Cal3DS2_Base.h
@@ -80,6 +80,10 @@ class GTSAM_EXPORT Cal3DS2_Base : public Cal3 {
   /// @name Testable
   /// @{
 
+  /// Output stream operator
+  GTSAM_EXPORT friend std::ostream& operator<<(std::ostream& os,
+                                               const Cal3DS2_Base& cal);
+
   /// print with optional string
   void print(const std::string& s = "") const override;
 
diff --git a/gtsam/geometry/Cal3Fisheye.cpp b/gtsam/geometry/Cal3Fisheye.cpp
index 55020f581..b9e60acee 100644
--- a/gtsam/geometry/Cal3Fisheye.cpp
+++ b/gtsam/geometry/Cal3Fisheye.cpp
@@ -139,6 +139,14 @@ Point2 Cal3Fisheye::calibrate(const Point2& uv, OptionalJacobian<2, 9> Dcal,
   return pi;
 }
 
+/* ************************************************************************* */
+std::ostream& operator<<(std::ostream& os, const Cal3Fisheye& cal) {
+  os << (Cal3&)cal;
+  os << ", k1: " << cal.k1() << ", k2: " << cal.k2() << ", k3: " << cal.k3()
+     << ", k4: " << cal.k4();
+  return os;
+}
+
 /* ************************************************************************* */
 void Cal3Fisheye::print(const std::string& s_) const {
   gtsam::print((Matrix)K(), s_ + ".K");
diff --git a/gtsam/geometry/Cal3Fisheye.h b/gtsam/geometry/Cal3Fisheye.h
index f41a9b0d7..738b2275d 100644
--- a/gtsam/geometry/Cal3Fisheye.h
+++ b/gtsam/geometry/Cal3Fisheye.h
@@ -137,6 +137,10 @@ class GTSAM_EXPORT Cal3Fisheye : public Cal3 {
   /// @name Testable
   /// @{
 
+  /// Output stream operator
+  GTSAM_EXPORT friend std::ostream& operator<<(std::ostream& os,
+                                               const Cal3Fisheye& cal);
+
   /// print with optional string
   virtual void print(const std::string& s = "") const override;
 
diff --git a/gtsam/geometry/Cal3Unified.cpp b/gtsam/geometry/Cal3Unified.cpp
index d260dd725..80613bbf2 100644
--- a/gtsam/geometry/Cal3Unified.cpp
+++ b/gtsam/geometry/Cal3Unified.cpp
@@ -32,6 +32,13 @@ Vector10 Cal3Unified::vector() const {
   return v;
 }
 
+/* ************************************************************************* */
+std::ostream& operator<<(std::ostream& os, const Cal3Unified& cal) {
+  os << (Cal3DS2_Base&)cal;
+  os << ", xi: " << cal.xi();
+  return os;
+}
+
 /* ************************************************************************* */
 void Cal3Unified::print(const std::string& s) const {
   Base::print(s);
diff --git a/gtsam/geometry/Cal3Unified.h b/gtsam/geometry/Cal3Unified.h
index e1368db4c..a2c5ebc5c 100644
--- a/gtsam/geometry/Cal3Unified.h
+++ b/gtsam/geometry/Cal3Unified.h
@@ -75,6 +75,10 @@ class GTSAM_EXPORT Cal3Unified : public Cal3DS2_Base {
   /// @name Testable
   /// @{
 
+  /// Output stream operator
+  GTSAM_EXPORT friend std::ostream& operator<<(std::ostream& os,
+                                               const Cal3Unified& cal);
+
   /// print with optional string
   void print(const std::string& s = "") const override;
 
diff --git a/gtsam/geometry/Cal3_S2.cpp b/gtsam/geometry/Cal3_S2.cpp
index 2d830a4a3..98c7bea38 100644
--- a/gtsam/geometry/Cal3_S2.cpp
+++ b/gtsam/geometry/Cal3_S2.cpp
@@ -25,8 +25,8 @@ namespace gtsam {
 
 /* ************************************************************************* */
 std::ostream& operator<<(std::ostream& os, const Cal3_S2& cal) {
-  os << "{ fx: " << cal.fx() << ", fy: " << cal.fy() << ", s: " << cal.skew()
-     << ", px: " << cal.px() << ", py: " << cal.py() << " }";
+  // Use the base class version since it is identical.
+  os << (Cal3&)cal;
   return os;
 }
 
diff --git a/gtsam/geometry/Cal3_S2Stereo.cpp b/gtsam/geometry/Cal3_S2Stereo.cpp
index 3bf7c3468..8bd733e95 100644
--- a/gtsam/geometry/Cal3_S2Stereo.cpp
+++ b/gtsam/geometry/Cal3_S2Stereo.cpp
@@ -23,16 +23,15 @@ namespace gtsam {
 
 /* ************************************************************************* */
 std::ostream& operator<<(std::ostream& os, const Cal3_S2Stereo& cal) {
-  os << "{ fx: " << cal.fx() << ", fy: " << cal.fy() << ", s: " << cal.skew()
-     << ", px: " << cal.px() << ", py: " << cal.py()
-     << ", b: " << cal.baseline() << " }";
+  os << (Cal3_S2&)cal;
+  os << ", b: " << cal.baseline();
   return os;
 }
 
 /* ************************************************************************* */
 void Cal3_S2Stereo::print(const std::string& s) const {
   std::cout << s << (s != "" ? " " : "");
-  print("K: ");
+  std::cout << "K: " << (Matrix)K() << std::endl;
   std::cout << "Baseline: " << b_ << std::endl;
 }
 
diff --git a/gtsam/geometry/Cal3_S2Stereo.h b/gtsam/geometry/Cal3_S2Stereo.h
index 0c68ef6b4..94341ec37 100644
--- a/gtsam/geometry/Cal3_S2Stereo.h
+++ b/gtsam/geometry/Cal3_S2Stereo.h
@@ -61,6 +61,10 @@ namespace gtsam {
     /// @name Testable
     /// @{
 
+    /// Output stream operator
+    GTSAM_EXPORT friend std::ostream& operator<<(std::ostream& os,
+                                                 const Cal3_S2Stereo& cal);
+
     /// print with optional string
     void print(const std::string& s = "") const override;
 
@@ -83,7 +87,7 @@ namespace gtsam {
     /// vectorized form (column-wise)
     Vector6 vector() const {
       Vector6 v;
-      v << vector(), b_;
+      v << Cal3_S2::vector(), b_;
       return v;
     }
 
@@ -108,7 +112,6 @@ namespace gtsam {
       return T2.vector() - vector();
     }
 
-
     /// @}
     /// @name Advanced Interface
     /// @{

From ecb0af57fdad66ea6e5c8bd1b1bbec5585e3fb96 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Wed, 2 Dec 2020 08:36:23 -0500
Subject: [PATCH 14/18] Improved constructor for loading parameters from file

---
 gtsam/geometry/Cal3.cpp |  5 ++---
 gtsam/geometry/Cal3.h   | 10 +++++++++-
 2 files changed, 11 insertions(+), 4 deletions(-)

diff --git a/gtsam/geometry/Cal3.cpp b/gtsam/geometry/Cal3.cpp
index ec39d7eda..6210a6065 100644
--- a/gtsam/geometry/Cal3.cpp
+++ b/gtsam/geometry/Cal3.cpp
@@ -33,12 +33,11 @@ Cal3::Cal3(double fov, int w, int h)
 }
 
 /* ************************************************************************* */
-Cal3::Cal3(const std::string& path)
-    : fx_(320), fy_(320), s_(0), u0_(320), v0_(140) {
+Cal3::Cal3(const std::string& path) {
   const auto buffer = path + std::string("/calibration_info.txt");
   std::ifstream infile(buffer, std::ios::in);
 
-  if (infile) {
+  if (infile && !infile.eof()) {
     infile >> fx_ >> fy_ >> s_ >> u0_ >> v0_;
   } else {
     throw std::runtime_error("Cal3: Unable to load the calibration");
diff --git a/gtsam/geometry/Cal3.h b/gtsam/geometry/Cal3.h
index b1a61c592..712414c82 100644
--- a/gtsam/geometry/Cal3.h
+++ b/gtsam/geometry/Cal3.h
@@ -103,7 +103,15 @@ class GTSAM_EXPORT Cal3 {
   /// @name Advanced Constructors
   /// @{
 
-  /// load calibration from location (default name is calibration_info.txt)
+  /**
+   * Load calibration parameters from `calibration_info.txt` file located in
+   * `path` directory.
+   *
+   * The contents of calibration file should be the 5 parameters in order:
+   * `fx, fy, s, u0, v0`
+   *
+   * @param path path to directory containing `calibration_info.txt`.
+   */
   Cal3(const std::string& path);
 
   /// @}

From fc0fd1a12533b3913f2e69b78eb74920310e6962 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Wed, 2 Dec 2020 16:19:05 -0500
Subject: [PATCH 15/18] Override dim(), cleanup, and add unicode

---
 gtsam/geometry/Cal3.cpp                  |  9 +++++++-
 gtsam/geometry/Cal3.h                    | 16 +++++++--------
 gtsam/geometry/Cal3Bundler.cpp           | 22 ++++++++++++--------
 gtsam/geometry/Cal3Bundler.h             | 26 ++++++++----------------
 gtsam/geometry/Cal3DS2.h                 |  4 ++--
 gtsam/geometry/Cal3DS2_Base.cpp          |  6 +++---
 gtsam/geometry/Cal3DS2_Base.h            | 15 +++++++++-----
 gtsam/geometry/Cal3Fisheye.h             |  8 ++++----
 gtsam/geometry/Cal3Unified.h             | 12 +++++------
 gtsam/geometry/Cal3_S2.cpp               |  5 +++--
 gtsam/geometry/Cal3_S2.h                 |  5 +----
 gtsam/geometry/Cal3_S2Stereo.h           |  6 +++---
 gtsam/geometry/tests/testCal3Bundler.cpp | 23 ++++++++-------------
 13 files changed, 78 insertions(+), 79 deletions(-)

diff --git a/gtsam/geometry/Cal3.cpp b/gtsam/geometry/Cal3.cpp
index 6210a6065..41de47f46 100644
--- a/gtsam/geometry/Cal3.cpp
+++ b/gtsam/geometry/Cal3.cpp
@@ -27,7 +27,6 @@ namespace gtsam {
 Cal3::Cal3(double fov, int w, int h)
     : s_(0), u0_((double)w / 2.0), v0_((double)h / 2.0) {
   double a = fov * M_PI / 360.0;  // fov/2 in radians
-  // old formula: fx_ = (double) w * tan(a);
   fx_ = double(w) / (2.0 * tan(a));
   fy_ = fx_;
 }
@@ -63,6 +62,14 @@ bool Cal3::equals(const Cal3& K, double tol) const {
           std::fabs(v0_ - K.v0_) < tol);
 }
 
+Matrix3 Cal3::inverse() const {
+  const double fxy = fx_ * fy_, sv0 = s_ * v0_, fyu0 = fy_ * u0_;
+  Matrix3 K_inverse;
+  K_inverse << 1.0 / fx_, -s_ / fxy, (sv0 - fyu0) / fxy, 0.0, 1.0 / fy_,
+      -v0_ / fy_, 0.0, 0.0, 1.0;
+  return K_inverse;
+}
+
 /* ************************************************************************* */
 
 }  // \ namespace gtsam
diff --git a/gtsam/geometry/Cal3.h b/gtsam/geometry/Cal3.h
index 712414c82..74c9868f3 100644
--- a/gtsam/geometry/Cal3.h
+++ b/gtsam/geometry/Cal3.h
@@ -161,7 +161,7 @@ class GTSAM_EXPORT Cal3 {
   }
 
   /// return calibration matrix K
-  Matrix3 K() const {
+  virtual Matrix3 K() const {
     Matrix3 K;
     K << fx_, s_, u0_, 0.0, fy_, v0_, 0.0, 0.0, 1.0;
     return K;
@@ -173,13 +173,13 @@ class GTSAM_EXPORT Cal3 {
 #endif
 
   /// Return inverted calibration matrix inv(K)
-  Matrix3 inverse() const {
-    const double fxy = fx_ * fy_, sv0 = s_ * v0_, fyu0 = fy_ * u0_;
-    Matrix3 K_inverse;
-    K_inverse << 1.0 / fx_, -s_ / fxy, (sv0 - fyu0) / fxy, 0.0, 1.0 / fy_,
-        -v0_ / fy_, 0.0, 0.0, 1.0;
-    return K_inverse;
-  }
+  Matrix3 inverse() const;
+
+  /// return DOF, dimensionality of tangent space
+  inline virtual size_t dim() const { return Dim(); }
+
+  /// return DOF, dimensionality of tangent space
+  inline static size_t Dim() { return dimension; }
 
   /// @}
   /// @name Advanced Interface
diff --git a/gtsam/geometry/Cal3Bundler.cpp b/gtsam/geometry/Cal3Bundler.cpp
index af2f881d8..31beac73e 100644
--- a/gtsam/geometry/Cal3Bundler.cpp
+++ b/gtsam/geometry/Cal3Bundler.cpp
@@ -25,8 +25,9 @@ namespace gtsam {
 
 /* ************************************************************************* */
 Matrix3 Cal3Bundler::K() const {
+  // This function is needed to ensure skew = 0;
   Matrix3 K;
-  K << f_, 0, u0_, 0, f_, v0_, 0, 0, 1;
+  K << fx_, 0, u0_, 0, fy_, v0_, 0, 0, 1.0;
   return K;
 }
 
@@ -39,7 +40,7 @@ Vector4 Cal3Bundler::k() const {
 
 /* ************************************************************************* */
 Vector3 Cal3Bundler::vector() const {
-  return Vector3(f_, k1_, k2_);
+  return Vector3(fx_, k1_, k2_);
 }
 
 /* ************************************************************************* */
@@ -51,12 +52,13 @@ std::ostream& operator<<(std::ostream& os, const Cal3Bundler& cal) {
 
 /* ************************************************************************* */
 void Cal3Bundler::print(const std::string& s) const {
-  gtsam::print((Vector)(Vector(5) << f_, k1_, k2_, u0_, v0_).finished(), s + ".K");
+  gtsam::print((Vector)(Vector(5) << fx_, k1_, k2_, u0_, v0_).finished(), s + ".K");
 }
 
 /* ************************************************************************* */
 bool Cal3Bundler::equals(const Cal3Bundler& K, double tol) const {
-  return (std::fabs(f_ - K.f_) < tol && std::fabs(k1_ - K.k1_) < tol &&
+  const Cal3* base = dynamic_cast<const Cal3*>(&K);
+  return (Cal3::equals(*base, tol) && std::fabs(k1_ - K.k1_) < tol &&
           std::fabs(k2_ - K.k2_) < tol && std::fabs(u0_ - K.u0_) < tol &&
           std::fabs(v0_ - K.v0_) < tol);
 }
@@ -64,14 +66,16 @@ bool Cal3Bundler::equals(const Cal3Bundler& K, double tol) const {
 /* ************************************************************************* */
 Point2 Cal3Bundler::uncalibrate(const Point2& p, //
     OptionalJacobian<2, 3> Dcal, OptionalJacobian<2, 2> Dp) const {
-  //  r = x^2 + y^2;
-  //  g = (1 + k(1)*r + k(2)*r^2);
+  //  r = x² + y²;
+  //  g = (1 + k(1)*r + k(2)*r²);
   //  pi(:,i) = g * pn(:,i)
   const double x = p.x(), y = p.y();
   const double r = x * x + y * y;
   const double g = 1. + (k1_ + k2_ * r) * r;
   const double u = g * x, v = g * y;
 
+  const double f_ = fx_;
+
   // Derivatives make use of intermediate variables above
   if (Dcal) {
     double rx = r * x, ry = r * y;
@@ -92,9 +96,9 @@ Point2 Cal3Bundler::uncalibrate(const Point2& p, //
 Point2 Cal3Bundler::calibrate(const Point2& pi,
                               OptionalJacobian<2, 3> Dcal,
                               OptionalJacobian<2, 2> Dp) const {
-  // Copied from Cal3DS2 :-(
-  // but specialized with k1,k2 non-zero only and fx=fy and s=0
-  double x = (pi.x() - u0_)/f_, y = (pi.y() - v0_)/f_;
+  // Copied from Cal3DS2
+  // but specialized with k1, k2 non-zero only and fx=fy and s=0
+  double x = (pi.x() - u0_) / fx_, y = (pi.y() - v0_) / fx_;
   const Point2 invKPi(x, y);
 
   // initialize by ignoring the distortion at all, might be problematic for pixels around boundary
diff --git a/gtsam/geometry/Cal3Bundler.h b/gtsam/geometry/Cal3Bundler.h
index 76703f96f..50b392096 100644
--- a/gtsam/geometry/Cal3Bundler.h
+++ b/gtsam/geometry/Cal3Bundler.h
@@ -14,6 +14,7 @@
  * @brief Calibration used by Bundler
  * @date Sep 25, 2010
  * @author Yong Dian Jian
+ * @author Varun Agrawal
  */
 
 #pragma once
@@ -31,11 +32,11 @@ namespace gtsam {
 class GTSAM_EXPORT Cal3Bundler : public Cal3 {
 
  private:
-  double f_ = 1.0f;               ///< focal length
   double k1_ = 0.0f, k2_ = 0.0f;  ///< radial distortion
   double tol_ = 1e-5;             ///< tolerance value when calibrating
 
-  // NOTE: image center parameters (u0, v0) are not optimized
+  // NOTE: We use the base class fx to represent the common focal length.
+  // Also, image center parameters (u0, v0) are not optimized
   // but are treated as constants.
 
  public:
@@ -59,7 +60,7 @@ class GTSAM_EXPORT Cal3Bundler : public Cal3 {
    */
   Cal3Bundler(double f, double k1, double k2, double u0 = 0, double v0 = 0,
               double tol = 1e-5)
-      : Cal3(f, f, 0, u0, v0), f_(f), k1_(k1), k2_(k2), tol_(tol) {}
+      : Cal3(f, f, 0, u0, v0), k1_(k1), k2_(k2), tol_(tol) {}
 
   virtual ~Cal3Bundler() {}
 
@@ -81,16 +82,6 @@ class GTSAM_EXPORT Cal3Bundler : public Cal3 {
   /// @name Standard Interface
   /// @{
 
-  /// focal length x
-  inline double fx() const {
-    return f_;
-  }
-
-  /// focal length y
-  inline double fy() const {
-    return f_;
-  }
-
   /// distorsion parameter k1
   inline double k1() const {
     return k1_;
@@ -111,7 +102,7 @@ class GTSAM_EXPORT Cal3Bundler : public Cal3 {
     return v0_;
   }
 
-  Matrix3 K() const; ///< Standard 3*3 calibration matrix
+  Matrix3 K() const override; ///< Standard 3*3 calibration matrix
   Vector4 k() const; ///< Radial distortion parameters (4 of them, 2 0)
 
   Vector3 vector() const;
@@ -165,14 +156,14 @@ class GTSAM_EXPORT Cal3Bundler : public Cal3 {
   /// @{
 
   /// return DOF, dimensionality of tangent space
-  virtual size_t dim() const { return dimension; }
+  virtual size_t dim() const override { return Dim(); }
 
   /// return DOF, dimensionality of tangent space
-  static size_t Dim() { return dimension; }
+  inline static size_t Dim() { return dimension; }
 
   /// Update calibration with tangent space delta
   inline Cal3Bundler retract(const Vector& d) const {
-    return Cal3Bundler(f_ + d(0), k1_ + d(1), k2_ + d(2), u0_, v0_);
+    return Cal3Bundler(fx_ + d(0), k1_ + d(1), k2_ + d(2), u0_, v0_);
   }
 
   /// Calculate local coordinates to another calibration
@@ -192,7 +183,6 @@ class GTSAM_EXPORT Cal3Bundler : public Cal3 {
   void serialize(Archive & ar, const unsigned int /*version*/) {
     ar& boost::serialization::make_nvp(
         "Cal3Bundler", boost::serialization::base_object<Cal3>(*this));
-    ar& BOOST_SERIALIZATION_NVP(f_);
     ar& BOOST_SERIALIZATION_NVP(k1_);
     ar& BOOST_SERIALIZATION_NVP(k2_);
     ar& BOOST_SERIALIZATION_NVP(tol_);
diff --git a/gtsam/geometry/Cal3DS2.h b/gtsam/geometry/Cal3DS2.h
index d0bed8652..ad4406b76 100644
--- a/gtsam/geometry/Cal3DS2.h
+++ b/gtsam/geometry/Cal3DS2.h
@@ -81,10 +81,10 @@ public:
   Vector localCoordinates(const Cal3DS2& T2) const ;
 
   /// Return dimensions of calibration manifold object
-  virtual size_t dim() const { return dimension ; }
+  virtual size_t dim() const override { return Dim(); }
 
   /// Return dimensions of calibration manifold object
-  static size_t Dim() { return dimension; }
+  inline static size_t Dim() { return dimension; }
 
   /// @}
   /// @name Clone
diff --git a/gtsam/geometry/Cal3DS2_Base.cpp b/gtsam/geometry/Cal3DS2_Base.cpp
index 2741d0736..71ce32ccb 100644
--- a/gtsam/geometry/Cal3DS2_Base.cpp
+++ b/gtsam/geometry/Cal3DS2_Base.cpp
@@ -93,9 +93,9 @@ static Matrix2 D2dintrinsic(double x, double y, double rr,
 /* ************************************************************************* */
 Point2 Cal3DS2_Base::uncalibrate(const Point2& p, OptionalJacobian<2, 9> Dcal,
                                  OptionalJacobian<2, 2> Dp) const {
-  //  rr = x^2 + y^2;
-  //  g = (1 + k(1)*rr + k(2)*rr^2);
-  //  dp = [2*k(3)*x*y + k(4)*(rr + 2*x^2); 2*k(4)*x*y + k(3)*(rr + 2*y^2)];
+  //  r² = x² + y²;
+  //  g = (1 + k(1)*r² + k(2)*r⁴);
+  //  dp = [2*k(3)*x*y + k(4)*(r² + 2*x²); 2*k(4)*x*y + k(3)*(r² + 2*y²)];
   //  pi(:,i) = g * pn(:,i) + dp;
   const double x = p.x(), y = p.y(), xy = x * y, xx = x * x, yy = y * y;
   const double rr = xx + yy;
diff --git a/gtsam/geometry/Cal3DS2_Base.h b/gtsam/geometry/Cal3DS2_Base.h
index 713fa4e0e..23e138838 100644
--- a/gtsam/geometry/Cal3DS2_Base.h
+++ b/gtsam/geometry/Cal3DS2_Base.h
@@ -33,11 +33,10 @@ namespace gtsam {
  * http://docs.opencv.org/modules/calib3d/doc/camera_calibration_and_3d_reconstruction.html
  * but using only k1,k2,p1, and p2 coefficients.
  * K = [ fx s u0 ; 0 fy v0 ; 0 0 1 ]
- * rr = Pn.x^2 + Pn.y^2
- * \hat{Pn} = (1 + k1*rr + k2*rr^2 ) Pn + [ 2*p1 Pn.x Pn.y + p2 (rr + 2 Pn.x^2)
- * ;
- *                      p1 (rr + 2 Pn.y^2) + 2*p2 Pn.x Pn.y  ]
- * pi = K*Pn
+ * r² = P.x² + P.y²
+ * P̂ = (1 + k1*r² + k2*r⁴) P + [ (2*p1 P.x P.y) + p2 (r² + 2 Pn.x²)
+ *                            p1 (r² + 2 Pn.y²) + (2*p2 Pn.x Pn.y) ]
+ * pi = K*P̂
  */
 class GTSAM_EXPORT Cal3DS2_Base : public Cal3 {
  protected:
@@ -132,6 +131,12 @@ class GTSAM_EXPORT Cal3DS2_Base : public Cal3 {
   /// Derivative of uncalibrate wrpt the calibration parameters
   Matrix29 D2d_calibration(const Point2& p) const;
 
+  /// return DOF, dimensionality of tangent space
+  virtual size_t dim() const override { return Dim(); }
+
+  /// return DOF, dimensionality of tangent space
+  inline static size_t Dim() { return dimension; }
+
   /// @}
   /// @name Clone
   /// @{
diff --git a/gtsam/geometry/Cal3Fisheye.h b/gtsam/geometry/Cal3Fisheye.h
index 738b2275d..a394d2000 100644
--- a/gtsam/geometry/Cal3Fisheye.h
+++ b/gtsam/geometry/Cal3Fisheye.h
@@ -38,9 +38,9 @@ namespace gtsam {
  * Intrinsic coordinates:
  *   [x_i;y_i] = [x/z; y/z]
  * Distorted coordinates:
- *   r^2 = (x_i)^2 + (y_i)^2
+ *   r² = (x_i)² + (y_i)²
  *   th = atan(r)
- *   th_d = th(1 + k1*th^2 + k2*th^4 + k3*th^6 + k4*th^8)
+ *   th_d = th(1 + k1*th² + k2*th⁴ + k3*th⁶ + k4*th⁸)
  *   [x_d; y_d] = (th_d / r)*[x_i; y_i]
  * Pixel coordinates:
  *   K = [fx s u0; 0 fy v0 ;0 0 1]
@@ -152,10 +152,10 @@ class GTSAM_EXPORT Cal3Fisheye : public Cal3 {
   /// @{
 
   /// Return dimensions of calibration manifold object
-  virtual size_t dim() const { return dimension; }
+  virtual size_t dim() const override { return Dim(); }
 
   /// Return dimensions of calibration manifold object
-  static size_t Dim() { return dimension; }
+  inline static size_t Dim() { return dimension; }
 
   /// Given delta vector, update calibration
   inline Cal3Fisheye retract(const Vector& d) const {
diff --git a/gtsam/geometry/Cal3Unified.h b/gtsam/geometry/Cal3Unified.h
index a2c5ebc5c..4c456ec24 100644
--- a/gtsam/geometry/Cal3Unified.h
+++ b/gtsam/geometry/Cal3Unified.h
@@ -34,10 +34,10 @@ namespace gtsam {
  *
  * Similar to Cal3DS2, does distortion but has additional mirror parameter xi
  * K = [ fx s u0 ; 0 fy v0 ; 0 0 1 ]
- * Pn = [ P.x / (1 + xi * \sqrt{P.x^2 + P.y^2 + 1}), P.y / (1 + xi * \sqrt{P.x^2 + P.y^2 + 1})]
- * rr = Pn.x^2 + Pn.y^2
- * \hat{pn} = (1 + k1*rr + k2*rr^2 ) pn + [ 2*k3 pn.x pn.y + k4 (rr + 2 Pn.x^2) ;
- *                      k3 (rr + 2 Pn.y^2) + 2*k4 pn.x pn.y  ]
+ * Pn = [ P.x / (1 + xi * \sqrt{P.x² + P.y² + 1}), P.y / (1 + xi * \sqrt{P.x² + P.y² + 1})]
+ * r² = Pn.x² + Pn.y²
+ * \hat{pn} = (1 + k1*r² + k2*r⁴ ) pn + [ 2*k3 pn.x pn.y + k4 (r² + 2 Pn.x²) ;
+ *                      k3 (rr + 2 Pn.y²) + 2*k4 pn.x pn.y  ]
  * pi = K*pn
  */
 class GTSAM_EXPORT Cal3Unified : public Cal3DS2_Base {
@@ -127,10 +127,10 @@ class GTSAM_EXPORT Cal3Unified : public Cal3DS2_Base {
   Vector localCoordinates(const Cal3Unified& T2) const ;
 
   /// Return dimensions of calibration manifold object
-  virtual size_t dim() const { return dimension ; }
+  virtual size_t dim() const override { return Dim(); }
 
   /// Return dimensions of calibration manifold object
-  static size_t Dim() { return dimension; }
+  inline static size_t Dim() { return dimension; }
 
   /// @}
 
diff --git a/gtsam/geometry/Cal3_S2.cpp b/gtsam/geometry/Cal3_S2.cpp
index 98c7bea38..f97082ddf 100644
--- a/gtsam/geometry/Cal3_S2.cpp
+++ b/gtsam/geometry/Cal3_S2.cpp
@@ -55,8 +55,9 @@ Point2 Cal3_S2::calibrate(const Point2& p, OptionalJacobian<2, 5> Dcal,
   const double u = p.x(), v = p.y();
   double delta_u = u - u0_, delta_v = v - v0_;
   double inv_fx = 1 / fx_, inv_fy = 1 / fy_;
-  double inv_fy_delta_v = inv_fy * delta_v,
-         inv_fx_s_inv_fy = inv_fx * s_ * inv_fy;
+  double inv_fy_delta_v = inv_fy * delta_v;
+  double inv_fx_s_inv_fy = inv_fx * s_ * inv_fy;
+
   Point2 point(inv_fx * (delta_u - s_ * inv_fy_delta_v), inv_fy_delta_v);
   if (Dcal)
     *Dcal << -inv_fx * point.x(), inv_fx * s_ * inv_fy * inv_fy_delta_v,
diff --git a/gtsam/geometry/Cal3_S2.h b/gtsam/geometry/Cal3_S2.h
index 37edc46c4..93b98a7e1 100644
--- a/gtsam/geometry/Cal3_S2.h
+++ b/gtsam/geometry/Cal3_S2.h
@@ -115,10 +115,7 @@ class GTSAM_EXPORT Cal3_S2 : public Cal3 {
   /// @{
 
   /// return DOF, dimensionality of tangent space
-  inline size_t dim() const { return dimension; }
-
-  /// return DOF, dimensionality of tangent space
-  static size_t Dim() { return dimension; }
+  inline static size_t Dim() { return dimension; }
 
   /// Given 5-dim tangent vector, create new calibration
   inline Cal3_S2 retract(const Vector& d) const {
diff --git a/gtsam/geometry/Cal3_S2Stereo.h b/gtsam/geometry/Cal3_S2Stereo.h
index 94341ec37..ee316251a 100644
--- a/gtsam/geometry/Cal3_S2Stereo.h
+++ b/gtsam/geometry/Cal3_S2Stereo.h
@@ -79,7 +79,7 @@ namespace gtsam {
     const Cal3_S2& calibration() const { return *this; }
 
     /// return calibration matrix K, same for left and right
-    Matrix3 K() const { return K(); }
+    Matrix3 K() const override { return Cal3_S2::K(); }
 
     /// return baseline
     inline double baseline() const { return b_; }
@@ -96,10 +96,10 @@ namespace gtsam {
     /// @{
 
     /// return DOF, dimensionality of tangent space
-    inline size_t dim() const { return dimension; }
+    inline size_t dim() const override { return Dim(); }
 
     /// return DOF, dimensionality of tangent space
-    static size_t Dim() { return dimension; }
+    inline static size_t Dim() { return dimension; }
 
     /// Given 6-dim tangent vector, create new calibration
     inline Cal3_S2Stereo retract(const Vector& d) const {
diff --git a/gtsam/geometry/tests/testCal3Bundler.cpp b/gtsam/geometry/tests/testCal3Bundler.cpp
index 75aa50a25..eac8d1044 100644
--- a/gtsam/geometry/tests/testCal3Bundler.cpp
+++ b/gtsam/geometry/tests/testCal3Bundler.cpp
@@ -29,8 +29,7 @@ static Cal3Bundler K(500, 1e-3, 1e-3, 1000, 2000);
 static Point2 p(2,3);
 
 /* ************************************************************************* */
-TEST(Cal3Bundler, vector)
-{
+TEST(Cal3Bundler, vector) {
   Cal3Bundler K;
   Vector expected(3);
   expected << 1, 0, 0;
@@ -38,8 +37,7 @@ TEST(Cal3Bundler, vector)
 }
 
 /* ************************************************************************* */
-TEST(Cal3Bundler, uncalibrate)
-{
+TEST(Cal3Bundler, uncalibrate) {
   Vector v = K.vector() ;
   double r = p.x()*p.x() + p.y()*p.y() ;
   double g = v[0]*(1+v[1]*r+v[2]*r*r) ;
@@ -48,8 +46,7 @@ TEST(Cal3Bundler, uncalibrate)
   CHECK(assert_equal(expected,actual));
 }
 
-TEST(Cal3Bundler, calibrate )
-{
+TEST(Cal3Bundler, calibrate ) {
   Point2 pn(0.5, 0.5);
   Point2 pi = K.uncalibrate(pn);
   Point2 pn_hat = K.calibrate(pi);
@@ -62,8 +59,7 @@ Point2 uncalibrate_(const Cal3Bundler& k, const Point2& pt) { return k.uncalibra
 Point2 calibrate_(const Cal3Bundler& k, const Point2& pt) { return k.calibrate(pt); }
 
 /* ************************************************************************* */
-TEST(Cal3Bundler, Duncalibrate)
-{
+TEST(Cal3Bundler, Duncalibrate) {
   Matrix Dcal, Dp;
   Point2 actual = K.uncalibrate(p, Dcal, Dp);
   Point2 expected(2182, 3773);
@@ -75,8 +71,7 @@ TEST(Cal3Bundler, Duncalibrate)
 }
 
 /* ************************************************************************* */
-TEST(Cal3Bundler, Dcalibrate)
-{
+TEST(Cal3Bundler, Dcalibrate) {
   Matrix Dcal, Dp;
   Point2 pn(0.5, 0.5);
   Point2 pi = K.uncalibrate(pn);
@@ -89,15 +84,15 @@ TEST(Cal3Bundler, Dcalibrate)
 }
 
 /* ************************************************************************* */
-TEST(Cal3Bundler, assert_equal)
-{
+TEST(Cal3Bundler, assert_equal) {
   CHECK(assert_equal(K,K,1e-7));
 }
 
 /* ************************************************************************* */
-TEST(Cal3Bundler, retract)
-{
+TEST(Cal3Bundler, retract) {
   Cal3Bundler expected(510, 2e-3, 2e-3, 1000, 2000);
+  EXPECT_LONGS_EQUAL(3, expected.dim());
+
   Vector3 d;
   d << 10, 1e-3, 1e-3;
   Cal3Bundler actual = K.retract(d);

From 0a55d31702e546ead6e82a4ea859992db0c7e9ce Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Wed, 2 Dec 2020 17:14:29 -0500
Subject: [PATCH 16/18] Added tests for checking calibration model dimensions

---
 gtsam/geometry/tests/testCal3Bundler.cpp   |  3 ++
 gtsam/geometry/tests/testCal3DFisheye.cpp  |  6 +++-
 gtsam/geometry/tests/testCal3DS2.cpp       |  9 +++--
 gtsam/geometry/tests/testCal3Unified.cpp   |  9 +++--
 gtsam/geometry/tests/testCal3_S2.cpp       | 42 +++++++++++-----------
 gtsam/geometry/tests/testCal3_S2Stereo.cpp |  3 ++
 6 files changed, 45 insertions(+), 27 deletions(-)

diff --git a/gtsam/geometry/tests/testCal3Bundler.cpp b/gtsam/geometry/tests/testCal3Bundler.cpp
index eac8d1044..8e6fe983f 100644
--- a/gtsam/geometry/tests/testCal3Bundler.cpp
+++ b/gtsam/geometry/tests/testCal3Bundler.cpp
@@ -93,6 +93,9 @@ TEST(Cal3Bundler, retract) {
   Cal3Bundler expected(510, 2e-3, 2e-3, 1000, 2000);
   EXPECT_LONGS_EQUAL(3, expected.dim());
 
+  EXPECT_LONGS_EQUAL(Cal3Bundler::Dim(), 3);
+  EXPECT_LONGS_EQUAL(expected.dim(), 3);
+
   Vector3 d;
   d << 10, 1e-3, 1e-3;
   Cal3Bundler actual = K.retract(d);
diff --git a/gtsam/geometry/tests/testCal3DFisheye.cpp b/gtsam/geometry/tests/testCal3DFisheye.cpp
index 85e661728..7a73e7490 100644
--- a/gtsam/geometry/tests/testCal3DFisheye.cpp
+++ b/gtsam/geometry/tests/testCal3DFisheye.cpp
@@ -42,7 +42,11 @@ TEST(Cal3Fisheye, retract) {
   Cal3Fisheye expected(K.fx() + 1, K.fy() + 2, K.skew() + 3, K.px() + 4,
                        K.py() + 5, K.k1() + 6, K.k2() + 7, K.k3() + 8,
                        K.k4() + 9);
-  Vector d(9);
+
+  EXPECT_LONGS_EQUAL(Cal3Fisheye::Dim(), 9);
+  EXPECT_LONGS_EQUAL(expected.dim(), 9);
+
+  Vector9 d;
   d << 1, 2, 3, 4, 5, 6, 7, 8, 9;
   Cal3Fisheye actual = K.retract(d);
   CHECK(assert_equal(expected, actual, 1e-7));
diff --git a/gtsam/geometry/tests/testCal3DS2.cpp b/gtsam/geometry/tests/testCal3DS2.cpp
index b382e85f3..e4dc3e806 100644
--- a/gtsam/geometry/tests/testCal3DS2.cpp
+++ b/gtsam/geometry/tests/testCal3DS2.cpp
@@ -96,11 +96,14 @@ TEST(Cal3DS2, Dcalibrate)
 TEST(Cal3DS2, assert_equal) { CHECK(assert_equal(K, K, 1e-5)); }
 
 /* ************************************************************************* */
-TEST(Cal3DS2, retract)
-{
+TEST(Cal3DS2, retract) {
   Cal3DS2 expected(500 + 1, 100 + 2, 0.1 + 3, 320 + 4, 240 + 5, 1e-3 + 6,
       2.0 * 1e-3 + 7, 3.0 * 1e-3 + 8, 4.0 * 1e-3 + 9);
-  Vector d(9);
+
+  EXPECT_LONGS_EQUAL(Cal3DS2::Dim(), 9);
+  EXPECT_LONGS_EQUAL(expected.dim(), 9);
+
+  Vector9 d;
   d << 1,2,3,4,5,6,7,8,9;
   Cal3DS2 actual = K.retract(d);
   CHECK(assert_equal(expected,actual,1e-7));
diff --git a/gtsam/geometry/tests/testCal3Unified.cpp b/gtsam/geometry/tests/testCal3Unified.cpp
index 41a1d3ad9..ff759d1cd 100644
--- a/gtsam/geometry/tests/testCal3Unified.cpp
+++ b/gtsam/geometry/tests/testCal3Unified.cpp
@@ -106,11 +106,14 @@ TEST(Cal3Unified, assert_equal)
 }
 
 /* ************************************************************************* */
-TEST(Cal3Unified, retract)
-{
+TEST(Cal3Unified, retract) {
   Cal3Unified expected(100 + 2, 105 + 3, 0.0 + 4, 320 + 5, 240 + 6,
       1e-3 + 7, 2.0*1e-3 + 8, 3.0*1e-3 + 9, 4.0*1e-3 + 10, 0.1 + 1);
-  Vector d(10);
+
+  EXPECT_LONGS_EQUAL(Cal3Unified::Dim(), 10);
+  EXPECT_LONGS_EQUAL(expected.dim(), 10);
+
+  Vector10 d;
   d << 2, 3, 4, 5, 6, 7, 8, 9, 10, 1;
   Cal3Unified actual = K.retract(d);
   CHECK(assert_equal(expected,actual,1e-9));
diff --git a/gtsam/geometry/tests/testCal3_S2.cpp b/gtsam/geometry/tests/testCal3_S2.cpp
index 3317addff..9b7941c91 100644
--- a/gtsam/geometry/tests/testCal3_S2.cpp
+++ b/gtsam/geometry/tests/testCal3_S2.cpp
@@ -31,8 +31,7 @@ static Point2 p_uv(1320.3, 1740);
 static Point2 p_xy(2, 3);
 
 /* ************************************************************************* */
-TEST(Cal3_S2, easy_constructor)
-{
+TEST(Cal3_S2, Constructor) {
   Cal3_S2 expected(554.256, 554.256, 0, 640 / 2, 480 / 2);
 
   double fov = 60; // degrees
@@ -43,8 +42,7 @@ TEST(Cal3_S2, easy_constructor)
 }
 
 /* ************************************************************************* */
-TEST(Cal3_S2, calibrate)
-{
+TEST(Cal3_S2, Calibrate) {
   Point2 intrinsic(2,3);
   Point2 expectedimage(1320.3, 1740);
   Point2 imagecoordinates = K.uncalibrate(intrinsic);
@@ -53,33 +51,36 @@ TEST(Cal3_S2, calibrate)
 }
 
 /* ************************************************************************* */
-TEST(Cal3_S2, calibrate_homogeneous) {
+TEST(Cal3_S2, CalibrateHomogeneous) {
   Vector3 intrinsic(2, 3, 1);
   Vector3 image(1320.3, 1740, 1);
   CHECK(assert_equal((Vector)intrinsic,(Vector)K.calibrate(image)));
 }
 
 /* ************************************************************************* */
-Point2 uncalibrate_(const Cal3_S2& k, const Point2& pt) { return k.uncalibrate(pt); }
-TEST(Cal3_S2, Duncalibrate1)
-{
+Point2 uncalibrate_(const Cal3_S2& k, const Point2& pt) {
+  return k.uncalibrate(pt);
+}
+
+TEST(Cal3_S2, Duncalibrate1) {
   Matrix25 computed; K.uncalibrate(p, computed, boost::none);
   Matrix numerical = numericalDerivative21(uncalibrate_, K, p);
   CHECK(assert_equal(numerical,computed,1e-8));
 }
 
 /* ************************************************************************* */
-TEST(Cal3_S2, Duncalibrate2)
-{
+TEST(Cal3_S2, Duncalibrate2) {
   Matrix computed; K.uncalibrate(p, boost::none, computed);
   Matrix numerical = numericalDerivative22(uncalibrate_, K, p);
   CHECK(assert_equal(numerical,computed,1e-9));
 }
 
-Point2 calibrate_(const Cal3_S2& k,  const Point2& pt) {return k.calibrate(pt); }
+Point2 calibrate_(const Cal3_S2& k, const Point2& pt) {
+  return k.calibrate(pt);
+}
+
 /* ************************************************************************* */
-TEST(Cal3_S2, Dcalibrate1)
-{
+TEST(Cal3_S2, Dcalibrate1) {
     Matrix computed;
     Point2 expected = K.calibrate(p_uv, computed, boost::none);
     Matrix numerical = numericalDerivative21(calibrate_, K, p_uv);
@@ -88,8 +89,7 @@ TEST(Cal3_S2, Dcalibrate1)
 }
 
 /* ************************************************************************* */
-TEST(Cal3_S2, Dcalibrate2)
-{
+TEST(Cal3_S2, Dcalibrate2) {
     Matrix computed;
     Point2 expected = K.calibrate(p_uv, boost::none, computed);
     Matrix numerical = numericalDerivative22(calibrate_, K, p_uv);
@@ -98,8 +98,7 @@ TEST(Cal3_S2, Dcalibrate2)
 }
 
 /* ************************************************************************* */
-TEST(Cal3_S2, assert_equal)
-{
+TEST(Cal3_S2, Equal) {
   CHECK(assert_equal(K,K,1e-9));
 
   Cal3_S2 K1(500, 500, 0.1, 640 / 2, 480 / 2);
@@ -107,10 +106,13 @@ TEST(Cal3_S2, assert_equal)
 }
 
 /* ************************************************************************* */
-TEST(Cal3_S2, retract)
-{
+TEST(Cal3_S2, Retract) {
   Cal3_S2 expected(500+1, 500+2, 0.1+3, 640 / 2+4, 480 / 2+5);
-  Vector d(5);
+
+  EXPECT_LONGS_EQUAL(Cal3_S2::Dim(), 5);
+  EXPECT_LONGS_EQUAL(expected.dim(), 5);
+
+  Vector5 d;
   d << 1,2,3,4,5;
   Cal3_S2 actual = K.retract(d);
   CHECK(assert_equal(expected,actual,1e-7));
diff --git a/gtsam/geometry/tests/testCal3_S2Stereo.cpp b/gtsam/geometry/tests/testCal3_S2Stereo.cpp
index 9c93b7496..f823a8b97 100644
--- a/gtsam/geometry/tests/testCal3_S2Stereo.cpp
+++ b/gtsam/geometry/tests/testCal3_S2Stereo.cpp
@@ -111,6 +111,9 @@ TEST(Cal3_S2Stereo, assert_equal) {
 TEST(Cal3_S2Stereo, retract) {
   Cal3_S2Stereo expected(500 + 1, 500 + 2, 0.1 + 3, 640 / 2 + 4, 480 / 2 + 5,
                          7);
+  EXPECT_LONGS_EQUAL(Cal3_S2Stereo::Dim(), 6);
+  EXPECT_LONGS_EQUAL(expected.dim(), 6);
+
   Vector6 d;
   d << 1, 2, 3, 4, 5, 6;
   Cal3_S2Stereo actual = K.retract(d);

From 70bab8e00c0e310c9bd2c9fa1486e6723861f093 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Wed, 2 Dec 2020 17:15:10 -0500
Subject: [PATCH 17/18] Consistent and better formatting

---
 gtsam/geometry/Cal3Bundler.cpp             |  27 ++--
 gtsam/geometry/Cal3Bundler.h               |  47 ++----
 gtsam/geometry/Cal3DS2.cpp                 |  11 +-
 gtsam/geometry/Cal3DS2.h                   |  33 ++--
 gtsam/geometry/Cal3DS2_Base.cpp            |  29 ++--
 gtsam/geometry/Cal3Unified.h               |  41 +++--
 gtsam/geometry/Cal3_S2Stereo.cpp           |   3 +-
 gtsam/geometry/Cal3_S2Stereo.h             | 179 ++++++++++-----------
 gtsam/geometry/tests/testCal3Bundler.cpp   |  47 +++---
 gtsam/geometry/tests/testCal3DFisheye.cpp  |   7 +-
 gtsam/geometry/tests/testCal3DS2.cpp       |  73 ++++-----
 gtsam/geometry/tests/testCal3Unified.cpp   |  67 ++++----
 gtsam/geometry/tests/testCal3_S2.cpp       |  61 +++----
 gtsam/geometry/tests/testCal3_S2Stereo.cpp |  52 +-----
 14 files changed, 298 insertions(+), 379 deletions(-)

diff --git a/gtsam/geometry/Cal3Bundler.cpp b/gtsam/geometry/Cal3Bundler.cpp
index 31beac73e..e03562452 100644
--- a/gtsam/geometry/Cal3Bundler.cpp
+++ b/gtsam/geometry/Cal3Bundler.cpp
@@ -15,11 +15,11 @@
  * @author ydjian
  */
 
-#include <gtsam/base/Vector.h>
 #include <gtsam/base/Matrix.h>
+#include <gtsam/base/Vector.h>
+#include <gtsam/geometry/Cal3Bundler.h>
 #include <gtsam/geometry/Point2.h>
 #include <gtsam/geometry/Point3.h>
-#include <gtsam/geometry/Cal3Bundler.h>
 
 namespace gtsam {
 
@@ -39,9 +39,7 @@ Vector4 Cal3Bundler::k() const {
 }
 
 /* ************************************************************************* */
-Vector3 Cal3Bundler::vector() const {
-  return Vector3(fx_, k1_, k2_);
-}
+Vector3 Cal3Bundler::vector() const { return Vector3(fx_, k1_, k2_); }
 
 /* ************************************************************************* */
 std::ostream& operator<<(std::ostream& os, const Cal3Bundler& cal) {
@@ -52,7 +50,8 @@ std::ostream& operator<<(std::ostream& os, const Cal3Bundler& cal) {
 
 /* ************************************************************************* */
 void Cal3Bundler::print(const std::string& s) const {
-  gtsam::print((Vector)(Vector(5) << fx_, k1_, k2_, u0_, v0_).finished(), s + ".K");
+  gtsam::print((Vector)(Vector(5) << fx_, k1_, k2_, u0_, v0_).finished(),
+               s + ".K");
 }
 
 /* ************************************************************************* */
@@ -64,8 +63,8 @@ bool Cal3Bundler::equals(const Cal3Bundler& K, double tol) const {
 }
 
 /* ************************************************************************* */
-Point2 Cal3Bundler::uncalibrate(const Point2& p, //
-    OptionalJacobian<2, 3> Dcal, OptionalJacobian<2, 2> Dp) const {
+Point2 Cal3Bundler::uncalibrate(const Point2& p, OptionalJacobian<2, 3> Dcal,
+                                OptionalJacobian<2, 2> Dp) const {
   //  r = x² + y²;
   //  g = (1 + k(1)*r + k(2)*r²);
   //  pi(:,i) = g * pn(:,i)
@@ -93,23 +92,22 @@ Point2 Cal3Bundler::uncalibrate(const Point2& p, //
 }
 
 /* ************************************************************************* */
-Point2 Cal3Bundler::calibrate(const Point2& pi,
-                              OptionalJacobian<2, 3> Dcal,
+Point2 Cal3Bundler::calibrate(const Point2& pi, OptionalJacobian<2, 3> Dcal,
                               OptionalJacobian<2, 2> Dp) const {
   // Copied from Cal3DS2
   // but specialized with k1, k2 non-zero only and fx=fy and s=0
   double x = (pi.x() - u0_) / fx_, y = (pi.y() - v0_) / fx_;
   const Point2 invKPi(x, y);
 
-  // initialize by ignoring the distortion at all, might be problematic for pixels around boundary
+  // initialize by ignoring the distortion at all, might be problematic for
+  // pixels around boundary
   Point2 pn(x, y);
 
   // iterate until the uncalibrate is close to the actual pixel coordinate
   const int maxIterations = 10;
   int iteration;
   for (iteration = 0; iteration < maxIterations; ++iteration) {
-    if (distance2(uncalibrate(pn), pi) <= tol_)
-      break;
+    if (distance2(uncalibrate(pn), pi) <= tol_) break;
     const double px = pn.x(), py = pn.y(), xx = px * px, yy = py * py;
     const double rr = xx + yy;
     const double g = (1 + k1_ * rr + k2_ * rr * rr);
@@ -118,7 +116,8 @@ Point2 Cal3Bundler::calibrate(const Point2& pi,
 
   if (iteration >= maxIterations)
     throw std::runtime_error(
-        "Cal3Bundler::calibrate fails to converge. need a better initialization");
+        "Cal3Bundler::calibrate fails to converge. need a better "
+        "initialization");
 
   calibrateJacobians<Cal3Bundler, dimension>(*this, pn, Dcal, Dp);
 
diff --git a/gtsam/geometry/Cal3Bundler.h b/gtsam/geometry/Cal3Bundler.h
index 50b392096..0016ded2d 100644
--- a/gtsam/geometry/Cal3Bundler.h
+++ b/gtsam/geometry/Cal3Bundler.h
@@ -30,7 +30,6 @@ namespace gtsam {
  * \nosubgrouping
  */
 class GTSAM_EXPORT Cal3Bundler : public Cal3 {
-
  private:
   double k1_ = 0.0f, k2_ = 0.0f;  ///< radial distortion
   double tol_ = 1e-5;             ///< tolerance value when calibrating
@@ -40,7 +39,6 @@ class GTSAM_EXPORT Cal3Bundler : public Cal3 {
   // but are treated as constants.
 
  public:
-
   enum { dimension = 3 };
 
   /// @name Standard Constructors
@@ -83,40 +81,28 @@ class GTSAM_EXPORT Cal3Bundler : public Cal3 {
   /// @{
 
   /// distorsion parameter k1
-  inline double k1() const {
-    return k1_;
-  }
+  inline double k1() const { return k1_; }
 
   /// distorsion parameter k2
-  inline double k2() const {
-    return k2_;
-  }
+  inline double k2() const { return k2_; }
 
   /// image center in x
-  inline double px() const {
-    return u0_;
-  }
+  inline double px() const { return u0_; }
 
   /// image center in y
-  inline double py() const {
-    return v0_;
-  }
+  inline double py() const { return v0_; }
 
-  Matrix3 K() const override; ///< Standard 3*3 calibration matrix
-  Vector4 k() const; ///< Radial distortion parameters (4 of them, 2 0)
+  Matrix3 K() const override;  ///< Standard 3*3 calibration matrix
+  Vector4 k() const;  ///< Radial distortion parameters (4 of them, 2 0)
 
   Vector3 vector() const;
 
 #ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V41
   /// get parameter u0
-  inline double u0() const {
-    return u0_;
-  }
+  inline double u0() const { return u0_; }
 
   /// get parameter v0
-  inline double v0() const {
-    return v0_;
-  }
+  inline double v0() const { return v0_; }
 #endif
 
   /**
@@ -128,7 +114,7 @@ class GTSAM_EXPORT Cal3Bundler : public Cal3 {
    * @return point in image coordinates
    */
   Point2 uncalibrate(const Point2& p, OptionalJacobian<2, 3> Dcal = boost::none,
-      OptionalJacobian<2, 2> Dp = boost::none) const;
+                     OptionalJacobian<2, 2> Dp = boost::none) const;
 
   /**
    * Convert a pixel coordinate to ideal coordinate xy
@@ -138,8 +124,7 @@ class GTSAM_EXPORT Cal3Bundler : public Cal3 {
    * @param Dp optional 2*2 Jacobian wrpt intrinsic coordinates
    * @return point in intrinsic coordinates
    */
-  Point2 calibrate(const Point2& pi,
-                   OptionalJacobian<2, 3> Dcal = boost::none,
+  Point2 calibrate(const Point2& pi, OptionalJacobian<2, 3> Dcal = boost::none,
                    OptionalJacobian<2, 2> Dp = boost::none) const;
 
   /// @deprecated might be removed in next release, use uncalibrate
@@ -172,15 +157,14 @@ class GTSAM_EXPORT Cal3Bundler : public Cal3 {
   }
 
  private:
-
   /// @}
   /// @name Advanced Interface
   /// @{
 
   /** Serialization function */
   friend class boost::serialization::access;
-  template<class Archive>
-  void serialize(Archive & ar, const unsigned int /*version*/) {
+  template <class Archive>
+  void serialize(Archive& ar, const unsigned int /*version*/) {
     ar& boost::serialization::make_nvp(
         "Cal3Bundler", boost::serialization::base_object<Cal3>(*this));
     ar& BOOST_SERIALIZATION_NVP(k1_);
@@ -189,13 +173,12 @@ class GTSAM_EXPORT Cal3Bundler : public Cal3 {
   }
 
   /// @}
-
 };
 
-template<>
+template <>
 struct traits<Cal3Bundler> : public internal::Manifold<Cal3Bundler> {};
 
-template<>
+template <>
 struct traits<const Cal3Bundler> : public internal::Manifold<Cal3Bundler> {};
 
-} // namespace gtsam
+}  // namespace gtsam
diff --git a/gtsam/geometry/Cal3DS2.cpp b/gtsam/geometry/Cal3DS2.cpp
index 71aa2738d..f93386ea7 100644
--- a/gtsam/geometry/Cal3DS2.cpp
+++ b/gtsam/geometry/Cal3DS2.cpp
@@ -16,11 +16,11 @@
  * @author Varun Agrawal
  */
 
-#include <gtsam/base/Vector.h>
 #include <gtsam/base/Matrix.h>
+#include <gtsam/base/Vector.h>
+#include <gtsam/geometry/Cal3DS2.h>
 #include <gtsam/geometry/Point2.h>
 #include <gtsam/geometry/Point3.h>
-#include <gtsam/geometry/Cal3DS2.h>
 
 namespace gtsam {
 
@@ -31,9 +31,7 @@ std::ostream& operator<<(std::ostream& os, const Cal3DS2& cal) {
 }
 
 /* ************************************************************************* */
-void Cal3DS2::print(const std::string& s_) const {
-  Base::print(s_);
-}
+void Cal3DS2::print(const std::string& s_) const { Base::print(s_); }
 
 /* ************************************************************************* */
 bool Cal3DS2::equals(const Cal3DS2& K, double tol) const {
@@ -50,8 +48,5 @@ Cal3DS2 Cal3DS2::retract(const Vector& d) const {
 Vector Cal3DS2::localCoordinates(const Cal3DS2& T2) const {
   return T2.vector() - vector();
 }
-
 }
 /* ************************************************************************* */
-
-
diff --git a/gtsam/geometry/Cal3DS2.h b/gtsam/geometry/Cal3DS2.h
index ad4406b76..58d35c2ec 100644
--- a/gtsam/geometry/Cal3DS2.h
+++ b/gtsam/geometry/Cal3DS2.h
@@ -11,7 +11,8 @@
 
 /**
  * @file Cal3DS2.h
- * @brief Calibration of a camera with radial distortion, calculations in base class Cal3DS2_Base
+ * @brief Calibration of a camera with radial distortion, calculations in base
+ * class Cal3DS2_Base
  * @date Feb 28, 2010
  * @author ydjian
  * @autho Varun Agrawal
@@ -31,11 +32,9 @@ namespace gtsam {
  * \nosubgrouping
  */
 class GTSAM_EXPORT Cal3DS2 : public Cal3DS2_Base {
-
   using Base = Cal3DS2_Base;
 
-public:
-
+ public:
   enum { dimension = 9 };
 
   /// @name Standard Constructors
@@ -54,7 +53,7 @@ public:
   /// @name Advanced Constructors
   /// @{
 
-  Cal3DS2(const Vector9 &v) : Base(v) {}
+  Cal3DS2(const Vector9& v) : Base(v) {}
 
   /// @}
   /// @name Testable
@@ -75,10 +74,10 @@ public:
   /// @{
 
   /// Given delta vector, update calibration
-  Cal3DS2 retract(const Vector& d) const ;
+  Cal3DS2 retract(const Vector& d) const;
 
   /// Given a different calibration, calculate update to obtain it
-  Vector localCoordinates(const Cal3DS2& T2) const ;
+  Vector localCoordinates(const Cal3DS2& T2) const;
 
   /// Return dimensions of calibration manifold object
   virtual size_t dim() const override { return Dim(); }
@@ -97,30 +96,24 @@ public:
 
   /// @}
 
-
-private:
-
+ private:
   /// @name Advanced Interface
   /// @{
 
   /** Serialization function */
   friend class boost::serialization::access;
-  template<class Archive>
-  void serialize(Archive & ar, const unsigned int /*version*/)
-  {
-    ar & boost::serialization::make_nvp("Cal3DS2",
-        boost::serialization::base_object<Cal3DS2_Base>(*this));
+  template <class Archive>
+  void serialize(Archive& ar, const unsigned int /*version*/) {
+    ar& boost::serialization::make_nvp(
+        "Cal3DS2", boost::serialization::base_object<Cal3DS2_Base>(*this));
   }
 
   /// @}
-
 };
 
-template<>
+template <>
 struct traits<Cal3DS2> : public internal::Manifold<Cal3DS2> {};
 
-template<>
+template <>
 struct traits<const Cal3DS2> : public internal::Manifold<Cal3DS2> {};
-
 }
-
diff --git a/gtsam/geometry/Cal3DS2_Base.cpp b/gtsam/geometry/Cal3DS2_Base.cpp
index 71ce32ccb..a3f7026b9 100644
--- a/gtsam/geometry/Cal3DS2_Base.cpp
+++ b/gtsam/geometry/Cal3DS2_Base.cpp
@@ -16,11 +16,11 @@
  * @author Varun Agrawal
  */
 
-#include <gtsam/base/Vector.h>
 #include <gtsam/base/Matrix.h>
+#include <gtsam/base/Vector.h>
+#include <gtsam/geometry/Cal3DS2_Base.h>
 #include <gtsam/geometry/Point2.h>
 #include <gtsam/geometry/Point3.h>
-#include <gtsam/geometry/Cal3DS2_Base.h>
 
 namespace gtsam {
 
@@ -54,23 +54,23 @@ bool Cal3DS2_Base::equals(const Cal3DS2_Base& K, double tol) const {
 }
 
 /* ************************************************************************* */
-static Matrix29 D2dcalibration(double x, double y, double xx,
-    double yy, double xy, double rr, double r4, double pnx, double pny,
-    const Matrix2& DK) {
+static Matrix29 D2dcalibration(double x, double y, double xx, double yy,
+                               double xy, double rr, double r4, double pnx,
+                               double pny, const Matrix2& DK) {
   Matrix25 DR1;
   DR1 << pnx, 0.0, pny, 1.0, 0.0, 0.0, pny, 0.0, 0.0, 1.0;
   Matrix24 DR2;
-  DR2 << x * rr, x * r4, 2 * xy, rr + 2 * xx, //
-         y * rr, y * r4, rr + 2 * yy, 2 * xy;
+  DR2 << x * rr, x * r4, 2 * xy, rr + 2 * xx,  //
+      y * rr, y * r4, rr + 2 * yy, 2 * xy;
   Matrix29 D;
   D << DR1, DK * DR2;
   return D;
 }
 
 /* ************************************************************************* */
-static Matrix2 D2dintrinsic(double x, double y, double rr,
-    double g, double k1, double k2, double p1, double p2,
-    const Matrix2& DK) {
+static Matrix2 D2dintrinsic(double x, double y, double rr, double g, double k1,
+                            double k2, double p1, double p2,
+                            const Matrix2& DK) {
   const double drdx = 2. * x;
   const double drdy = 2. * y;
   const double dgdx = k1 * drdx + k2 * 2. * rr * drdx;
@@ -84,8 +84,8 @@ static Matrix2 D2dintrinsic(double x, double y, double rr,
   const double dDydy = 2. * p2 * x + p1 * (drdy + 4. * y);
 
   Matrix2 DR;
-  DR << g + x * dgdx + dDxdx, x * dgdy + dDxdy, //
-        y * dgdx + dDydx, g + y * dgdy + dDydy;
+  DR << g + x * dgdx + dDxdx, x * dgdy + dDxdy,  //
+      y * dgdx + dDydx, g + y * dgdy + dDydy;
 
   return DK * DR;
 }
@@ -100,7 +100,7 @@ Point2 Cal3DS2_Base::uncalibrate(const Point2& p, OptionalJacobian<2, 9> Dcal,
   const double x = p.x(), y = p.y(), xy = x * y, xx = x * x, yy = y * y;
   const double rr = xx + yy;
   const double r4 = rr * rr;
-  const double g = 1. + k1_ * rr + k2_ * r4; // scaling factor
+  const double g = 1. + k1_ * rr + k2_ * r4;  // scaling factor
 
   // tangential component
   const double dx = 2. * p1_ * xy + p2_ * (rr + 2. * xx);
@@ -190,8 +190,5 @@ Matrix29 Cal3DS2_Base::D2d_calibration(const Point2& p) const {
   DK << fx_, s_, 0.0, fy_;
   return D2dcalibration(x, y, xx, yy, xy, rr, r4, pnx, pny, DK);
 }
-
 }
 /* ************************************************************************* */
-
-
diff --git a/gtsam/geometry/Cal3Unified.h b/gtsam/geometry/Cal3Unified.h
index 4c456ec24..ee388c8c1 100644
--- a/gtsam/geometry/Cal3Unified.h
+++ b/gtsam/geometry/Cal3Unified.h
@@ -28,20 +28,21 @@
 namespace gtsam {
 
 /**
- * @brief Calibration of a omni-directional camera with mirror + lens radial distortion
+ * @brief Calibration of a omni-directional camera with mirror + lens radial
+ * distortion
  * @addtogroup geometry
  * \nosubgrouping
  *
  * Similar to Cal3DS2, does distortion but has additional mirror parameter xi
  * K = [ fx s u0 ; 0 fy v0 ; 0 0 1 ]
- * Pn = [ P.x / (1 + xi * \sqrt{P.x² + P.y² + 1}), P.y / (1 + xi * \sqrt{P.x² + P.y² + 1})]
+ * Pn = [ P.x / (1 + xi * \sqrt{P.x² + P.y² + 1}), P.y / (1 + xi * \sqrt{P.x² +
+ * P.y² + 1})]
  * r² = Pn.x² + Pn.y²
  * \hat{pn} = (1 + k1*r² + k2*r⁴ ) pn + [ 2*k3 pn.x pn.y + k4 (r² + 2 Pn.x²) ;
  *                      k3 (rr + 2 Pn.y²) + 2*k4 pn.x pn.y  ]
  * pi = K*pn
  */
 class GTSAM_EXPORT Cal3Unified : public Cal3DS2_Base {
-
   using This = Cal3Unified;
   using Base = Cal3DS2_Base;
 
@@ -49,7 +50,6 @@ class GTSAM_EXPORT Cal3Unified : public Cal3DS2_Base {
   double xi_ = 0.0f;  ///< mirror parameter
 
  public:
-
   enum { dimension = 10 };
 
   /// @name Standard Constructors
@@ -90,10 +90,10 @@ class GTSAM_EXPORT Cal3Unified : public Cal3DS2_Base {
   /// @{
 
   /// mirror parameter
-  inline double xi() const { return xi_;}
+  inline double xi() const { return xi_; }
 
   /// Return all parameters as a vector
-  Vector10 vector() const ;
+  Vector10 vector() const;
 
   /**
    * convert intrinsic coordinates xy to image coordinates uv
@@ -103,8 +103,8 @@ class GTSAM_EXPORT Cal3Unified : public Cal3DS2_Base {
    * @return point in image coordinates
    */
   Point2 uncalibrate(const Point2& p,
-      OptionalJacobian<2,10> Dcal = boost::none,
-      OptionalJacobian<2,2> Dp = boost::none) const ;
+                     OptionalJacobian<2, 10> Dcal = boost::none,
+                     OptionalJacobian<2, 2> Dp = boost::none) const;
 
   /// Conver a pixel coordinate to ideal coordinate
   Point2 calibrate(const Point2& p, OptionalJacobian<2, 10> Dcal = boost::none,
@@ -121,10 +121,10 @@ class GTSAM_EXPORT Cal3Unified : public Cal3DS2_Base {
   /// @{
 
   /// Given delta vector, update calibration
-  Cal3Unified retract(const Vector& d) const ;
+  Cal3Unified retract(const Vector& d) const;
 
   /// Given a different calibration, calculate update to obtain it
-  Vector localCoordinates(const Cal3Unified& T2) const ;
+  Vector localCoordinates(const Cal3Unified& T2) const;
 
   /// Return dimensions of calibration manifold object
   virtual size_t dim() const override { return Dim(); }
@@ -134,25 +134,20 @@ class GTSAM_EXPORT Cal3Unified : public Cal3DS2_Base {
 
   /// @}
 
-private:
-
+ private:
   /** Serialization function */
   friend class boost::serialization::access;
-  template<class Archive>
-  void serialize(Archive & ar, const unsigned int /*version*/)
-  {
-    ar & boost::serialization::make_nvp("Cal3Unified",
-        boost::serialization::base_object<Cal3DS2_Base>(*this));
-    ar & BOOST_SERIALIZATION_NVP(xi_);
+  template <class Archive>
+  void serialize(Archive& ar, const unsigned int /*version*/) {
+    ar& boost::serialization::make_nvp(
+        "Cal3Unified", boost::serialization::base_object<Cal3DS2_Base>(*this));
+    ar& BOOST_SERIALIZATION_NVP(xi_);
   }
-
 };
 
-template<>
+template <>
 struct traits<Cal3Unified> : public internal::Manifold<Cal3Unified> {};
 
-template<>
+template <>
 struct traits<const Cal3Unified> : public internal::Manifold<Cal3Unified> {};
-
 }
-
diff --git a/gtsam/geometry/Cal3_S2Stereo.cpp b/gtsam/geometry/Cal3_S2Stereo.cpp
index 8bd733e95..56ceaf516 100644
--- a/gtsam/geometry/Cal3_S2Stereo.cpp
+++ b/gtsam/geometry/Cal3_S2Stereo.cpp
@@ -38,7 +38,8 @@ void Cal3_S2Stereo::print(const std::string& s) const {
 /* ************************************************************************* */
 bool Cal3_S2Stereo::equals(const Cal3_S2Stereo& other, double tol) const {
   const Cal3_S2* base = dynamic_cast<const Cal3_S2*>(&other);
-  return (Cal3_S2::equals(*base, tol) && std::fabs(b_ - other.baseline()) < tol);
+  return (Cal3_S2::equals(*base, tol) &&
+          std::fabs(b_ - other.baseline()) < tol);
 }
 
 /* ************************************************************************* */
diff --git a/gtsam/geometry/Cal3_S2Stereo.h b/gtsam/geometry/Cal3_S2Stereo.h
index ee316251a..d7bf34e61 100644
--- a/gtsam/geometry/Cal3_S2Stereo.h
+++ b/gtsam/geometry/Cal3_S2Stereo.h
@@ -22,121 +22,116 @@
 
 namespace gtsam {
 
-  /**
-   * @brief The most common 5DOF 3D->2D calibration, stereo version
-   * @addtogroup geometry
-   * \nosubgrouping
-   */
-  class GTSAM_EXPORT Cal3_S2Stereo : public Cal3_S2 {
-   private:
-    double b_ = 1.0f;  ///< Stereo baseline.
+/**
+ * @brief The most common 5DOF 3D->2D calibration, stereo version
+ * @addtogroup geometry
+ * \nosubgrouping
+ */
+class GTSAM_EXPORT Cal3_S2Stereo : public Cal3_S2 {
+ private:
+  double b_ = 1.0f;  ///< Stereo baseline.
 
-   public:
+ public:
+  enum { dimension = 6 };
 
-    enum { dimension = 6 };
+  ///< shared pointer to stereo calibration object
+  using shared_ptr = boost::shared_ptr<Cal3_S2Stereo>;
 
-    ///< shared pointer to stereo calibration object
-    using shared_ptr = boost::shared_ptr<Cal3_S2Stereo>;
+  /// @name Standard Constructors
+  /// @
 
-    /// @name Standard Constructors
-    /// @
+  /// default calibration leaves coordinates unchanged
+  Cal3_S2Stereo() = default;
 
-    /// default calibration leaves coordinates unchanged
-    Cal3_S2Stereo() = default;
+  /// constructor from doubles
+  Cal3_S2Stereo(double fx, double fy, double s, double u0, double v0, double b)
+      : Cal3_S2(fx, fy, s, u0, v0), b_(b) {}
 
-    /// constructor from doubles
-    Cal3_S2Stereo(double fx, double fy, double s, double u0, double v0,
-                  double b)
-        : Cal3_S2(fx, fy, s, u0, v0), b_(b) {}
+  /// constructor from vector
+  Cal3_S2Stereo(const Vector6& d)
+      : Cal3_S2(d(0), d(1), d(2), d(3), d(4)), b_(d(5)) {}
 
-    /// constructor from vector
-    Cal3_S2Stereo(const Vector6& d)
-        : Cal3_S2(d(0), d(1), d(2), d(3), d(4)), b_(d(5)) {}
+  /// easy constructor; field-of-view in degrees, assumes zero skew
+  Cal3_S2Stereo(double fov, int w, int h, double b)
+      : Cal3_S2(fov, w, h), b_(b) {}
 
-    /// easy constructor; field-of-view in degrees, assumes zero skew
-    Cal3_S2Stereo(double fov, int w, int h, double b)
-        : Cal3_S2(fov, w, h), b_(b) {}
+  /// @}
+  /// @name Testable
+  /// @{
 
-    /// @}
-    /// @name Testable
-    /// @{
+  /// Output stream operator
+  GTSAM_EXPORT friend std::ostream& operator<<(std::ostream& os,
+                                               const Cal3_S2Stereo& cal);
 
-    /// Output stream operator
-    GTSAM_EXPORT friend std::ostream& operator<<(std::ostream& os,
-                                                 const Cal3_S2Stereo& cal);
+  /// print with optional string
+  void print(const std::string& s = "") const override;
 
-    /// print with optional string
-    void print(const std::string& s = "") const override;
+  /// Check if equal up to specified tolerance
+  bool equals(const Cal3_S2Stereo& other, double tol = 10e-9) const;
 
-    /// Check if equal up to specified tolerance
-    bool equals(const Cal3_S2Stereo& other, double tol = 10e-9) const;
+  /// @}
+  /// @name Standard Interface
+  /// @{
 
-   /// @}
-    /// @name Standard Interface
-    /// @{
+  /// return calibration, same for left and right
+  const Cal3_S2& calibration() const { return *this; }
 
-    /// return calibration, same for left and right
-    const Cal3_S2& calibration() const { return *this; }
+  /// return calibration matrix K, same for left and right
+  Matrix3 K() const override { return Cal3_S2::K(); }
 
-    /// return calibration matrix K, same for left and right
-    Matrix3 K() const override { return Cal3_S2::K(); }
+  /// return baseline
+  inline double baseline() const { return b_; }
 
-    /// return baseline
-    inline double baseline() const { return b_; }
+  /// vectorized form (column-wise)
+  Vector6 vector() const {
+    Vector6 v;
+    v << Cal3_S2::vector(), b_;
+    return v;
+  }
 
-    /// vectorized form (column-wise)
-    Vector6 vector() const {
-      Vector6 v;
-      v << Cal3_S2::vector(), b_;
-      return v;
-    }
+  /// @}
+  /// @name Manifold
+  /// @{
 
-    /// @}
-    /// @name Manifold
-    /// @{
+  /// return DOF, dimensionality of tangent space
+  inline size_t dim() const override { return Dim(); }
 
-    /// return DOF, dimensionality of tangent space
-    inline size_t dim() const override { return Dim(); }
+  /// return DOF, dimensionality of tangent space
+  inline static size_t Dim() { return dimension; }
 
-    /// return DOF, dimensionality of tangent space
-    inline static size_t Dim() { return dimension; }
+  /// Given 6-dim tangent vector, create new calibration
+  inline Cal3_S2Stereo retract(const Vector& d) const {
+    return Cal3_S2Stereo(fx() + d(0), fy() + d(1), skew() + d(2), px() + d(3),
+                         py() + d(4), b_ + d(5));
+  }
 
-    /// Given 6-dim tangent vector, create new calibration
-    inline Cal3_S2Stereo retract(const Vector& d) const {
-      return Cal3_S2Stereo(fx() + d(0), fy() + d(1), skew() + d(2), px() + d(3),
-                           py() + d(4), b_ + d(5));
-    }
+  /// Unretraction for the calibration
+  Vector6 localCoordinates(const Cal3_S2Stereo& T2) const {
+    return T2.vector() - vector();
+  }
 
-    /// Unretraction for the calibration
-    Vector6 localCoordinates(const Cal3_S2Stereo& T2) const {
-      return T2.vector() - vector();
-    }
+  /// @}
+  /// @name Advanced Interface
+  /// @{
 
-    /// @}
-    /// @name Advanced Interface
-    /// @{
+ private:
+  /** Serialization function */
+  friend class boost::serialization::access;
+  template <class Archive>
+  void serialize(Archive& ar, const unsigned int /*version*/) {
+    ar& boost::serialization::make_nvp(
+        "Cal3_S2", boost::serialization::base_object<Cal3_S2>(*this));
+    ar& BOOST_SERIALIZATION_NVP(b_);
+  }
+  /// @}
+};
 
-   private:
-    /** Serialization function */
-    friend class boost::serialization::access;
-    template<class Archive>
-    void serialize(Archive & ar, const unsigned int /*version*/)
-    {
-      ar& boost::serialization::make_nvp(
-          "Cal3_S2", boost::serialization::base_object<Cal3_S2>(*this));
-      ar& BOOST_SERIALIZATION_NVP(b_);
-    }
-    /// @}
+// Define GTSAM traits
+template <>
+struct traits<Cal3_S2Stereo> : public internal::Manifold<Cal3_S2Stereo> {};
 
-  };
+template <>
+struct traits<const Cal3_S2Stereo> : public internal::Manifold<Cal3_S2Stereo> {
+};
 
-  // Define GTSAM traits
-  template<>
-  struct traits<Cal3_S2Stereo> : public internal::Manifold<Cal3_S2Stereo> {
-  };
-
-  template<>
-  struct traits<const Cal3_S2Stereo> : public internal::Manifold<Cal3_S2Stereo> {
-  };
-
-} // \ namespace gtsam
+}  // \ namespace gtsam
diff --git a/gtsam/geometry/tests/testCal3Bundler.cpp b/gtsam/geometry/tests/testCal3Bundler.cpp
index 8e6fe983f..b821d295b 100644
--- a/gtsam/geometry/tests/testCal3Bundler.cpp
+++ b/gtsam/geometry/tests/testCal3Bundler.cpp
@@ -26,27 +26,27 @@ GTSAM_CONCEPT_TESTABLE_INST(Cal3Bundler)
 GTSAM_CONCEPT_MANIFOLD_INST(Cal3Bundler)
 
 static Cal3Bundler K(500, 1e-3, 1e-3, 1000, 2000);
-static Point2 p(2,3);
+static Point2 p(2, 3);
 
 /* ************************************************************************* */
 TEST(Cal3Bundler, vector) {
   Cal3Bundler K;
   Vector expected(3);
   expected << 1, 0, 0;
-  CHECK(assert_equal(expected,K.vector()));
+  CHECK(assert_equal(expected, K.vector()));
 }
 
 /* ************************************************************************* */
 TEST(Cal3Bundler, uncalibrate) {
-  Vector v = K.vector() ;
-  double r = p.x()*p.x() + p.y()*p.y() ;
-  double g = v[0]*(1+v[1]*r+v[2]*r*r) ;
-  Point2 expected (1000+g*p.x(), 2000+g*p.y()) ;
+  Vector v = K.vector();
+  double r = p.x() * p.x() + p.y() * p.y();
+  double g = v[0] * (1 + v[1] * r + v[2] * r * r);
+  Point2 expected(1000 + g * p.x(), 2000 + g * p.y());
   Point2 actual = K.uncalibrate(p);
-  CHECK(assert_equal(expected,actual));
+  CHECK(assert_equal(expected, actual));
 }
 
-TEST(Cal3Bundler, calibrate ) {
+TEST(Cal3Bundler, calibrate) {
   Point2 pn(0.5, 0.5);
   Point2 pi = K.uncalibrate(pn);
   Point2 pn_hat = K.calibrate(pi);
@@ -54,20 +54,24 @@ TEST(Cal3Bundler, calibrate ) {
 }
 
 /* ************************************************************************* */
-Point2 uncalibrate_(const Cal3Bundler& k, const Point2& pt) { return k.uncalibrate(pt); }
+Point2 uncalibrate_(const Cal3Bundler& k, const Point2& pt) {
+  return k.uncalibrate(pt);
+}
 
-Point2 calibrate_(const Cal3Bundler& k, const Point2& pt) { return k.calibrate(pt); }
+Point2 calibrate_(const Cal3Bundler& k, const Point2& pt) {
+  return k.calibrate(pt);
+}
 
 /* ************************************************************************* */
 TEST(Cal3Bundler, Duncalibrate) {
   Matrix Dcal, Dp;
   Point2 actual = K.uncalibrate(p, Dcal, Dp);
   Point2 expected(2182, 3773);
-  CHECK(assert_equal(expected,actual,1e-7));
+  CHECK(assert_equal(expected, actual, 1e-7));
   Matrix numerical1 = numericalDerivative21(uncalibrate_, K, p);
   Matrix numerical2 = numericalDerivative22(uncalibrate_, K, p);
-  CHECK(assert_equal(numerical1,Dcal,1e-7));
-  CHECK(assert_equal(numerical2,Dp,1e-7));
+  CHECK(assert_equal(numerical1, Dcal, 1e-7));
+  CHECK(assert_equal(numerical2, Dp, 1e-7));
 }
 
 /* ************************************************************************* */
@@ -79,14 +83,12 @@ TEST(Cal3Bundler, Dcalibrate) {
   CHECK(assert_equal(pn, actual, 1e-7));
   Matrix numerical1 = numericalDerivative21(calibrate_, K, pi);
   Matrix numerical2 = numericalDerivative22(calibrate_, K, pi);
-  CHECK(assert_equal(numerical1,Dcal,1e-5));
-  CHECK(assert_equal(numerical2,Dp,1e-5));
+  CHECK(assert_equal(numerical1, Dcal, 1e-5));
+  CHECK(assert_equal(numerical2, Dp, 1e-5));
 }
 
 /* ************************************************************************* */
-TEST(Cal3Bundler, assert_equal) {
-  CHECK(assert_equal(K,K,1e-7));
-}
+TEST(Cal3Bundler, assert_equal) { CHECK(assert_equal(K, K, 1e-7)); }
 
 /* ************************************************************************* */
 TEST(Cal3Bundler, retract) {
@@ -99,8 +101,8 @@ TEST(Cal3Bundler, retract) {
   Vector3 d;
   d << 10, 1e-3, 1e-3;
   Cal3Bundler actual = K.retract(d);
-  CHECK(assert_equal(expected,actual,1e-7));
-  CHECK(assert_equal(d,K.localCoordinates(actual),1e-7));
+  CHECK(assert_equal(expected, actual, 1e-7));
+  CHECK(assert_equal(d, K.localCoordinates(actual), 1e-7));
 }
 
 /* ************************************************************************* */
@@ -114,5 +116,8 @@ TEST(Cal3_S2, Print) {
 }
 
 /* ************************************************************************* */
-int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
+int main() {
+  TestResult tr;
+  return TestRegistry::runAllTests(tr);
+}
 /* ************************************************************************* */
diff --git a/gtsam/geometry/tests/testCal3DFisheye.cpp b/gtsam/geometry/tests/testCal3DFisheye.cpp
index 7a73e7490..28064a92c 100644
--- a/gtsam/geometry/tests/testCal3DFisheye.cpp
+++ b/gtsam/geometry/tests/testCal3DFisheye.cpp
@@ -191,16 +191,15 @@ Point2 calibrate_(const Cal3Fisheye& k, const Point2& pt) {
 }
 
 /* ************************************************************************* */
-TEST(Cal3Fisheye, Dcalibrate)
-{
+TEST(Cal3Fisheye, Dcalibrate) {
   Point2 p(0.5, 0.5);
   Point2 pi = K.uncalibrate(p);
   Matrix Dcal, Dp;
   K.calibrate(pi, Dcal, Dp);
   Matrix numerical1 = numericalDerivative21(calibrate_, K, pi);
-  CHECK(assert_equal(numerical1,Dcal,1e-5));
+  CHECK(assert_equal(numerical1, Dcal, 1e-5));
   Matrix numerical2 = numericalDerivative22(calibrate_, K, pi);
-  CHECK(assert_equal(numerical2,Dp,1e-5));
+  CHECK(assert_equal(numerical2, Dp, 1e-5));
 }
 
 /* ************************************************************************* */
diff --git a/gtsam/geometry/tests/testCal3DS2.cpp b/gtsam/geometry/tests/testCal3DS2.cpp
index e4dc3e806..7ef6e5001 100644
--- a/gtsam/geometry/tests/testCal3DS2.cpp
+++ b/gtsam/geometry/tests/testCal3DS2.cpp
@@ -14,7 +14,6 @@
  * @brief Unit tests for Cal3DS2 calibration model.
  */
 
-
 #include <CppUnitLite/TestHarness.h>
 #include <gtsam/base/Testable.h>
 #include <gtsam/base/TestableAssertions.h>
@@ -26,53 +25,53 @@ using namespace gtsam;
 GTSAM_CONCEPT_TESTABLE_INST(Cal3DS2)
 GTSAM_CONCEPT_MANIFOLD_INST(Cal3DS2)
 
-static Cal3DS2 K(500, 100, 0.1, 320, 240, 1e-3, 2.0*1e-3, 3.0*1e-3, 4.0*1e-3);
-static Point2 p(2,3);
+static Cal3DS2 K(500, 100, 0.1, 320, 240, 1e-3, 2.0 * 1e-3, 3.0 * 1e-3,
+                 4.0 * 1e-3);
+static Point2 p(2, 3);
 
 /* ************************************************************************* */
-TEST(Cal3DS2, uncalibrate)
-{
-  Vector k = K.k() ;
-  double r = p.x()*p.x() + p.y()*p.y() ;
-  double g = 1+k[0]*r+k[1]*r*r ;
-  double tx = 2*k[2]*p.x()*p.y()       +   k[3]*(r+2*p.x()*p.x()) ;
-  double ty =   k[2]*(r+2*p.y()*p.y()) + 2*k[3]*p.x()*p.y() ;
-  Vector v_hat = (Vector(3) << g*p.x() + tx, g*p.y() + ty, 1.0).finished();
-  Vector v_i = K.K() * v_hat ;
-  Point2 p_i(v_i(0)/v_i(2), v_i(1)/v_i(2)) ;
+TEST(Cal3DS2, Uncalibrate) {
+  Vector k = K.k();
+  double r = p.x() * p.x() + p.y() * p.y();
+  double g = 1 + k[0] * r + k[1] * r * r;
+  double tx = 2 * k[2] * p.x() * p.y() + k[3] * (r + 2 * p.x() * p.x());
+  double ty = k[2] * (r + 2 * p.y() * p.y()) + 2 * k[3] * p.x() * p.y();
+  Vector v_hat = (Vector(3) << g * p.x() + tx, g * p.y() + ty, 1.0).finished();
+  Vector v_i = K.K() * v_hat;
+  Point2 p_i(v_i(0) / v_i(2), v_i(1) / v_i(2));
   Point2 q = K.uncalibrate(p);
-  CHECK(assert_equal(q,p_i));
+  CHECK(assert_equal(q, p_i));
 }
 
-TEST(Cal3DS2, calibrate )
-{
+TEST(Cal3DS2, Calibrate) {
   Point2 pn(0.5, 0.5);
   Point2 pi = K.uncalibrate(pn);
   Point2 pn_hat = K.calibrate(pi);
-  CHECK( traits<Point2>::Equals(pn, pn_hat, 1e-5));
+  CHECK(traits<Point2>::Equals(pn, pn_hat, 1e-5));
 }
 
-Point2 uncalibrate_(const Cal3DS2& k, const Point2& pt) { return k.uncalibrate(pt); }
+Point2 uncalibrate_(const Cal3DS2& k, const Point2& pt) {
+  return k.uncalibrate(pt);
+}
 
 /* ************************************************************************* */
-TEST(Cal3DS2, Duncalibrate1)
-{
+TEST(Cal3DS2, Duncalibrate1) {
   Matrix computed;
   K.uncalibrate(p, computed, boost::none);
   Matrix numerical = numericalDerivative21(uncalibrate_, K, p, 1e-7);
-  CHECK(assert_equal(numerical,computed,1e-5));
+  CHECK(assert_equal(numerical, computed, 1e-5));
   Matrix separate = K.D2d_calibration(p);
-  CHECK(assert_equal(numerical,separate,1e-5));
+  CHECK(assert_equal(numerical, separate, 1e-5));
 }
 
 /* ************************************************************************* */
-TEST(Cal3DS2, Duncalibrate2)
-{
-  Matrix computed; K.uncalibrate(p, boost::none, computed);
+TEST(Cal3DS2, Duncalibrate2) {
+  Matrix computed;
+  K.uncalibrate(p, boost::none, computed);
   Matrix numerical = numericalDerivative22(uncalibrate_, K, p, 1e-7);
-  CHECK(assert_equal(numerical,computed,1e-5));
+  CHECK(assert_equal(numerical, computed, 1e-5));
   Matrix separate = K.D2d_intrinsic(p);
-  CHECK(assert_equal(numerical,separate,1e-5));
+  CHECK(assert_equal(numerical, separate, 1e-5));
 }
 
 Point2 calibrate_(const Cal3DS2& k, const Point2& pt) {
@@ -80,8 +79,7 @@ Point2 calibrate_(const Cal3DS2& k, const Point2& pt) {
 }
 
 /* ************************************************************************* */
-TEST(Cal3DS2, Dcalibrate)
-{
+TEST(Cal3DS2, Dcalibrate) {
   Point2 pn(0.5, 0.5);
   Point2 pi = K.uncalibrate(pn);
   Matrix Dcal, Dp;
@@ -93,21 +91,21 @@ TEST(Cal3DS2, Dcalibrate)
 }
 
 /* ************************************************************************* */
-TEST(Cal3DS2, assert_equal) { CHECK(assert_equal(K, K, 1e-5)); }
+TEST(Cal3DS2, Equal) { CHECK(assert_equal(K, K, 1e-5)); }
 
 /* ************************************************************************* */
-TEST(Cal3DS2, retract) {
+TEST(Cal3DS2, Retract) {
   Cal3DS2 expected(500 + 1, 100 + 2, 0.1 + 3, 320 + 4, 240 + 5, 1e-3 + 6,
-      2.0 * 1e-3 + 7, 3.0 * 1e-3 + 8, 4.0 * 1e-3 + 9);
+                   2.0 * 1e-3 + 7, 3.0 * 1e-3 + 8, 4.0 * 1e-3 + 9);
 
   EXPECT_LONGS_EQUAL(Cal3DS2::Dim(), 9);
   EXPECT_LONGS_EQUAL(expected.dim(), 9);
 
   Vector9 d;
-  d << 1,2,3,4,5,6,7,8,9;
+  d << 1, 2, 3, 4, 5, 6, 7, 8, 9;
   Cal3DS2 actual = K.retract(d);
-  CHECK(assert_equal(expected,actual,1e-7));
-  CHECK(assert_equal(d,K.localCoordinates(actual),1e-7));
+  CHECK(assert_equal(expected, actual, 1e-7));
+  CHECK(assert_equal(d, K.localCoordinates(actual), 1e-7));
 }
 
 /* ************************************************************************* */
@@ -122,5 +120,8 @@ TEST(Cal3DS2, Print) {
 }
 
 /* ************************************************************************* */
-int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
+int main() {
+  TestResult tr;
+  return TestRegistry::runAllTests(tr);
+}
 /* ************************************************************************* */
diff --git a/gtsam/geometry/tests/testCal3Unified.cpp b/gtsam/geometry/tests/testCal3Unified.cpp
index ff759d1cd..648bb358c 100644
--- a/gtsam/geometry/tests/testCal3Unified.cpp
+++ b/gtsam/geometry/tests/testCal3Unified.cpp
@@ -20,8 +20,8 @@
 #include <gtsam/base/numericalDerivative.h>
 #include <gtsam/geometry/Cal3Unified.h>
 
-#include <gtsam/nonlinear/Values.h>
 #include <gtsam/inference/Key.h>
+#include <gtsam/nonlinear/Values.h>
 
 using namespace gtsam;
 
@@ -36,51 +36,49 @@ V = [0.1, 1e-3, 2.0*1e-3, 3.0*1e-3, 4.0*1e-3, 0, 0, 100, 105, 320, 240];
 matlab toolbox available at http://homepages.laas.fr/~cmei/index.php/Toolbox
 */
 
-static Cal3Unified K(100, 105, 0.0, 320, 240, 1e-3, 2.0*1e-3, 3.0*1e-3, 4.0*1e-3, 0.1);
+static Cal3Unified K(100, 105, 0.0, 320, 240, 1e-3, 2.0 * 1e-3, 3.0 * 1e-3,
+                     4.0 * 1e-3, 0.1);
 static Point2 p(0.5, 0.7);
 
 /* ************************************************************************* */
-TEST(Cal3Unified, uncalibrate)
-{
-  Point2 p_i(364.7791831734982, 305.6677211952602) ;
+TEST(Cal3Unified, Uncalibrate) {
+  Point2 p_i(364.7791831734982, 305.6677211952602);
   Point2 q = K.uncalibrate(p);
-  CHECK(assert_equal(q,p_i));
+  CHECK(assert_equal(q, p_i));
 }
 
 /* ************************************************************************* */
-TEST(Cal3Unified, spaceNplane)
-{
+TEST(Cal3Unified, SpaceNplane) {
   Point2 q = K.spaceToNPlane(p);
   CHECK(assert_equal(Point2(0.441731600049497, 0.618424240069295), q));
   CHECK(assert_equal(p, K.nPlaneToSpace(q)));
 }
 
 /* ************************************************************************* */
-TEST(Cal3Unified, calibrate)
-{
+TEST(Cal3Unified, Calibrate) {
   Point2 pi = K.uncalibrate(p);
   Point2 pn_hat = K.calibrate(pi);
-  CHECK( traits<Point2>::Equals(p, pn_hat, 1e-8));
+  CHECK(traits<Point2>::Equals(p, pn_hat, 1e-8));
 }
 
-Point2 uncalibrate_(const Cal3Unified& k, const Point2& pt) { return k.uncalibrate(pt); }
+Point2 uncalibrate_(const Cal3Unified& k, const Point2& pt) {
+  return k.uncalibrate(pt);
+}
 
 /* ************************************************************************* */
-TEST(Cal3Unified, Duncalibrate1)
-{
+TEST(Cal3Unified, Duncalibrate1) {
   Matrix computed;
   K.uncalibrate(p, computed, boost::none);
   Matrix numerical = numericalDerivative21(uncalibrate_, K, p, 1e-7);
-  CHECK(assert_equal(numerical,computed,1e-6));
+  CHECK(assert_equal(numerical, computed, 1e-6));
 }
 
 /* ************************************************************************* */
-TEST(Cal3Unified, Duncalibrate2)
-{
+TEST(Cal3Unified, Duncalibrate2) {
   Matrix computed;
   K.uncalibrate(p, boost::none, computed);
   Matrix numerical = numericalDerivative22(uncalibrate_, K, p, 1e-7);
-  CHECK(assert_equal(numerical,computed,1e-6));
+  CHECK(assert_equal(numerical, computed, 1e-6));
 }
 
 Point2 calibrate_(const Cal3Unified& k, const Point2& pt) {
@@ -88,27 +86,24 @@ Point2 calibrate_(const Cal3Unified& k, const Point2& pt) {
 }
 
 /* ************************************************************************* */
-TEST(Cal3Unified, Dcalibrate)
-{
+TEST(Cal3Unified, Dcalibrate) {
   Point2 pi = K.uncalibrate(p);
   Matrix Dcal, Dp;
   K.calibrate(pi, Dcal, Dp);
   Matrix numerical1 = numericalDerivative21(calibrate_, K, pi);
-  CHECK(assert_equal(numerical1,Dcal,1e-5));
+  CHECK(assert_equal(numerical1, Dcal, 1e-5));
   Matrix numerical2 = numericalDerivative22(calibrate_, K, pi);
-  CHECK(assert_equal(numerical2,Dp,1e-5));
+  CHECK(assert_equal(numerical2, Dp, 1e-5));
 }
 
 /* ************************************************************************* */
-TEST(Cal3Unified, assert_equal)
-{
-  CHECK(assert_equal(K,K,1e-9));
-}
+TEST(Cal3Unified, Equal) { CHECK(assert_equal(K, K, 1e-9)); }
 
 /* ************************************************************************* */
-TEST(Cal3Unified, retract) {
-  Cal3Unified expected(100 + 2, 105 + 3, 0.0 + 4, 320 + 5, 240 + 6,
-      1e-3 + 7, 2.0*1e-3 + 8, 3.0*1e-3 + 9, 4.0*1e-3 + 10, 0.1 + 1);
+TEST(Cal3Unified, Retract) {
+  Cal3Unified expected(100 + 2, 105 + 3, 0.0 + 4, 320 + 5, 240 + 6, 1e-3 + 7,
+                       2.0 * 1e-3 + 8, 3.0 * 1e-3 + 9, 4.0 * 1e-3 + 10,
+                       0.1 + 1);
 
   EXPECT_LONGS_EQUAL(Cal3Unified::Dim(), 10);
   EXPECT_LONGS_EQUAL(expected.dim(), 10);
@@ -116,13 +111,12 @@ TEST(Cal3Unified, retract) {
   Vector10 d;
   d << 2, 3, 4, 5, 6, 7, 8, 9, 10, 1;
   Cal3Unified actual = K.retract(d);
-  CHECK(assert_equal(expected,actual,1e-9));
-  CHECK(assert_equal(d,K.localCoordinates(actual),1e-9));
+  CHECK(assert_equal(expected, actual, 1e-9));
+  CHECK(assert_equal(d, K.localCoordinates(actual), 1e-9));
 }
 
 /* ************************************************************************* */
-TEST(Cal3Unified, DerivedValue)
-{
+TEST(Cal3Unified, DerivedValue) {
   Values values;
   Cal3Unified cal(1, 2, 3, 4, 5, 6, 7, 8, 9, 10);
   Key key = 1;
@@ -130,7 +124,7 @@ TEST(Cal3Unified, DerivedValue)
 
   Cal3Unified calafter = values.at<Cal3Unified>(key);
 
-  CHECK(assert_equal(cal,calafter,1e-9));
+  CHECK(assert_equal(cal, calafter, 1e-9));
 }
 
 /* ************************************************************************* */
@@ -146,5 +140,8 @@ TEST(Cal3Unified, Print) {
 }
 
 /* ************************************************************************* */
-int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
+int main() {
+  TestResult tr;
+  return TestRegistry::runAllTests(tr);
+}
 /* ************************************************************************* */
diff --git a/gtsam/geometry/tests/testCal3_S2.cpp b/gtsam/geometry/tests/testCal3_S2.cpp
index 9b7941c91..41be5ea8e 100644
--- a/gtsam/geometry/tests/testCal3_S2.cpp
+++ b/gtsam/geometry/tests/testCal3_S2.cpp
@@ -34,27 +34,27 @@ static Point2 p_xy(2, 3);
 TEST(Cal3_S2, Constructor) {
   Cal3_S2 expected(554.256, 554.256, 0, 640 / 2, 480 / 2);
 
-  double fov = 60; // degrees
-  size_t w=640,h=480;
-  Cal3_S2 actual(fov,w,h);
+  double fov = 60;  // degrees
+  size_t w = 640, h = 480;
+  Cal3_S2 actual(fov, w, h);
 
-  CHECK(assert_equal(expected,actual,1e-3));
+  CHECK(assert_equal(expected, actual, 1e-3));
 }
 
 /* ************************************************************************* */
 TEST(Cal3_S2, Calibrate) {
-  Point2 intrinsic(2,3);
+  Point2 intrinsic(2, 3);
   Point2 expectedimage(1320.3, 1740);
   Point2 imagecoordinates = K.uncalibrate(intrinsic);
-  CHECK(assert_equal(expectedimage,imagecoordinates));
-  CHECK(assert_equal(intrinsic,K.calibrate(imagecoordinates)));
+  CHECK(assert_equal(expectedimage, imagecoordinates));
+  CHECK(assert_equal(intrinsic, K.calibrate(imagecoordinates)));
 }
 
 /* ************************************************************************* */
 TEST(Cal3_S2, CalibrateHomogeneous) {
   Vector3 intrinsic(2, 3, 1);
   Vector3 image(1320.3, 1740, 1);
-  CHECK(assert_equal((Vector)intrinsic,(Vector)K.calibrate(image)));
+  CHECK(assert_equal((Vector)intrinsic, (Vector)K.calibrate(image)));
 }
 
 /* ************************************************************************* */
@@ -63,16 +63,18 @@ Point2 uncalibrate_(const Cal3_S2& k, const Point2& pt) {
 }
 
 TEST(Cal3_S2, Duncalibrate1) {
-  Matrix25 computed; K.uncalibrate(p, computed, boost::none);
+  Matrix25 computed;
+  K.uncalibrate(p, computed, boost::none);
   Matrix numerical = numericalDerivative21(uncalibrate_, K, p);
-  CHECK(assert_equal(numerical,computed,1e-8));
+  CHECK(assert_equal(numerical, computed, 1e-8));
 }
 
 /* ************************************************************************* */
 TEST(Cal3_S2, Duncalibrate2) {
-  Matrix computed; K.uncalibrate(p, boost::none, computed);
+  Matrix computed;
+  K.uncalibrate(p, boost::none, computed);
   Matrix numerical = numericalDerivative22(uncalibrate_, K, p);
-  CHECK(assert_equal(numerical,computed,1e-9));
+  CHECK(assert_equal(numerical, computed, 1e-9));
 }
 
 Point2 calibrate_(const Cal3_S2& k, const Point2& pt) {
@@ -81,42 +83,42 @@ Point2 calibrate_(const Cal3_S2& k, const Point2& pt) {
 
 /* ************************************************************************* */
 TEST(Cal3_S2, Dcalibrate1) {
-    Matrix computed;
-    Point2 expected = K.calibrate(p_uv, computed, boost::none);
-    Matrix numerical = numericalDerivative21(calibrate_, K, p_uv);
-    CHECK(assert_equal(expected, p_xy, 1e-8));
-    CHECK(assert_equal(numerical, computed, 1e-8));
+  Matrix computed;
+  Point2 expected = K.calibrate(p_uv, computed, boost::none);
+  Matrix numerical = numericalDerivative21(calibrate_, K, p_uv);
+  CHECK(assert_equal(expected, p_xy, 1e-8));
+  CHECK(assert_equal(numerical, computed, 1e-8));
 }
 
 /* ************************************************************************* */
 TEST(Cal3_S2, Dcalibrate2) {
-    Matrix computed;
-    Point2 expected = K.calibrate(p_uv, boost::none, computed);
-    Matrix numerical = numericalDerivative22(calibrate_, K, p_uv);
-    CHECK(assert_equal(expected, p_xy, 1e-8));
-    CHECK(assert_equal(numerical, computed, 1e-8));
+  Matrix computed;
+  Point2 expected = K.calibrate(p_uv, boost::none, computed);
+  Matrix numerical = numericalDerivative22(calibrate_, K, p_uv);
+  CHECK(assert_equal(expected, p_xy, 1e-8));
+  CHECK(assert_equal(numerical, computed, 1e-8));
 }
 
 /* ************************************************************************* */
 TEST(Cal3_S2, Equal) {
-  CHECK(assert_equal(K,K,1e-9));
+  CHECK(assert_equal(K, K, 1e-9));
 
   Cal3_S2 K1(500, 500, 0.1, 640 / 2, 480 / 2);
-  CHECK(assert_equal(K,K1,1e-9));
+  CHECK(assert_equal(K, K1, 1e-9));
 }
 
 /* ************************************************************************* */
 TEST(Cal3_S2, Retract) {
-  Cal3_S2 expected(500+1, 500+2, 0.1+3, 640 / 2+4, 480 / 2+5);
+  Cal3_S2 expected(500 + 1, 500 + 2, 0.1 + 3, 640 / 2 + 4, 480 / 2 + 5);
 
   EXPECT_LONGS_EQUAL(Cal3_S2::Dim(), 5);
   EXPECT_LONGS_EQUAL(expected.dim(), 5);
 
   Vector5 d;
-  d << 1,2,3,4,5;
+  d << 1, 2, 3, 4, 5;
   Cal3_S2 actual = K.retract(d);
-  CHECK(assert_equal(expected,actual,1e-7));
-  CHECK(assert_equal(d,K.localCoordinates(actual),1e-7));
+  CHECK(assert_equal(expected, actual, 1e-7));
+  CHECK(assert_equal(d, K.localCoordinates(actual), 1e-7));
 }
 
 /* ************************************************************************* */
@@ -124,7 +126,7 @@ TEST(Cal3_S2, between) {
   Cal3_S2 k1(5, 5, 5, 5, 5), k2(5, 6, 7, 8, 9);
   Matrix H1, H2;
 
-  EXPECT(assert_equal(Cal3_S2(0,1,2,3,4), k1.between(k2, H1, H2)));
+  EXPECT(assert_equal(Cal3_S2(0, 1, 2, 3, 4), k1.between(k2, H1, H2)));
   EXPECT(assert_equal(-I_5x5, H1));
   EXPECT(assert_equal(I_5x5, H2));
 }
@@ -145,4 +147,3 @@ int main() {
   return TestRegistry::runAllTests(tr);
 }
 /* ************************************************************************* */
-
diff --git a/gtsam/geometry/tests/testCal3_S2Stereo.cpp b/gtsam/geometry/tests/testCal3_S2Stereo.cpp
index f823a8b97..e6a591b5f 100644
--- a/gtsam/geometry/tests/testCal3_S2Stereo.cpp
+++ b/gtsam/geometry/tests/testCal3_S2Stereo.cpp
@@ -31,7 +31,7 @@ static Point2 p_uv(1320.3, 1740);
 static Point2 p_xy(2, 3);
 
 /* ************************************************************************* */
-TEST(Cal3_S2Stereo, easy_constructor) {
+TEST(Cal3_S2Stereo, Constructor) {
   Cal3_S2Stereo expected(554.256, 554.256, 0, 640 / 2, 480 / 2, 3);
 
   double fov = 60;  // degrees
@@ -42,7 +42,7 @@ TEST(Cal3_S2Stereo, easy_constructor) {
 }
 
 /* ************************************************************************* */
-TEST(Cal3_S2Stereo, calibrate) {
+TEST(Cal3_S2Stereo, Calibrate) {
   Point2 intrinsic(2, 3);
   Point2 expectedimage(1320.3, 1740);
   Point2 imagecoordinates = K.uncalibrate(intrinsic);
@@ -51,56 +51,14 @@ TEST(Cal3_S2Stereo, calibrate) {
 }
 
 /* ************************************************************************* */
-TEST(Cal3_S2Stereo, calibrate_homogeneous) {
+TEST(Cal3_S2Stereo, CalibrateHomogeneous) {
   Vector3 intrinsic(2, 3, 1);
   Vector3 image(1320.3, 1740, 1);
   CHECK(assert_equal((Vector)intrinsic, (Vector)K.calibrate(image)));
 }
 
-//TODO(Varun) Add calibrate and uncalibrate methods
-// /* ************************************************************************* */
-// Point2 uncalibrate_(const Cal3_S2Stereo& k, const Point2& pt) {
-//   return k.uncalibrate(pt);
-// }
-
-// TEST(Cal3_S2Stereo, Duncalibrate1) {
-//   Matrix26 computed;
-//   K.uncalibrate(p, computed, boost::none);
-//   Matrix numerical = numericalDerivative21(uncalibrate_, K, p);
-//   CHECK(assert_equal(numerical, computed, 1e-8));
-// }
-
-// /* ************************************************************************* */
-// TEST(Cal3_S2Stereo, Duncalibrate2) {
-//   Matrix computed;
-//   K.uncalibrate(p, boost::none, computed);
-//   Matrix numerical = numericalDerivative22(uncalibrate_, K, p);
-//   CHECK(assert_equal(numerical, computed, 1e-9));
-// }
-
-// Point2 calibrate_(const Cal3_S2Stereo& k, const Point2& pt) {
-//   return k.calibrate(pt);
-// }
-// /* ************************************************************************* */
-// TEST(Cal3_S2Stereo, Dcalibrate1) {
-//   Matrix computed;
-//   Point2 expected = K.calibrate(p_uv, computed, boost::none);
-//   Matrix numerical = numericalDerivative21(calibrate_, K, p_uv);
-//   CHECK(assert_equal(expected, p_xy, 1e-8));
-//   CHECK(assert_equal(numerical, computed, 1e-8));
-// }
-
-// /* ************************************************************************* */
-// TEST(Cal3_S2Stereo, Dcalibrate2) {
-//   Matrix computed;
-//   Point2 expected = K.calibrate(p_uv, boost::none, computed);
-//   Matrix numerical = numericalDerivative22(calibrate_, K, p_uv);
-//   CHECK(assert_equal(expected, p_xy, 1e-8));
-//   CHECK(assert_equal(numerical, computed, 1e-8));
-// }
-
 /* ************************************************************************* */
-TEST(Cal3_S2Stereo, assert_equal) {
+TEST(Cal3_S2Stereo, Equal) {
   CHECK(assert_equal(K, K, 1e-9));
 
   Cal3_S2Stereo K1(500, 500, 0.1, 640 / 2, 480 / 2, 1);
@@ -108,7 +66,7 @@ TEST(Cal3_S2Stereo, assert_equal) {
 }
 
 /* ************************************************************************* */
-TEST(Cal3_S2Stereo, retract) {
+TEST(Cal3_S2Stereo, Retract) {
   Cal3_S2Stereo expected(500 + 1, 500 + 2, 0.1 + 3, 640 / 2 + 4, 480 / 2 + 5,
                          7);
   EXPECT_LONGS_EQUAL(Cal3_S2Stereo::Dim(), 6);

From 3ddc999f43e1bc7add36ab9e115f1b14b6c21548 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Wed, 2 Dec 2020 17:24:21 -0500
Subject: [PATCH 18/18] additional formatting

---
 gtsam/geometry/Cal3Unified.cpp | 26 +++++++++++---------------
 1 file changed, 11 insertions(+), 15 deletions(-)

diff --git a/gtsam/geometry/Cal3Unified.cpp b/gtsam/geometry/Cal3Unified.cpp
index 80613bbf2..11aabcaa7 100644
--- a/gtsam/geometry/Cal3Unified.cpp
+++ b/gtsam/geometry/Cal3Unified.cpp
@@ -16,10 +16,10 @@
  * @author Varun Agrawal
  */
 
-#include <gtsam/base/Vector.h>
 #include <gtsam/base/Matrix.h>
-#include <gtsam/geometry/Point2.h>
+#include <gtsam/base/Vector.h>
 #include <gtsam/geometry/Cal3Unified.h>
+#include <gtsam/geometry/Point2.h>
 
 #include <cmath>
 
@@ -53,10 +53,8 @@ bool Cal3Unified::equals(const Cal3Unified& K, double tol) const {
 
 /* ************************************************************************* */
 // todo: make a fixed sized jacobian version of this
-Point2 Cal3Unified::uncalibrate(const Point2& p,
-       OptionalJacobian<2,10> Dcal,
-       OptionalJacobian<2,2> Dp) const {
-
+Point2 Cal3Unified::uncalibrate(const Point2& p, OptionalJacobian<2, 10> Dcal,
+                                OptionalJacobian<2, 2> Dp) const {
   // this part of code is modified from Cal3DS2,
   // since the second part of this model (after project to normalized plane)
   // is same as Cal3DS2
@@ -69,19 +67,19 @@ Point2 Cal3Unified::uncalibrate(const Point2& p,
   const double sqrt_nx = sqrt(xs * xs + ys * ys + 1.0);
   const double xi_sqrt_nx = 1.0 / (1 + xi * sqrt_nx);
   const double xi_sqrt_nx2 = xi_sqrt_nx * xi_sqrt_nx;
-  const double x = xs * xi_sqrt_nx, y = ys * xi_sqrt_nx; // points on NPlane
+  const double x = xs * xi_sqrt_nx, y = ys * xi_sqrt_nx;  // points on NPlane
 
   // Part2: project NPlane point to pixel plane: use Cal3DS2
-  Point2 m(x,y);
+  Point2 m(x, y);
   Matrix29 H1base;
-  Matrix2 H2base;    // jacobians from Base class
+  Matrix2 H2base;  // jacobians from Base class
   Point2 puncalib = Base::uncalibrate(m, H1base, H2base);
 
   // Inlined derivative for calibration
   if (Dcal) {
     // part1
     Vector2 DU;
-    DU << -xs * sqrt_nx * xi_sqrt_nx2, //
+    DU << -xs * sqrt_nx * xi_sqrt_nx2,  //
         -ys * sqrt_nx * xi_sqrt_nx2;
     *Dcal << H1base, H2base * DU;
   }
@@ -90,10 +88,10 @@ Point2 Cal3Unified::uncalibrate(const Point2& p,
   if (Dp) {
     // part1
     const double denom = 1.0 * xi_sqrt_nx2 / sqrt_nx;
-    const double mid = -(xi * xs*ys) * denom;
+    const double mid = -(xi * xs * ys) * denom;
     Matrix2 DU;
-    DU << (sqrt_nx + xi*(ys*ys + 1)) * denom, mid, //
-        mid, (sqrt_nx + xi*(xs*xs + 1)) * denom;
+    DU << (sqrt_nx + xi * (ys * ys + 1)) * denom, mid,  //
+        mid, (sqrt_nx + xi * (xs * xs + 1)) * denom;
 
     *Dp << H2base * DU;
   }
@@ -116,7 +114,6 @@ Point2 Cal3Unified::calibrate(const Point2& pi, OptionalJacobian<2, 10> Dcal,
 }
 /* ************************************************************************* */
 Point2 Cal3Unified::nPlaneToSpace(const Point2& p) const {
-
   const double x = p.x(), y = p.y();
   const double xy2 = x * x + y * y;
   const double sq_xy = (xi_ + sqrt(1 + (1 - xi_ * xi_) * xy2)) / (xy2 + 1);
@@ -126,7 +123,6 @@ Point2 Cal3Unified::nPlaneToSpace(const Point2& p) const {
 
 /* ************************************************************************* */
 Point2 Cal3Unified::spaceToNPlane(const Point2& p) const {
-
   const double x = p.x(), y = p.y();
   const double sq_xy = 1 + xi_ * sqrt(x * x + y * y + 1);