From 9acf8d4ebca32e2f13e7c7deb75e9d449d86cffc Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Thu, 10 Sep 2020 19:59:18 -0400
Subject: [PATCH 01/14] ISAM2 helper methods and wrapper to evaluate nonlinear
 error

---
 gtsam/gtsam.i                 | 2 ++
 gtsam/nonlinear/ISAM2Result.h | 2 ++
 2 files changed, 4 insertions(+)

diff --git a/gtsam/gtsam.i b/gtsam/gtsam.i
index 52f5901ee..8779c946f 100644
--- a/gtsam/gtsam.i
+++ b/gtsam/gtsam.i
@@ -2551,6 +2551,8 @@ class ISAM2Result {
   size_t getVariablesRelinearized() const;
   size_t getVariablesReeliminated() const;
   size_t getCliques() const;
+  double getErrorBefore() const;
+  double getErrorAfter() const;
 };
 
 class ISAM2 {
diff --git a/gtsam/nonlinear/ISAM2Result.h b/gtsam/nonlinear/ISAM2Result.h
index e45b17e4a..b249af5c5 100644
--- a/gtsam/nonlinear/ISAM2Result.h
+++ b/gtsam/nonlinear/ISAM2Result.h
@@ -176,6 +176,8 @@ struct ISAM2Result {
   size_t getVariablesRelinearized() const { return variablesRelinearized; }
   size_t getVariablesReeliminated() const { return variablesReeliminated; }
   size_t getCliques() const { return cliques; }
+  double getErrorBefore() const { return errorBefore ? *errorBefore : std::nan(""); }
+  double getErrorAfter() const { return errorAfter ? *errorAfter : std::nan(""); }
 };
 
 }  // namespace gtsam

From 921dc848a08ba08dc369ef461d9646a9a11f63cb Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Tue, 22 Sep 2020 20:17:09 -0400
Subject: [PATCH 02/14] added calibrate with jacobians for Cal3Bundler

---
 gtsam/geometry/Cal3Bundler.cpp             | 33 ++++++++++++++++++++++
 gtsam/geometry/Cal3Bundler.h               | 10 +++++++
 gtsam/geometry/tests/testPinholeCamera.cpp |  9 ++++++
 3 files changed, 52 insertions(+)

diff --git a/gtsam/geometry/Cal3Bundler.cpp b/gtsam/geometry/Cal3Bundler.cpp
index 0d64d0184..56f62e3ac 100644
--- a/gtsam/geometry/Cal3Bundler.cpp
+++ b/gtsam/geometry/Cal3Bundler.cpp
@@ -121,6 +121,39 @@ Point2 Cal3Bundler::calibrate(const Point2& pi, const double tol) const {
   return pn;
 }
 
+/* ************************************************************************* */
+Point2 Cal3Bundler::calibrate(const Point2& p, OptionalJacobian<2, 3> Dcal,
+                              OptionalJacobian<2, 2> Dp) const {
+  Point2 pn = calibrate(p, 1e-5);
+
+  // Approximate the jacobians via a single iteration of g.
+  if (Dcal) {
+    const double u = p.x(), v = p.y(), x = (u - u0_) / f_, y = (v - v0_) / f_;
+    const double xx = x * x, yy = y * y;
+    const double rr = xx + yy;
+    const double g = (1 + k1_ * rr + k2_ * rr * rr);
+    const double inv_f = 1 / f_, inv_g = 1 / g;
+
+    *Dcal << -inv_f * x, -x * inv_g * rr, -x * inv_g * rr * rr, -inv_f * y,
+        -y * inv_g * rr, -y * inv_g * rr * rr;
+  }
+  if (Dp) {
+    const double u = p.x(), v = p.y(), x = (u - u0_) / f_, y = (v - v0_) / f_;
+    const double xx = x * x, yy = y * y;
+    const double rr = xx + yy;
+    const double g = (1 + k1_ * rr + k2_ * rr * rr);
+    const double inv_f = 1 / f_, inv_g = 1 / g;
+
+    const double dg_du = (2 * k1_ * x * inv_f) + (4 * k2_ * rr * x * inv_f);
+    const double dg_dv = (2 * k1_ * y * inv_f) + (4 * k2_ * rr * y * inv_f);
+
+    *Dp << (inv_f * inv_g) - (pn.x() * inv_g * dg_du), -pn.x() * inv_g * dg_dv,
+        -pn.y() * inv_g * dg_du, (inv_f * inv_g) - (pn.x() * inv_g * dg_dv);
+  }
+
+  return pn;
+}
+
 /* ************************************************************************* */
 Matrix2 Cal3Bundler::D2d_intrinsic(const Point2& p) const {
   Matrix2 Dp;
diff --git a/gtsam/geometry/Cal3Bundler.h b/gtsam/geometry/Cal3Bundler.h
index 4787f565b..0042b710f 100644
--- a/gtsam/geometry/Cal3Bundler.h
+++ b/gtsam/geometry/Cal3Bundler.h
@@ -120,6 +120,16 @@ public:
   /// Convert a pixel coordinate to ideal coordinate
   Point2 calibrate(const Point2& pi, const double tol = 1e-5) const;
 
+  /**
+   * Convert image coordinates uv to intrinsic coordinates xy
+   * @param p point in image coordinates
+   * @param Dcal optional 2*3 Jacobian wrpt Cal3Bundler parameters
+   * @param Dp optional 2*2 Jacobian wrpt intrinsic coordinates
+   * @return point in intrinsic coordinates
+   */
+  Point2 calibrate(const Point2& p, OptionalJacobian<2, 3> Dcal = boost::none,
+                   OptionalJacobian<2, 2> Dp = boost::none) const;
+
   /// @deprecated might be removed in next release, use uncalibrate
   Matrix2 D2d_intrinsic(const Point2& p) const;
 
diff --git a/gtsam/geometry/tests/testPinholeCamera.cpp b/gtsam/geometry/tests/testPinholeCamera.cpp
index a9b68bdec..ad6f4e921 100644
--- a/gtsam/geometry/tests/testPinholeCamera.cpp
+++ b/gtsam/geometry/tests/testPinholeCamera.cpp
@@ -336,6 +336,15 @@ TEST( PinholeCamera, range3) {
   EXPECT(assert_equal(Hexpected2, D2, 1e-7));
 }
 
+/* ************************************************************************* */
+TEST( PinholeCamera, Cal3Bundler) {
+  Cal3Bundler calibration;
+  Pose3 wTc;
+  PinholeCamera<Cal3Bundler> camera(wTc, calibration);
+  Point2 p(50, 100);
+  camera.backproject(p, 10);
+}
+
 /* ************************************************************************* */
 int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
 /* ************************************************************************* */

From 3fab304191b04b80e7525f980c28b6244ef14bef Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Wed, 23 Sep 2020 09:59:46 -0400
Subject: [PATCH 03/14] combine the calibrate functions into one

---
 gtsam/geometry/Cal3Bundler.cpp | 28 ++++++++--------------------
 gtsam/geometry/Cal3Bundler.h   | 11 +++++------
 2 files changed, 13 insertions(+), 26 deletions(-)

diff --git a/gtsam/geometry/Cal3Bundler.cpp b/gtsam/geometry/Cal3Bundler.cpp
index 56f62e3ac..4677dbe45 100644
--- a/gtsam/geometry/Cal3Bundler.cpp
+++ b/gtsam/geometry/Cal3Bundler.cpp
@@ -94,7 +94,9 @@ Point2 Cal3Bundler::uncalibrate(const Point2& p, //
 }
 
 /* ************************************************************************* */
-Point2 Cal3Bundler::calibrate(const Point2& pi, const double tol) const {
+Point2 Cal3Bundler::calibrate(const Point2& pi, OptionalJacobian<2, 3> Dcal,
+                              OptionalJacobian<2, 2> Dp,
+                              const double tol) const {
   // Copied from Cal3DS2 :-(
   // but specialized with k1,k2 non-zero only and fx=fy and s=0
   const Point2 invKPi((pi.x() - u0_)/f_, (pi.y() - v0_)/f_);
@@ -118,32 +120,18 @@ Point2 Cal3Bundler::calibrate(const Point2& pi, const double tol) const {
     throw std::runtime_error(
         "Cal3Bundler::calibrate fails to converge. need a better initialization");
 
-  return pn;
-}
-
-/* ************************************************************************* */
-Point2 Cal3Bundler::calibrate(const Point2& p, OptionalJacobian<2, 3> Dcal,
-                              OptionalJacobian<2, 2> Dp) const {
-  Point2 pn = calibrate(p, 1e-5);
+  const double x = invKPi.x(), y = invKPi.y();
+  const double xx = x * x, yy = y * y;
+  const double rr = xx + yy;
+  const double g = (1 + k1_ * rr + k2_ * rr * rr);
+  const double inv_f = 1 / f_, inv_g = 1 / g;
 
   // Approximate the jacobians via a single iteration of g.
   if (Dcal) {
-    const double u = p.x(), v = p.y(), x = (u - u0_) / f_, y = (v - v0_) / f_;
-    const double xx = x * x, yy = y * y;
-    const double rr = xx + yy;
-    const double g = (1 + k1_ * rr + k2_ * rr * rr);
-    const double inv_f = 1 / f_, inv_g = 1 / g;
-
     *Dcal << -inv_f * x, -x * inv_g * rr, -x * inv_g * rr * rr, -inv_f * y,
         -y * inv_g * rr, -y * inv_g * rr * rr;
   }
   if (Dp) {
-    const double u = p.x(), v = p.y(), x = (u - u0_) / f_, y = (v - v0_) / f_;
-    const double xx = x * x, yy = y * y;
-    const double rr = xx + yy;
-    const double g = (1 + k1_ * rr + k2_ * rr * rr);
-    const double inv_f = 1 / f_, inv_g = 1 / g;
-
     const double dg_du = (2 * k1_ * x * inv_f) + (4 * k2_ * rr * x * inv_f);
     const double dg_dv = (2 * k1_ * y * inv_f) + (4 * k2_ * rr * y * inv_f);
 
diff --git a/gtsam/geometry/Cal3Bundler.h b/gtsam/geometry/Cal3Bundler.h
index 0042b710f..0636399a2 100644
--- a/gtsam/geometry/Cal3Bundler.h
+++ b/gtsam/geometry/Cal3Bundler.h
@@ -117,18 +117,17 @@ public:
   Point2 uncalibrate(const Point2& p, OptionalJacobian<2, 3> Dcal = boost::none,
       OptionalJacobian<2, 2> Dp = boost::none) const;
 
-  /// Convert a pixel coordinate to ideal coordinate
-  Point2 calibrate(const Point2& pi, const double tol = 1e-5) const;
-
   /**
-   * Convert image coordinates uv to intrinsic coordinates xy
+   * Convert a pixel coordinate to ideal coordinate xy
    * @param p point in image coordinates
    * @param Dcal optional 2*3 Jacobian wrpt Cal3Bundler parameters
    * @param Dp optional 2*2 Jacobian wrpt intrinsic coordinates
+   * @param tol optional tolerance threshold value for iterative minimization
    * @return point in intrinsic coordinates
    */
-  Point2 calibrate(const Point2& p, OptionalJacobian<2, 3> Dcal = boost::none,
-                   OptionalJacobian<2, 2> Dp = boost::none) const;
+  Point2 calibrate(const Point2& pi, OptionalJacobian<2, 3> Dcal = boost::none,
+                   OptionalJacobian<2, 2> Dp = boost::none,
+                   const double tol = 1e-5) const;
 
   /// @deprecated might be removed in next release, use uncalibrate
   Matrix2 D2d_intrinsic(const Point2& p) const;

From 611974c63ad7469973fa847138979a09e9a102f7 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Wed, 23 Sep 2020 10:45:57 -0400
Subject: [PATCH 04/14] put the jacobians at the end so that the calibrate
 function can be wrapped

---
 gtsam/geometry/Cal3Bundler.cpp | 6 +++---
 gtsam/geometry/Cal3Bundler.h   | 8 ++++----
 2 files changed, 7 insertions(+), 7 deletions(-)

diff --git a/gtsam/geometry/Cal3Bundler.cpp b/gtsam/geometry/Cal3Bundler.cpp
index 4677dbe45..837ce35e0 100644
--- a/gtsam/geometry/Cal3Bundler.cpp
+++ b/gtsam/geometry/Cal3Bundler.cpp
@@ -94,9 +94,9 @@ Point2 Cal3Bundler::uncalibrate(const Point2& p, //
 }
 
 /* ************************************************************************* */
-Point2 Cal3Bundler::calibrate(const Point2& pi, OptionalJacobian<2, 3> Dcal,
-                              OptionalJacobian<2, 2> Dp,
-                              const double tol) const {
+Point2 Cal3Bundler::calibrate(const Point2& pi, const double tol,
+                              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
   const Point2 invKPi((pi.x() - u0_)/f_, (pi.y() - v0_)/f_);
diff --git a/gtsam/geometry/Cal3Bundler.h b/gtsam/geometry/Cal3Bundler.h
index 0636399a2..35398303f 100644
--- a/gtsam/geometry/Cal3Bundler.h
+++ b/gtsam/geometry/Cal3Bundler.h
@@ -120,14 +120,14 @@ public:
   /**
    * Convert a pixel coordinate to ideal coordinate xy
    * @param p point in image coordinates
+   * @param tol optional tolerance threshold value for iterative minimization
    * @param Dcal optional 2*3 Jacobian wrpt Cal3Bundler parameters
    * @param Dp optional 2*2 Jacobian wrpt intrinsic coordinates
-   * @param tol optional tolerance threshold value for iterative minimization
    * @return point in intrinsic coordinates
    */
-  Point2 calibrate(const Point2& pi, OptionalJacobian<2, 3> Dcal = boost::none,
-                   OptionalJacobian<2, 2> Dp = boost::none,
-                   const double tol = 1e-5) const;
+  Point2 calibrate(const Point2& pi, const double tol = 1e-5,
+                   OptionalJacobian<2, 3> Dcal = boost::none,
+                   OptionalJacobian<2, 2> Dp = boost::none) const;
 
   /// @deprecated might be removed in next release, use uncalibrate
   Matrix2 D2d_intrinsic(const Point2& p) const;

From b499006b12dbab9b1d7b1af366a74c1c554f2437 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Wed, 23 Sep 2020 13:48:40 -0400
Subject: [PATCH 05/14] make tolerance as a constructor param

---
 gtsam/geometry/Cal3Bundler.cpp | 21 +++++++++++----------
 gtsam/geometry/Cal3Bundler.h   |  8 ++++++--
 2 files changed, 17 insertions(+), 12 deletions(-)

diff --git a/gtsam/geometry/Cal3Bundler.cpp b/gtsam/geometry/Cal3Bundler.cpp
index 837ce35e0..470c97c75 100644
--- a/gtsam/geometry/Cal3Bundler.cpp
+++ b/gtsam/geometry/Cal3Bundler.cpp
@@ -25,13 +25,13 @@ namespace gtsam {
 
 /* ************************************************************************* */
 Cal3Bundler::Cal3Bundler() :
-    f_(1), k1_(0), k2_(0), u0_(0), v0_(0) {
+    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) :
-    f_(f), k1_(k1), k2_(k2), u0_(u0), v0_(v0) {
-}
+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 {
@@ -94,23 +94,24 @@ Point2 Cal3Bundler::uncalibrate(const Point2& p, //
 }
 
 /* ************************************************************************* */
-Point2 Cal3Bundler::calibrate(const Point2& pi, const double tol,
+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
-  const Point2 invKPi((pi.x() - u0_)/f_, (pi.y() - v0_)/f_);
+  double x = (pi.x() - u0_)/f_, y = (pi.y() - v0_)/f_;
+  const Point2 invKPi(x, y);
 
   // initialize by ignoring the distortion at all, might be problematic for pixels around boundary
-  Point2 pn = invKPi;
+  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)
+    if (distance2(uncalibrate(pn), pi) <= tol_)
       break;
-    const double x = pn.x(), y = pn.y(), xx = x * x, yy = y * y;
+    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);
     pn = invKPi / g;
@@ -120,7 +121,7 @@ Point2 Cal3Bundler::calibrate(const Point2& pi, const double tol,
     throw std::runtime_error(
         "Cal3Bundler::calibrate fails to converge. need a better initialization");
 
-  const double x = invKPi.x(), y = invKPi.y();
+  x = invKPi.x(), y = invKPi.y();
   const double xx = x * x, yy = y * y;
   const double rr = xx + yy;
   const double g = (1 + k1_ * rr + k2_ * rr * rr);
diff --git a/gtsam/geometry/Cal3Bundler.h b/gtsam/geometry/Cal3Bundler.h
index 35398303f..96765f899 100644
--- a/gtsam/geometry/Cal3Bundler.h
+++ b/gtsam/geometry/Cal3Bundler.h
@@ -33,6 +33,7 @@ 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:
 
@@ -51,8 +52,10 @@ public:
    *  @param k2 second radial distortion coefficient (quartic)
    *  @param u0 optional image center (default 0), considered a constant
    *  @param v0 optional image center (default 0), considered a constant
+   *  @param tol optional calibration tolerance value
    */
-  Cal3Bundler(double f, double k1, double k2, double u0 = 0, double v0 = 0);
+  Cal3Bundler(double f, double k1, double k2, double u0 = 0, double v0 = 0,
+              double tol = 1e-5);
 
   virtual ~Cal3Bundler() {}
 
@@ -125,7 +128,7 @@ public:
    * @param Dp optional 2*2 Jacobian wrpt intrinsic coordinates
    * @return point in intrinsic coordinates
    */
-  Point2 calibrate(const Point2& pi, const double tol = 1e-5,
+  Point2 calibrate(const Point2& pi,
                    OptionalJacobian<2, 3> Dcal = boost::none,
                    OptionalJacobian<2, 2> Dp = boost::none) const;
 
@@ -173,6 +176,7 @@ private:
     ar & BOOST_SERIALIZATION_NVP(k2_);
     ar & BOOST_SERIALIZATION_NVP(u0_);
     ar & BOOST_SERIALIZATION_NVP(v0_);
+    ar & BOOST_SERIALIZATION_NVP(tol_);
   }
 
   /// @}

From cb78b281a57f125ccbdd5383aa454425ed3dbbfe Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Thu, 24 Sep 2020 00:09:39 -0400
Subject: [PATCH 06/14] update calibrate in wrapper

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

diff --git a/gtsam/gtsam.i b/gtsam/gtsam.i
index d8297c43b..369ea983c 100644
--- a/gtsam/gtsam.i
+++ b/gtsam/gtsam.i
@@ -1053,7 +1053,7 @@ class Cal3_S2Stereo {
 class Cal3Bundler {
   // Standard Constructors
   Cal3Bundler();
-  Cal3Bundler(double fx, double k1, double k2, double u0, double v0);
+  Cal3Bundler(double fx, double k1, double k2, double u0, double v0, double tol);
 
   // Testable
   void print(string s) const;
@@ -1066,7 +1066,7 @@ class Cal3Bundler {
   Vector localCoordinates(const gtsam::Cal3Bundler& c) const;
 
   // Action on Point2
-  gtsam::Point2 calibrate(const gtsam::Point2& p, double tol) const;
+  gtsam::Point2 calibrate(const gtsam::Point2& p) const;
   gtsam::Point2 uncalibrate(const gtsam::Point2& p) const;
 
   // Standard Interface

From 49ba9a79907da70d11a4316ae1af8fc3f5b323a5 Mon Sep 17 00:00:00 2001
From: alexma3312 <alexma3312@gmail.com>
Date: Mon, 28 Sep 2020 20:52:31 -0400
Subject: [PATCH 07/14] Throw an exception when n=0 .

---
 gtsam/geometry/Point3.cpp | 1 +
 gtsam/geometry/Point3.h   | 1 +
 2 files changed, 2 insertions(+)

diff --git a/gtsam/geometry/Point3.cpp b/gtsam/geometry/Point3.cpp
index ce4ceee89..783741262 100644
--- a/gtsam/geometry/Point3.cpp
+++ b/gtsam/geometry/Point3.cpp
@@ -77,6 +77,7 @@ double dot(const Point3 &p, const Point3 &q, OptionalJacobian<1, 3> H1,
 
 Point3Pair mean(const std::vector<Point3Pair> &abPointPairs) {
   const size_t n = abPointPairs.size();
+  if (n == 0) throw std::invalid_argument("input should have at least 1 pair of points");
   Point3 aCentroid(0, 0, 0), bCentroid(0, 0, 0);
   for (const Point3Pair &abPair : abPointPairs) {
     aCentroid += abPair.first;
diff --git a/gtsam/geometry/Point3.h b/gtsam/geometry/Point3.h
index 7f58497e9..b400470ea 100644
--- a/gtsam/geometry/Point3.h
+++ b/gtsam/geometry/Point3.h
@@ -62,6 +62,7 @@ GTSAM_EXPORT double dot(const Point3& p, const Point3& q,
 /// mean
 template <class CONTAINER>
 GTSAM_EXPORT Point3 mean(const CONTAINER& points) {
+  if (points.size() == 0) throw std::invalid_argument("input should have at least 1 point");
   Point3 sum(0, 0, 0);
   sum = std::accumulate(points.begin(), points.end(), sum);
   return sum / points.size();

From 3743202a0daa2f1c356ef8ee9f47fd04593d4482 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Tue, 29 Sep 2020 01:50:04 -0400
Subject: [PATCH 08/14] use implicit function theorem to compute jacobians of
 Cal3Bundler::calibrate

---
 gtsam/geometry/Cal3Bundler.cpp | 28 ++++++++++++++--------------
 1 file changed, 14 insertions(+), 14 deletions(-)

diff --git a/gtsam/geometry/Cal3Bundler.cpp b/gtsam/geometry/Cal3Bundler.cpp
index 470c97c75..dad2a78d1 100644
--- a/gtsam/geometry/Cal3Bundler.cpp
+++ b/gtsam/geometry/Cal3Bundler.cpp
@@ -121,23 +121,23 @@ Point2 Cal3Bundler::calibrate(const Point2& pi,
     throw std::runtime_error(
         "Cal3Bundler::calibrate fails to converge. need a better initialization");
 
-  x = invKPi.x(), y = invKPi.y();
-  const double xx = x * x, yy = y * y;
-  const double rr = xx + yy;
-  const double g = (1 + k1_ * rr + k2_ * rr * rr);
-  const double inv_f = 1 / f_, inv_g = 1 / g;
+  // 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
+  Matrix H_uncal_K, H_uncal_pn;
 
-  // Approximate the jacobians via a single iteration of g.
-  if (Dcal) {
-    *Dcal << -inv_f * x, -x * inv_g * rr, -x * inv_g * rr * rr, -inv_f * y,
-        -y * inv_g * rr, -y * inv_g * rr * rr;
+  if (Dcal || Dp) {
+    // Compute uncalibrate Jacobians
+    uncalibrate(pn, H_uncal_K, H_uncal_pn);
+  }
+
+  if (Dcal) { 
+    *Dcal = -H_uncal_pn.inverse() * H_uncal_K;
   }
   if (Dp) {
-    const double dg_du = (2 * k1_ * x * inv_f) + (4 * k2_ * rr * x * inv_f);
-    const double dg_dv = (2 * k1_ * y * inv_f) + (4 * k2_ * rr * y * inv_f);
-
-    *Dp << (inv_f * inv_g) - (pn.x() * inv_g * dg_du), -pn.x() * inv_g * dg_dv,
-        -pn.y() * inv_g * dg_du, (inv_f * inv_g) - (pn.x() * inv_g * dg_dv);
+    *Dp = H_uncal_pn.inverse();
   }
 
   return pn;

From c52c217592d9195b16b232354ef78e5d3f398dc8 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Tue, 29 Sep 2020 01:50:14 -0400
Subject: [PATCH 09/14] added test for Cal3Bundler::calibrate Jacobians1

---
 gtsam/geometry/tests/testCal3Bundler.cpp | 23 +++++++++++++++++------
 1 file changed, 17 insertions(+), 6 deletions(-)

diff --git a/gtsam/geometry/tests/testCal3Bundler.cpp b/gtsam/geometry/tests/testCal3Bundler.cpp
index 8de049fa4..448600266 100644
--- a/gtsam/geometry/tests/testCal3Bundler.cpp
+++ b/gtsam/geometry/tests/testCal3Bundler.cpp
@@ -52,12 +52,14 @@ TEST( Cal3Bundler, 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 Cal3Bundler& k, const Point2& pt) { return k.uncalibrate(pt); }
 
+Point2 calibrate_(const Cal3Bundler& k, const Point2& pt) { return k.calibrate(pt); }
+
 /* ************************************************************************* */
 TEST( Cal3Bundler, Duncalibrate)
 {
@@ -69,11 +71,20 @@ TEST( Cal3Bundler, Duncalibrate)
   Matrix numerical2 = numericalDerivative22(uncalibrate_, K, p);
   CHECK(assert_equal(numerical1,Dcal,1e-7));
   CHECK(assert_equal(numerical2,Dp,1e-7));
-  CHECK(assert_equal(numerical1,K.D2d_calibration(p),1e-7));
-  CHECK(assert_equal(numerical2,K.D2d_intrinsic(p),1e-7));
-  Matrix Dcombined(2,5);
-  Dcombined << Dp, Dcal;
-  CHECK(assert_equal(Dcombined,K.D2d_intrinsic_calibration(p),1e-7));
+}
+
+/* ************************************************************************* */
+TEST( Cal3Bundler, Dcalibrate)
+{
+  Matrix Dcal, Dp;
+  Point2 pn(0.5, 0.5);
+  Point2 pi = K.uncalibrate(pn);
+  Point2 actual = K.calibrate(pi, Dcal, Dp);
+  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));
 }
 
 /* ************************************************************************* */

From 8634d3f42ea2202d34a16f4d295935f03a28873a Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Tue, 29 Sep 2020 01:50:21 -0400
Subject: [PATCH 10/14] make Cal3Bundler wrapper constructor backwards
 compatible

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

diff --git a/gtsam/gtsam.i b/gtsam/gtsam.i
index 369ea983c..5be275133 100644
--- a/gtsam/gtsam.i
+++ b/gtsam/gtsam.i
@@ -1053,6 +1053,7 @@ class Cal3_S2Stereo {
 class Cal3Bundler {
   // Standard Constructors
   Cal3Bundler();
+  Cal3Bundler(double fx, double k1, double k2, double u0, double v0);
   Cal3Bundler(double fx, double k1, double k2, double u0, double v0, double tol);
 
   // Testable

From 7dab718861880fcdec4417ceb5133843f9e58a52 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Tue, 29 Sep 2020 10:49:44 -0400
Subject: [PATCH 11/14] fixed sized matrices and minor improvements

---
 gtsam/geometry/Cal3Bundler.cpp | 15 ++++++---------
 1 file changed, 6 insertions(+), 9 deletions(-)

diff --git a/gtsam/geometry/Cal3Bundler.cpp b/gtsam/geometry/Cal3Bundler.cpp
index dad2a78d1..e986bc820 100644
--- a/gtsam/geometry/Cal3Bundler.cpp
+++ b/gtsam/geometry/Cal3Bundler.cpp
@@ -126,20 +126,17 @@ Point2 Cal3Bundler::calibrate(const Point2& pi,
   // 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
-  Matrix H_uncal_K, H_uncal_pn;
+  Matrix23 H_uncal_K;
+  Matrix22 H_uncal_pn;
 
   if (Dcal || Dp) {
     // Compute uncalibrate Jacobians
-    uncalibrate(pn, H_uncal_K, H_uncal_pn);
-  }
+    uncalibrate(pn, Dcal ? &H_uncal_K : nullptr, H_uncal_pn);
 
-  if (Dcal) { 
-    *Dcal = -H_uncal_pn.inverse() * H_uncal_K;
-  }
-  if (Dp) {
-    *Dp = H_uncal_pn.inverse();
-  }
+    if (Dp) *Dp = H_uncal_pn.inverse();
+    if (Dcal) *Dcal = -H_uncal_pn.inverse() * H_uncal_K;
 
+  }
   return pn;
 }
 

From 31ad107053b148bec48b931b08afb1e16df273c2 Mon Sep 17 00:00:00 2001
From: alexma3312 <alexma3312@gmail.com>
Date: Tue, 29 Sep 2020 12:04:42 -0400
Subject: [PATCH 12/14] Modify error message to be more descriptive.

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

diff --git a/gtsam/geometry/Point3.cpp b/gtsam/geometry/Point3.cpp
index 783741262..9ff220e63 100644
--- a/gtsam/geometry/Point3.cpp
+++ b/gtsam/geometry/Point3.cpp
@@ -77,7 +77,7 @@ double dot(const Point3 &p, const Point3 &q, OptionalJacobian<1, 3> H1,
 
 Point3Pair mean(const std::vector<Point3Pair> &abPointPairs) {
   const size_t n = abPointPairs.size();
-  if (n == 0) throw std::invalid_argument("input should have at least 1 pair of points");
+  if (n == 0) throw std::invalid_argument("Point3::mean input Point3Pair vector is empty");
   Point3 aCentroid(0, 0, 0), bCentroid(0, 0, 0);
   for (const Point3Pair &abPair : abPointPairs) {
     aCentroid += abPair.first;
diff --git a/gtsam/geometry/Point3.h b/gtsam/geometry/Point3.h
index b400470ea..510a5fa80 100644
--- a/gtsam/geometry/Point3.h
+++ b/gtsam/geometry/Point3.h
@@ -62,7 +62,7 @@ GTSAM_EXPORT double dot(const Point3& p, const Point3& q,
 /// mean
 template <class CONTAINER>
 GTSAM_EXPORT Point3 mean(const CONTAINER& points) {
-  if (points.size() == 0) throw std::invalid_argument("input should have at least 1 point");
+  if (points.size() == 0) throw std::invalid_argument("Point3::mean input container is empty");
   Point3 sum(0, 0, 0);
   sum = std::accumulate(points.begin(), points.end(), sum);
   return sum / points.size();

From 0ae5a92d424d52ba4f2b378da37a9f9f54e76b8a Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Tue, 29 Sep 2020 12:57:55 -0400
Subject: [PATCH 13/14] compute inverse only once

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

diff --git a/gtsam/geometry/Cal3Bundler.cpp b/gtsam/geometry/Cal3Bundler.cpp
index e986bc820..b198643b0 100644
--- a/gtsam/geometry/Cal3Bundler.cpp
+++ b/gtsam/geometry/Cal3Bundler.cpp
@@ -127,16 +127,19 @@ Point2 Cal3Bundler::calibrate(const Point2& pi,
   // 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
   Matrix23 H_uncal_K;
-  Matrix22 H_uncal_pn;
+  Matrix22 H_uncal_pn, H_uncal_pn_inv;
 
   if (Dcal || Dp) {
     // Compute uncalibrate Jacobians
     uncalibrate(pn, Dcal ? &H_uncal_K : nullptr, H_uncal_pn);
 
-    if (Dp) *Dp = H_uncal_pn.inverse();
-    if (Dcal) *Dcal = -H_uncal_pn.inverse() * H_uncal_K;
+    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;
 }
 

From bb08c62693c47ef6b6478d648a1d93c19e2a7c69 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Tue, 29 Sep 2020 15:51:47 -0400
Subject: [PATCH 14/14] wrap PinholeCameraCal3Bundler

---
 gtsam/gtsam.i | 5 ++---
 1 file changed, 2 insertions(+), 3 deletions(-)

diff --git a/gtsam/gtsam.i b/gtsam/gtsam.i
index 5be275133..dc7c2add2 100644
--- a/gtsam/gtsam.i
+++ b/gtsam/gtsam.i
@@ -1201,7 +1201,7 @@ typedef gtsam::PinholeCamera<gtsam::Cal3_S2> PinholeCameraCal3_S2;
 //TODO (Issue 237) 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::Cal3Bundler> PinholeCameraCal3Bundler;
+typedef gtsam::PinholeCamera<gtsam::Cal3Bundler> PinholeCameraCal3Bundler;
 
 #include <gtsam/geometry/StereoCamera.h>
 class StereoCamera {
@@ -2862,8 +2862,7 @@ class SfmTrack {
 class SfmData {
   size_t number_cameras() const;
   size_t number_tracks() const;
-  //TODO(Varun) Need to fix issue #237 first before this can work
-  // gtsam::PinholeCamera<gtsam::Cal3Bundler> camera(size_t idx) const;
+  gtsam::PinholeCamera<gtsam::Cal3Bundler> camera(size_t idx) const;
   gtsam::SfmTrack track(size_t idx) const;
 };