From e98acff79ac5825d6fae35b2f2209a4e52ecd64a Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@gmail.com>
Date: Sat, 26 Oct 2024 15:57:16 -0700
Subject: [PATCH] Wrap FundamentalMatrix classes

---
 gtsam/geometry/geometry.i   | 48 +++++++++++++++++++++++++++++++++++++
 gtsam/nonlinear/nonlinear.i |  3 +++
 2 files changed, 51 insertions(+)

diff --git a/gtsam/geometry/geometry.i b/gtsam/geometry/geometry.i
index cf86723ae..8fd1e46b2 100644
--- a/gtsam/geometry/geometry.i
+++ b/gtsam/geometry/geometry.i
@@ -903,6 +903,54 @@ class Cal3Bundler {
   void serialize() const;
 };
 
+#include <gtsam/geometry/FundamentalMatrix.h>
+
+// FundamentalMatrix class
+class FundamentalMatrix {
+  // Constructors
+  FundamentalMatrix();
+  FundamentalMatrix(const gtsam::Rot3& U, double s, const gtsam::Rot3& V);
+  FundamentalMatrix(const gtsam::Matrix3& F);
+
+  // Overloaded constructors for specific calibration types
+  FundamentalMatrix(const gtsam::Cal3_S2& Ka, const gtsam::Pose3& aPb,
+                    const gtsam::Cal3_S2& Kb);
+
+  // Methods
+  gtsam::Matrix3 matrix() const;
+
+  // Testable
+  void print(const std::string& s = "") const;
+  bool equals(const gtsam::FundamentalMatrix& other, double tol = 1e-9) const;
+
+  // Manifold
+  static size_t Dim();
+  size_t dim() const;
+  gtsam::Vector localCoordinates(const gtsam::FundamentalMatrix& F) const;
+  gtsam::FundamentalMatrix retract(const gtsam::Vector& delta) const;
+};
+
+// SimpleFundamentalMatrix class
+class SimpleFundamentalMatrix {
+  // Constructors
+  SimpleFundamentalMatrix();
+  SimpleFundamentalMatrix(const gtsam::EssentialMatrix& E, double fa, double fb,
+                          const gtsam::Point2& ca, const gtsam::Point2& cb);
+
+  // Methods
+  gtsam::Matrix3 matrix() const;
+
+  // Testable
+  void print(const std::string& s = "") const;
+  bool equals(const gtsam::SimpleFundamentalMatrix& other, double tol = 1e-9) const;
+
+  // Manifold
+  static size_t Dim();
+  size_t dim() const;
+  gtsam::Vector localCoordinates(const gtsam::SimpleFundamentalMatrix& F) const;
+  gtsam::SimpleFundamentalMatrix retract(const gtsam::Vector& delta) const;
+};
+
 #include <gtsam/geometry/CalibratedCamera.h>
 class CalibratedCamera {
   // Standard Constructors and Named Constructors
diff --git a/gtsam/nonlinear/nonlinear.i b/gtsam/nonlinear/nonlinear.i
index f40deab5a..f493fe8f6 100644
--- a/gtsam/nonlinear/nonlinear.i
+++ b/gtsam/nonlinear/nonlinear.i
@@ -11,6 +11,7 @@ namespace gtsam {
 #include <gtsam/geometry/Cal3_S2.h>
 #include <gtsam/geometry/CalibratedCamera.h>
 #include <gtsam/geometry/EssentialMatrix.h>
+#include <gtsam/geometry/FundamentalMatrix.h>
 #include <gtsam/geometry/PinholeCamera.h>
 #include <gtsam/geometry/Point2.h>
 #include <gtsam/geometry/Point3.h>
@@ -537,6 +538,8 @@ class ISAM2 {
   template <VALUE = {gtsam::Point2, gtsam::Rot2, gtsam::Pose2, gtsam::Point3,
                      gtsam::Rot3, gtsam::Pose3, gtsam::Cal3_S2, gtsam::Cal3DS2,
                      gtsam::Cal3Bundler, gtsam::EssentialMatrix,
+                     gtsam::Cal3Bundler, gtsam::FundamentalMatrix,
+                     gtsam::Cal3Bundler, gtsam::SimpleFundamentalMatrix,
                      gtsam::PinholeCamera<gtsam::Cal3_S2>,
                      gtsam::PinholeCamera<gtsam::Cal3Bundler>,
                      gtsam::PinholeCamera<gtsam::Cal3Fisheye>,