diff --git a/python/gtsam/examples/ViewGraphComparison.py b/python/gtsam/examples/ViewGraphComparison.py index e2cd77c95..d11fdf6f9 100644 --- a/python/gtsam/examples/ViewGraphComparison.py +++ b/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 - F1_perturbed = F1.retract(np.random.normal(0, std, F1.dim())) - 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) + initialEstimate.insert(EdgeKey(a, b).key(), F1) + initialEstimate.insert(EdgeKey(a, c).key(), F2) 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 - E1_perturbed = E1.retract(np.random.normal(0, std, E1.dim())) - 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) + initialEstimate.insert(EdgeKey(a, b).key(), E1) + initialEstimate.insert(EdgeKey(a, c).key(), E2) 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.setAbsoluteErrorTol(1.0) + params.setlambdaInitial(1e10) # Initialize lambda to a high value + 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_ac = gtsam.FundamentalMatrix(cal.K(), E_est_ac, cal.K()) + F_est_ab = gtsam.FundamentalMatrix(cal_a.K(), E_est_ab, cal_b.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]: - initial_error = graph.error(initial_estimate[method]) + if method == methods[0]: + error0 = graph.error(initial_estimate[method]) else: current_error = graph.error(initial_estimate[method]) - if not np.allclose(initial_error, current_error): - print(f"Initial error for {method} ({current_error}) does not match ") - print(f"initial error for {methods[0]} ({initial_error})") - exit(1) + assert np.allclose(error0, current_error) # 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)