From 7d95505d11fd03b2d4ff06da11f8ac9720b2621b Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@gmail.com>
Date: Thu, 24 Oct 2024 17:04:11 -0700
Subject: [PATCH] Init from pose and Ks

---
 gtsam/geometry/FundamentalMatrix.cpp |  4 +---
 gtsam/geometry/FundamentalMatrix.h   | 17 +++++++++++++++++
 2 files changed, 18 insertions(+), 3 deletions(-)

diff --git a/gtsam/geometry/FundamentalMatrix.cpp b/gtsam/geometry/FundamentalMatrix.cpp
index 57d59d0a0..af322a3c0 100644
--- a/gtsam/geometry/FundamentalMatrix.cpp
+++ b/gtsam/geometry/FundamentalMatrix.cpp
@@ -72,9 +72,7 @@ Matrix3 FundamentalMatrix::matrix() const {
 }
 
 void FundamentalMatrix::print(const std::string& s) const {
-  std::cout << s << "U:\n"
-            << U_.matrix() << "\ns: " << s_ << "\nV:\n"
-            << V_.matrix() << std::endl;
+  std::cout << s << matrix() << std::endl;
 }
 
 bool FundamentalMatrix::equals(const FundamentalMatrix& other,
diff --git a/gtsam/geometry/FundamentalMatrix.h b/gtsam/geometry/FundamentalMatrix.h
index 497f95cf7..f8aa37bf8 100644
--- a/gtsam/geometry/FundamentalMatrix.h
+++ b/gtsam/geometry/FundamentalMatrix.h
@@ -54,6 +54,23 @@ class GTSAM_EXPORT FundamentalMatrix {
    */
   FundamentalMatrix(const Matrix3& F);
 
+  /**
+   * @brief Construct from calibration matrices Ka, Kb, and pose aPb
+   *
+   * Initializes the GeneralFundamentalMatrix from the given calibration
+   * matrices Ka and Kb, and the pose aPb.
+   *
+   * @tparam CAL Calibration type, expected to have a matrix() method
+   * @param Ka Calibration matrix for the left camera
+   * @param aPb Pose from the left to the right camera
+   * @param Kb Calibration matrix for the right camera
+   */
+  template <typename CAL>
+  GeneralFundamentalMatrix(const CAL& Ka, const Pose3& aPb, const CAL& Kb)
+      : GeneralFundamentalMatrix(Ka.K().transpose().inverse() *
+                                 EssentialMatrix::FromPose3(aPb).matrix() *
+                                 Kb.K().inverse()) {}
+
   /// Return the fundamental matrix representation
   Matrix3 matrix() const;