Add SimpleF case
							parent
							
								
									bbba497ca1
								
							
						
					
					
						commit
						aa0db60a52
					
				|  | @ -1,7 +1,9 @@ | |||
| """ | ||||
|   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 F-manifold. It also plots the final error of the optimization. | ||||
|   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 | ||||
| """ | ||||
|  | @ -13,15 +15,24 @@ import numpy as np | |||
| from gtsam.examples import SFMdata | ||||
| 
 | ||||
| import gtsam | ||||
| from gtsam import (Cal3f, EdgeKey, EssentialMatrix, FundamentalMatrix, | ||||
|                    LevenbergMarquardtOptimizer, LevenbergMarquardtParams, | ||||
|                    NonlinearFactorGraph, PinholeCameraCal3f, Values) | ||||
| 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 = ["FundamentalMatrix", "EssentialMatrix"] | ||||
| methods = ["FundamentalMatrix", "SimpleFundamentalMatrix", "EssentialMatrix"] | ||||
| 
 | ||||
| 
 | ||||
| # Formatter function for printing keys | ||||
|  | @ -63,41 +74,38 @@ def simulate_data(points, poses, cal, rng, noise_std): | |||
| 
 | ||||
| # Function to compute ground truth matrices | ||||
| def compute_ground_truth(method, poses, cal): | ||||
|     F1 = FundamentalMatrix(cal.K(), poses[0].between(poses[1]), cal.K()) | ||||
|     F2 = FundamentalMatrix(cal.K(), poses[0].between(poses[2]), cal.K()) | ||||
|     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 == "FundamentalMatrix": | ||||
|         return F1, F2 | ||||
|     elif method == "SimpleFundamentalMatrix": | ||||
|         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 == "EssentialMatrix": | ||||
|         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, with known calibration | ||||
|         invK = np.linalg.inv(cal.K()) | ||||
|         G1 = invK.transpose() @ E1.matrix() @ invK | ||||
|         G2 = invK.transpose() @ E2.matrix() @ invK | ||||
|         assert np.allclose( | ||||
|             G1 / np.linalg.norm(G1), F1.matrix() / np.linalg.norm(F1.matrix()) | ||||
|         ), "E1 and F1 are not the same" | ||||
|         assert np.allclose( | ||||
|             G2 / np.linalg.norm(G2), F2.matrix() / np.linalg.norm(F2.matrix()) | ||||
|         ), "E2 and F2 are not the same" | ||||
|         return E1, E2 | ||||
|     else: | ||||
|         raise ValueError(f"Unknown method {method}") | ||||
| 
 | ||||
| 
 | ||||
| def build_factor_graph(method, num_cameras, measurements): | ||||
| def build_factor_graph(method, num_cameras, measurements, cal): | ||||
|     """build the factor graph""" | ||||
|     graph = NonlinearFactorGraph() | ||||
| 
 | ||||
|     if method == "FundamentalMatrix": | ||||
|         FactorClass = gtsam.TransferFactorFundamentalMatrix | ||||
|     elif method == "SimpleFundamentalMatrix": | ||||
|         FactorClass = gtsam.TransferFactorSimpleFundamentalMatrix | ||||
|     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) | ||||
|             graph.addPriorCal3f(K(i), cal, model) | ||||
|     else: | ||||
|         raise ValueError(f"Unknown method {method}") | ||||
| 
 | ||||
|  | @ -129,7 +137,7 @@ def get_initial_estimate(method, num_cameras, ground_truth, cal): | |||
|     initialEstimate = Values() | ||||
|     total_dimension = 0 | ||||
| 
 | ||||
|     if method == "FundamentalMatrix": | ||||
|     if method == "FundamentalMatrix" or method == "SimpleFundamentalMatrix": | ||||
|         F1, F2 = ground_truth | ||||
|         for a in range(num_cameras): | ||||
|             b = (a + 1) % num_cameras  # Next camera | ||||
|  | @ -174,13 +182,13 @@ def compute_distances(method, result, ground_truth, num_cameras, cal): | |||
|     """Compute geodesic distances from ground truth""" | ||||
|     distances = [] | ||||
| 
 | ||||
|     if method == "FundamentalMatrix": | ||||
|     if method == "FundamentalMatrix" or method == "SimpleFundamentalMatrix": | ||||
|         F1, F2 = ground_truth | ||||
|     elif method == "EssentialMatrix": | ||||
|         E1, E2 = ground_truth | ||||
|         # Convert ground truth EssentialMatrices to FundamentalMatrices | ||||
|         F1 = gtsam.FundamentalMatrix(cal.K(), E1, cal.K()) | ||||
|         F2 = gtsam.FundamentalMatrix(cal.K(), E2, cal.K()) | ||||
|         F1 = FundamentalMatrix(cal.K(), E1, cal.K()) | ||||
|         F2 = FundamentalMatrix(cal.K(), E2, cal.K()) | ||||
|     else: | ||||
|         raise ValueError(f"Unknown method {method}") | ||||
| 
 | ||||
|  | @ -193,6 +201,9 @@ def compute_distances(method, result, ground_truth, num_cameras, cal): | |||
|         if method == "FundamentalMatrix": | ||||
|             F_est_ab = result.atFundamentalMatrix(key_ab) | ||||
|             F_est_ac = result.atFundamentalMatrix(key_ac) | ||||
|         elif method == "SimpleFundamentalMatrix": | ||||
|             F_est_ab = result.atSimpleFundamentalMatrix(key_ab) | ||||
|             F_est_ac = result.atSimpleFundamentalMatrix(key_ac) | ||||
|         elif method == "EssentialMatrix": | ||||
|             E_est_ab = result.atEssentialMatrix(key_ab) | ||||
|             E_est_ac = result.atEssentialMatrix(key_ac) | ||||
|  | @ -203,8 +214,8 @@ def compute_distances(method, result, ground_truth, num_cameras, cal): | |||
|             cal_c = result.atCal3f(K(c)) | ||||
| 
 | ||||
|             # Convert estimated EssentialMatrices to FundamentalMatrices | ||||
|             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()) | ||||
|             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()) | ||||
| 
 | ||||
|         # Compute local coordinates (geodesic distance on the F-manifold) | ||||
|         dist_ab = np.linalg.norm(F1.localCoordinates(F_est_ab)) | ||||
|  | @ -219,7 +230,7 @@ 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] | ||||
|     distances = [results[method]["distances"] for method in methods] | ||||
| 
 | ||||
|     fig, ax1 = plt.subplots() | ||||
| 
 | ||||
|  | @ -277,14 +288,14 @@ def main(): | |||
|             print(f"\nRunning method: {method}") | ||||
| 
 | ||||
|             # Build the factor graph | ||||
|             graph = build_factor_graph(method, args.num_cameras, measurements) | ||||
|             graph = build_factor_graph(method, args.num_cameras, measurements, cal) | ||||
| 
 | ||||
|             # Assert that the initial error is the same for both methods: | ||||
|             # Assert that the initial error is the same for all methods: | ||||
|             if method == methods[0]: | ||||
|                 error0 = graph.error(initial_estimate[method]) | ||||
|             else: | ||||
|                 current_error = graph.error(initial_estimate[method]) | ||||
|                 assert np.allclose(error0, current_error) | ||||
|                 assert np.allclose(error0, current_error), "Initial errors do not match among methods." | ||||
| 
 | ||||
|             # Optimize the graph | ||||
|             result = optimize(graph, initial_estimate[method], method) | ||||
|  |  | |||
		Loading…
	
		Reference in New Issue