Batched version

Author: dellaert

examples/ViewGraphExample.cpp

@@ -13,7 +13,7 @@
  * @file    ViewGraphExample.cpp
  * @brief   View-graph calibration on a simulated dataset, a la Sweeney 2015
  * @author  Frank Dellaert
- * @author  October 2024
+ * @date    October 2024
  */
 
 #include <gtsam/geometry/Cal3_S2.h>
@@ -68,21 +68,28 @@ int main(int argc, char* argv[]) {
   // In this version, we only include triplets between 3 successive cameras.
   NonlinearFactorGraph graph;
   using Factor = TransferFactor<FundamentalMatrix>;
+
   for (size_t a = 0; a < 4; ++a) {
     size_t b = (a + 1) % 4;  // Next camera
     size_t c = (a + 2) % 4;  // Camera after next
-    for (size_t j = 0; j < 4; ++j) {
-      // Add transfer factors between views a, b, and c. Note that the EdgeKeys
-      // are crucial in performing the transfer in the right direction. We use
-      // exactly 8 unique EdgeKeys, corresponding to 8 unknown fundamental
-      // matrices we will optimize for.
-      graph.emplace_shared<Factor>(EdgeKey(a, c), EdgeKey(b, c), p[a][j],
-                                   p[b][j], p[c][j]);
-      graph.emplace_shared<Factor>(EdgeKey(a, b), EdgeKey(b, c), p[a][j],
-                                   p[c][j], p[b][j]);
-      graph.emplace_shared<Factor>(EdgeKey(a, c), EdgeKey(a, b), p[c][j],
-                                   p[b][j], p[a][j]);
+
+    // Vectors to collect tuples for each factor
+    std::vector<std::tuple<Point2, Point2, Point2>> tuples1, tuples2, tuples3;
+
+    // Collect data for the three factors
+    for (size_t j = 0; j < 8; ++j) {
+      tuples1.emplace_back(p[a][j], p[b][j], p[c][j]);
+      tuples2.emplace_back(p[a][j], p[c][j], p[b][j]);
+      tuples3.emplace_back(p[c][j], p[b][j], p[a][j]);
     }
+
+    // Add transfer factors between views a, b, and c. Note that the EdgeKeys
+    // are crucial in performing the transfer in the right direction. We use
+    // exactly 8 unique EdgeKeys, corresponding to 8 unknown fundamental
+    // matrices we will optimize for.
+    graph.emplace_shared<Factor>(EdgeKey(a, c), EdgeKey(b, c), tuples1);
+    graph.emplace_shared<Factor>(EdgeKey(a, b), EdgeKey(b, c), tuples2);
+    graph.emplace_shared<Factor>(EdgeKey(a, c), EdgeKey(a, b), tuples3);
   }
 
   auto formatter = [](Key key) {
@@ -96,7 +103,7 @@ int main(int argc, char* argv[]) {
   // We can't really go far before convergence becomes problematic :-(
   Vector7 delta;
   delta << 1, 2, 3, 4, 5, 6, 7;
-  delta *= 5e-5;
+  delta *= 1e-5;
 
   // Create the data structure to hold the initial estimate to the solution
   Values initialEstimate;
@@ -107,7 +114,7 @@ int main(int argc, char* argv[]) {
     initialEstimate.insert(EdgeKey(a, c), F2.retract(delta));
   }
   initialEstimate.print("Initial Estimates:\n", formatter);
-  //   graph.printErrors(initialEstimate, "errors: ", formatter);
+    graph.printErrors(initialEstimate, "errors: ", formatter);
 
   /* Optimize the graph and print results */
   LevenbergMarquardtParams params;

gtsam/sfm/TransferFactor.h

@@ -32,22 +32,14 @@ namespace gtsam {
 template <typename F>
 class TransferFactor : public NoiseModelFactorN<F, F> {
   EdgeKey key1_, key2_;  ///< the two EdgeKeys
-  Point2 pa, pb, pc;     ///< The points in the three views
+  std::vector<std::tuple<Point2, Point2, Point2>>
+      triplets_;  ///< Point triplets
 
  public:
   /**
-   * @brief Constructor for the TransferFactor class.
+   * @brief Constructor for a single triplet of points
    *
-   * Uses EdgeKeys to determine how to use the two fundamental matrix unknowns
-   * F1 and F2, to transfer points pa and pb to the third view, and minimize the
-   * difference with pc.
-   *
-   * The edge keys must represent valid edges for the transfer operation,
-   * specifically one of the following configurations:
-   * - (a, c) and (b, c)
-   * - (a, c) and (c, b)
-   * - (c, a) and (b, c)
-   * - (c, a) and (c, b)
+   * @note: batching all points for the same transfer will be much faster.
    *
    * @param key1 First EdgeKey specifying F1: (a, c) or (c, a).
    * @param key2 Second EdgeKey specifying F2: (b, c) or (c, b).
@@ -62,9 +54,25 @@ class TransferFactor : public NoiseModelFactorN<F, F> {
       : NoiseModelFactorN<F, F>(model, key1, key2),
         key1_(key1),
         key2_(key2),
-        pa(pa),
-        pb(pb),
-        pc(pc) {}
+        triplets_({std::make_tuple(pa, pb, pc)}) {}
+
+  /**
+   * @brief Constructor that accepts a vector of point triplets.
+   *
+   * @param key1 First EdgeKey specifying F1: (a, c) or (c, a).
+   * @param key2 Second EdgeKey specifying F2: (b, c) or (c, b).
+   * @param triplets A vector of triplets containing (pa, pb, pc).
+   * @param model An optional SharedNoiseModel that defines the noise model
+   *              for this factor. Defaults to nullptr.
+   */
+  TransferFactor(
+      EdgeKey key1, EdgeKey key2,
+      const std::vector<std::tuple<Point2, Point2, Point2>>& triplets,
+      const SharedNoiseModel& model = nullptr)
+      : NoiseModelFactorN<F, F>(model, key1, key2),
+        key1_(key1),
+        key2_(key2),
+        triplets_(triplets) {}
 
   // Create Matrix3 objects based on EdgeKey configurations
   std::pair<Matrix3, Matrix3> getMatrices(const F& F1, const F& F2) const {
@@ -83,17 +91,27 @@ class TransferFactor : public NoiseModelFactorN<F, F> {
     }
   }
 
-  /// vector of errors returns 2D vector
+  /// vector of errors returns 2*N vector
   Vector evaluateError(const F& F1, const F& F2,
                        OptionalMatrixType H1 = nullptr,
                        OptionalMatrixType H2 = nullptr) const override {
-    std::function<Point2(F, F)> transfer = [&](const F& F1, const F& F2) {
+    std::function<Vector(const F&, const F&)> transfer = [&](const F& F1,
+                                                             const F& F2) {
+      Vector errors(2 * triplets_.size());
+      size_t idx = 0;
       auto [Fca, Fcb] = getMatrices(F1, F2);
-      return Transfer(Fca, pa, Fcb, pb);
+      for (const auto& tuple : triplets_) {
+        const auto& [pa, pb, pc] = tuple;
+        Point2 transferredPoint = Transfer(Fca, pa, Fcb, pb);
+        Vector2 error = transferredPoint - pc;
+        errors.segment<2>(idx) = error;
+        idx += 2;
+      }
+      return errors;
     };
-    if (H1) *H1 = numericalDerivative21<Point2, F, F>(transfer, F1, F2);
-    if (H2) *H2 = numericalDerivative22<Point2, F, F>(transfer, F1, F2);
-    return transfer(F1, F2) - pc;
+    if (H1) *H1 = numericalDerivative21<Vector, F, F>(transfer, F1, F2);
+    if (H2) *H2 = numericalDerivative22<Vector, F, F>(transfer, F1, F2);
+    return transfer(F1, F2);
   }
 };
 

gtsam/sfm/tests/testTransferFactor.cpp

@@ -89,6 +89,64 @@ TEST(TransferFactor, Jacobians) {
   }
 }
 
+//*************************************************************************
+// Test for TransferFactor with multiple tuples
+TEST(TransferFactor, MultipleTuples) {
+  // Generate cameras on a circle
+  std::array<Pose3, 3> cameraPoses = generateCameraPoses();
+  auto triplet = generateTripleF(cameraPoses);
+
+  // Now project multiple points into the three cameras
+  const size_t numPoints = 5;  // Number of points to test
+  std::vector<Point3> points3D;
+  std::vector<std::array<Point2, 3>> projectedPoints;
+
+  const Cal3_S2 K(focalLength, focalLength, 0.0,  //
+                  principalPoint.x(), principalPoint.y());
+
+  // Generate random 3D points and project them
+  for (size_t n = 0; n < numPoints; ++n) {
+    Point3 P(0.1 * n, 0.2 * n, 0.3 + 0.1 * n);
+    points3D.push_back(P);
+
+    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);
+    }
+    projectedPoints.push_back(p);
+  }
+
+  // Create a vector of tuples for the TransferFactor
+  EdgeKey key01(0, 1), key12(1, 2), key20(2, 0);
+  std::vector<std::tuple<Point2, Point2, Point2>> tuples0, tuples1, tuples2;
+
+  for (size_t n = 0; n < numPoints; ++n) {
+    const auto& p = projectedPoints[n];
+    tuples0.emplace_back(p[1], p[2], p[0]);
+  }
+
+  // Create TransferFactors using the new constructor
+  TransferFactor<SimpleFundamentalMatrix> factor{key01, key20, tuples0};
+
+  // Create Values with edge keys
+  Values values;
+  values.insert(key01, triplet.Fab);
+  values.insert(key12, triplet.Fbc);
+  values.insert(key20, triplet.Fca);
+
+  // Check error and Jacobians for multiple tuples
+  Vector error = factor.unwhitenedError(values);
+  // The error vector should be of size 2 * numPoints
+  EXPECT_LONGS_EQUAL(2 * numPoints, error.size());
+  // Since the points are perfectly matched, the error should be zero
+  EXPECT(assert_equal<Vector>(Vector::Zero(2 * numPoints), error, 1e-9));
+
+  // Check the Jacobians
+  EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-5, 1e-7);
+}
+
 //*************************************************************************
 int main() {
   TestResult tr;