From e1c3314e485b82ba231ea853ac8da01a1d43d14d Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Mon, 30 Nov 2020 10:35:34 -0500
Subject: [PATCH 1/4] Jacobians for Camera models

- Add jacobians for calibrate function using implicit function theorem
- Consistent naming of jacobian parameters
- Added tests for jacobians
- Some simple formatting
- Fixed docs for implicit function theorem
- Added parentheses to conform with Google style
---
 gtsam/geometry/Cal3Bundler.cpp           |  4 +-
 gtsam/geometry/Cal3DS2.h                 |  4 +-
 gtsam/geometry/Cal3DS2_Base.cpp          | 72 ++++++++++++++++++------
 gtsam/geometry/Cal3DS2_Base.h            | 42 ++++++++++----
 gtsam/geometry/Cal3Unified.cpp           | 42 ++++++++++----
 gtsam/geometry/Cal3Unified.h             |  4 +-
 gtsam/geometry/tests/testCal3DS2.cpp     | 22 ++++++--
 gtsam/geometry/tests/testCal3Unified.cpp | 16 ++++++
 8 files changed, 159 insertions(+), 47 deletions(-)

diff --git a/gtsam/geometry/Cal3Bundler.cpp b/gtsam/geometry/Cal3Bundler.cpp
index b198643b0..36e7bf62d 100644
--- a/gtsam/geometry/Cal3Bundler.cpp
+++ b/gtsam/geometry/Cal3Bundler.cpp
@@ -124,8 +124,8 @@ Point2 Cal3Bundler::calibrate(const Point2& pi,
   // We make use of the Implicit Function Theorem to compute the Jacobians from uncalibrate
   // 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)
-  // Dcal = -inv(H_uncal_pn) * df/pi = -inv(H_uncal_pn) * (-I) = inv(H_uncal_pn)
-  // Dp = -inv(H_uncal_pn) * df/K = -inv(H_uncal_pn) * H_uncal_K
+  // Dp = -inv(H_uncal_pn) * df/pi = -inv(H_uncal_pn) * (-I) = inv(H_uncal_pn)
+  // Dcal = -inv(H_uncal_pn) * df/K = -inv(H_uncal_pn) * H_uncal_K
   Matrix23 H_uncal_K;
   Matrix22 H_uncal_pn, H_uncal_pn_inv;
 
diff --git a/gtsam/geometry/Cal3DS2.h b/gtsam/geometry/Cal3DS2.h
index 7fd453d45..e66c3d124 100644
--- a/gtsam/geometry/Cal3DS2.h
+++ b/gtsam/geometry/Cal3DS2.h
@@ -44,8 +44,8 @@ public:
   Cal3DS2() : Base() {}
 
   Cal3DS2(double fx, double fy, double s, double u0, double v0,
-      double k1, double k2, double p1 = 0.0, double p2 = 0.0) :
-        Base(fx, fy, s, u0, v0, k1, k2, p1, p2) {}
+      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() {}
 
diff --git a/gtsam/geometry/Cal3DS2_Base.cpp b/gtsam/geometry/Cal3DS2_Base.cpp
index 6c03883ce..c5ef117a7 100644
--- a/gtsam/geometry/Cal3DS2_Base.cpp
+++ b/gtsam/geometry/Cal3DS2_Base.cpp
@@ -13,6 +13,7 @@
  * @file Cal3DS2_Base.cpp
  * @date Feb 28, 2010
  * @author ydjian
+ * @author Varun Agrawal
  */
 
 #include <gtsam/base/Vector.h>
@@ -24,8 +25,17 @@
 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]){}
+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)),
+      tol_(1e-5) {}
 
 /* ************************************************************************* */
 Matrix3 Cal3DS2_Base::K() const {
@@ -94,9 +104,8 @@ static Matrix2 D2dintrinsic(double x, double y, double rr,
 }
 
 /* ************************************************************************* */
-Point2 Cal3DS2_Base::uncalibrate(const Point2& p,
-    OptionalJacobian<2,9> H1, OptionalJacobian<2,2> H2) const {
-
+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)];
@@ -115,37 +124,44 @@ Point2 Cal3DS2_Base::uncalibrate(const Point2& p,
   const double pny = g * y + dy;
 
   Matrix2 DK;
-  if (H1 || H2) DK << fx_, s_, 0.0, fy_;
+  if (Dcal || Dp) {
+    DK << fx_, s_, 0.0, fy_;
+  }
 
   // Derivative for calibration
-  if (H1)
-    *H1 = D2dcalibration(x, y, xx, yy, xy, rr, r4, pnx, pny, DK);
+  if (Dcal) {
+    *Dcal = D2dcalibration(x, y, xx, yy, xy, rr, r4, pnx, pny, DK);
+  }
 
   // Derivative for points
-  if (H2)
-    *H2 = D2dintrinsic(x, y, rr, g, k1_, k2_, p1_, p2_, DK);
+  if (Dp) {
+    *Dp = D2dintrinsic(x, y, rr, g, k1_, k2_, p1_, p2_, DK);
+  }
 
   // Regular uncalibrate after distortion
   return Point2(fx_ * pnx + s_ * pny + u0_, fy_ * pny + v0_);
 }
 
 /* ************************************************************************* */
-Point2 Cal3DS2_Base::calibrate(const Point2& pi, const double tol) const {
+Point2 Cal3DS2_Base::calibrate(const Point2& pi, OptionalJacobian<2, 9> Dcal,
+                               OptionalJacobian<2, 2> Dp) const {
   // Use the following fixed point iteration to invert the radial distortion.
   // pn_{t+1} = (inv(K)*pi - dp(pn_{t})) / g(pn_{t})
 
-  const Point2 invKPi ((1 / fx_) * (pi.x() - u0_ - (s_ / fy_) * (pi.y() - v0_)),
-                       (1 / fy_) * (pi.y() - v0_));
+  const Point2 invKPi((1 / fx_) * (pi.x() - u0_ - (s_ / fy_) * (pi.y() - v0_)),
+                      (1 / fy_) * (pi.y() - v0_));
 
-  // 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 = invKPi;
 
   // 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;
-    const double x = pn.x(), y = pn.y(), xy = x * y, xx = x * x, yy = y * y;
+    if (distance2(uncalibrate(pn), pi) <= tol_) break;
+    const double px = pn.x(), py = pn.y(), xy = px * py, xx = px * px,
+                 yy = py * py;
     const double rr = xx + yy;
     const double g = (1 + k1_ * rr + k2_ * rr * rr);
     const double dx = 2 * p1_ * xy + p2_ * (rr + 2 * xx);
@@ -153,8 +169,28 @@ Point2 Cal3DS2_Base::calibrate(const Point2& pi, const double tol) const {
     pn = (invKPi - Point2(dx, dy)) / g;
   }
 
-  if ( iteration >= maxIterations )
-    throw std::runtime_error("Cal3DS2::calibrate fails to converge. need a better initialization");
+  if (iteration >= maxIterations)
+    throw std::runtime_error(
+        "Cal3DS2::calibrate fails to converge. need a better initialization");
+
+  // We make use of the Implicit Function Theorem to compute the Jacobians from uncalibrate
+  // 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)
+  // Dp = -inv(H_uncal_pn) * df/pi = -inv(H_uncal_pn) * (-I) = inv(H_uncal_pn)
+  // Dcal = -inv(H_uncal_pn) * df/K = -inv(H_uncal_pn) * H_uncal_K
+  Matrix29 H_uncal_K;
+  Matrix22 H_uncal_pn, H_uncal_pn_inv;
+
+  if (Dcal || Dp) {
+    // Compute uncalibrate Jacobians
+    uncalibrate(pn, Dcal ? &H_uncal_K : nullptr, H_uncal_pn);
+
+    H_uncal_pn_inv = H_uncal_pn.inverse();
+
+    if (Dp) *Dp = H_uncal_pn_inv;
+    if (Dcal) *Dcal = -H_uncal_pn_inv * H_uncal_K;
+
+  }
 
   return pn;
 }
diff --git a/gtsam/geometry/Cal3DS2_Base.h b/gtsam/geometry/Cal3DS2_Base.h
index a0ece8bdb..b6d27cda1 100644
--- a/gtsam/geometry/Cal3DS2_Base.h
+++ b/gtsam/geometry/Cal3DS2_Base.h
@@ -14,6 +14,7 @@
  * @brief Calibration of a camera with radial distortion
  * @date Feb 28, 2010
  * @author ydjian
+ * @author Varun Agrawal
  */
 
 #pragma once
@@ -43,18 +44,38 @@ 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_; // tolerance value when calibrating
 
 public:
 
   /// @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) {}
+ /// 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) {}
 
-  Cal3DS2_Base(double fx, double fy, double s, double u0, double v0,
-      double k1, double k2, double p1 = 0.0, double p2 = 0.0) :
-  fx_(fx), fy_(fy), s_(s), u0_(u0), v0_(v0), k1_(k1), k2_(k2), p1_(p1), p2_(p2) {}
+ 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) {}
 
   virtual ~Cal3DS2_Base() {}
 
@@ -72,7 +93,7 @@ public:
   virtual void print(const std::string& s = "") const;
 
   /// assert equality up to a tolerance
-  bool equals(const Cal3DS2_Base& K, double tol = 10e-9) const;
+  bool equals(const Cal3DS2_Base& K, double tol = 1e-8) const;
 
   /// @}
   /// @name Standard Interface
@@ -121,12 +142,12 @@ public:
    * @param Dp optional 2*2 Jacobian wrpt intrinsic coordinates
    * @return point in (distorted) image coordinates
    */
-  Point2 uncalibrate(const Point2& p,
-       OptionalJacobian<2,9> Dcal = boost::none,
-       OptionalJacobian<2,2> Dp = boost::none) const ;
+  Point2 uncalibrate(const Point2& p, OptionalJacobian<2, 9> Dcal = boost::none,
+                     OptionalJacobian<2, 2> Dp = boost::none) const;
 
   /// Convert (distorted) image coordinates uv to intrinsic coordinates xy
-  Point2 calibrate(const Point2& p, const double tol=1e-5) const;
+  Point2 calibrate(const Point2& p, OptionalJacobian<2, 9> Dcal = boost::none,
+                   OptionalJacobian<2, 2> Dp = boost::none) const;
 
   /// Derivative of uncalibrate wrpt intrinsic coordinates
   Matrix2 D2d_intrinsic(const Point2& p) const ;
@@ -164,6 +185,7 @@ private:
     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 b1b9c3722..247e77ae1 100644
--- a/gtsam/geometry/Cal3Unified.cpp
+++ b/gtsam/geometry/Cal3Unified.cpp
@@ -13,6 +13,7 @@
  * @file Cal3Unified.cpp
  * @date Mar 8, 2014
  * @author Jing Dong
+ * @author Varun Agrawal
  */
 
 #include <gtsam/base/Vector.h>
@@ -54,8 +55,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> H1,
-       OptionalJacobian<2,2> H2) const {
+       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)
@@ -78,16 +79,16 @@ Point2 Cal3Unified::uncalibrate(const Point2& p,
   Point2 puncalib = Base::uncalibrate(m, H1base, H2base);
 
   // Inlined derivative for calibration
-  if (H1) {
+  if (Dcal) {
     // part1
     Vector2 DU;
     DU << -xs * sqrt_nx * xi_sqrt_nx2, //
         -ys * sqrt_nx * xi_sqrt_nx2;
-    *H1 << H1base, H2base * DU;
+    *Dcal << H1base, H2base * DU;
   }
 
   // Inlined derivative for points
-  if (H2) {
+  if (Dp) {
     // part1
     const double denom = 1.0 * xi_sqrt_nx2 / sqrt_nx;
     const double mid = -(xi * xs*ys) * denom;
@@ -95,20 +96,41 @@ Point2 Cal3Unified::uncalibrate(const Point2& p,
     DU << (sqrt_nx + xi*(ys*ys + 1)) * denom, mid, //
         mid, (sqrt_nx + xi*(xs*xs + 1)) * denom;
 
-    *H2 << H2base * DU;
+    *Dp << H2base * DU;
   }
 
   return puncalib;
 }
 
 /* ************************************************************************* */
-Point2 Cal3Unified::calibrate(const Point2& pi, const double tol) const {
-
+Point2 Cal3Unified::calibrate(const Point2& pi, OptionalJacobian<2, 10> Dcal,
+                              OptionalJacobian<2, 2> Dp) const {
   // calibrate point to Nplane use base class::calibrate()
-  Point2 pnplane = Base::calibrate(pi, tol);
+  Point2 pnplane = Base::calibrate(pi);
 
   // call nplane to space
-  return this->nPlaneToSpace(pnplane);
+  Point2 pn = this->nPlaneToSpace(pnplane);
+
+  // We make use of the Implicit Function Theorem to compute the Jacobians from uncalibrate
+  // 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)
+  // Dp = -inv(H_uncal_pn) * df/pi = -inv(H_uncal_pn) * (-I) = inv(H_uncal_pn)
+  // Dcal = -inv(H_uncal_pn) * df/K = -inv(H_uncal_pn) * H_uncal_K
+  Eigen::Matrix<double, 2, 10> H_uncal_K;
+  Matrix22 H_uncal_pn, H_uncal_pn_inv;
+
+  if (Dcal || Dp) {
+    // Compute uncalibrate Jacobians
+    uncalibrate(pn, Dcal ? &H_uncal_K : nullptr, H_uncal_pn);
+
+    H_uncal_pn_inv = H_uncal_pn.inverse();
+
+    if (Dp) *Dp = H_uncal_pn_inv;
+    if (Dcal) *Dcal = -H_uncal_pn_inv * H_uncal_K;
+
+  }
+
+  return pn;
 }
 /* ************************************************************************* */
 Point2 Cal3Unified::nPlaneToSpace(const Point2& p) const {
diff --git a/gtsam/geometry/Cal3Unified.h b/gtsam/geometry/Cal3Unified.h
index 381405d20..6fc37b0d1 100644
--- a/gtsam/geometry/Cal3Unified.h
+++ b/gtsam/geometry/Cal3Unified.h
@@ -14,6 +14,7 @@
  * @brief Unified Calibration Model, see Mei07icra for details
  * @date Mar 8, 2014
  * @author Jing Dong
+ * @author Varun Agrawal
  */
 
 /**
@@ -99,7 +100,8 @@ public:
       OptionalJacobian<2,2> Dp = boost::none) const ;
 
   /// Conver a pixel coordinate to ideal coordinate
-  Point2 calibrate(const Point2& p, const double tol=1e-5) const;
+  Point2 calibrate(const Point2& p, OptionalJacobian<2, 10> Dcal = boost::none,
+                   OptionalJacobian<2, 2> Dp = boost::none) const;
 
   /// Convert a 3D point to normalized unit plane
   Point2 spaceToNPlane(const Point2& p) const;
diff --git a/gtsam/geometry/tests/testCal3DS2.cpp b/gtsam/geometry/tests/testCal3DS2.cpp
index 416665d46..beed09883 100644
--- a/gtsam/geometry/tests/testCal3DS2.cpp
+++ b/gtsam/geometry/tests/testCal3DS2.cpp
@@ -74,12 +74,26 @@ TEST( Cal3DS2, Duncalibrate2)
   CHECK(assert_equal(numerical,separate,1e-5));
 }
 
-/* ************************************************************************* */
-TEST( Cal3DS2, assert_equal)
-{
-  CHECK(assert_equal(K,K,1e-5));
+Point2 calibrate_(const Cal3DS2& k, const Point2& pt) {
+  return k.calibrate(pt);
 }
 
+/* ************************************************************************* */
+TEST( Cal3DS2, Dcalibrate)
+{
+  Point2 pn(0.5, 0.5);
+  Point2 pi = K.uncalibrate(pn);
+  Matrix Dcal, Dp;
+  K.calibrate(pi, Dcal, Dp);
+  Matrix numerical1 = numericalDerivative21(calibrate_, K, pi, 1e-7);
+  CHECK(assert_equal(numerical1, Dcal, 1e-5));
+  Matrix numerical2 = numericalDerivative22(calibrate_, K, pi, 1e-7);
+  CHECK(assert_equal(numerical2, Dp, 1e-5));
+}
+
+/* ************************************************************************* */
+TEST(Cal3DS2, assert_equal) { CHECK(assert_equal(K, K, 1e-5)); }
+
 /* ************************************************************************* */
 TEST( Cal3DS2, retract)
 {
diff --git a/gtsam/geometry/tests/testCal3Unified.cpp b/gtsam/geometry/tests/testCal3Unified.cpp
index 2c5ffd7fb..8abb6fe04 100644
--- a/gtsam/geometry/tests/testCal3Unified.cpp
+++ b/gtsam/geometry/tests/testCal3Unified.cpp
@@ -82,6 +82,22 @@ TEST( Cal3Unified, Duncalibrate2)
   CHECK(assert_equal(numerical,computed,1e-6));
 }
 
+Point2 calibrate_(const Cal3Unified& k, const Point2& pt) {
+  return k.calibrate(pt);
+}
+
+/* ************************************************************************* */
+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));
+  Matrix numerical2 = numericalDerivative22(calibrate_, K, pi);
+  CHECK(assert_equal(numerical2,Dp,1e-5));
+}
+
 /* ************************************************************************* */
 TEST( Cal3Unified, assert_equal)
 {

From cb3a766b30677d662f462a2ee2086a8fb329c892 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Mon, 30 Nov 2020 13:19:13 -0500
Subject: [PATCH 2/4] uncomment calibration applications

---
 gtsam/geometry/SimpleCamera.h         |  5 ++---
 gtsam/gtsam.i                         | 10 ++++------
 gtsam_unstable/slam/serialization.cpp |  6 ++----
 tests/testSerializationSLAM.cpp       |  6 ++----
 4 files changed, 10 insertions(+), 17 deletions(-)

diff --git a/gtsam/geometry/SimpleCamera.h b/gtsam/geometry/SimpleCamera.h
index 04746ba6f..aa00222c7 100644
--- a/gtsam/geometry/SimpleCamera.h
+++ b/gtsam/geometry/SimpleCamera.h
@@ -31,9 +31,8 @@ namespace gtsam {
   /// Also needed as forward declarations in the wrapper.
   using PinholeCameraCal3_S2 = gtsam::PinholeCamera<gtsam::Cal3_S2>;
   using PinholeCameraCal3Bundler = gtsam::PinholeCamera<gtsam::Cal3Bundler>;
-  //TODO Need to fix issue 621 for this to work with wrapper
-  // using PinholeCameraCal3DS2 = gtsam::PinholeCamera<gtsam::Cal3DS2>;
-  // using PinholeCameraCal3Unified = gtsam::PinholeCamera<gtsam::Cal3Unified>;
+  using PinholeCameraCal3DS2 = gtsam::PinholeCamera<gtsam::Cal3DS2>;
+  using PinholeCameraCal3Unified = gtsam::PinholeCamera<gtsam::Cal3Unified>;
 
 #ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V41
 /**
diff --git a/gtsam/gtsam.i b/gtsam/gtsam.i
index eb36e73a3..2e1920641 100644
--- a/gtsam/gtsam.i
+++ b/gtsam/gtsam.i
@@ -881,7 +881,7 @@ virtual class Cal3DS2_Base {
 
   // Action on Point2
   gtsam::Point2 uncalibrate(const gtsam::Point2& p) const;
-  gtsam::Point2 calibrate(const gtsam::Point2& p, double tol) const;
+  gtsam::Point2 calibrate(const gtsam::Point2& p) const;
 
   // enabling serialization functionality
   void serialize() const;
@@ -1064,9 +1064,8 @@ class PinholeCamera {
 // Some typedefs for common camera types
 // PinholeCameraCal3_S2 is the same as SimpleCamera above
 typedef gtsam::PinholeCamera<gtsam::Cal3_S2> PinholeCameraCal3_S2;
-//TODO (Issue 621) due to lack of jacobians of Cal3DS2_Base::calibrate, PinholeCamera does not apply to Cal3DS2/Unified
-//typedef gtsam::PinholeCamera<gtsam::Cal3DS2> PinholeCameraCal3DS2;
-//typedef gtsam::PinholeCamera<gtsam::Cal3Unified> PinholeCameraCal3Unified;
+typedef gtsam::PinholeCamera<gtsam::Cal3DS2> PinholeCameraCal3DS2;
+typedef gtsam::PinholeCamera<gtsam::Cal3Unified> PinholeCameraCal3Unified;
 typedef gtsam::PinholeCamera<gtsam::Cal3Bundler> PinholeCameraCal3Bundler;
 
 #include <gtsam/geometry/StereoCamera.h>
@@ -2634,8 +2633,7 @@ virtual class GeneralSFMFactor : gtsam::NoiseModelFactor {
   gtsam::Point2 measured() const;
 };
 typedef gtsam::GeneralSFMFactor<gtsam::PinholeCameraCal3_S2, gtsam::Point3> GeneralSFMFactorCal3_S2;
-//TODO (Issue 621) due to lack of jacobians of Cal3DS2_Base::calibrate, GeneralSFMFactor does not apply to Cal3DS2
-//typedef gtsam::GeneralSFMFactor<gtsam::PinholeCameraCal3DS2, gtsam::Point3> GeneralSFMFactorCal3DS2;
+typedef gtsam::GeneralSFMFactor<gtsam::PinholeCameraCal3DS2, gtsam::Point3> GeneralSFMFactorCal3DS2;
 typedef gtsam::GeneralSFMFactor<gtsam::PinholeCamera<gtsam::Cal3Bundler>, gtsam::Point3> GeneralSFMFactorCal3Bundler;
 
 template<CALIBRATION = {gtsam::Cal3_S2}>
diff --git a/gtsam_unstable/slam/serialization.cpp b/gtsam_unstable/slam/serialization.cpp
index 803e4353a..88a94fd51 100644
--- a/gtsam_unstable/slam/serialization.cpp
+++ b/gtsam_unstable/slam/serialization.cpp
@@ -86,8 +86,7 @@ typedef GenericProjectionFactor<Pose3, Point3, Cal3_S2> GenericProjectionFactorC
 typedef GenericProjectionFactor<Pose3, Point3, Cal3DS2> GenericProjectionFactorCal3DS2;
 
 typedef gtsam::GeneralSFMFactor<gtsam::PinholeCameraCal3_S2, gtsam::Point3> GeneralSFMFactorCal3_S2;
-//TODO fix issue 621
-//typedef gtsam::GeneralSFMFactor<gtsam::PinholeCameraCal3DS2, gtsam::Point3> GeneralSFMFactorCal3DS2;
+typedef gtsam::GeneralSFMFactor<gtsam::PinholeCameraCal3DS2, gtsam::Point3> GeneralSFMFactorCal3DS2;
 
 typedef gtsam::GeneralSFMFactor2<gtsam::Cal3_S2> GeneralSFMFactor2Cal3_S2;
 
@@ -184,8 +183,7 @@ BOOST_CLASS_EXPORT_GUID(GenericProjectionFactorCal3_S2, "gtsam::GenericProjectio
 BOOST_CLASS_EXPORT_GUID(GenericProjectionFactorCal3DS2, "gtsam::GenericProjectionFactorCal3DS2");
 
 BOOST_CLASS_EXPORT_GUID(GeneralSFMFactorCal3_S2, "gtsam::GeneralSFMFactorCal3_S2");
-//TODO Fix issue 621
-//BOOST_CLASS_EXPORT_GUID(GeneralSFMFactorCal3DS2, "gtsam::GeneralSFMFactorCal3DS2");
+BOOST_CLASS_EXPORT_GUID(GeneralSFMFactorCal3DS2, "gtsam::GeneralSFMFactorCal3DS2");
 
 BOOST_CLASS_EXPORT_GUID(GeneralSFMFactor2Cal3_S2, "gtsam::GeneralSFMFactor2Cal3_S2");
 
diff --git a/tests/testSerializationSLAM.cpp b/tests/testSerializationSLAM.cpp
index 53086e921..2e99aff71 100644
--- a/tests/testSerializationSLAM.cpp
+++ b/tests/testSerializationSLAM.cpp
@@ -100,8 +100,7 @@ typedef GenericProjectionFactor<Pose3, Point3, Cal3_S2> GenericProjectionFactorC
 typedef GenericProjectionFactor<Pose3, Point3, Cal3DS2> GenericProjectionFactorCal3DS2;
 
 typedef gtsam::GeneralSFMFactor<gtsam::PinholeCameraCal3_S2, gtsam::Point3> GeneralSFMFactorCal3_S2;
-//TODO Fix issue 621 for this to work
-//typedef gtsam::GeneralSFMFactor<gtsam::PinholeCameraCal3DS2, gtsam::Point3> GeneralSFMFactorCal3DS2;
+typedef gtsam::GeneralSFMFactor<gtsam::PinholeCameraCal3DS2, gtsam::Point3> GeneralSFMFactorCal3DS2;
 
 typedef gtsam::GeneralSFMFactor2<gtsam::Cal3_S2> GeneralSFMFactor2Cal3_S2;
 
@@ -198,8 +197,7 @@ BOOST_CLASS_EXPORT_GUID(GenericProjectionFactorCal3_S2, "gtsam::GenericProjectio
 BOOST_CLASS_EXPORT_GUID(GenericProjectionFactorCal3DS2, "gtsam::GenericProjectionFactorCal3DS2");
 
 BOOST_CLASS_EXPORT_GUID(GeneralSFMFactorCal3_S2, "gtsam::GeneralSFMFactorCal3_S2");
-//TODO fix issue 621
-//BOOST_CLASS_EXPORT_GUID(GeneralSFMFactorCal3DS2, "gtsam::GeneralSFMFactorCal3DS2");
+BOOST_CLASS_EXPORT_GUID(GeneralSFMFactorCal3DS2, "gtsam::GeneralSFMFactorCal3DS2");
 
 BOOST_CLASS_EXPORT_GUID(GeneralSFMFactor2Cal3_S2, "gtsam::GeneralSFMFactor2Cal3_S2");
 

From 012820b7fa6d7c5f5339036f6f2da38e9335e3ac Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Tue, 1 Dec 2020 00:39:32 -0500
Subject: [PATCH 3/4] Common function to compute Jacobians of calibrate method

---
 gtsam/geometry/Cal3Bundler.cpp            | 19 +---------
 gtsam/geometry/Cal3Bundler.h              |  1 +
 gtsam/geometry/Cal3DS2_Base.cpp           | 22 ++----------
 gtsam/geometry/Cal3DS2_Base.h             | 44 ++++++++++++++++++++---
 gtsam/geometry/Cal3Fisheye.cpp            |  9 +++--
 gtsam/geometry/Cal3Fisheye.h              | 13 ++++---
 gtsam/geometry/Cal3Unified.cpp            | 19 +---------
 gtsam/geometry/tests/testCal3DFisheye.cpp | 17 +++++++++
 8 files changed, 76 insertions(+), 68 deletions(-)

diff --git a/gtsam/geometry/Cal3Bundler.cpp b/gtsam/geometry/Cal3Bundler.cpp
index 36e7bf62d..a73bfec52 100644
--- a/gtsam/geometry/Cal3Bundler.cpp
+++ b/gtsam/geometry/Cal3Bundler.cpp
@@ -121,24 +121,7 @@ Point2 Cal3Bundler::calibrate(const Point2& pi,
     throw std::runtime_error(
         "Cal3Bundler::calibrate fails to converge. need a better initialization");
 
-  // We make use of the Implicit Function Theorem to compute the Jacobians from uncalibrate
-  // 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)
-  // Dp = -inv(H_uncal_pn) * df/pi = -inv(H_uncal_pn) * (-I) = inv(H_uncal_pn)
-  // Dcal = -inv(H_uncal_pn) * df/K = -inv(H_uncal_pn) * H_uncal_K
-  Matrix23 H_uncal_K;
-  Matrix22 H_uncal_pn, H_uncal_pn_inv;
-
-  if (Dcal || Dp) {
-    // Compute uncalibrate Jacobians
-    uncalibrate(pn, Dcal ? &H_uncal_K : nullptr, H_uncal_pn);
-
-    H_uncal_pn_inv = H_uncal_pn.inverse();
-
-    if (Dp) *Dp = H_uncal_pn_inv;
-    if (Dcal) *Dcal = -H_uncal_pn_inv * H_uncal_K;
-
-  }
+  calibrateJacobians<Cal3Bundler, dimension>(*this, pn, Dcal, Dp);
 
   return pn;
 }
diff --git a/gtsam/geometry/Cal3Bundler.h b/gtsam/geometry/Cal3Bundler.h
index 2e3fab002..da43112d9 100644
--- a/gtsam/geometry/Cal3Bundler.h
+++ b/gtsam/geometry/Cal3Bundler.h
@@ -18,6 +18,7 @@
 
 #pragma once
 
+#include <gtsam/geometry/Cal3DS2_Base.h>
 #include <gtsam/geometry/Point2.h>
 
 namespace gtsam {
diff --git a/gtsam/geometry/Cal3DS2_Base.cpp b/gtsam/geometry/Cal3DS2_Base.cpp
index c5ef117a7..d175259f2 100644
--- a/gtsam/geometry/Cal3DS2_Base.cpp
+++ b/gtsam/geometry/Cal3DS2_Base.cpp
@@ -34,8 +34,7 @@ Cal3DS2_Base::Cal3DS2_Base(const Vector& v)
       k1_(v(5)),
       k2_(v(6)),
       p1_(v(7)),
-      p2_(v(8)),
-      tol_(1e-5) {}
+      p2_(v(8)) {}
 
 /* ************************************************************************* */
 Matrix3 Cal3DS2_Base::K() const {
@@ -173,24 +172,7 @@ Point2 Cal3DS2_Base::calibrate(const Point2& pi, OptionalJacobian<2, 9> Dcal,
     throw std::runtime_error(
         "Cal3DS2::calibrate fails to converge. need a better initialization");
 
-  // We make use of the Implicit Function Theorem to compute the Jacobians from uncalibrate
-  // 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)
-  // Dp = -inv(H_uncal_pn) * df/pi = -inv(H_uncal_pn) * (-I) = inv(H_uncal_pn)
-  // Dcal = -inv(H_uncal_pn) * df/K = -inv(H_uncal_pn) * H_uncal_K
-  Matrix29 H_uncal_K;
-  Matrix22 H_uncal_pn, H_uncal_pn_inv;
-
-  if (Dcal || Dp) {
-    // Compute uncalibrate Jacobians
-    uncalibrate(pn, Dcal ? &H_uncal_K : nullptr, H_uncal_pn);
-
-    H_uncal_pn_inv = H_uncal_pn.inverse();
-
-    if (Dp) *Dp = H_uncal_pn_inv;
-    if (Dcal) *Dcal = -H_uncal_pn_inv * H_uncal_K;
-
-  }
+  calibrateJacobians<Cal3DS2_Base, dimension>(*this, pn, Dcal, Dp);
 
   return pn;
 }
diff --git a/gtsam/geometry/Cal3DS2_Base.h b/gtsam/geometry/Cal3DS2_Base.h
index b6d27cda1..c9b53c29b 100644
--- a/gtsam/geometry/Cal3DS2_Base.h
+++ b/gtsam/geometry/Cal3DS2_Base.h
@@ -23,6 +23,39 @@
 
 namespace gtsam {
 
+/**
+ * Function which makes use of the Implicit Function Theorem to compute the
+ * Jacobians of `calibrate` using `uncalibrate`.
+ * 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)
+ * Dp = -inv(H_uncal_pn) * df/pi = -inv(H_uncal_pn) * (-I) = inv(H_uncal_pn)
+ * Dcal = -inv(H_uncal_pn) * df/K = -inv(H_uncal_pn) * H_uncal_K
+ *
+ * @tparam T Calibration model.
+ * @tparam Dim The number of parameters in the calibration model.
+ * @param p Calibrated point.
+ * @param Dcal optional 2*p Jacobian wrpt `p` Cal3DS2 parameters.
+ * @param Dp optional 2*2 Jacobian wrpt intrinsic coordinates.
+ */
+template <typename T, size_t Dim>
+void calibrateJacobians(const T& calibration, const Point2& pn,
+                        OptionalJacobian<2, Dim> Dcal = boost::none,
+                        OptionalJacobian<2, 2> Dp = boost::none) {
+  if (Dcal || Dp) {
+    Eigen::Matrix<double, 2, Dim> H_uncal_K;
+    Matrix22 H_uncal_pn, H_uncal_pn_inv;
+
+    // Compute uncalibrate Jacobians
+    calibration.uncalibrate(pn, H_uncal_K, H_uncal_pn);
+
+    H_uncal_pn_inv = H_uncal_pn.inverse();
+
+    if (Dp) *Dp = H_uncal_pn_inv;
+    if (Dcal) *Dcal = -H_uncal_pn_inv * H_uncal_K;
+  }
+}
+
 /**
  * @brief Calibration of a camera with radial distortion
  * @addtogroup geometry
@@ -40,14 +73,15 @@ namespace gtsam {
 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_; // tolerance value when calibrating
+ 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:
 
+  enum { dimension = 9 };
+
   /// @name Standard Constructors
   /// @{
 
diff --git a/gtsam/geometry/Cal3Fisheye.cpp b/gtsam/geometry/Cal3Fisheye.cpp
index f7794fafb..1ed1826ad 100644
--- a/gtsam/geometry/Cal3Fisheye.cpp
+++ b/gtsam/geometry/Cal3Fisheye.cpp
@@ -122,14 +122,15 @@ Point2 Cal3Fisheye::uncalibrate(const Point2& p, OptionalJacobian<2, 9> H1,
 }
 
 /* ************************************************************************* */
-Point2 Cal3Fisheye::calibrate(const Point2& uv, const double tol) const {
+Point2 Cal3Fisheye::calibrate(const Point2& uv, OptionalJacobian<2, 9> Dcal,
+                              OptionalJacobian<2, 2> Dp) const {
   // initial gues just inverts the pinhole model
   const double u = uv.x(), v = uv.y();
   const double yd = (v - v0_) / fy_;
   const double xd = (u - s_ * yd - u0_) / fx_;
   Point2 pi(xd, yd);
 
-  // Perform newtons method, break when solution converges past tol,
+  // Perform newtons method, break when solution converges past tol_,
   // throw exception if max iterations are reached
   const int maxIterations = 10;
   int iteration;
@@ -140,7 +141,7 @@ Point2 Cal3Fisheye::calibrate(const Point2& uv, const double tol) const {
     const Point2 uv_hat = uncalibrate(pi, boost::none, jac);
 
     // Test convergence
-    if ((uv_hat - uv).norm() < tol) break;
+    if ((uv_hat - uv).norm() < tol_) break;
 
     // Newton's method update step
     pi = pi - jac.inverse() * (uv_hat - uv);
@@ -151,6 +152,8 @@ Point2 Cal3Fisheye::calibrate(const Point2& uv, const double tol) const {
         "Cal3Fisheye::calibrate fails to converge. need a better "
         "initialization");
 
+  calibrateJacobians<Cal3Fisheye, dimension>(*this, pi, Dcal, Dp);
+
   return pi;
 }
 
diff --git a/gtsam/geometry/Cal3Fisheye.h b/gtsam/geometry/Cal3Fisheye.h
index e24fe074f..5487019f6 100644
--- a/gtsam/geometry/Cal3Fisheye.h
+++ b/gtsam/geometry/Cal3Fisheye.h
@@ -14,10 +14,12 @@
  * @brief Calibration of a fisheye camera
  * @date Apr 8, 2020
  * @author ghaggin
+ * @author Varun Agrawal
  */
 
 #pragma once
 
+#include <gtsam/geometry/Cal3DS2_Base.h>
 #include <gtsam/geometry/Point2.h>
 
 #include <string>
@@ -48,6 +50,7 @@ class GTSAM_EXPORT Cal3Fisheye {
  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
 
  public:
   enum { dimension = 9 };
@@ -59,11 +62,11 @@ 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) {}
+      : fx_(1), fy_(1), s_(0), u0_(0), v0_(0), 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)
+              const double k3, const double k4, double tol = 1e-5)
       : fx_(fx),
         fy_(fy),
         s_(s),
@@ -72,7 +75,8 @@ class GTSAM_EXPORT Cal3Fisheye {
         k1_(k1),
         k2_(k2),
         k3_(k3),
-        k4_(k4) {}
+        k4_(k4),
+        tol_(tol) {}
 
   virtual ~Cal3Fisheye() {}
 
@@ -139,7 +143,8 @@ class GTSAM_EXPORT Cal3Fisheye {
 
   /// Convert (distorted) image coordinates [u;v] to intrinsic coordinates [x_i,
   /// y_i]
-  Point2 calibrate(const Point2& p, const double tol = 1e-5) const;
+  Point2 calibrate(const Point2& p, OptionalJacobian<2, 9> Dcal = boost::none,
+                   OptionalJacobian<2, 2> Dp = boost::none) const;
 
   /// @}
   /// @name Testable
diff --git a/gtsam/geometry/Cal3Unified.cpp b/gtsam/geometry/Cal3Unified.cpp
index 247e77ae1..f4ce0ed75 100644
--- a/gtsam/geometry/Cal3Unified.cpp
+++ b/gtsam/geometry/Cal3Unified.cpp
@@ -111,24 +111,7 @@ Point2 Cal3Unified::calibrate(const Point2& pi, OptionalJacobian<2, 10> Dcal,
   // call nplane to space
   Point2 pn = this->nPlaneToSpace(pnplane);
 
-  // We make use of the Implicit Function Theorem to compute the Jacobians from uncalibrate
-  // 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)
-  // Dp = -inv(H_uncal_pn) * df/pi = -inv(H_uncal_pn) * (-I) = inv(H_uncal_pn)
-  // Dcal = -inv(H_uncal_pn) * df/K = -inv(H_uncal_pn) * H_uncal_K
-  Eigen::Matrix<double, 2, 10> H_uncal_K;
-  Matrix22 H_uncal_pn, H_uncal_pn_inv;
-
-  if (Dcal || Dp) {
-    // Compute uncalibrate Jacobians
-    uncalibrate(pn, Dcal ? &H_uncal_K : nullptr, H_uncal_pn);
-
-    H_uncal_pn_inv = H_uncal_pn.inverse();
-
-    if (Dp) *Dp = H_uncal_pn_inv;
-    if (Dcal) *Dcal = -H_uncal_pn_inv * H_uncal_K;
-
-  }
+  calibrateJacobians<Cal3Unified, dimension>(*this, pn, Dcal, Dp);
 
   return pn;
 }
diff --git a/gtsam/geometry/tests/testCal3DFisheye.cpp b/gtsam/geometry/tests/testCal3DFisheye.cpp
index 9317fb737..6bfbe3e46 100644
--- a/gtsam/geometry/tests/testCal3DFisheye.cpp
+++ b/gtsam/geometry/tests/testCal3DFisheye.cpp
@@ -181,6 +181,23 @@ TEST(Cal3Fisheye, calibrate3) {
   CHECK(assert_equal(xi_hat, xi));
 }
 
+Point2 calibrate_(const Cal3Fisheye& k, const Point2& pt) {
+  return k.calibrate(pt);
+}
+
+/* ************************************************************************* */
+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));
+  Matrix numerical2 = numericalDerivative22(calibrate_, K, pi);
+  CHECK(assert_equal(numerical2,Dp,1e-5));
+}
+
 /* ************************************************************************* */
 int main() {
   TestResult tr;

From 06d4933e1bf6c80830f079f3c6dc0baa43e7c1e1 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Tue, 1 Dec 2020 12:46:05 -0500
Subject: [PATCH 4/4] common header file for all calibration models

---
 gtsam/geometry/Cal3.h         | 66 +++++++++++++++++++++++++++++++++++
 gtsam/geometry/Cal3Bundler.h  |  2 +-
 gtsam/geometry/Cal3DS2_Base.h | 34 +-----------------
 gtsam/geometry/Cal3Fisheye.h  |  2 +-
 4 files changed, 69 insertions(+), 35 deletions(-)
 create mode 100644 gtsam/geometry/Cal3.h

diff --git a/gtsam/geometry/Cal3.h b/gtsam/geometry/Cal3.h
new file mode 100644
index 000000000..d9e12f7d2
--- /dev/null
+++ b/gtsam/geometry/Cal3.h
@@ -0,0 +1,66 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation,
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
+/**
+ * @file   Cal3.h
+ * @brief  Common code for all Calibration models.
+ * @author Varun Agrawal
+ */
+
+/**
+ * @addtogroup geometry
+ */
+
+#pragma once
+
+#include <gtsam/geometry/Point2.h>
+
+namespace gtsam {
+
+/**
+ * Function which makes use of the Implicit Function Theorem to compute the
+ * 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)
+ * Dp = -inv(H_uncal_pn) * df/pi = -inv(H_uncal_pn) * (-I) = inv(H_uncal_pn)
+ * Dcal = -inv(H_uncal_pn) * df/K = -inv(H_uncal_pn) * H_uncal_K
+ *
+ * @tparam Cal Calibration model.
+ * @tparam Dim The number of parameters in the calibration model.
+ * @param p Calibrated point.
+ * @param Dcal optional 2*p Jacobian wrpt `p` Cal3DS2 parameters.
+ * @param Dp optional 2*2 Jacobian wrpt intrinsic coordinates.
+ */
+template <typename Cal, size_t Dim>
+void calibrateJacobians(const Cal& calibration, const Point2& pn,
+                        OptionalJacobian<2, Dim> Dcal = boost::none,
+                        OptionalJacobian<2, 2> Dp = boost::none) {
+  if (Dcal || Dp) {
+    Eigen::Matrix<double, 2, Dim> H_uncal_K;
+    Matrix22 H_uncal_pn, H_uncal_pn_inv;
+
+    // Compute uncalibrate Jacobians
+    calibration.uncalibrate(pn, Dcal ? &H_uncal_K : nullptr, H_uncal_pn);
+
+    H_uncal_pn_inv = H_uncal_pn.inverse();
+
+    if (Dp) *Dp = H_uncal_pn_inv;
+    if (Dcal) *Dcal = -H_uncal_pn_inv * H_uncal_K;
+  }
+}
+
+//TODO(Varun) Make common base class for all calibration models.
+
+}  // \ namespace gtsam
diff --git a/gtsam/geometry/Cal3Bundler.h b/gtsam/geometry/Cal3Bundler.h
index da43112d9..8c836b504 100644
--- a/gtsam/geometry/Cal3Bundler.h
+++ b/gtsam/geometry/Cal3Bundler.h
@@ -18,7 +18,7 @@
 
 #pragma once
 
-#include <gtsam/geometry/Cal3DS2_Base.h>
+#include <gtsam/geometry/Cal3.h>
 #include <gtsam/geometry/Point2.h>
 
 namespace gtsam {
diff --git a/gtsam/geometry/Cal3DS2_Base.h b/gtsam/geometry/Cal3DS2_Base.h
index c9b53c29b..dbd6478e1 100644
--- a/gtsam/geometry/Cal3DS2_Base.h
+++ b/gtsam/geometry/Cal3DS2_Base.h
@@ -19,43 +19,11 @@
 
 #pragma once
 
+#include <gtsam/geometry/Cal3.h>
 #include <gtsam/geometry/Point2.h>
 
 namespace gtsam {
 
-/**
- * Function which makes use of the Implicit Function Theorem to compute the
- * Jacobians of `calibrate` using `uncalibrate`.
- * 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)
- * Dp = -inv(H_uncal_pn) * df/pi = -inv(H_uncal_pn) * (-I) = inv(H_uncal_pn)
- * Dcal = -inv(H_uncal_pn) * df/K = -inv(H_uncal_pn) * H_uncal_K
- *
- * @tparam T Calibration model.
- * @tparam Dim The number of parameters in the calibration model.
- * @param p Calibrated point.
- * @param Dcal optional 2*p Jacobian wrpt `p` Cal3DS2 parameters.
- * @param Dp optional 2*2 Jacobian wrpt intrinsic coordinates.
- */
-template <typename T, size_t Dim>
-void calibrateJacobians(const T& calibration, const Point2& pn,
-                        OptionalJacobian<2, Dim> Dcal = boost::none,
-                        OptionalJacobian<2, 2> Dp = boost::none) {
-  if (Dcal || Dp) {
-    Eigen::Matrix<double, 2, Dim> H_uncal_K;
-    Matrix22 H_uncal_pn, H_uncal_pn_inv;
-
-    // Compute uncalibrate Jacobians
-    calibration.uncalibrate(pn, H_uncal_K, H_uncal_pn);
-
-    H_uncal_pn_inv = H_uncal_pn.inverse();
-
-    if (Dp) *Dp = H_uncal_pn_inv;
-    if (Dcal) *Dcal = -H_uncal_pn_inv * H_uncal_K;
-  }
-}
-
 /**
  * @brief Calibration of a camera with radial distortion
  * @addtogroup geometry
diff --git a/gtsam/geometry/Cal3Fisheye.h b/gtsam/geometry/Cal3Fisheye.h
index 5487019f6..77e122f21 100644
--- a/gtsam/geometry/Cal3Fisheye.h
+++ b/gtsam/geometry/Cal3Fisheye.h
@@ -19,7 +19,7 @@
 
 #pragma once
 
-#include <gtsam/geometry/Cal3DS2_Base.h>
+#include <gtsam/geometry/Cal3.h>
 #include <gtsam/geometry/Point2.h>
 
 #include <string>