ViewGraphComparison.py

Go to the documentation of this file.

"""
  Compare several methods for optimizing the view-graph.
  We measure the distance from the ground truth in terms of the norm of 
  local coordinates (geodesic distance) on the F-manifold. 
  We also plot the final error of the optimization.
  
  Author: Frank Dellaert (with heavy assist from ChatGPT)
  Date: October 2024
"""
 
import argparse
 
import matplotlib.pyplot as plt
import numpy as np
from gtsam.examples import SFMdata
 
import gtsam
from gtsam import (
    Cal3f,
    EdgeKey,
    EssentialMatrix,
    FundamentalMatrix,
    LevenbergMarquardtOptimizer,
    LevenbergMarquardtParams,
    NonlinearFactorGraph,
    PinholeCameraCal3f,
    SimpleFundamentalMatrix,
    Values,
)
 
# For symbol shorthand (e.g., K(0), K(1))
K = gtsam.symbol_shorthand.K
 
# Methods to compare
methods = ["SimpleF", "Fundamental", "Essential+Ks", "Calibrated"]
 
 
# 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()})"
 
 
def simulate_geometry(num_cameras, rng, num_random_points=12):
    """simulate geometry (points and poses)"""
    # Define the camera calibration parameters
    cal = Cal3f(50.0, 50.0, 50.0)
 
    # Create the set of 8 ground-truth landmarks
    points = SFMdata.createPoints()
 
    # Create extra random points in the -10,10 cube around the origin
    extra_points = rng.uniform(-10, 10, (num_random_points, 3))
    points.extend([gtsam.Point3(p) for p in extra_points])
 
    # Create the set of ground-truth poses
    poses = SFMdata.posesOnCircle(num_cameras, 30)
 
    return points, poses, cal
 
 
def simulate_data(points, poses, cal, rng, noise_std):
    """Simulate measurements from each camera pose"""
    measurements = [[None for _ in points] for _ in poses]
    for i, pose in enumerate(poses):
        camera = PinholeCameraCal3f(pose, cal)
        for j, point in enumerate(points):
            projection = camera.project(point)
            noise = rng.normal(0, noise_std, size=2)
            measurements[i][j] = projection + noise
 
    return measurements
 
 
# Function to compute ground truth matrices
def compute_ground_truth(method, poses, cal):
    E1 = EssentialMatrix.FromPose3(poses[0].between(poses[1]))
    E2 = EssentialMatrix.FromPose3(poses[0].between(poses[2]))
    F1 = FundamentalMatrix(cal.K(), E1, cal.K())
    F2 = FundamentalMatrix(cal.K(), E2, cal.K())
    if method == "Fundamental":
        return F1, F2
    elif method == "SimpleF":
        f = cal.fx()
        c = cal.principalPoint()
        SF1 = SimpleFundamentalMatrix(E1, f, f, c, c)
        SF2 = SimpleFundamentalMatrix(E2, f, f, c, c)
        return SF1, SF2
    elif method == "Essential+Ks" or method == "Calibrated":
        return E1, E2
    else:
        raise ValueError(f"Unknown method {method}")
 
 
def build_factor_graph(method, num_cameras, measurements, cal):
    """build the factor graph"""
    graph = NonlinearFactorGraph()
 
    if method == "Fundamental":
        FactorClass = gtsam.TransferFactorFundamentalMatrix
    elif method == "SimpleF":
        FactorClass = gtsam.TransferFactorSimpleFundamentalMatrix
    elif method == "Essential+Ks":
        FactorClass = gtsam.EssentialTransferFactorKCal3f
        # add priors on all calibrations:
        for i in range(num_cameras):
            model = gtsam.noiseModel.Isotropic.Sigma(1, 10.0)
            graph.addPriorCal3f(K(i), cal, model)
    elif method == "Calibrated":
        FactorClass = gtsam.EssentialTransferFactorCal3f
        # No priors on calibration needed
    else:
        raise ValueError(f"Unknown method {method}")
 
    z = measurements  # shorthand
 
    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((z[a][j], z[b][j], z[c][j]))
            tuples2.append((z[a][j], z[c][j], z[b][j]))
            tuples3.append((z[c][j], z[b][j], z[a][j]))
 
        # Add transfer factors between views a, b, and c.
        if method in ["Calibrated"]:
            graph.add(FactorClass(EdgeKey(a, c), EdgeKey(b, c), tuples1, cal))
            graph.add(FactorClass(EdgeKey(a, b), EdgeKey(b, c), tuples2, cal))
            graph.add(FactorClass(EdgeKey(a, c), EdgeKey(a, b), tuples3, cal))
        else:
            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
 
 
def get_initial_estimate(method, num_cameras, ground_truth, cal):
    """get initial estimate for method"""
    initialEstimate = Values()
    total_dimension = 0
 
    if method in ["Fundamental", "SimpleF"]:
        F1, F2 = ground_truth
        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)
            total_dimension += F1.dim() + F2.dim()
    elif method in ["Essential+Ks", "Calibrated"]:
        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
            initialEstimate.insert(EdgeKey(a, b).key(), E1)
            initialEstimate.insert(EdgeKey(a, c).key(), E2)
            total_dimension += E1.dim() + E2.dim()
    else:
        raise ValueError(f"Unknown method {method}")
 
    if method == "Essential+Ks":
        # Insert initial calibrations
        for i in range(num_cameras):
            initialEstimate.insert(K(i), cal)
            total_dimension += cal.dim()
 
    print(f"Total dimension of the problem: {total_dimension}")
    return initialEstimate
 
 
def optimize(graph, initialEstimate, method):
    """optimize the graph"""
    params = LevenbergMarquardtParams()
    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()
    iterations = optimizer.iterations()
    return result, iterations
 
 
def compute_distances(method, result, ground_truth, num_cameras, cal):
    """Compute geodesic distances from ground truth"""
    distances = []
 
    F1, F2 = ground_truth["Fundamental"]
 
    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()
 
        if method in ["Essential+Ks", "Calibrated"]:
            E_est_ab = result.atEssentialMatrix(key_ab)
            E_est_ac = result.atEssentialMatrix(key_ac)
 
        # Compute estimated FundamentalMatrices
        if method == "Fundamental":
            F_est_ab = result.atFundamentalMatrix(key_ab)
            F_est_ac = result.atFundamentalMatrix(key_ac)
        elif method == "SimpleF":
            SF_est_ab = result.atSimpleFundamentalMatrix(key_ab).matrix()
            SF_est_ac = result.atSimpleFundamentalMatrix(key_ac).matrix()
            F_est_ab = FundamentalMatrix(SF_est_ab)
            F_est_ac = FundamentalMatrix(SF_est_ac)
        elif method == "Essential+Ks":
            # Retrieve 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 = FundamentalMatrix(cal_a.K(), E_est_ab, cal_b.K())
            F_est_ac = FundamentalMatrix(cal_a.K(), E_est_ac, cal_c.K())
        elif method == "Calibrated":
            # Use ground truth calibration
            F_est_ab = FundamentalMatrix(cal.K(), E_est_ab, cal.K())
            F_est_ac = FundamentalMatrix(cal.K(), E_est_ac, cal.K())
        else:
            raise ValueError(f"Unknown method {method}")
 
        # Compute local coordinates (geodesic distance on the F-manifold)
        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)
 
    return distances
 
 
def plot_results(results):
    """plot results"""
    methods = list(results.keys())
    final_errors = [results[method]["final_error"] for method in methods]
    distances = [results[method]["distances"] for method in methods]
    iterations = [results[method]["iterations"] for method in methods]
 
    fig, ax1 = plt.subplots()
 
    color = "tab:red"
    ax1.set_xlabel("Method")
    ax1.set_ylabel("Median Error (log scale)", color=color)
    ax1.set_yscale("log")
    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("Median Geodesic Distance", color=color)
    ax2.plot(methods, distances, color=color, marker="o", linestyle="-")
    ax2.tick_params(axis="y", labelcolor=color)
 
    # Annotate the blue data points with the average number of iterations
    for i, method in enumerate(methods):
        ax2.annotate(
            f"{iterations[i]:.1f}",
            (i, distances[i]),
            textcoords="offset points",
            xytext=(0, 10),
            ha="center",
            color=color,
        )
 
    plt.title("Comparison of Methods (Labels show avg iterations)")
    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)")
    parser.add_argument("--num_extra_points", type=int, default=12, help="Number of extra random points (default: 12)")
    parser.add_argument("--num_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=0.5, help="Standard deviation of noise (default: 0.5)")
    args = parser.parse_args()
 
    # Initialize the random number generator
    rng = np.random.default_rng(seed=args.seed)
 
    # Initialize results dictionary
    results = {method: {"distances": [], "final_error": [], "iterations": []} for method in methods}
 
    # Simulate geometry
    points, poses, cal = simulate_geometry(args.num_cameras, rng, args.num_extra_points)
 
    # Compute ground truth matrices
    ground_truth = {method: compute_ground_truth(method, poses, cal) for method in methods}
 
    # Get initial estimates
    initial_estimate: dict[Values] = {
        method: get_initial_estimate(method, args.num_cameras, ground_truth[method], cal) for method in methods
    }
    simple_f_result: Values = Values()
 
    for trial in range(args.num_trials):
        print(f"\nTrial {trial + 1}/{args.num_trials}")
 
        # Simulate data
        measurements = simulate_data(points, poses, cal, rng, args.noise_std)
 
        for method in methods:
            print(f"\nRunning method: {method}")
 
            # Build the factor graph
            graph = build_factor_graph(method, args.num_cameras, measurements, cal)
 
            # For F, initialize from SimpleF:
            if method == "Fundamental":
                initial_estimate[method] = simple_f_result
 
            # Optimize the graph
            result, iterations = optimize(graph, initial_estimate[method], method)
 
            # Store SimpleF result as a set of FundamentalMatrices
            if method == "SimpleF":
                simple_f_result = Values()
                for a in range(args.num_cameras):
                    b = (a + 1) % args.num_cameras  # Next camera
                    c = (a + 2) % args.num_cameras  # Camera after next
                    key_ab = EdgeKey(a, b).key()
                    key_ac = EdgeKey(a, c).key()
                    F1 = result.atSimpleFundamentalMatrix(key_ab).matrix()
                    F2 = result.atSimpleFundamentalMatrix(key_ac).matrix()
                    simple_f_result.insert(key_ab, FundamentalMatrix(F1))
                    simple_f_result.insert(key_ac, FundamentalMatrix(F2))
 
            # Compute distances from ground truth
            distances = compute_distances(method, result, ground_truth, args.num_cameras, cal)
 
            # Compute final error
            final_error = graph.error(result)
 
            # Store results
            results[method]["distances"].extend(distances)
            results[method]["final_error"].append(final_error)
            results[method]["iterations"].append(iterations)
 
            print(f"Method: {method}")
            print(f"Final Error: {final_error:.3f}")
            print(f"Mean Geodesic Distance: {np.mean(distances):.3f}")
            print(f"Number of Iterations: {iterations}\n")
 
    # Average results over trials
    for method in methods:
        results[method]["final_error"] = np.median(results[method]["final_error"])
        results[method]["distances"] = np.median(results[method]["distances"])
        results[method]["iterations"] = np.median(results[method]["iterations"])
 
    # Plot results
    plot_results(results)
 
 
if __name__ == "__main__":
    main()