diff --git a/python/gtsam/examples/ViewGraphComparison.py b/python/gtsam/examples/ViewGraphComparison.py new file mode 100644 index 000000000..f9c433595 --- /dev/null +++ b/python/gtsam/examples/ViewGraphComparison.py @@ -0,0 +1,259 @@ +""" + Compare the Fundamental Matrix and Essential Matrix methods for optimizing the view-graph. + It measures the distance from the ground truth matrices in terms of the norm of local coordinates (geodesic distance) + on the respective manifolds. It also plots the final error of the optimization. + Author: Frank Dellaert (with heavy assist from ChatGPT) + Date: October 2024 +""" + +import matplotlib.pyplot as plt +import numpy as np +from gtsam.examples import SFMdata + +import gtsam +import argparse +from gtsam import ( + Cal3_S2, + EdgeKey, + EssentialMatrix, + FundamentalMatrix, + LevenbergMarquardtOptimizer, + LevenbergMarquardtParams, + NonlinearFactorGraph, + PinholeCameraCal3_S2, + Values, +) + +# For symbol shorthand (e.g., K(0), K(1)) +K_sym = gtsam.symbol_shorthand.K + +# Methods to compare +methods = ["FundamentalMatrix", "EssentialMatrix"] + + +# Formatter function for printing keys +def formatter(key): + sym = gtsam.Symbol(key) + if sym.chr() == ord("k"): + return f"k{sym.index()}" + else: + edge = EdgeKey(key) + return f"({edge.i()},{edge.j()})" + + +# 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) + + # Create the set of 8 ground-truth landmarks + points = SFMdata.createPoints() + + # Create the set of ground-truth poses + poses = SFMdata.posesOnCircle(num_cameras, 30) + + # 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) + for j in range(len(points)): + measurements[i][j] = camera.project(points[j]) + + return points, poses, measurements, K + + +# Function to compute ground truth matrices +def compute_ground_truth_matrices(method, poses, K): + if method == "FundamentalMatrix": + F1 = FundamentalMatrix(K, poses[0].between(poses[1]), K) + F2 = FundamentalMatrix(K, poses[0].between(poses[2]), K) + return F1, F2 + elif method == "EssentialMatrix": + E1 = EssentialMatrix.FromPose3(poses[0].between(poses[1])) + E2 = EssentialMatrix.FromPose3(poses[0].between(poses[2])) + return E1, E2 + else: + raise ValueError(f"Unknown method {method}") + + +# Function to build the factor graph +def build_factor_graph(method, num_cameras, measurements): + graph = NonlinearFactorGraph() + + if method == "FundamentalMatrix": + # Use TransferFactorFundamentalMatrix + FactorClass = gtsam.TransferFactorFundamentalMatrix + elif method == "EssentialMatrix": + # Use EssentialTransferFactorCal3_S2 + FactorClass = gtsam.EssentialTransferFactorCal3_S2 + else: + raise ValueError(f"Unknown method {method}") + + for a in range(num_cameras): + b = (a + 1) % num_cameras # Next camera + c = (a + 2) % num_cameras # Camera after next + + # Vectors to collect tuples for each factor + tuples1 = [] + tuples2 = [] + tuples3 = [] + + # Collect data for the three factors + for j in range(len(measurements[0])): + tuples1.append((measurements[a][j], measurements[b][j], measurements[c][j])) + tuples2.append((measurements[a][j], measurements[c][j], measurements[b][j])) + tuples3.append((measurements[c][j], measurements[b][j], measurements[a][j])) + + # Add transfer factors between views a, b, and c. + graph.add(FactorClass(EdgeKey(a, c), EdgeKey(b, c), tuples1)) + graph.add(FactorClass(EdgeKey(a, b), EdgeKey(b, c), tuples2)) + graph.add(FactorClass(EdgeKey(a, c), EdgeKey(a, b), tuples3)) + + return graph + + +# Function to get initial estimates +def get_initial_estimate(method, num_cameras, ground_truth_matrices, K_initial): + initialEstimate = Values() + + if method == "FundamentalMatrix": + F1, F2 = ground_truth_matrices + for a in range(num_cameras): + b = (a + 1) % num_cameras # Next camera + c = (a + 2) % num_cameras # Camera after next + initialEstimate.insert(EdgeKey(a, b).key(), F1) + initialEstimate.insert(EdgeKey(a, c).key(), F2) + elif method == "EssentialMatrix": + E1, E2 = ground_truth_matrices + for a in range(num_cameras): + b = (a + 1) % num_cameras # Next camera + c = (a + 2) % num_cameras # Camera after next + initialEstimate.insert(EdgeKey(a, b).key(), E1) + initialEstimate.insert(EdgeKey(a, c).key(), E2) + # Insert initial calibrations + for i in range(num_cameras): + initialEstimate.insert(K_sym(i), K_initial) + else: + raise ValueError(f"Unknown method {method}") + + return initialEstimate + + +# Function to optimize the graph +def optimize(graph, initialEstimate): + params = LevenbergMarquardtParams() + params.setlambdaInitial(1000.0) # Initialize lambda to a high value + params.setVerbosityLM("SUMMARY") + optimizer = LevenbergMarquardtOptimizer(graph, initialEstimate, params) + result = optimizer.optimize() + return result + + +# Function to compute distances from ground truth +def compute_distances(method, result, ground_truth_matrices, num_cameras): + distances = [] + if method == "FundamentalMatrix": + F1, F2 = ground_truth_matrices + for a in range(num_cameras): + b = (a + 1) % num_cameras + c = (a + 2) % num_cameras + key_ab = EdgeKey(a, b).key() + key_ac = EdgeKey(a, c).key() + F_est_ab = result.atFundamentalMatrix(key_ab) + F_est_ac = result.atFundamentalMatrix(key_ac) + # Compute local coordinates + dist_ab = np.linalg.norm(F1.localCoordinates(F_est_ab)) + dist_ac = np.linalg.norm(F2.localCoordinates(F_est_ac)) + distances.append(dist_ab) + distances.append(dist_ac) + elif method == "EssentialMatrix": + E1, E2 = ground_truth_matrices + for a in range(num_cameras): + b = (a + 1) % num_cameras + c = (a + 2) % num_cameras + key_ab = EdgeKey(a, b).key() + key_ac = EdgeKey(a, c).key() + E_est_ab = result.atEssentialMatrix(key_ab) + E_est_ac = result.atEssentialMatrix(key_ac) + # Compute local coordinates + dist_ab = np.linalg.norm(E1.localCoordinates(E_est_ab)) + dist_ac = np.linalg.norm(E2.localCoordinates(E_est_ac)) + distances.append(dist_ab) + distances.append(dist_ac) + else: + raise ValueError(f"Unknown method {method}") + return distances + + +# Function to plot results +def plot_results(results): + methods = list(results.keys()) + final_errors = [results[method]["final_error"] for method in methods] + distances = [np.mean(results[method]["distances"]) for method in methods] + + fig, ax1 = plt.subplots() + + color = "tab:red" + ax1.set_xlabel("Method") + ax1.set_ylabel("Final Error", color=color) + ax1.bar(methods, final_errors, color=color, alpha=0.6) + ax1.tick_params(axis="y", labelcolor=color) + + ax2 = ax1.twinx() + color = "tab:blue" + ax2.set_ylabel("Mean Geodesic Distance", color=color) + ax2.plot(methods, distances, color=color, marker="o") + ax2.tick_params(axis="y", labelcolor=color) + + plt.title("Comparison of Methods") + fig.tight_layout() + plt.show() + + +# Main function +def main(): + # Parse command line arguments + parser = argparse.ArgumentParser(description="Compare Fundamental and Essential Matrix Methods") + parser.add_argument("--num_cameras", type=int, default=4, help="Number of cameras (default: 4)") + args = parser.parse_args() + + # Initialize results dictionary + results = {} + + for method in methods: + print(f"Running method: {method}") + + # Simulate data + points, poses, measurements, K_initial = simulate_data(args.num_cameras) + + # Compute ground truth matrices + ground_truth_matrices = compute_ground_truth_matrices(method, poses, K_initial) + + # 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_matrices, K_initial) + + # Optimize the graph + result = optimize(graph, initialEstimate) + + # Compute distances from ground truth + distances = compute_distances(method, result, ground_truth_matrices, args.num_cameras) + + # Compute final error + final_error = graph.error(result) + + # Store results + results[method] = {"distances": distances, "final_error": final_error} + + print(f"Method: {method}") + print(f"Final Error: {final_error}") + print(f"Mean Geodesic Distance: {np.mean(distances)}\n") + + # Plot results + plot_results(results) + + +if __name__ == "__main__": + main()