TransferFactor in sfm

gtsam/geometry/tests/testFundamentalMatrix.cpp

@@ -197,35 +197,6 @@ 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

gtsam/sfm/TransferFactor.h

@@ -0,0 +1,48 @@
+/* ----------------------------------------------------------------------------
+ * GTSAM Copyright 2010-2024, Georgia Tech Research Corporation,
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+ * See LICENSE for the license information
+ * -------------------------------------------------------------------------- */
+
+/*
+ * @file TransferFactor.h
+ * @brief TransferFactor class
+ * @author Frank Dellaert
+ * @date October 24, 2024
+ */
+
+#pragma once
+
+#include <gtsam/base/numericalDerivative.h>
+#include <gtsam/geometry/FundamentalMatrix.h>
+#include <gtsam/nonlinear/NonlinearFactor.h>
+
+namespace gtsam {
+
+template <typename F>
+struct TransferFactor {
+  Point2 p0, p1, p2;
+
+  /// vector of errors returns 6D vector
+  Vector evaluateError(const F& F01, const F& F12, const F& F20,  //
+                       Matrix* H01, Matrix* H12, Matrix* H20) const {
+    Vector error(6);
+    std::function<Vector6(F, F, F)> fn = [&](const F& F01, const F& F12,
+                                             const F& 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, F, F, F>(fn, F01, F12, F20);
+    if (H12) *H12 = numericalDerivative32<Vector6, F, F, F>(fn, F01, F12, F20);
+    if (H20) *H20 = numericalDerivative33<Vector6, F, F, F>(fn, F01, F12, F20);
+    return fn(F01, F12, F20);
+  }
+};
+
+}  // namespace gtsam

gtsam/sfm/tests/testTransferFactor.cpp

@@ -0,0 +1,95 @@
+/*
+ * @file testTransferFactor.cpp
+ * @brief Test TransferFactor class
+ * @author Your Name
+ * @date October 23, 2024
+ */
+
+#include <CppUnitLite/TestHarness.h>
+#include <gtsam/geometry/Cal3_S2.h>
+#include <gtsam/geometry/EssentialMatrix.h>
+#include <gtsam/geometry/FundamentalMatrix.h>
+#include <gtsam/geometry/Point2.h>
+#include <gtsam/geometry/Point3.h>
+#include <gtsam/geometry/Rot3.h>
+#include <gtsam/geometry/SimpleCamera.h>
+#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
+#include <gtsam/nonlinear/NonlinearFactorGraph.h>
+#include <gtsam/nonlinear/factorTesting.h>
+#include <gtsam/sfm/TransferFactor.h>
+
+using namespace gtsam;
+
+//*************************************************************************
+/// Generate three cameras on a circle, looking in
+std::array<Pose3, 3> generateCameraPoses() {
+  std::array<Pose3, 3> cameraPoses;
+  const double radius = 1.0;
+  for (int i = 0; i < 3; ++i) {
+    double angle = i * 2.0 * M_PI / 3.0;
+    double c = cos(angle), s = sin(angle);
+    Rot3 aRb({-s, c, 0}, {0, 0, -1}, {-c, -s, 0});
+    cameraPoses[i] = {aRb, Point3(radius * c, radius * s, 0)};
+  }
+  return cameraPoses;
+}
+
+// Function to generate a TripleF from camera poses
+TripleF<SimpleFundamentalMatrix> generateTripleF(
+    const std::array<Pose3, 3>& cameraPoses) {
+  std::array<SimpleFundamentalMatrix, 3> F;
+  for (size_t i = 0; i < 3; ++i) {
+    size_t j = (i + 1) % 3;
+    const Pose3 iPj = cameraPoses[i].between(cameraPoses[j]);
+    EssentialMatrix E(iPj.rotation(), Unit3(iPj.translation()));
+    F[i] = {E, 1000.0, 1000.0, Point2(640 / 2, 480 / 2),
+            Point2(640 / 2, 480 / 2)};
+  }
+  return {F[0], F[1], F[2]};  // Return a TripleF instance
+}
+
+double focalLength = 1000;
+Point2 principalPoint(640 / 2, 480 / 2);
+
+// Test for TransferFactor
+TEST(TransferFactor, Jacobians) {
+  // Generate cameras on a circle
+  std::array<Pose3, 3> cameraPoses = generateCameraPoses();
+  auto triplet = generateTripleF(cameraPoses);
+
+  // Now project a point into the three cameras
+  const Point3 P(0.1, 0.2, 0.3);
+  const Cal3_S2 K(focalLength, focalLength, 0.0,  //
+                  principalPoint.x(), principalPoint.y());
+
+  std::array<Point2, 3> p;
+  for (size_t i = 0; i < 3; ++i) {
+    // Project the point into each camera
+    PinholeCameraCal3_S2 camera(cameraPoses[i], K);
+    p[i] = camera.project(P);
+  }
+
+  // Create a TransferFactor
+  TransferFactor<SimpleFundamentalMatrix> factor{p[0], p[1], p[2]};
+  Matrix H01, H12, H20;
+  Vector error = factor.evaluateError(triplet.F01, triplet.F12, triplet.F20,
+                                      &H01, &H12, &H20);
+  std::cout << "Error: " << error << std::endl;
+  std::cout << H01 << std::endl << std::endl;
+  std::cout << H12 << std::endl << std::endl;
+  std::cout << H20 << std::endl;
+
+  // Check Jacobians
+  Values values;
+  values.insert(0, triplet.F01);
+  values.insert(1, triplet.F12);
+  values.insert(2, triplet.F20);
+  // EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-5, 1e-7);
+}
+
+//*************************************************************************
+int main() {
+  TestResult tr;
+  return TestRegistry::runAllTests(tr);
+}
+//*************************************************************************