12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Run many trials

release/4.3a0
Frank Dellaert 2024-10-27 00:34:40 -07:00
parent b9dc9ae459
commit 77dfa446a9
1 changed files with 70 additions and 42 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

98
python/gtsam/examples/ViewGraphComparison.py
View File

@ -34,8 +34,8 @@ def formatter(key):
return f"({edge.i()},{edge.j()})"
# Function to simulate data
def simulate_data(num_cameras):
def simulate_geometry(num_cameras):
"""simulate geometry (points and poses)"""
# Define the camera calibration parameters
cal = Cal3f(50.0, 50.0, 50.0)
@ -45,14 +45,20 @@ def simulate_data(num_cameras):
# 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 = PinholeCameraCal3f(poses[i], cal)
for j in range(len(points)):
measurements[i][j] = camera.project(points[j])
return points, poses, cal
return points, poses, measurements, 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
@ -69,15 +75,13 @@ def compute_ground_truth(method, poses, cal):
raise ValueError(f"Unknown method {method}")
# Function to build the factor graph
def build_factor_graph(method, num_cameras, measurements):
"""build the factor graph"""
graph = NonlinearFactorGraph()
if method == "FundamentalMatrix":
# Use TransferFactorFundamentalMatrix
FactorClass = gtsam.TransferFactorFundamentalMatrix
elif method == "EssentialMatrix":
# Use EssentialTransferFactorCal3f
FactorClass = gtsam.EssentialTransferFactorCal3f
else:
raise ValueError(f"Unknown method {method}")
@ -105,9 +109,10 @@ def build_factor_graph(method, num_cameras, measurements):
return graph
# Function to get initial estimates
def get_initial_estimate(method, num_cameras, ground_truth, cal):
"""get initial estimate for method"""
initialEstimate = Values()
total_dimension = 0
if method == "FundamentalMatrix":
F1, F2 = ground_truth
@ -116,6 +121,7 @@ def get_initial_estimate(method, num_cameras, ground_truth, cal):
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 == "EssentialMatrix":
E1, E2 = ground_truth
for a in range(num_cameras):
@ -123,34 +129,38 @@ def get_initial_estimate(method, num_cameras, ground_truth, cal):
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()
# Insert initial calibrations
for i in range(num_cameras):
initialEstimate.insert(K(i), cal)
total_dimension += cal.dim()
else:
raise ValueError(f"Unknown method {method}")
print(f"Total dimension of the problem: {total_dimension}")
return initialEstimate
# Function to optimize the graph
def optimize(graph, initialEstimate):
"""optimize the graph"""
params = LevenbergMarquardtParams()
params.setlambdaInitial(1000.0) # Initialize lambda to a high value
params.setAbsoluteErrorTol(1.0)
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, num_cameras, cal):
"""Compute geodesic distances from ground truth"""
distances = []
if method == "FundamentalMatrix":
F1, F2 = ground_truth
elif method == "EssentialMatrix":
E1, E2 = ground_truth
# Convert ground truth EssentialMatrices to FundamentalMatrices using GTSAM method
# Convert ground truth EssentialMatrices to FundamentalMatrices
F1 = gtsam.FundamentalMatrix(cal.K(), E1, cal.K())
F2 = gtsam.FundamentalMatrix(cal.K(), E2, cal.K())
else:
@ -168,11 +178,9 @@ 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)
# Convert estimated EssentialMatrices to FundamentalMatrices using GTSAM method
# 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())
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))
@ -183,8 +191,8 @@ def compute_distances(method, result, ground_truth, num_cameras, cal):
return distances
# Function to plot results
def plot_results(results):
"""plot 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]
@ -213,41 +221,61 @@ 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("--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)")
args = parser.parse_args()
# Initialize the random number generator
rng = np.random.default_rng(seed=args.seed)
# Initialize results dictionary
results = {}
results = {method: {"distances": [], "final_error": []} for method in methods}
for method in methods:
print(f"Running method: {method}")
# Simulate data
points, poses, measurements, cal = simulate_data(args.num_cameras)
# Simulate geometry
points, poses, cal = simulate_geometry(args.num_cameras)
# Compute ground truth matrices
ground_truth = compute_ground_truth(method, poses, cal)
ground_truth = {method: compute_ground_truth(method, poses, cal) for method in methods}
# Get initial estimates
initial_estimate = {
method: get_initial_estimate(method, args.num_cameras, ground_truth[method], cal) for method in methods
}
for trial in range(args.nr_trials):
print(f"\nTrial {trial + 1}/{args.nr_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)
# Get initial estimates
initialEstimate = get_initial_estimate(method, args.num_cameras, ground_truth, cal)
# Optimize the graph
result = optimize(graph, initialEstimate)
result = optimize(graph, initial_estimate[method])
# Compute distances from ground truth
distances = compute_distances(method, result, ground_truth, args.num_cameras, cal)
distances = compute_distances(method, result, ground_truth[method], args.num_cameras, cal)
# Compute final error
final_error = graph.error(result)
# Store results
results[method] = {"distances": distances, "final_error": final_error}
results[method]["distances"].extend(distances)
results[method]["final_error"].append(final_error)
print(f"Method: {method}")
print(f"Final Error: {final_error}")
print(f"Mean Geodesic Distance: {np.mean(distances)}\n")
print(f"Final Error: {final_error:.3f}")
print(f"Mean Geodesic Distance: {np.mean(distances):.3f}\n")
# Average results over trials
for method in methods:
results[method]["final_error"] = np.mean(results[method]["final_error"])
results[method]["distances"] = np.mean(results[method]["distances"])
# Plot results
plot_results(results)