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

Abbreviate methods

release/4.3a0
Frank Dellaert 2024-10-29 11:07:32 -07:00
parent 6b96ae217f
commit 005efb3f07
1 changed files with 20 additions and 20 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -32,7 +32,7 @@ from gtsam import (
K = gtsam.symbol_shorthand.K K = gtsam.symbol_shorthand.K
# Methods to compare # Methods to compare
methods = ["FundamentalMatrix", "SimpleFundamentalMatrix", "EssentialMatrix", "CalibratedEssentialMatrix"] methods = ["Fundamental", "SimpleF", "Essential+Ks", "Calibrated"]
# Formatter function for printing keys # Formatter function for printing keys
@ -82,15 +82,15 @@ def compute_ground_truth(method, poses, cal):
E2 = EssentialMatrix.FromPose3(poses[0].between(poses[2])) E2 = EssentialMatrix.FromPose3(poses[0].between(poses[2]))
F1 = FundamentalMatrix(cal.K(), E1, cal.K()) F1 = FundamentalMatrix(cal.K(), E1, cal.K())
F2 = FundamentalMatrix(cal.K(), E2, cal.K()) F2 = FundamentalMatrix(cal.K(), E2, cal.K())
if method == "FundamentalMatrix": if method == "Fundamental":
return F1, F2 return F1, F2
elif method == "SimpleFundamentalMatrix": elif method == "SimpleF":
f = cal.fx() f = cal.fx()
c = cal.principalPoint() c = cal.principalPoint()
SF1 = SimpleFundamentalMatrix(E1, f, f, c, c) SF1 = SimpleFundamentalMatrix(E1, f, f, c, c)
SF2 = SimpleFundamentalMatrix(E2, f, f, c, c) SF2 = SimpleFundamentalMatrix(E2, f, f, c, c)
return SF1, SF2 return SF1, SF2
elif method == "EssentialMatrix" or method == "CalibratedEssentialMatrix": elif method == "Essential+Ks" or method == "Calibrated":
return E1, E2 return E1, E2
else: else:
raise ValueError(f"Unknown method {method}") raise ValueError(f"Unknown method {method}")
@ -100,23 +100,23 @@ def build_factor_graph(method, num_cameras, measurements, cal):
"""build the factor graph""" """build the factor graph"""
graph = NonlinearFactorGraph() graph = NonlinearFactorGraph()
if method == "FundamentalMatrix": if method == "Fundamental":
FactorClass = gtsam.TransferFactorFundamentalMatrix FactorClass = gtsam.TransferFactorFundamentalMatrix
elif method == "SimpleFundamentalMatrix": elif method == "SimpleF":
FactorClass = gtsam.TransferFactorSimpleFundamentalMatrix FactorClass = gtsam.TransferFactorSimpleFundamentalMatrix
elif method == "EssentialMatrix": elif method == "Essential+Ks":
FactorClass = gtsam.EssentialTransferFactorCal3f FactorClass = gtsam.EssentialTransferFactorCal3f
# add priors on all calibrations: # add priors on all calibrations:
for i in range(num_cameras): for i in range(num_cameras):
model = gtsam.noiseModel.Isotropic.Sigma(1, 10.0) model = gtsam.noiseModel.Isotropic.Sigma(1, 10.0)
graph.addPriorCal3f(K(i), cal, model) graph.addPriorCal3f(K(i), cal, model)
elif method == "CalibratedEssentialMatrix": elif method == "Calibrated":
FactorClass = gtsam.TransferFactorEssentialMatrix FactorClass = gtsam.TransferFactorEssentialMatrix
# No priors on calibration needed # No priors on calibration needed
else: else:
raise ValueError(f"Unknown method {method}") raise ValueError(f"Unknown method {method}")
if method == "CalibratedEssentialMatrix": if method == "Calibrated":
# Calibrate measurements using ground truth calibration # Calibrate measurements using ground truth calibration
z = [[cal.calibrate(m) for m in cam_measurements] for cam_measurements in measurements] z = [[cal.calibrate(m) for m in cam_measurements] for cam_measurements in measurements]
else: else:
@ -150,7 +150,7 @@ def get_initial_estimate(method, num_cameras, ground_truth, cal):
initialEstimate = Values() initialEstimate = Values()
total_dimension = 0 total_dimension = 0
if method in ["FundamentalMatrix", "SimpleFundamentalMatrix"]: if method in ["Fundamental", "SimpleF"]:
F1, F2 = ground_truth F1, F2 = ground_truth
for a in range(num_cameras): for a in range(num_cameras):
b = (a + 1) % num_cameras # Next camera b = (a + 1) % num_cameras # Next camera
@ -158,7 +158,7 @@ def get_initial_estimate(method, num_cameras, ground_truth, cal):
initialEstimate.insert(EdgeKey(a, b).key(), F1) initialEstimate.insert(EdgeKey(a, b).key(), F1)
initialEstimate.insert(EdgeKey(a, c).key(), F2) initialEstimate.insert(EdgeKey(a, c).key(), F2)
total_dimension += F1.dim() + F2.dim() total_dimension += F1.dim() + F2.dim()
elif method in ["EssentialMatrix", "CalibratedEssentialMatrix"]: elif method in ["Essential+Ks", "Calibrated"]:
E1, E2 = ground_truth E1, E2 = ground_truth
for a in range(num_cameras): for a in range(num_cameras):
b = (a + 1) % num_cameras # Next camera b = (a + 1) % num_cameras # Next camera
@ -169,7 +169,7 @@ def get_initial_estimate(method, num_cameras, ground_truth, cal):
else: else:
raise ValueError(f"Unknown method {method}") raise ValueError(f"Unknown method {method}")
if method == "EssentialMatrix": if method == "Essential+Ks":
# Insert initial calibrations # Insert initial calibrations
for i in range(num_cameras): for i in range(num_cameras):
initialEstimate.insert(K(i), cal) initialEstimate.insert(K(i), cal)
@ -196,7 +196,7 @@ def compute_distances(method, result, ground_truth, num_cameras, cal):
"""Compute geodesic distances from ground truth""" """Compute geodesic distances from ground truth"""
distances = [] distances = []
F1, F2 = ground_truth["FundamentalMatrix"] F1, F2 = ground_truth["Fundamental"]
for a in range(num_cameras): for a in range(num_cameras):
b = (a + 1) % num_cameras b = (a + 1) % num_cameras
@ -204,20 +204,20 @@ def compute_distances(method, result, ground_truth, num_cameras, cal):
key_ab = EdgeKey(a, b).key() key_ab = EdgeKey(a, b).key()
key_ac = EdgeKey(a, c).key() key_ac = EdgeKey(a, c).key()
if method in ["EssentialMatrix", "CalibratedEssentialMatrix"]: if method in ["Essential+Ks", "Calibrated"]:
E_est_ab = result.atEssentialMatrix(key_ab) E_est_ab = result.atEssentialMatrix(key_ab)
E_est_ac = result.atEssentialMatrix(key_ac) E_est_ac = result.atEssentialMatrix(key_ac)
# Compute estimated FundamentalMatrices # Compute estimated FundamentalMatrices
if method == "FundamentalMatrix": if method == "Fundamental":
F_est_ab = result.atFundamentalMatrix(key_ab) F_est_ab = result.atFundamentalMatrix(key_ab)
F_est_ac = result.atFundamentalMatrix(key_ac) F_est_ac = result.atFundamentalMatrix(key_ac)
elif method == "SimpleFundamentalMatrix": elif method == "SimpleF":
SF_est_ab = result.atSimpleFundamentalMatrix(key_ab).matrix() SF_est_ab = result.atSimpleFundamentalMatrix(key_ab).matrix()
SF_est_ac = result.atSimpleFundamentalMatrix(key_ac).matrix() SF_est_ac = result.atSimpleFundamentalMatrix(key_ac).matrix()
F_est_ab = FundamentalMatrix(SF_est_ab) F_est_ab = FundamentalMatrix(SF_est_ab)
F_est_ac = FundamentalMatrix(SF_est_ac) F_est_ac = FundamentalMatrix(SF_est_ac)
elif method == "EssentialMatrix": elif method == "Essential+Ks":
# Retrieve calibrations from result: # Retrieve calibrations from result:
cal_a = result.atCal3f(K(a)) cal_a = result.atCal3f(K(a))
cal_b = result.atCal3f(K(b)) cal_b = result.atCal3f(K(b))
@ -226,7 +226,7 @@ def compute_distances(method, result, ground_truth, num_cameras, cal):
# Convert estimated EssentialMatrices to FundamentalMatrices # Convert estimated EssentialMatrices to FundamentalMatrices
F_est_ab = FundamentalMatrix(cal_a.K(), E_est_ab, cal_b.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()) F_est_ac = FundamentalMatrix(cal_a.K(), E_est_ac, cal_c.K())
elif method == "CalibratedEssentialMatrix": elif method == "Calibrated":
# Use ground truth calibration # Use ground truth calibration
F_est_ab = FundamentalMatrix(cal.K(), E_est_ab, cal.K()) F_est_ab = FundamentalMatrix(cal.K(), E_est_ab, cal.K())
F_est_ac = FundamentalMatrix(cal.K(), E_est_ac, cal.K()) F_est_ac = FundamentalMatrix(cal.K(), E_est_ac, cal.K())
@ -322,7 +322,7 @@ def main():
# Assert that the initial error is the same for all methods: # Assert that the initial error is the same for all methods:
if method == methods[0]: if method == methods[0]:
error0 = graph.error(initial_estimate[method]) error0 = graph.error(initial_estimate[method])
elif method == "CalibratedEssentialMatrix": elif method == "Calibrated":
current_error = graph.error(initial_estimate[method]) * cal.f() * cal.f() current_error = graph.error(initial_estimate[method]) * cal.f() * cal.f()
print(error0, current_error) print(error0, current_error)
assert np.allclose(error0, current_error), "Initial errors do not match among methods." assert np.allclose(error0, current_error), "Initial errors do not match among methods."
@ -338,7 +338,7 @@ def main():
# Compute final error # Compute final error
final_error = graph.error(result) final_error = graph.error(result)
if method == "CalibratedEssentialMatrix": if method == "Calibrated":
final_error *= cal.f() * cal.f() final_error *= cal.f() * cal.f()
# Store results # Store results