Switch to using Cal3f

examples/EssentialViewGraphExample.cpp

@@ -16,7 +16,7 @@
  * @date    October 2024
  */
 
-#include <gtsam/geometry/Cal3_S2.h>
+#include <gtsam/geometry/Cal3f.h>
 #include <gtsam/geometry/EssentialMatrix.h>
 #include <gtsam/geometry/PinholeCamera.h>
 #include <gtsam/geometry/Point2.h>
@@ -40,7 +40,7 @@ using namespace symbol_shorthand;  // For K(symbol)
 // Main function
 int main(int argc, char* argv[]) {
   // Define the camera calibration parameters
-  Cal3_S2 K_initial(50.0, 50.0, 0.0, 50.0, 50.0);
+  Cal3f cal(50.0, 50.0, 50.0);
 
   // Create the set of 8 ground-truth landmarks
   vector<Point3> points = createPoints();
@@ -55,7 +55,7 @@ int main(int argc, char* argv[]) {
   // Simulate measurements from each camera pose
   std::array<std::array<Point2, 8>, 4> p;
   for (size_t i = 0; i < 4; ++i) {
-    PinholeCamera<Cal3_S2> camera(poses[i], K_initial);
+    PinholeCamera<Cal3f> camera(poses[i], cal);
     for (size_t j = 0; j < 8; ++j) {
       p[i][j] = camera.project(points[j]);
     }
@@ -63,7 +63,7 @@ int main(int argc, char* argv[]) {
 
   // Create the factor graph
   NonlinearFactorGraph graph;
-  using Factor = EssentialTransferFactor<Cal3_S2>;
+  using Factor = EssentialTransferFactor<Cal3f>;
 
   for (size_t a = 0; a < 4; ++a) {
     size_t b = (a + 1) % 4;  // Next camera
@@ -113,7 +113,7 @@ int main(int argc, char* argv[]) {
 
   // Insert initial calibrations (using K symbol)
   for (size_t i = 0; i < 4; ++i) {
-    initialEstimate.insert(K(i), K_initial);
+    initialEstimate.insert(K(i), cal);
   }
 
   initialEstimate.print("Initial Estimates:\n", formatter);

examples/ViewGraphExample.cpp

@@ -114,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;

python/gtsam/examples/EssentialViewGraphExample.py

@@ -18,10 +18,10 @@ import numpy as np
 from gtsam.examples import SFMdata
 
 import gtsam
-from gtsam import Cal3_S2, EdgeKey, EssentialMatrix
+from gtsam import Cal3f, EdgeKey, EssentialMatrix
-from gtsam import EssentialTransferFactorCal3_S2 as Factor
+from gtsam import EssentialTransferFactorCal3f as Factor
 from gtsam import (LevenbergMarquardtOptimizer, LevenbergMarquardtParams,
-                   NonlinearFactorGraph, PinholeCameraCal3_S2, Values)
+                   NonlinearFactorGraph, PinholeCameraCal3f, Values)
 
 # For symbol shorthand (e.g., X(0), L(1))
 K = gtsam.symbol_shorthand.K
@@ -39,7 +39,7 @@ def formatter(key):
 
 def main():
     # Define the camera calibration parameters
-    K_initial = Cal3_S2(50.0, 50.0, 0.0, 50.0, 50.0)
+    cal = Cal3f(50.0, 50.0, 50.0)
 
     # Create the set of 8 ground-truth landmarks
     points = SFMdata.createPoints()
@@ -54,7 +54,7 @@ def main():
     # 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_initial)
+        camera = PinholeCameraCal3f(poses[i], cal)
         for j in range(8):
             p[i][j] = camera.project(points[j])
 
@@ -95,7 +95,7 @@ def main():
 
     # Insert initial calibrations
     for i in range(4):
-        initialEstimate.insert(K(i), K_initial)
+        initialEstimate.insert(K(i), cal)
 
     # Optimize the graph and print results
     params = LevenbergMarquardtParams()

python/gtsam/examples/ViewGraphComparison.py

@@ -6,26 +6,19 @@
   Date: October 2024
 """
 
+import argparse
+
 import matplotlib.pyplot as plt
 import numpy as np
 from gtsam.examples import SFMdata
 
 import gtsam
-import argparse
+from gtsam import (Cal3f, EdgeKey, EssentialMatrix, FundamentalMatrix,
-from gtsam import (
+                   LevenbergMarquardtOptimizer, LevenbergMarquardtParams,
-    Cal3_S2,
+                   NonlinearFactorGraph, PinholeCameraCal3f, Values)
-    EdgeKey,
-    EssentialMatrix,
-    FundamentalMatrix,
-    LevenbergMarquardtOptimizer,
-    LevenbergMarquardtParams,
-    NonlinearFactorGraph,
-    PinholeCameraCal3_S2,
-    Values,
-)
 
 # For symbol shorthand (e.g., K(0), K(1))
-K_sym = gtsam.symbol_shorthand.K
+K = gtsam.symbol_shorthand.K
 
 # Methods to compare
 methods = ["FundamentalMatrix", "EssentialMatrix"]
@@ -44,7 +37,7 @@ def formatter(key):
 # Function to simulate data
 def simulate_data(num_cameras):
     # Define the camera calibration parameters
-    K = Cal3_S2(50.0, 50.0, 0.0, 50.0, 50.0)
+    cal = Cal3f(50.0, 50.0, 50.0)
 
     # Create the set of 8 ground-truth landmarks
     points = SFMdata.createPoints()
@@ -55,18 +48,18 @@ def simulate_data(num_cameras):
     # Simulate measurements from each camera pose
     measurements = [[None for _ in range(len(points))] for _ in range(num_cameras)]
     for i in range(num_cameras):
-        camera = PinholeCameraCal3_S2(poses[i], K)
+        camera = PinholeCameraCal3f(poses[i], cal)
         for j in range(len(points)):
             measurements[i][j] = camera.project(points[j])
 
-    return points, poses, measurements, K
+    return points, poses, measurements, cal
 
 
 # Function to compute ground truth matrices
-def compute_ground_truth_matrices(method, poses, K):
+def compute_ground_truth(method, poses, cal):
     if method == "FundamentalMatrix":
-        F1 = FundamentalMatrix(K, poses[0].between(poses[1]), K)
+        F1 = FundamentalMatrix(cal.K(), poses[0].between(poses[1]), cal.K())
-        F2 = FundamentalMatrix(K, poses[0].between(poses[2]), K)
+        F2 = FundamentalMatrix(cal.K(), poses[0].between(poses[2]), cal.K())
         return F1, F2
     elif method == "EssentialMatrix":
         E1 = EssentialMatrix.FromPose3(poses[0].between(poses[1]))
@@ -84,8 +77,8 @@ def build_factor_graph(method, num_cameras, measurements):
         # Use TransferFactorFundamentalMatrix
         FactorClass = gtsam.TransferFactorFundamentalMatrix
     elif method == "EssentialMatrix":
-        # Use EssentialTransferFactorCal3_S2
+        # Use EssentialTransferFactorCal3f
-        FactorClass = gtsam.EssentialTransferFactorCal3_S2
+        FactorClass = gtsam.EssentialTransferFactorCal3f
     else:
         raise ValueError(f"Unknown method {method}")
 
@@ -113,7 +106,7 @@ def build_factor_graph(method, num_cameras, measurements):
 
 
 # Function to get initial estimates
-def get_initial_estimate(method, num_cameras, ground_truth, K):
+def get_initial_estimate(method, num_cameras, ground_truth, cal):
     initialEstimate = Values()
 
     if method == "FundamentalMatrix":
@@ -132,7 +125,7 @@ def get_initial_estimate(method, num_cameras, ground_truth, K):
             initialEstimate.insert(EdgeKey(a, c).key(), E2)
         # Insert initial calibrations
         for i in range(num_cameras):
-            initialEstimate.insert(K_sym(i), K)
+            initialEstimate.insert(K(i), cal)
     else:
         raise ValueError(f"Unknown method {method}")
 
@@ -150,7 +143,7 @@ def optimize(graph, initialEstimate):
 
 
 # Function to compute distances from ground truth
-def compute_distances(method, result, ground_truth, num_cameras, K):
+def compute_distances(method, result, ground_truth, num_cameras, cal):
     distances = []
 
     if method == "FundamentalMatrix":
@@ -158,8 +151,8 @@ def compute_distances(method, result, ground_truth, num_cameras, K):
     elif method == "EssentialMatrix":
         E1, E2 = ground_truth
         # Convert ground truth EssentialMatrices to FundamentalMatrices using GTSAM method
-        F1 = gtsam.FundamentalMatrix(K, E1, K)
+        F1 = gtsam.FundamentalMatrix(cal.K(), E1, cal.K())
-        F2 = gtsam.FundamentalMatrix(K, E2, K)
+        F2 = gtsam.FundamentalMatrix(cal.K(), E2, cal.K())
     else:
         raise ValueError(f"Unknown method {method}")
 
@@ -176,8 +169,8 @@ def compute_distances(method, result, ground_truth, num_cameras, K):
             E_est_ab = result.atEssentialMatrix(key_ab)
             E_est_ac = result.atEssentialMatrix(key_ac)
             # Convert estimated EssentialMatrices to FundamentalMatrices using GTSAM method
-            F_est_ab = gtsam.FundamentalMatrix(K, E_est_ab, K)
+            F_est_ab = gtsam.FundamentalMatrix(cal.K(), E_est_ab, cal.K())
-            F_est_ac = gtsam.FundamentalMatrix(K, E_est_ac, K)
+            F_est_ac = gtsam.FundamentalMatrix(cal.K(), E_est_ac, cal.K())
         else:
             raise ValueError(f"Unknown method {method}")
 
@@ -229,22 +222,22 @@ def main():
         print(f"Running method: {method}")
 
         # Simulate data
-        points, poses, measurements, K = simulate_data(args.num_cameras)
+        points, poses, measurements, cal = simulate_data(args.num_cameras)
 
         # Compute ground truth matrices
-        ground_truth = compute_ground_truth_matrices(method, poses, K)
+        ground_truth = compute_ground_truth(method, poses, cal)
 
         # Build the factor graph
         graph = build_factor_graph(method, args.num_cameras, measurements)
 
         # Get initial estimates
-        initialEstimate = get_initial_estimate(method, args.num_cameras, ground_truth, K)
+        initialEstimate = get_initial_estimate(method, args.num_cameras, ground_truth, cal)
 
         # Optimize the graph
         result = optimize(graph, initialEstimate)
 
         # Compute distances from ground truth
-        distances = compute_distances(method, result, ground_truth, args.num_cameras, K)
+        distances = compute_distances(method, result, ground_truth, args.num_cameras, cal)
 
         # Compute final error
         final_error = graph.error(result)

python/gtsam/examples/ViewGraphExample.py

@@ -32,7 +32,7 @@ def formatter(key):
 
 def main():
     # Define the camera calibration parameters
-    K = Cal3_S2(50.0, 50.0, 0.0, 50.0, 50.0)
+    cal = Cal3_S2(50.0, 50.0, 0.0, 50.0, 50.0)
 
     # Create the set of 8 ground-truth landmarks
     points = SFMdata.createPoints()
@@ -41,13 +41,13 @@ def main():
     poses = SFMdata.posesOnCircle(4, 30)
 
     # Calculate ground truth fundamental matrices, 1 and 2 poses apart
-    F1 = FundamentalMatrix(K, poses[0].between(poses[1]), K)
+    F1 = FundamentalMatrix(cal.K(), poses[0].between(poses[1]), cal.K())
-    F2 = FundamentalMatrix(K, poses[0].between(poses[2]), K)
+    F2 = FundamentalMatrix(cal.K(), poses[0].between(poses[2]), cal.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)
+        camera = PinholeCameraCal3_S2(poses[i], cal)
         for j in range(8):
             p[i][j] = camera.project(points[j])