From ce196d962fbd62283cda8295cbc93f7cb6722657 Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@gmail.com>
Date: Wed, 30 Oct 2024 16:07:22 -0700
Subject: [PATCH] Wrap EMFs

---
 gtsam/slam/EssentialMatrixFactor.h |  4 +--
 gtsam/slam/slam.i                  | 50 +++++++++++++++++++++++++++---
 2 files changed, 47 insertions(+), 7 deletions(-)

diff --git a/gtsam/slam/EssentialMatrixFactor.h b/gtsam/slam/EssentialMatrixFactor.h
index c8f94681c..7a0b46906 100644
--- a/gtsam/slam/EssentialMatrixFactor.h
+++ b/gtsam/slam/EssentialMatrixFactor.h
@@ -354,7 +354,7 @@ class EssentialMatrixFactor4
    * coordinates
    */
   EssentialMatrixFactor4(Key keyE, Key keyK, const Point2& pA, const Point2& pB,
-                         const SharedNoiseModel& model)
+                         const SharedNoiseModel& model = nullptr)
       : Base(model, keyE, keyK), pA_(pA), pB_(pB) {}
 
   /// @return a deep copy of this factor
@@ -452,7 +452,7 @@ class EssentialMatrixFactor5
    * coordinates
    */
   EssentialMatrixFactor5(Key keyE, Key keyKa, Key keyKb, const Point2& pA,
-                         const Point2& pB, const SharedNoiseModel& model)
+                         const Point2& pB, const SharedNoiseModel& model = nullptr)
       : Base(model, keyE, keyKa, keyKb), pA_(pA), pB_(pB) {}
 
   /// @return a deep copy of this factor
diff --git a/gtsam/slam/slam.i b/gtsam/slam/slam.i
index 97dcfcae7..a226f06ec 100644
--- a/gtsam/slam/slam.i
+++ b/gtsam/slam/slam.i
@@ -107,7 +107,7 @@ typedef gtsam::GeneralSFMFactor<gtsam::PinholePose<gtsam::Cal3Unified>,
                                 gtsam::Point3>
     GeneralSFMFactorPoseCal3Unified;
 
-template <CALIBRATION = {gtsam::Cal3_S2, gtsam::Cal3DS2, gtsam::Cal3Bundler,
+template <CALIBRATION = {gtsam::Cal3_S2, gtsam::Cal3DS2, gtsam::Cal3f, gtsam::Cal3Bundler,
                          gtsam::Cal3Fisheye, gtsam::Cal3Unified}>
 virtual class GeneralSFMFactor2 : gtsam::NoiseModelFactor {
   GeneralSFMFactor2(const gtsam::Point2& measured,
@@ -221,15 +221,55 @@ typedef gtsam::PoseRotationPrior<gtsam::Pose3> PoseRotationPrior3D;
 
 #include <gtsam/slam/EssentialMatrixFactor.h>
 virtual class EssentialMatrixFactor : gtsam::NoiseModelFactor {
-  EssentialMatrixFactor(size_t key, const gtsam::Point2& pA,
-                        const gtsam::Point2& pB,
-                        const gtsam::noiseModel::Base* noiseModel);
+  EssentialMatrixFactor(size_t key, 
+                        const gtsam::Point2& pA, const gtsam::Point2& pB,
+                        const gtsam::noiseModel::Base* model);
   void print(string s = "", const gtsam::KeyFormatter& keyFormatter =
                                 gtsam::DefaultKeyFormatter) const;
-  bool equals(const gtsam::EssentialMatrixFactor& other, double tol) const;
   gtsam::Vector evaluateError(const gtsam::EssentialMatrix& E) const;
 };
 
+virtual class EssentialMatrixFactor2 : gtsam::NoiseModelFactor {
+  EssentialMatrixFactor2(size_t key1, size_t key2, 
+                         const gtsam::Point2& pA, const gtsam::Point2& pB,
+                         const gtsam::noiseModel::Base* model);
+  void print(string s = "", const gtsam::KeyFormatter& keyFormatter =
+                                gtsam::DefaultKeyFormatter) const;
+  gtsam::Vector evaluateError(const gtsam::EssentialMatrix& E, double d) const;
+};
+
+virtual class EssentialMatrixFactor3 : gtsam::EssentialMatrixFactor2 {
+  EssentialMatrixFactor3(size_t key1, size_t key2, 
+                         const gtsam::Point2& pA, const gtsam::Point2& pB,
+                         const gtsam::Rot3& cRb, const gtsam::noiseModel::Base* model);
+  void print(string s = "", const gtsam::KeyFormatter& keyFormatter =
+                                gtsam::DefaultKeyFormatter) const;
+  gtsam::Vector evaluateError(const gtsam::EssentialMatrix& E, double d) const;
+};
+
+template <CALIBRATION = {gtsam::Cal3_S2, gtsam::Cal3DS2, gtsam::Cal3f, gtsam::Cal3Bundler,
+                         gtsam::Cal3Fisheye, gtsam::Cal3Unified}>
+virtual class EssentialMatrixFactor4 : gtsam::NoiseModelFactor {
+  EssentialMatrixFactor4(size_t keyE, size_t keyK,
+                         const gtsam::Point2& pA, const gtsam::Point2& pB,
+                         const gtsam::noiseModel::Base* model = nullptr);
+  void print(string s = "", const gtsam::KeyFormatter& keyFormatter =
+                                gtsam::DefaultKeyFormatter) const;
+  gtsam::Vector evaluateError(const gtsam::EssentialMatrix& E, const CALIBRATION& K) const;
+};
+
+template <CALIBRATION = {gtsam::Cal3_S2, gtsam::Cal3DS2, gtsam::Cal3f, gtsam::Cal3Bundler,
+                         gtsam::Cal3Fisheye, gtsam::Cal3Unified}>
+virtual class EssentialMatrixFactor5 : gtsam::NoiseModelFactor {
+  EssentialMatrixFactor5(size_t keyE, size_t keyKa, size_t keyKb,
+                         const gtsam::Point2& pA, const gtsam::Point2& pB,
+                         const gtsam::noiseModel::Base* model = nullptr);
+  void print(string s = "", const gtsam::KeyFormatter& keyFormatter =
+                                gtsam::DefaultKeyFormatter) const;
+  gtsam::Vector evaluateError(const gtsam::EssentialMatrix& E, 
+                              const CALIBRATION& Ka, const CALIBRATION& Kb) const;
+};
+
 #include <gtsam/slam/EssentialMatrixConstraint.h>
 virtual class EssentialMatrixConstraint : gtsam::NoiseModelFactor {
   EssentialMatrixConstraint(size_t key1, size_t key2, const gtsam::EssentialMatrix &measuredE,