From e6bfcada40375bd8a742e111bcdfa58d994b0e3b Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@gmail.com>
Date: Thu, 24 Oct 2024 13:23:00 -0700
Subject: [PATCH] True factor

---
 gtsam/sfm/TransferFactor.h             | 11 +++++++++--
 gtsam/sfm/tests/testTransferFactor.cpp |  8 ++++----
 2 files changed, 13 insertions(+), 6 deletions(-)

diff --git a/gtsam/sfm/TransferFactor.h b/gtsam/sfm/TransferFactor.h
index cec7a445e..fd65db8b9 100644
--- a/gtsam/sfm/TransferFactor.h
+++ b/gtsam/sfm/TransferFactor.h
@@ -45,12 +45,19 @@ struct TripletError {
 };
 
 template <typename F>
-struct TransferFactor {
+class TransferFactor : public NoiseModelFactorN<F, F> {
   Point2 p0, p1, p2;
 
+ public:
+  // Constructor
+  TransferFactor(Key key1, Key key2, const Point2& p0, const Point2& p1,
+                 const Point2& p2, const SharedNoiseModel& model = nullptr)
+      : NoiseModelFactorN<F, F>(model, key1, key2), p0(p0), p1(p1), p2(p2) {}
+
   /// vector of errors returns 2D vector
   Vector evaluateError(const F& F12, const F& F20,  //
-                       Matrix* H12, Matrix* H20) const {
+                       OptionalMatrixType H12 = nullptr,
+                       OptionalMatrixType H20 = nullptr) const override {
     std::function<Vector2(F, F)> fn = [&](const F& F12, const F& F20) {
       Vector2 error;
       error <<  //
diff --git a/gtsam/sfm/tests/testTransferFactor.cpp b/gtsam/sfm/tests/testTransferFactor.cpp
index 44b03ad5d..afceb8fdb 100644
--- a/gtsam/sfm/tests/testTransferFactor.cpp
+++ b/gtsam/sfm/tests/testTransferFactor.cpp
@@ -80,7 +80,7 @@ TEST(TransferFactor, Jacobians) {
   std::cout << H20 << std::endl;
 
   // Create a TransferFactor
-  TransferFactor<SimpleFundamentalMatrix> factor{p[0], p[1], p[2]};
+  TransferFactor<SimpleFundamentalMatrix> factor{0, 1, p[0], p[1], p[2]};
   Matrix H0, H1;
   Vector e2 = factor.evaluateError(triplet.F12, triplet.F20, &H0, &H1);
   std::cout << "Error: " << e2 << std::endl;
@@ -89,9 +89,9 @@ TEST(TransferFactor, Jacobians) {
 
   // Check Jacobians
   Values values;
-  values.insert(1, triplet.F12);
-  values.insert(2, triplet.F20);
-  // EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-5, 1e-7);
+  values.insert(0, triplet.F12);
+  values.insert(1, triplet.F20);
+  EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-5, 1e-7);
 }
 
 //*************************************************************************