From 8a0ab6e094ed3be2387f44610b7fca7a4adbf236 Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@gmail.com>
Date: Thu, 24 Oct 2024 01:09:01 -0700
Subject: [PATCH] proto-factor with numerical derivative

---
 .../geometry/tests/testFundamentalMatrix.cpp  | 29 +++++++++++++++++++
 1 file changed, 29 insertions(+)

diff --git a/gtsam/geometry/tests/testFundamentalMatrix.cpp b/gtsam/geometry/tests/testFundamentalMatrix.cpp
index a8884e791..80e6b1d3a 100644
--- a/gtsam/geometry/tests/testFundamentalMatrix.cpp
+++ b/gtsam/geometry/tests/testFundamentalMatrix.cpp
@@ -197,6 +197,35 @@ TripleF<SimpleFundamentalMatrix> generateTripleF(
   return {F[0], F[1], F[2]};  // Return a TripleF instance
 }
 
+//*************************************************************************
+
+struct TripletFactor {
+  using F = FundamentalMatrix;
+  using SF = SimpleFundamentalMatrix;
+  Point2 p0, p1, p2;
+
+  /// vector of errors returns 6D vector
+  Vector evaluateError(const SF& F01, const SF& F12, const SF& F20,  //
+                       Matrix* H01, Matrix* H12, Matrix* H20) const {
+    Vector error(6);
+    std::function<Vector6(SF, SF, SF)> fn = [&](const SF& F01, const SF& F12,
+                                                const SF& F20) {
+      Vector6 error;
+      error << F::transfer(F01.matrix(), p1, F20.matrix().transpose(), p2) - p0,
+          F::transfer(F01.matrix().transpose(), p0, F12.matrix(), p2) - p1,
+          F::transfer(F20.matrix(), p0, F12.matrix().transpose(), p1) - p2;
+      return error;
+    };
+    if (H01)
+      *H01 = numericalDerivative31<Vector6, SF, SF, SF>(fn, F01, F12, F20);
+    if (H12)
+      *H12 = numericalDerivative32<Vector6, SF, SF, SF>(fn, F01, F12, F20);
+    if (H20)
+      *H20 = numericalDerivative33<Vector6, SF, SF, SF>(fn, F01, F12, F20);
+    return fn(F01, F12, F20);
+  }
+};
+
 //*************************************************************************
 TEST(TripleF, Transfer) {
   // Generate cameras on a circle, as well as fundamental matrices