Add prior, use calibrations

python/gtsam/examples/ViewGraphComparison.py

@@ -71,7 +71,7 @@ def compute_ground_truth(method, poses, cal):
         E1 = EssentialMatrix.FromPose3(poses[0].between(poses[1]))
         E2 = EssentialMatrix.FromPose3(poses[0].between(poses[2]))
 
-        # Assert that E1.matrix and F1 are the same
+        # Assert that E1.matrix and F1 are the same, with known calibration
         invK = np.linalg.inv(cal.K())
         G1 = invK.transpose() @ E1.matrix() @ invK
         G2 = invK.transpose() @ E2.matrix() @ invK
@@ -94,6 +94,10 @@ def build_factor_graph(method, num_cameras, measurements):
         FactorClass = gtsam.TransferFactorFundamentalMatrix
     elif method == "EssentialMatrix":
         FactorClass = gtsam.EssentialTransferFactorCal3f
+        # add priors on all calibrations:
+        for i in range(num_cameras):
+            model = gtsam.noiseModel.Isotropic.Sigma(1, 10.0)
+            graph.addPriorCal3f(K(i), Cal3f(50.0, 50.0, 50.0), model)
     else:
         raise ValueError(f"Unknown method {method}")
 
@@ -120,7 +124,7 @@ def build_factor_graph(method, num_cameras, measurements):
     return graph
 
 
-def get_initial_estimate(method, num_cameras, ground_truth, cal, std=1e-6):
+def get_initial_estimate(method, num_cameras, ground_truth, cal):
     """get initial estimate for method"""
     initialEstimate = Values()
     total_dimension = 0
@@ -130,22 +134,16 @@ def get_initial_estimate(method, num_cameras, ground_truth, cal, std=1e-6):
         for a in range(num_cameras):
             b = (a + 1) % num_cameras  # Next camera
             c = (a + 2) % num_cameras  # Camera after next
-            # Retract with delta drawn from zero-mean normal
+            initialEstimate.insert(EdgeKey(a, b).key(), F1)
-            F1_perturbed = F1.retract(np.random.normal(0, std, F1.dim()))
+            initialEstimate.insert(EdgeKey(a, c).key(), F2)
-            F2_perturbed = F2.retract(np.random.normal(0, 1e-4, F2.dim()))
-            initialEstimate.insert(EdgeKey(a, b).key(), F1_perturbed)
-            initialEstimate.insert(EdgeKey(a, c).key(), F2_perturbed)
             total_dimension += F1.dim() + F2.dim()
     elif method == "EssentialMatrix":
         E1, E2 = ground_truth
         for a in range(num_cameras):
             b = (a + 1) % num_cameras  # Next camera
             c = (a + 2) % num_cameras  # Camera after next
-            # Retract with delta drawn from zero-mean normal
+            initialEstimate.insert(EdgeKey(a, b).key(), E1)
-            E1_perturbed = E1.retract(np.random.normal(0, std, E1.dim()))
+            initialEstimate.insert(EdgeKey(a, c).key(), E2)
-            E2_perturbed = E2.retract(np.random.normal(0, 1e-4, E2.dim()))
-            initialEstimate.insert(EdgeKey(a, b).key(), E1_perturbed)
-            initialEstimate.insert(EdgeKey(a, c).key(), E2_perturbed)
             total_dimension += E1.dim() + E2.dim()
         # Insert initial calibrations
         for i in range(num_cameras):
@@ -158,14 +156,17 @@ def get_initial_estimate(method, num_cameras, ground_truth, cal, std=1e-6):
     return initialEstimate
 
 
-def optimize(graph, initialEstimate):
+def optimize(graph, initialEstimate, method):
     """optimize the graph"""
     params = LevenbergMarquardtParams()
-    params.setlambdaInitial(1000.0)  # Initialize lambda to a high value
+    params.setlambdaInitial(1e10)  # Initialize lambda to a high value
-    params.setAbsoluteErrorTol(1.0)
+    params.setlambdaUpperBound(1e10)
+    # params.setAbsoluteErrorTol(0.1)
     params.setVerbosityLM("SUMMARY")
     optimizer = LevenbergMarquardtOptimizer(graph, initialEstimate, params)
     result = optimizer.optimize()
+    # if method == "EssentialMatrix":
+    #     result.print("Final results:\n", formatter)
     return result
 
 
@@ -195,9 +196,15 @@ def compute_distances(method, result, ground_truth, num_cameras, cal):
         elif method == "EssentialMatrix":
             E_est_ab = result.atEssentialMatrix(key_ab)
             E_est_ac = result.atEssentialMatrix(key_ac)
+
+            # Retrieve correct calibrations from result:
+            cal_a = result.atCal3f(K(a))
+            cal_b = result.atCal3f(K(b))
+            cal_c = result.atCal3f(K(c))
+
             # Convert estimated EssentialMatrices to FundamentalMatrices
-            F_est_ab = gtsam.FundamentalMatrix(cal.K(), E_est_ab, cal.K())
+            F_est_ab = gtsam.FundamentalMatrix(cal_a.K(), E_est_ab, cal_b.K())
-            F_est_ac = gtsam.FundamentalMatrix(cal.K(), E_est_ac, cal.K())
+            F_est_ac = gtsam.FundamentalMatrix(cal_a.K(), E_est_ac, cal_c.K())
 
         # Compute local coordinates (geodesic distance on the F-manifold)
         dist_ab = np.linalg.norm(F1.localCoordinates(F_est_ab))
@@ -240,7 +247,7 @@ def main():
     parser.add_argument("--num_cameras", type=int, default=4, help="Number of cameras (default: 4)")
     parser.add_argument("--nr_trials", type=int, default=5, help="Number of trials (default: 5)")
     parser.add_argument("--seed", type=int, default=42, help="Random seed (default: 42)")
-    parser.add_argument("--noise_std", type=float, default=1.0, help="Standard deviation of noise (default: 1.0)")
+    parser.add_argument("--noise_std", type=float, default=0.5, help="Standard deviation of noise (default: 0.5)")
     args = parser.parse_args()
 
     # Initialize the random number generator
@@ -273,17 +280,14 @@ def main():
             graph = build_factor_graph(method, args.num_cameras, measurements)
 
             # Assert that the initial error is the same for both methods:
-            if trial == 0 and method == methods[0]:
+            if method == methods[0]:
-                initial_error = graph.error(initial_estimate[method])
+                error0 = graph.error(initial_estimate[method])
             else:
                 current_error = graph.error(initial_estimate[method])
-                if not np.allclose(initial_error, current_error):
+                assert np.allclose(error0, current_error)
-                    print(f"Initial error for {method} ({current_error}) does not match ")
-                    print(f"initial error for {methods[0]} ({initial_error})")
-                    exit(1)
 
             # Optimize the graph
-            result = optimize(graph, initial_estimate[method])
+            result = optimize(graph, initial_estimate[method], method)
 
             # Compute distances from ground truth
             distances = compute_distances(method, result, ground_truth[method], args.num_cameras, cal)