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