Differentiated EssentialTransferFactor and EssentialTransferFactorK

examples/EssentialViewGraphExample.cpp

@@ -27,7 +27,7 @@
 #include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
 #include <gtsam/nonlinear/NonlinearFactorGraph.h>
 #include <gtsam/nonlinear/Values.h>
-#include <gtsam/sfm/TransferFactor.h>  // Contains EssentialTransferFactor
+#include <gtsam/sfm/TransferFactor.h>  // Contains EssentialTransferFactorK
 
 #include <vector>
 
@@ -63,7 +63,7 @@ int main(int argc, char* argv[]) {
 
   // Create the factor graph
   NonlinearFactorGraph graph;
-  using Factor = EssentialTransferFactor<Cal3f>;
+  using Factor = EssentialTransferFactorK<Cal3f>;
 
   for (size_t a = 0; a < 4; ++a) {
     size_t b = (a + 1) % 4;  // Next camera

gtsam/sfm/TransferFactor.h

@@ -59,7 +59,7 @@ class TransferEdges {
       return edge1.j();
     else
       throw std::runtime_error(
-          "EssentialTransferFactor: No common key in edge keys.");
+          "EssentialTransferFactorK: No common key in edge keys.");
   }
 
   /// Create Matrix3 objects based on EdgeKey configurations.
@@ -85,8 +85,11 @@ class TransferEdges {
  */
 template <typename F>
 class TransferFactor : public NoiseModelFactorN<F, F>, public TransferEdges<F> {
+ public:
   using Base = NoiseModelFactorN<F, F>;
   using Triplet = std::tuple<Point2, Point2, Point2>;
+
+ protected:
   std::vector<Triplet> triplets_;  ///< Point triplets.
 
  public:
@@ -130,6 +133,81 @@ class TransferFactor : public NoiseModelFactorN<F, F>, public TransferEdges<F> {
 
 /**
  * @class EssentialTransferFactor
+ * @brief Transfers points between views using essential matrices with a shared
+ * calibration.
+ *
+ * This factor is templated on the calibration class K and extends
+ * the TransferFactor for EssentialMatrices. It involves two essential matrices
+ * and a shared calibration object (K). 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 TransferFactor<EssentialMatrix> {
+  using EM = EssentialMatrix;
+  using Triplet = std::tuple<Point2, Point2, Point2>;
+  std::shared_ptr<K> calibration_;  ///< Shared pointer to calibration object
+
+ public:
+  using Base = TransferFactor<EM>;
+  using This = EssentialTransferFactor<K>;
+  using shared_ptr = std::shared_ptr<This>;
+
+  /**
+   * @brief Constructor that accepts a vector of point triplets and a shared
+   * calibration.
+   *
+   * @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 calibration Shared pointer to calibration object
+   * @param model An optional SharedNoiseModel
+   */
+  EssentialTransferFactor(EdgeKey edge1, EdgeKey edge2,
+                          const std::vector<Triplet>& triplets,
+                          const std::shared_ptr<K>& calibration,
+                          const SharedNoiseModel& model = nullptr)
+      : Base(edge1, edge2, triplets, model), calibration_(calibration) {}
+
+  /// Transfer points pa and pb to view c and evaluate error.
+  Vector2 TransferError(const Matrix3& Eca, const Point2& pa,
+                        const Matrix3& Ecb, const Point2& pb,
+                        const Point2& pc) const {
+    const Point2 pA = calibration_->calibrate(pa);
+    const Point2 pB = calibration_->calibrate(pb);
+    const Point2 pC = EpipolarTransfer(Eca, pA, Ecb, pB);
+    return calibration_->uncalibrate(pC) - pc;
+  }
+
+  /// Evaluate error function
+  Vector evaluateError(const EM& E1, const EM& E2,
+                       OptionalMatrixType H1 = nullptr,
+                       OptionalMatrixType H2 = nullptr) const override {
+    std::function<Vector(const EM&, const EM&)> errorFunction =
+        [&](const EM& e1, const EM& e2) {
+          Vector errors(2 * this->triplets_.size());
+          size_t idx = 0;
+          auto [Eca, Ecb] = this->getMatrices(e1, e2);
+          for (const auto& [pa, pb, pc] : this->triplets_) {
+            errors.segment<2>(idx) = TransferError(Eca, pa, Ecb, pb, pc);
+            idx += 2;
+          }
+          return errors;
+        };
+
+    // Compute error
+    Vector errors = errorFunction(E1, E2);
+
+    // Compute Jacobians if requested
+    if (H1) *H1 = numericalDerivative21(errorFunction, E1, E2);
+    if (H2) *H2 = numericalDerivative22(errorFunction, E1, E2);
+
+    return errors;
+  }
+};
+
+/**
+ * @class EssentialTransferFactorK
  * @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.
@@ -143,7 +221,7 @@ class TransferFactor : public NoiseModelFactorN<F, F>, public TransferEdges<F> {
  * and computes the error using bulk numerical differentiation.
  */
 template <typename K>
-class EssentialTransferFactor
+class EssentialTransferFactorK
     : public NoiseModelFactorN<EssentialMatrix, EssentialMatrix, K, K, K>,
       TransferEdges<EssentialMatrix> {
   using EM = EssentialMatrix;
@@ -152,7 +230,7 @@ class EssentialTransferFactor
 
  public:
   using Base = NoiseModelFactorN<EM, EM, K, K, K>;
-  using This = EssentialTransferFactor<K>;
+  using This = EssentialTransferFactorK<K>;
   using shared_ptr = std::shared_ptr<This>;
 
   /**
@@ -163,9 +241,9 @@ class EssentialTransferFactor
    * @param triplets A vector of triplets containing (pa, pb, pc)
    * @param model An optional SharedNoiseModel
    */
-  EssentialTransferFactor(EdgeKey edge1, EdgeKey edge2,
+  EssentialTransferFactorK(EdgeKey edge1, EdgeKey edge2,
-                          const std::vector<Triplet>& triplets,
+                           const std::vector<Triplet>& triplets,
-                          const SharedNoiseModel& model = nullptr)
+                           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
@@ -173,6 +251,16 @@ class EssentialTransferFactor
         TransferEdges<EM>(edge1, edge2),
         triplets_(triplets) {}
 
+  /// Transfer points pa and pb to view c and evaluate error.
+  Vector2 TransferError(const Matrix3& Eca, const K& Ka, const Point2& pa,
+                        const Matrix3& Ecb, const K& Kb, const Point2& pb,
+                        const K& Kc, const Point2& pc) const {
+    const Point2 pA = Ka.calibrate(pa);
+    const Point2 pB = Kb.calibrate(pb);
+    const Point2 pC = EpipolarTransfer(Eca, pA, Ecb, pB);
+    return Kc.uncalibrate(pC) - pc;
+  }
+
   /// Evaluate error function
   Vector evaluateError(const EM& E1, const EM& E2, const K& Ka, const K& Kb,
                        const K& Kc, OptionalMatrixType H1 = nullptr,
@@ -187,10 +275,8 @@ class EssentialTransferFactor
           size_t idx = 0;
           auto [Eca, Ecb] = this->getMatrices(e1, e2);
           for (const auto& [pa, pb, pc] : triplets_) {
-            const Point2 pA = kA.calibrate(pa);
+            errors.segment<2>(idx) =
-            const Point2 pB = kB.calibrate(pb);
+                TransferError(Eca, kA, pa, Ecb, kB, pb, kC, pc);
-            const Point2 pC = EpipolarTransfer(Eca, pA, Ecb, pB);
-            errors.segment<2>(idx) = kC.uncalibrate(pC) - pc;
             idx += 2;
           }
           return errors;

gtsam/sfm/sfm.i

@@ -77,11 +77,11 @@ gtsam::Values initialCamerasAndPointsEstimate(const gtsam::SfmData& db);
 
 #include <gtsam/sfm/TransferFactor.h>
 #include <gtsam/geometry/FundamentalMatrix.h>
-template <F = {gtsam::EssentialMatrix, gtsam::SimpleFundamentalMatrix, gtsam::FundamentalMatrix}>
+template <F = {gtsam::SimpleFundamentalMatrix, gtsam::FundamentalMatrix}>
 virtual class TransferFactor : gtsam::NoiseModelFactor {
   TransferFactor(gtsam::EdgeKey edge1, gtsam::EdgeKey edge2,
-                 const std::vector<std::tuple<gtsam::Point2, gtsam::Point2, gtsam::Point2>>& triplets
+                 const std::vector<std::tuple<gtsam::Point2, gtsam::Point2, gtsam::Point2>>& triplets,
-                );
+                 const gtsam::noiseModel::Base* model = nullptr);
 };
 
 #include <gtsam/geometry/Cal3_S2.h>
@@ -90,8 +90,16 @@ virtual class TransferFactor : gtsam::NoiseModelFactor {
 template <K = {gtsam::Cal3_S2, gtsam::Cal3f, gtsam::Cal3Bundler}>
 virtual class EssentialTransferFactor : gtsam::NoiseModelFactor {
   EssentialTransferFactor(gtsam::EdgeKey edge1, gtsam::EdgeKey edge2,
-                          const std::vector<std::tuple<gtsam::Point2, gtsam::Point2, gtsam::Point2>>& triplets
+                          const std::vector<std::tuple<gtsam::Point2, gtsam::Point2, gtsam::Point2>>& triplets,
-                         );
+                          const K* calibration,
+                          const gtsam::noiseModel::Base* model = nullptr);
+};
+
+template <K = {gtsam::Cal3_S2, gtsam::Cal3f, gtsam::Cal3Bundler}>
+virtual class EssentialTransferFactorK : gtsam::NoiseModelFactor {
+  EssentialTransferFactorK(gtsam::EdgeKey edge1, gtsam::EdgeKey edge2,
+                          const std::vector<std::tuple<gtsam::Point2, gtsam::Point2, gtsam::Point2>>& triplets,
+                          const gtsam::noiseModel::Base* model = nullptr);
 };
 
 #include <gtsam/sfm/ShonanFactor.h>

gtsam/sfm/tests/testTransferFactor.cpp

@@ -6,10 +6,13 @@
  */
 
 #include <CppUnitLite/TestHarness.h>
+#include <gtsam/geometry/Cal3_S2.h>
 #include <gtsam/geometry/SimpleCamera.h>
 #include <gtsam/nonlinear/factorTesting.h>
 #include <gtsam/sfm/TransferFactor.h>
 
+#include <memory>
+
 using namespace gtsam;
 using symbol_shorthand::K;
 
@@ -173,7 +176,51 @@ TEST(EssentialTransferFactor, Jacobians) {
   EdgeKey key02(0, 2);
 
   // Create an EssentialTransferFactor
-  EssentialTransferFactor<Cal3_S2> factor(key01, key02, {{p[1], p[2], p[0]}});
+  auto sharedCal = std::make_shared<Cal3_S2>(cal);
+  EssentialTransferFactor<Cal3_S2> factor(key01, key02, {{p[1], p[2], p[0]}},
+                                          sharedCal);
+
+  // 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
+
+  // Check error
+  Matrix H1, H2, H3, H4, H5;
+  Vector error = factor.evaluateError(E01, E02,  //
+                                      &H1, &H2);
+  EXPECT(H1.rows() == 2 && H1.cols() == 5)
+  EXPECT(H2.rows() == 2 && H2.cols() == 5)
+  EXPECT(assert_equal(Vector::Zero(2), error, 1e-9))
+
+  // Check Jacobians
+  EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-5, 1e-7);
+}
+
+//*************************************************************************
+// Test for EssentialTransferFactorK
+TEST(EssentialTransferFactorK, Jacobians) {
+  using namespace fixture;
+
+  // 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 EssentialTransferFactorK
+  EssentialTransferFactorK<Cal3_S2> factor(key01, key02, {{p[1], p[2], p[0]}});
 
   // Create Values and insert variables
   Values values;
@@ -185,10 +232,9 @@ TEST(EssentialTransferFactor, Jacobians) {
 
   // Check error
   Matrix H1, H2, H3, H4, H5;
-  Vector error =
+  Vector error = factor.evaluateError(E01, E02,       //
-      factor.evaluateError(E01, E02,                                  //
+                                      cal, cal, cal,  //
-                           fixture::cal, fixture::cal, fixture::cal,  //
+                                      &H1, &H2, &H3, &H4, &H5);
-                           &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)

python/gtsam/examples/ViewGraphComparison.py

@@ -105,22 +105,18 @@ def build_factor_graph(method, num_cameras, measurements, cal):
     elif method == "SimpleF":
         FactorClass = gtsam.TransferFactorSimpleFundamentalMatrix
     elif method == "Essential+Ks":
-        FactorClass = gtsam.EssentialTransferFactorCal3f
+        FactorClass = gtsam.EssentialTransferFactorKCal3f
         # add priors on all calibrations:
         for i in range(num_cameras):
             model = gtsam.noiseModel.Isotropic.Sigma(1, 10.0)
             graph.addPriorCal3f(K(i), cal, model)
     elif method == "Calibrated":
-        FactorClass = gtsam.TransferFactorEssentialMatrix
+        FactorClass = gtsam.EssentialTransferFactorCal3f
         # No priors on calibration needed
     else:
         raise ValueError(f"Unknown method {method}")
 
-    if method == "Calibrated":
+    z = measurements  # shorthand
-        # Calibrate measurements using ground truth calibration
-        z = [[cal.calibrate(m) for m in cam_measurements] for cam_measurements in measurements]
-    else:
-        z = measurements
 
     for a in range(num_cameras):
         b = (a + 1) % num_cameras  # Next camera
@@ -138,9 +134,14 @@ def build_factor_graph(method, num_cameras, measurements, cal):
             tuples3.append((z[c][j], z[b][j], z[a][j]))
 
         # Add transfer factors between views a, b, and c.
-        graph.add(FactorClass(EdgeKey(a, c), EdgeKey(b, c), tuples1))
+        if method in ["Calibrated"]:
-        graph.add(FactorClass(EdgeKey(a, b), EdgeKey(b, c), tuples2))
+            graph.add(FactorClass(EdgeKey(a, c), EdgeKey(b, c), tuples1, cal))
-        graph.add(FactorClass(EdgeKey(a, c), EdgeKey(a, b), tuples3))
+            graph.add(FactorClass(EdgeKey(a, b), EdgeKey(b, c), tuples2, cal))
+            graph.add(FactorClass(EdgeKey(a, c), EdgeKey(a, b), tuples3, cal))
+        else:
+            graph.add(FactorClass(EdgeKey(a, c), EdgeKey(b, c), tuples1))
+            graph.add(FactorClass(EdgeKey(a, b), EdgeKey(b, c), tuples2))
+            graph.add(FactorClass(EdgeKey(a, c), EdgeKey(a, b), tuples3))
 
     return graph
 
@@ -279,6 +280,7 @@ def plot_results(results):
     fig.tight_layout()
     plt.show()
 
+
 # Main function
 def main():
     # Parse command line arguments
@@ -345,8 +347,6 @@ def main():
 
             # Compute final error
             final_error = graph.error(result)
-            if method == "Calibrated":
-                final_error *= cal.f() * cal.f()
 
             # Store results
             results[method]["distances"].extend(distances)