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] 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;
 }