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

Compare 3 more cases

release/4.3a0
Frank Dellaert 2024-10-30 16:07:40 -07:00
parent ce196d962f
commit 360dc4138c
1 changed files with 70 additions and 44 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

114
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 = ["SimpleF", "Fundamental", "Essential+Ks", "Calibrated"] methods = ["SimpleF", "Fundamental", "Essential+Ks", "Essential+K", "Calibrated", "Binary+Ks", "Binary+K"]
# Formatter function for printing keys # Formatter function for printing keys
@ -76,8 +76,8 @@ def simulate_data(points, poses, cal, rng, noise_std):
return measurements return measurements
# Function to compute ground truth matrices
def compute_ground_truth(method, poses, cal): def compute_ground_truth(method, poses, cal):
"""Function to compute ground truth edge variables."""
E1 = EssentialMatrix.FromPose3(poses[0].between(poses[1])) E1 = EssentialMatrix.FromPose3(poses[0].between(poses[1]))
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())
@ -90,58 +90,80 @@ def compute_ground_truth(method, poses, cal):
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 == "Essential+Ks" or method == "Calibrated":
return E1, E2
else: else:
raise ValueError(f"Unknown method {method}") return E1, E2
def build_factor_graph(method, num_cameras, measurements, cal): def build_factor_graph(method, num_cameras, measurements, cal):
"""build the factor graph""" """build the factor graph"""
graph = NonlinearFactorGraph() graph = NonlinearFactorGraph()
# Determine the FactorClass based on the method
if method == "Fundamental": if method == "Fundamental":
FactorClass = gtsam.TransferFactorFundamentalMatrix FactorClass = gtsam.TransferFactorFundamentalMatrix
elif method == "SimpleF": elif method == "SimpleF":
FactorClass = gtsam.TransferFactorSimpleFundamentalMatrix FactorClass = gtsam.TransferFactorSimpleFundamentalMatrix
elif method == "Essential+Ks": elif method in ["Essential+Ks", "Essential+K"]:
FactorClass = gtsam.EssentialTransferFactorKCal3f FactorClass = gtsam.EssentialTransferFactorKCal3f
# add priors on all calibrations: elif method == "Binary+K":
FactorClass = gtsam.EssentialMatrixFactor4Cal3f
elif method == "Binary+Ks":
FactorClass = gtsam.EssentialMatrixFactor5Cal3f
elif method == "Calibrated":
FactorClass = gtsam.EssentialTransferFactorCal3f
else:
raise ValueError(f"Unknown method {method}")
# Add priors on calibrations if necessary
if method in ["Essential+Ks", "Binary+Ks"]:
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 == "Calibrated": elif method in ["Essential+K", "Binary+K"]:
FactorClass = gtsam.EssentialTransferFactorCal3f model = gtsam.noiseModel.Isotropic.Sigma(1, 10.0)
# No priors on calibration needed graph.addPriorCal3f(K(0), cal, model)
else:
raise ValueError(f"Unknown method {method}")
z = measurements # shorthand z = measurements # shorthand
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
c = (a + 2) % num_cameras # Camera after next c = (a + 2) % num_cameras # Camera after next
if method in ["Binary+Ks", "Binary+K"]:
# Vectors to collect tuples for each factor # Add binary Essential Matrix factors
tuples1 = [] ab, ac = EdgeKey(a, b).key(), EdgeKey(a, c).key()
tuples2 = [] for j in range(len(measurements[0])):
tuples3 = [] if method == "Binary+Ks":
graph.add(FactorClass(ab, K(a), K(b), z[a][j], z[b][j]))
# Collect data for the three factors graph.add(FactorClass(ac, K(a), K(c), z[a][j], z[c][j]))
for j in range(len(measurements[0])): else: # Binary+K
tuples1.append((z[a][j], z[b][j], z[c][j])) graph.add(FactorClass(ab, K(0), z[a][j], z[b][j]))
tuples2.append((z[a][j], z[c][j], z[b][j])) graph.add(FactorClass(ac, K(0), z[a][j], z[c][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: else:
graph.add(FactorClass(EdgeKey(a, c), EdgeKey(b, c), tuples1)) # Add transfer factors between views a, b, and c
graph.add(FactorClass(EdgeKey(a, b), EdgeKey(b, c), tuples2))
graph.add(FactorClass(EdgeKey(a, c), EdgeKey(a, b), tuples3)) # Vectors to collect tuples for each factor
tuples1 = []
tuples2 = []
tuples3 = []
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))
elif method == "Essential+K":
graph.add(FactorClass(EdgeKey(a, c), EdgeKey(b, c), K(0), tuples1))
graph.add(FactorClass(EdgeKey(a, b), EdgeKey(b, c), K(0), tuples2))
graph.add(FactorClass(EdgeKey(a, c), EdgeKey(a, b), K(0), tuples3))
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 return graph
@ -159,22 +181,23 @@ 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 ["Essential+Ks", "Calibrated"]: elif method in ["Essential+Ks", "Essential+K", "Binary+Ks", "Binary+K", "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
c = (a + 2) % num_cameras # Camera after next c = (a + 2) % num_cameras
initialEstimate.insert(EdgeKey(a, b).key(), E1) initialEstimate.insert(EdgeKey(a, b).key(), E1)
initialEstimate.insert(EdgeKey(a, c).key(), E2) initialEstimate.insert(EdgeKey(a, c).key(), E2)
total_dimension += E1.dim() + E2.dim() total_dimension += E1.dim() + E2.dim()
else:
raise ValueError(f"Unknown method {method}")
if method == "Essential+Ks": # Insert initial calibrations
# Insert initial calibrations if method in ["Essential+Ks", "Binary+Ks"]:
for i in range(num_cameras): for i in range(num_cameras):
initialEstimate.insert(K(i), cal) initialEstimate.insert(K(i), cal)
total_dimension += cal.dim() total_dimension += cal.dim()
elif method in ["Essential+K", "Binary+K"]:
initialEstimate.insert(K(0), cal)
total_dimension += cal.dim()
print(f"Total dimension of the problem: {total_dimension}") print(f"Total dimension of the problem: {total_dimension}")
return initialEstimate return initialEstimate
@ -205,7 +228,7 @@ 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 ["Essential+Ks", "Calibrated"]: if method in ["Essential+Ks", "Essential+K", "Binary+Ks", "Binary+K", "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)
@ -218,15 +241,18 @@ def compute_distances(method, result, ground_truth, num_cameras, cal):
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 == "Essential+Ks": elif method in ["Essential+Ks", "Binary+Ks"]:
# Retrieve calibrations from result: # Retrieve calibrations from result for each camera
cal_a = result.atCal3f(K(a)) cal_a = result.atCal3f(K(a))
cal_b = result.atCal3f(K(b)) cal_b = result.atCal3f(K(b))
cal_c = result.atCal3f(K(c)) 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_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 in ["Essential+K", "Binary+K"]:
# Use single shared calibration
cal_shared = result.atCal3f(K(0))
F_est_ab = FundamentalMatrix(cal_shared.K(), E_est_ab, cal_shared.K())
F_est_ac = FundamentalMatrix(cal_shared.K(), E_est_ac, cal_shared.K())
elif method == "Calibrated": 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())