EssentialTransferFactor

gtsam/sfm/TransferFactor.h

@@ -16,37 +16,63 @@
 #pragma once
 
 #include <gtsam/base/numericalDerivative.h>
+#include <gtsam/geometry/EssentialMatrix.h>
 #include <gtsam/geometry/FundamentalMatrix.h>
 #include <gtsam/inference/EdgeKey.h>
+#include <gtsam/inference/Symbol.h>
 #include <gtsam/nonlinear/NonlinearFactor.h>
 
+#include <cstdint>
+
 namespace gtsam {
 
 /**
  * Base class that holds the EdgeKeys and provides the getMatrices method.
  */
 template <typename F>
-struct TransferEdges {
+class TransferEdges {
-  EdgeKey edge1, edge2;  ///< The two EdgeKeys.
+ protected:
+  EdgeKey edge1_, edge2_;  ///< The two EdgeKeys.
+  uint32_t c_;             ///< The transfer target
 
-  TransferEdges(EdgeKey edge1, EdgeKey edge2) : edge1(edge1), edge2(edge2) {}
+ public:
+  TransferEdges(EdgeKey edge1, EdgeKey edge2)
+      : edge1_(edge1), edge2_(edge2), c_(viewC(edge1, edge2)) {}
 
-  // Create Matrix3 objects based on EdgeKey configurations.
+  /// Returns the view A index based on the EdgeKeys
-  std::pair<Matrix3, Matrix3> getMatrices(const F& F1, const F& F2) const {
+  static size_t viewA(const EdgeKey& edge1, const EdgeKey& edge2) {
-    // Fill Fca and Fcb based on EdgeKey configurations.
+    size_t c = viewC(edge1, edge2);
-    if (edge1.i() == edge2.i()) {
+    return (edge1.i() == c) ? edge1.j() : edge1.i();
-      return {F1.matrix(), F2.matrix()};
+  }
-    } else if (edge1.i() == edge2.j()) {
+
-      return {F1.matrix(), F2.matrix().transpose()};
+  /// Returns the view B index based on the EdgeKeys
-    } else if (edge1.j() == edge2.i()) {
+  static size_t viewB(const EdgeKey& edge1, const EdgeKey& edge2) {
-      return {F1.matrix().transpose(), F2.matrix()};
+    size_t c = viewC(edge1, edge2);
-    } else if (edge1.j() == edge2.j()) {
+    return (edge2.i() == c) ? edge2.j() : edge2.i();
-      return {F1.matrix().transpose(), F2.matrix().transpose()};
+  }
-    } else {
+
+  /// Returns the view C index based on the EdgeKeys
+  static size_t viewC(const EdgeKey& edge1, const EdgeKey& edge2) {
+    if (edge1.i() == edge2.i() || edge1.i() == edge2.j())
+      return edge1.i();
+    else if (edge1.j() == edge2.i() || edge1.j() == edge2.j())
+      return edge1.j();
+    else
       throw std::runtime_error(
-          "TransferEdges: invalid EdgeKey configuration between edge1 (" +
+          "EssentialTransferFactor: No common key in edge keys.");
-          std::string(edge1) + ") and edge2 (" + std::string(edge2) + ").");
+  }
-    }
+
+  /// Create Matrix3 objects based on EdgeKey configurations.
+  std::pair<Matrix3, Matrix3> getMatrices(const F& F1, const F& F2) const {
+    // Determine whether to transpose F1
+    const Matrix3 Fca =
+        edge1_.i() == c_ ? F1.matrix() : F1.matrix().transpose();
+
+    // Determine whether to transpose F2
+    const Matrix3 Fcb =
+        edge2_.i() == c_ ? F2.matrix() : F2.matrix().transpose();
+
+    return {Fca, Fcb};
   }
 };
 
@@ -64,26 +90,6 @@ class TransferFactor : public NoiseModelFactorN<F, F>, public TransferEdges<F> {
   std::vector<Triplet> triplets_;  ///< Point triplets.
 
  public:
-  /**
-   * @brief Constructor for a single triplet of points.
-   *
-   * @note Batching all points for the same transfer will be much faster.
-   *
-   * @param edge1 First EdgeKey specifying F1: (a, c) or (c, a).
-   * @param edge2 Second EdgeKey specifying F2: (b, c) or (c, b).
-   * @param pa The point in the first view (a).
-   * @param pb The point in the second view (b).
-   * @param pc The point in the third (and transfer target) view (c).
-   * @param model An optional SharedNoiseModel that defines the noise model
-   *              for this factor. Defaults to nullptr.
-   */
-  TransferFactor(EdgeKey edge1, EdgeKey edge2, const Point2& pa,
-                 const Point2& pb, const Point2& pc,
-                 const SharedNoiseModel& model = nullptr)
-      : Base(model, edge1, edge2),
-        TransferEdges<F>(edge1, edge2),
-        triplets_({std::make_tuple(pa, pb, pc)}) {}
-
   /**
    * @brief Constructor that accepts a vector of point triplets.
    *
@@ -104,25 +110,107 @@ class TransferFactor : public NoiseModelFactorN<F, F>, public TransferEdges<F> {
   Vector evaluateError(const F& F1, const F& F2,
                        OptionalMatrixType H1 = nullptr,
                        OptionalMatrixType H2 = nullptr) const override {
-    std::function<Vector(const F&, const F&)> errorFunction = [&](const F& F1,
+    std::function<Vector(const F&, const F&)> errorFunction = [&](const F& f1,
-                                                                  const F& F2) {
+                                                                  const F& f2) {
       Vector errors(2 * triplets_.size());
       size_t idx = 0;
-      auto [Fca, Fcb] = this->getMatrices(F1, F2);
+      auto [Fca, Fcb] = this->getMatrices(f1, f2);
-      for (const auto& tuple : triplets_) {
+      for (const auto& [pa, pb, pc] : triplets_) {
-        const auto& [pa, pb, pc] = tuple;
+        errors.segment<2>(idx) = EpipolarTransfer(Fca, pa, Fcb, pb) - pc;
-        Point2 transferredPoint = EpipolarTransfer(Fca, pa, Fcb, pb);
-        Vector2 error = transferredPoint - pc;
-        errors.segment<2>(idx) = error;
         idx += 2;
       }
       return errors;
     };
-    if (H1) *H1 = numericalDerivative21<Vector, F, F>(errorFunction, F1, F2);
-    if (H2) *H2 = numericalDerivative22<Vector, F, F>(errorFunction, F1, F2);
-    return errorFunction(F1, F2);
 
+    if (H1) *H1 = numericalDerivative21(errorFunction, F1, F2);
+    if (H2) *H2 = numericalDerivative22(errorFunction, F1, F2);
+    return errorFunction(F1, F2);
   }
 };
 
+/**
+ * @class EssentialTransferFactor
+ * @brief Transfers points between views using essential matrices, optimizes for
+ * calibrations of the views, as well. Note that the EssentialMatrixFactor4 does
+ * something similar but without transfer.
+ *
+ * @note Derives calibration keys from edges, and uses symbol 'k'.
+ *
+ * This factor is templated on the calibration class K and extends
+ * the TransferFactor for EssentialMatrices. It involves two essential matrices
+ * and three calibration objects (Ka, Kb, Kc). The evaluateError
+ * function calibrates the image points, calls the base class's transfer method,
+ * and computes the error using bulk numerical differentiation.
+ */
+template <typename K>
+class EssentialTransferFactor
+    : public NoiseModelFactorN<EssentialMatrix, EssentialMatrix, K, K, K>,
+      TransferEdges<EssentialMatrix> {
+  using EM = EssentialMatrix;
+  using Triplet = std::tuple<Point2, Point2, Point2>;
+  std::vector<Triplet> triplets_;  ///< Point triplets
+
+ public:
+  using Base = NoiseModelFactorN<EM, EM, K, K, K>;
+  using This = EssentialTransferFactor<K>;
+  using shared_ptr = std::shared_ptr<This>;
+
+  /**
+   * @brief Constructor that accepts a vector of point triplets.
+   *
+   * @param edge1 First EdgeKey specifying E1: (a, c) or (c, a)
+   * @param edge2 Second EdgeKey specifying E2: (b, c) or (c, b)
+   * @param triplets A vector of triplets containing (pa, pb, pc)
+   * @param model An optional SharedNoiseModel
+   */
+  EssentialTransferFactor(EdgeKey edge1, EdgeKey edge2,
+                          const std::vector<Triplet>& triplets,
+                          const SharedNoiseModel& model = nullptr)
+      : Base(model, edge1, edge2,
+             Symbol('k', viewA(edge1, edge2)),   // calibration key for view a
+             Symbol('k', viewB(edge1, edge2)),   // calibration key for view b
+             Symbol('k', viewC(edge1, edge2))),  // calibration key for target c
+        TransferEdges<EM>(edge1, edge2),
+        triplets_(triplets) {}
+
+  /// Evaluate error function
+  Vector evaluateError(const EM& E1, const EM& E2, const K& Ka, const K& Kb,
+                       const K& Kc, OptionalMatrixType H1 = nullptr,
+                       OptionalMatrixType H2 = nullptr,
+                       OptionalMatrixType H3 = nullptr,
+                       OptionalMatrixType H4 = nullptr,
+                       OptionalMatrixType H5 = nullptr) const override {
+    std::function<Vector(const EM&, const EM&, const K&, const K&, const K&)>
+        errorFunction = [&](const EM& e1, const EM& e2, const K& kA,
+                            const K& kB, const K& kC) {
+          Vector errors(2 * triplets_.size());
+          size_t idx = 0;
+          auto [Eca, Ecb] = this->getMatrices(e1, e2);
+          for (const auto& [pa, pb, pc] : triplets_) {
+            const Point2 pA = kA.calibrate(pa);
+            const Point2 pB = kB.calibrate(pb);
+            const Point2 pC = EpipolarTransfer(Eca, pA, Ecb, pB);
+            errors.segment<2>(idx) = kC.uncalibrate(pC) - pc;
+            idx += 2;
+          }
+          return errors;
+        };
+
+    // Compute error
+    Vector errors = errorFunction(E1, E2, Ka, Kb, Kc);
+
+    // Compute Jacobians if requested
+    if (H1) *H1 = numericalDerivative51(errorFunction, E1, E2, Ka, Kb, Kc);
+    if (H2) *H2 = numericalDerivative52(errorFunction, E1, E2, Ka, Kb, Kc);
+    if (H3) *H3 = numericalDerivative53(errorFunction, E1, E2, Ka, Kb, Kc);
+    if (H4) *H4 = numericalDerivative54(errorFunction, E1, E2, Ka, Kb, Kc);
+    if (H5) *H5 = numericalDerivative55(errorFunction, E1, E2, Ka, Kb, Kc);
+
+    return errors;
+  }
+
+  /// Return the dimension of the factor
+  size_t dim() const override { return 2 * triplets_.size(); }
+};
+
 }  // namespace gtsam

gtsam/sfm/tests/testTransferFactor.cpp

@@ -1,8 +1,8 @@
 /*
  * @file testTransferFactor.cpp
- * @brief Test TransferFactor class
+ * @brief Test TransferFactor classes
- * @author Your Name
+ * @author Frank Dellaert
- * @date October 23, 2024
+ * @date October 2024
  */
 
 #include <CppUnitLite/TestHarness.h>
@@ -11,9 +11,10 @@
 #include <gtsam/sfm/TransferFactor.h>
 
 using namespace gtsam;
+using symbol_shorthand::K;
 
 //*************************************************************************
-/// Generate three cameras on a circle, looking in
+/// Generate three cameras on a circle, looking inwards
 std::array<Pose3, 3> generateCameraPoses() {
   std::array<Pose3, 3> cameraPoses;
   const double radius = 1.0;
@@ -82,9 +83,9 @@ TEST(TransferFactor, Jacobians) {
   // Create a TransferFactor
   EdgeKey key01(0, 1), key12(1, 2), key20(2, 0);
   TransferFactor<SimpleFundamentalMatrix>  //
-      factor0{key01, key20, p[1], p[2], p[0]},
+      factor0{key01, key20, {{p[1], p[2], p[0]}}},
-      factor1{key12, key01, p[2], p[0], p[1]},
+      factor1{key12, key01, {{p[2], p[0], p[1]}}},
-      factor2{key20, key12, p[0], p[1], p[2]};
+      factor2{key20, key12, {{p[0], p[1], p[2]}}};
 
   // Create Values with edge keys
   Values values;
@@ -153,9 +154,68 @@ TEST(TransferFactor, MultipleTuples) {
   EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-5, 1e-7);
 }
 
+//*************************************************************************
+// Test for EssentialTransferFactor
+TEST(EssentialTransferFactor, Jacobians) {
+  // Generate cameras on a circle
+  std::array<Pose3, 3> cameraPoses = generateCameraPoses();
+
+  // Create calibration
+  const Cal3_S2 commonK(focalLength, focalLength, 0.0, principalPoint.x(),
+                        principalPoint.y());
+
+  // Create cameras
+  std::array<PinholeCamera<Cal3_S2>, 3> cameras;
+  for (size_t i = 0; i < 3; ++i) {
+    cameras[i] = PinholeCamera<Cal3_S2>(cameraPoses[i], commonK);
+  }
+
+  // Create EssentialMatrices between cameras
+  EssentialMatrix E01 =
+      EssentialMatrix::FromPose3(cameraPoses[0].between(cameraPoses[1]));
+  EssentialMatrix E02 =
+      EssentialMatrix::FromPose3(cameraPoses[0].between(cameraPoses[2]));
+
+  // Project a point into the three cameras
+  const Point3 P(0.1, 0.2, 0.3);
+  std::array<Point2, 3> p;
+  for (size_t i = 0; i < 3; ++i) {
+    p[i] = cameras[i].project(P);
+  }
+
+  // Create EdgeKeys
+  EdgeKey key01(0, 1);
+  EdgeKey key02(0, 2);
+
+  // Create an EssentialTransferFactor
+  EssentialTransferFactor<Cal3_S2> factor(key01, key02, {{p[1], p[2], p[0]}});
+
+  // Create Values and insert variables
+  Values values;
+  values.insert(key01, E01);     // Essential matrix between views 0 and 1
+  values.insert(key02, E02);     // Essential matrix between views 0 and 2
+  values.insert(K(1), commonK);  // Calibration for view A (view 1)
+  values.insert(K(2), commonK);  // Calibration for view B (view 2)
+  values.insert(K(0), commonK);  // Calibration for view C (view 0)
+
+  // Check error
+  Matrix H1, H2, H3, H4, H5;
+  Vector error = factor.evaluateError(E01, E02, commonK, commonK, commonK,  //
+                                      &H1, &H2, &H3, &H4, &H5);
+  EXPECT(H1.rows() == 2 && H1.cols() == 5)
+  EXPECT(H2.rows() == 2 && H2.cols() == 5)
+  EXPECT(H3.rows() == 2 && H3.cols() == 5)
+  EXPECT(H4.rows() == 2 && H4.cols() == 5)
+  EXPECT(H5.rows() == 2 && H5.cols() == 5)
+  EXPECT(assert_equal(Vector::Zero(2), error, 1e-9))
+
+  // Check Jacobians
+  EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-5, 1e-7);
+}
+
 //*************************************************************************
 int main() {
   TestResult tr;
   return TestRegistry::runAllTests(tr);
 }
-//*************************************************************************
+//*************************************************************************