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 1/2] 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 e986bc820b..b198643b04 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 2/2] wrap PinholeCameraCal3Bundler

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

diff --git a/gtsam/gtsam.i b/gtsam/gtsam.i
index 5be275133d..dc7c2add24 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;
 };