Python version of ViewGraphExample.cpp

gtsam/sfm/sfm.i

@@ -76,9 +76,18 @@ gtsam::Values initialCamerasEstimate(const gtsam::SfmData& db);
 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}>
+virtual class TransferFactor : gtsam::NoiseModelFactor {
+  TransferFactor(gtsam::EdgeKey edge1, gtsam::EdgeKey edge2,
+                 const std::vector<std::tuple<gtsam::Point2, gtsam::Point2, gtsam::Point2>>& triplets
+                );
+};
+
 #include <gtsam/geometry/Cal3_S2.h>
+#include <gtsam/geometry/Cal3f.h>
 #include <gtsam/geometry/Cal3Bundler.h>
-template <K = {gtsam::Cal3_S2, gtsam::Cal3Bundler}>
+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

python/gtsam/examples/EssentialViewGraphExample.py

@@ -24,25 +24,20 @@ import gtsam
 from gtsam import Cal3_S2, EdgeKey, EssentialMatrix
 from gtsam import EssentialTransferFactorCal3_S2 as Factor
 from gtsam import (LevenbergMarquardtOptimizer, LevenbergMarquardtParams,
-                   NonlinearFactorGraph, PinholeCameraCal3_S2, Point2, Point3,
+                   NonlinearFactorGraph, PinholeCameraCal3_S2, Values)
-                   Pose3, Values, symbol_shorthand)
 
 # For symbol shorthand (e.g., X(0), L(1))
-K = symbol_shorthand.K
+K = gtsam.symbol_shorthand.K
 
 
 # Formatter function for printing keys
 def formatter(key):
     sym = gtsam.Symbol(key)
-    if sym.chr() == ord("K"):
+    if sym.chr() == ord("k"):
-        return str(sym)
+        return f"k{sym.index()}"
-    elif sym.chr() == ord("E"):
-        idx = sym.index()
-        a = idx // 10
-        b = idx % 10
-        return f"E({a},{b})"
     else:
-        return str(sym)
+        edge = EdgeKey(key)
+        return f"({edge.i()},{edge.j()})"
 
 
 def main():
@@ -88,6 +83,8 @@ def main():
         graph.add(Factor(EdgeKey(a, b), EdgeKey(b, c), tuples2))
         graph.add(Factor(EdgeKey(a, c), EdgeKey(a, b), tuples3))
 
+    graph.print("graph", formatter)
+
     # Create a delta vector to perturb the ground truth (small perturbation)
     delta = np.ones(5) * 1e-2
 

python/gtsam/examples/ViewGraphExample.py

@@ -0,0 +1,114 @@
+"""
+  GTSAM Copyright 2010, 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
+
+  Solve a structure-from-motion problem from a "Bundle Adjustment in the Large" file
+  Author: Frank Dellaert (Python: Akshay Krishnan, John Lambert, Varun Agrawal)
+"""
+
+"""
+Python version of ViewGraphExample.cpp
+View-graph calibration on a simulated dataset, a la Sweeney 2015
+Author: Frank Dellaert
+Date: October 2024
+"""
+
+import numpy as np
+from gtsam.examples import SFMdata
+
+from gtsam import (Cal3_S2, EdgeKey, FundamentalMatrix,
+                   LevenbergMarquardtOptimizer, LevenbergMarquardtParams,
+                   NonlinearFactorGraph, PinholeCameraCal3_S2)
+from gtsam import TransferFactorFundamentalMatrix as Factor
+from gtsam import Values
+
+
+# Formatter function for printing keys
+def formatter(key):
+    edge = EdgeKey(key)
+    return f"({edge.i()},{edge.j()})"
+
+
+def main():
+    # Define the camera calibration parameters
+    K = Cal3_S2(50.0, 50.0, 0.0, 50.0, 50.0)
+
+    # Create the set of 8 ground-truth landmarks
+    points = SFMdata.createPoints()
+
+    # Create the set of 4 ground-truth poses
+    poses = SFMdata.posesOnCircle(4, 30)
+
+    # Calculate ground truth fundamental matrices, 1 and 2 poses apart
+    F1 = FundamentalMatrix(K, poses[0].between(poses[1]), K)
+    F2 = FundamentalMatrix(K, poses[0].between(poses[2]), K)
+
+    # Simulate measurements from each camera pose
+    p = [[None for _ in range(8)] for _ in range(4)]
+    for i in range(4):
+        camera = PinholeCameraCal3_S2(poses[i], K)
+        for j in range(8):
+            p[i][j] = camera.project(points[j])
+
+    # Create the factor graph
+    graph = NonlinearFactorGraph()
+
+    for a in range(4):
+        b = (a + 1) % 4  # Next camera
+        c = (a + 2) % 4  # Camera after next
+
+        # Vectors to collect tuples for each factor
+        tuples1 = []
+        tuples2 = []
+        tuples3 = []
+
+        # Collect data for the three factors
+        for j in range(8):
+            tuples1.append((p[a][j], p[b][j], p[c][j]))
+            tuples2.append((p[a][j], p[c][j], p[b][j]))
+            tuples3.append((p[c][j], p[b][j], p[a][j]))
+
+        # Add transfer factors between views a, b, and c.
+        graph.add(Factor(EdgeKey(a, c), EdgeKey(b, c), tuples1))
+        graph.add(Factor(EdgeKey(a, b), EdgeKey(b, c), tuples2))
+        graph.add(Factor(EdgeKey(a, c), EdgeKey(a, b), tuples3))
+
+    # Print the factor graph
+    graph.print("Factor Graph:\n", formatter)
+
+    # Create a delta vector to perturb the ground truth
+    delta = np.array([1, 2, 3, 4, 5, 6, 7]) * 1e-5
+
+    # Create the data structure to hold the initial estimate to the solution
+    initialEstimate = Values()
+    for a in range(4):
+        b = (a + 1) % 4  # Next camera
+        c = (a + 2) % 4  # Camera after next
+        initialEstimate.insert(EdgeKey(a, b).key(), F1.retract(delta))
+        initialEstimate.insert(EdgeKey(a, c).key(), F2.retract(delta))
+
+    initialEstimate.print("Initial Estimates:\n", formatter)
+    graph.printErrors(initialEstimate, "Initial Errors:\n", formatter)
+
+    # Optimize the graph and print results
+    params = LevenbergMarquardtParams()
+    params.setLambdaInitial(1000.0)  # Initialize lambda to a high value
+    params.setVerbosityLM("SUMMARY")
+    optimizer = LevenbergMarquardtOptimizer(graph, initialEstimate, params)
+    result = optimizer.optimize()
+
+    print(f"Initial error = {graph.error(initialEstimate)}")
+    print(f"Final error = {graph.error(result)}")
+
+    result.print("Final Results:\n", formatter)
+
+    print("Ground Truth F1:\n", F1.matrix())
+    print("Ground Truth F2:\n", F2.matrix())
+
+
+if __name__ == "__main__":
+    main()