Use EdgeKey logic

gtsam/sfm/TransferFactor.h

@@ -17,56 +17,83 @@
 
 #include <gtsam/base/numericalDerivative.h>
 #include <gtsam/geometry/FundamentalMatrix.h>
+#include <gtsam/inference/EdgeKey.h>
 #include <gtsam/nonlinear/NonlinearFactor.h>
 
 namespace gtsam {
 
-template <typename F>
+/**
-struct TripletError {
+ * Binary factor in the context of Structure from Motion (SfM).
-  Point2 p0, p1, p2;
+ * It is used to transfer points between different views based on the
-
+ * fundamental matrices between these views. The factor computes the error
-  /// vector of errors returns 6D vector
+ * between the transferred points `pi` and `pj`, and the actual point `pk` in
-  Vector evaluateError(const F& F01, const F& F12, const F& F20,  //
+ * the target view. Jacobians are done using numerical differentiation.
-                       Matrix* H01, Matrix* H12, Matrix* H20) const {
+ */
-    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);
-  }
-};
-
 template <typename F>
 class TransferFactor : public NoiseModelFactorN<F, F> {
-  Point2 p0, p1, p2;
+  EdgeKey key1_, key2_;  ///< the two EdgeKeys
+  Point2 pi, pj, pk;     ///< The points in the three views
 
  public:
-  // Constructor
+  /**
-  TransferFactor(Key key1, Key key2, const Point2& p0, const Point2& p1,
+   * @brief Constructor for the TransferFactor class.
-                 const Point2& p2, const SharedNoiseModel& model = nullptr)
+   *
-      : NoiseModelFactorN<F, F>(model, key1, key2), p0(p0), p1(p1), p2(p2) {}
+   * Uses EdgeKeys to determine how to use the two fundamental matrix unknowns
+   * F1 and F2, to transfer points pi and pj to the third view, and minimize the
+   * difference with pk.
+   *
+   * The edge keys must represent valid edges for the transfer operation,
+   * specifically one of the following configurations:
+   * - (i, k) and (j, k)
+   * - (i, k) and (k, j)
+   * - (k, i) and (j, k)
+   * - (k, i) and (k, j)
+   *
+   * @param key1 First EdgeKey specifying F1: (i, k) or (k, i).
+   * @param key2 Second EdgeKey specifying F2: (j, k) or (k, j).
+   * @param pi The point in the first view (i).
+   * @param pj The point in the second view (j).
+   * @param pk The point in the third (and transfer target) view (k).
+   * @param model An optional SharedNoiseModel that defines the noise model
+   *              for this factor. Defaults to nullptr.
+   */
+  TransferFactor(EdgeKey key1, EdgeKey key2, const Point2& pi, const Point2& pj,
+                 const Point2& pk, const SharedNoiseModel& model = nullptr)
+      : NoiseModelFactorN<F, F>(model, key1, key2),
+        key1_(key1),
+        key2_(key2),
+        pi(pi),
+        pj(pj),
+        pk(pk) {}
+
+  // Create Matrix3 objects based on EdgeKey configurations
+  std::pair<Matrix3, Matrix3> getMatrices(const F& F1, const F& F2) const {
+    // Fill Fki and Fkj based on EdgeKey configurations
+    if (key1_.i() == key2_.i()) {
+      return {F1.matrix(), F2.matrix()};
+    } else if (key1_.i() == key2_.j()) {
+      return {F1.matrix(), F2.matrix().transpose()};
+    } else if (key1_.j() == key2_.i()) {
+      return {F1.matrix().transpose(), F2.matrix()};
+    } else if (key1_.j() == key2_.j()) {
+      return {F1.matrix().transpose(), F2.matrix().transpose()};
+    } else {
+      throw std::runtime_error(
+          "TransferFactor: invalid EdgeKey configuration.");
+    }
+  }
 
   /// vector of errors returns 2D vector
-  Vector evaluateError(const F& F12, const F& F20,  //
+  Vector evaluateError(const F& F1, const F& F2,
-                       OptionalMatrixType H12 = nullptr,
+                       OptionalMatrixType H1 = nullptr,
-                       OptionalMatrixType H20 = nullptr) const override {
+                       OptionalMatrixType H2 = nullptr) const override {
-    std::function<Vector2(F, F)> fn = [&](const F& F12, const F& F20) {
+    std::function<Point2(F, F)> transfer = [&](const F& F1, const F& F2) {
-      Vector2 error;
+      auto [Fki, Fkj] = getMatrices(F1, F2);
-      error <<  //
+      return F::transfer(Fki, pi, Fkj, pj);
-          F::transfer(F20.matrix(), p0, F12.matrix().transpose(), p1) - p2;
-      return error;
     };
-    if (H12) *H12 = numericalDerivative21<Vector2, F, F>(fn, F12, F20);
+    if (H1) *H1 = numericalDerivative21<Point2, F, F>(transfer, F1, F2);
-    if (H20) *H20 = numericalDerivative22<Vector2, F, F>(fn, F12, F20);
+    if (H2) *H2 = numericalDerivative22<Point2, F, F>(transfer, F1, F2);
-    return fn(F12, F20);
+    return transfer(F1, F2) - pk;
   }
 };
 

gtsam/sfm/tests/testTransferFactor.cpp

@@ -6,20 +6,15 @@
  */
 
 #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;
 
+double focalLength = 1000;
+Point2 principalPoint(640 / 2, 480 / 2);
+
 //*************************************************************************
 /// Generate three cameras on a circle, looking in
 std::array<Pose3, 3> generateCameraPoses() {
@@ -34,6 +29,7 @@ std::array<Pose3, 3> generateCameraPoses() {
   return cameraPoses;
 }
 
+//*************************************************************************
 // Function to generate a TripleF from camera poses
 TripleF<SimpleFundamentalMatrix> generateTripleF(
     const std::array<Pose3, 3>& cameraPoses) {
@@ -48,9 +44,7 @@ TripleF<SimpleFundamentalMatrix> generateTripleF(
   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
@@ -70,28 +64,29 @@ TEST(TransferFactor, Jacobians) {
   }
 
   // Create a TransferFactor
-  TripletError<SimpleFundamentalMatrix> error{p[0], p[1], p[2]};
+  EdgeKey key01(0, 1), key12(1, 2), key20(2, 0);
-  Matrix H01, H12, H20;
+  TransferFactor<SimpleFundamentalMatrix>  //
-  Vector e = error.evaluateError(triplet.F01, triplet.F12, triplet.F20, &H01,
+      factor0{key01, key20, p[1], p[2], p[0]},
-                                 &H12, &H20);
+      factor1{key12, key01, p[2], p[0], p[1]},
-  std::cout << "Error: " << e << std::endl;
+      factor2{key20, key12, p[0], p[1], p[2]};
-  std::cout << H01 << std::endl << std::endl;
-  std::cout << H12 << std::endl << std::endl;
-  std::cout << H20 << std::endl;
 
-  // Create a TransferFactor
+  // Check that getMatrices is correct
-  TransferFactor<SimpleFundamentalMatrix> factor{0, 1, p[0], p[1], p[2]};
+  auto [Fki, Fkj] = factor2.getMatrices(triplet.Fca, triplet.Fbc);
-  Matrix H0, H1;
+  EXPECT(assert_equal<Matrix3>(triplet.Fca.matrix(), Fki));
-  Vector e2 = factor.evaluateError(triplet.F12, triplet.F20, &H0, &H1);
+  EXPECT(assert_equal<Matrix3>(triplet.Fbc.matrix().transpose(), Fkj));
-  std::cout << "Error: " << e2 << std::endl;
-  std::cout << H0 << std::endl << std::endl;
-  std::cout << H1 << std::endl << std::endl;
 
-  // Check Jacobians
+  // Create Values with edge keys
   Values values;
-  values.insert(0, triplet.F12);
+  values.insert(key01, triplet.Fab);
-  values.insert(1, triplet.F20);
+  values.insert(key12, triplet.Fbc);
-  EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-5, 1e-7);
+  values.insert(key20, triplet.Fca);
+
+  // Check error and Jacobians
+  for (auto&& f : {factor0, factor1, factor2}) {
+    Vector error = f.unwhitenedError(values);
+    EXPECT(assert_equal<Vector>(Z_2x1, error));
+    EXPECT_CORRECT_FACTOR_JACOBIANS(f, values, 1e-5, 1e-7);
+  }
 }
 
 //*************************************************************************