From 9cfca976bab21cde5f9369947eec3d2f4a104e76 Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@gmail.com>
Date: Sat, 26 Oct 2024 16:55:23 -0700
Subject: [PATCH] New constructor

---
 gtsam/geometry/FundamentalMatrix.h | 20 +++++++++++++++++---
 gtsam/geometry/geometry.i          |  2 ++
 2 files changed, 19 insertions(+), 3 deletions(-)

diff --git a/gtsam/geometry/FundamentalMatrix.h b/gtsam/geometry/FundamentalMatrix.h
index 6f04f45e8..770cd711d 100644
--- a/gtsam/geometry/FundamentalMatrix.h
+++ b/gtsam/geometry/FundamentalMatrix.h
@@ -54,6 +54,22 @@ class GTSAM_EXPORT FundamentalMatrix {
    */
   FundamentalMatrix(const Matrix3& F);
 
+  /**
+   * @brief Construct from essential matrix and calibration matrices
+   *
+   * Initializes the FundamentalMatrix from the given essential matrix E
+   * and calibration matrices Ka and Kb.
+   *
+   * @tparam CAL Calibration type, expected to have a matrix() method
+   * @param E Essential matrix
+   * @param Ka Calibration matrix for the left camera
+   * @param Kb Calibration matrix for the right camera
+   */
+  template <typename CAL>
+  FundamentalMatrix(const CAL& Ka, const EssentialMatrix& E, const CAL& Kb)
+      : FundamentalMatrix(Ka.K().transpose().inverse() * E.matrix() *
+                          Kb.K().inverse()) {}
+
   /**
    * @brief Construct from calibration matrices Ka, Kb, and pose aPb
    *
@@ -67,9 +83,7 @@ class GTSAM_EXPORT FundamentalMatrix {
    */
   template <typename CAL>
   FundamentalMatrix(const CAL& Ka, const Pose3& aPb, const CAL& Kb)
-      : FundamentalMatrix(Ka.K().transpose().inverse() *
-                          EssentialMatrix::FromPose3(aPb).matrix() *
-                          Kb.K().inverse()) {}
+      : FundamentalMatrix(Ka, EssentialMatrix::FromPose3(aPb), Kb) {}
 
   /// Return the fundamental matrix representation
   Matrix3 matrix() const;
diff --git a/gtsam/geometry/geometry.i b/gtsam/geometry/geometry.i
index 8fd1e46b2..13cdcf70d 100644
--- a/gtsam/geometry/geometry.i
+++ b/gtsam/geometry/geometry.i
@@ -913,6 +913,8 @@ class FundamentalMatrix {
   FundamentalMatrix(const gtsam::Matrix3& F);
 
   // Overloaded constructors for specific calibration types
+  FundamentalMatrix(const gtsam::Cal3_S2& Ka, const gtsam::EssentialMatrix& E,
+                    const gtsam::Cal3_S2& Kb);
   FundamentalMatrix(const gtsam::Cal3_S2& Ka, const gtsam::Pose3& aPb,
                     const gtsam::Cal3_S2& Kb);