Skip to content

Commit 360dc41

Browse files
dellaertdellaert
committed
Compare 3 more cases
1 parent ce196d9 commit 360dc41

File tree

1 file changed

+69
-43
lines changed

1 file changed

+69
-43
lines changed

‎python/gtsam/examples/ViewGraphComparison.py

+69-43
Original file line numberDiff line numberDiff line change
@@ -32,7 +32,7 @@
3232
K = gtsam.symbol_shorthand.K
3333

3434
# Methods to compare
35-
methods = ["SimpleF", "Fundamental", "Essential+Ks", "Calibrated"]
35+
methods = ["SimpleF", "Fundamental", "Essential+Ks", "Essential+K", "Calibrated", "Binary+Ks", "Binary+K"]
3636

3737

3838
# Formatter function for printing keys
@@ -76,8 +76,8 @@ def simulate_data(points, poses, cal, rng, noise_std):
7676
return measurements
7777

7878

79-
# Function to compute ground truth matrices
8079
def compute_ground_truth(method, poses, cal):
80+
"""Function to compute ground truth edge variables."""
8181
E1 = EssentialMatrix.FromPose3(poses[0].between(poses[1]))
8282
E2 = EssentialMatrix.FromPose3(poses[0].between(poses[2]))
8383
F1 = FundamentalMatrix(cal.K(), E1, cal.K())
@@ -90,58 +90,80 @@ def compute_ground_truth(method, poses, cal):
9090
SF1 = SimpleFundamentalMatrix(E1, f, f, c, c)
9191
SF2 = SimpleFundamentalMatrix(E2, f, f, c, c)
9292
return SF1, SF2
93-
elif method == "Essential+Ks" or method == "Calibrated":
94-
return E1, E2
9593
else:
96-
raise ValueError(f"Unknown method {method}")
94+
return E1, E2
9795

9896

9997
def build_factor_graph(method, num_cameras, measurements, cal):
10098
"""build the factor graph"""
10199
graph = NonlinearFactorGraph()
102100

101+
# Determine the FactorClass based on the method
103102
if method == "Fundamental":
104103
FactorClass = gtsam.TransferFactorFundamentalMatrix
105104
elif method == "SimpleF":
106105
FactorClass = gtsam.TransferFactorSimpleFundamentalMatrix
107-
elif method == "Essential+Ks":
106+
elif method in ["Essential+Ks", "Essential+K"]:
108107
FactorClass = gtsam.EssentialTransferFactorKCal3f
109-
# add priors on all calibrations:
110-
for i in range(num_cameras):
111-
model = gtsam.noiseModel.Isotropic.Sigma(1, 10.0)
112-
graph.addPriorCal3f(K(i), cal, model)
108+
elif method == "Binary+K":
109+
FactorClass = gtsam.EssentialMatrixFactor4Cal3f
110+
elif method == "Binary+Ks":
111+
FactorClass = gtsam.EssentialMatrixFactor5Cal3f
113112
elif method == "Calibrated":
114113
FactorClass = gtsam.EssentialTransferFactorCal3f
115-
# No priors on calibration needed
116114
else:
117115
raise ValueError(f"Unknown method {method}")
118116

117+
# Add priors on calibrations if necessary
118+
if method in ["Essential+Ks", "Binary+Ks"]:
119+
for i in range(num_cameras):
120+
model = gtsam.noiseModel.Isotropic.Sigma(1, 10.0)
121+
graph.addPriorCal3f(K(i), cal, model)
122+
elif method in ["Essential+K", "Binary+K"]:
123+
model = gtsam.noiseModel.Isotropic.Sigma(1, 10.0)
124+
graph.addPriorCal3f(K(0), cal, model)
125+
119126
z = measurements # shorthand
120127

121128
for a in range(num_cameras):
122129
b = (a + 1) % num_cameras # Next camera
123130
c = (a + 2) % num_cameras # Camera after next
124-
125-
# Vectors to collect tuples for each factor
126-
tuples1 = []
127-
tuples2 = []
128-
tuples3 = []
129-
130-
# Collect data for the three factors
131-
for j in range(len(measurements[0])):
132-
tuples1.append((z[a][j], z[b][j], z[c][j]))
133-
tuples2.append((z[a][j], z[c][j], z[b][j]))
134-
tuples3.append((z[c][j], z[b][j], z[a][j]))
135-
136-
# Add transfer factors between views a, b, and c.
137-
if method in ["Calibrated"]:
138-
graph.add(FactorClass(EdgeKey(a, c), EdgeKey(b, c), tuples1, cal))
139-
graph.add(FactorClass(EdgeKey(a, b), EdgeKey(b, c), tuples2, cal))
140-
graph.add(FactorClass(EdgeKey(a, c), EdgeKey(a, b), tuples3, cal))
131+
if method in ["Binary+Ks", "Binary+K"]:
132+
# Add binary Essential Matrix factors
133+
ab, ac = EdgeKey(a, b).key(), EdgeKey(a, c).key()
134+
for j in range(len(measurements[0])):
135+
if method == "Binary+Ks":
136+
graph.add(FactorClass(ab, K(a), K(b), z[a][j], z[b][j]))
137+
graph.add(FactorClass(ac, K(a), K(c), z[a][j], z[c][j]))
138+
else: # Binary+K
139+
graph.add(FactorClass(ab, K(0), z[a][j], z[b][j]))
140+
graph.add(FactorClass(ac, K(0), z[a][j], z[c][j]))
141141
else:
142-
graph.add(FactorClass(EdgeKey(a, c), EdgeKey(b, c), tuples1))
143-
graph.add(FactorClass(EdgeKey(a, b), EdgeKey(b, c), tuples2))
144-
graph.add(FactorClass(EdgeKey(a, c), EdgeKey(a, b), tuples3))
142+
# Add transfer factors between views a, b, and c
143+
144+
# Vectors to collect tuples for each factor
145+
tuples1 = []
146+
tuples2 = []
147+
tuples3 = []
148+
149+
for j in range(len(measurements[0])):
150+
tuples1.append((z[a][j], z[b][j], z[c][j]))
151+
tuples2.append((z[a][j], z[c][j], z[b][j]))
152+
tuples3.append((z[c][j], z[b][j], z[a][j]))
153+
154+
# Add transfer factors between views a, b, and c.
155+
if method in ["Calibrated"]:
156+
graph.add(FactorClass(EdgeKey(a, c), EdgeKey(b, c), tuples1, cal))
157+
graph.add(FactorClass(EdgeKey(a, b), EdgeKey(b, c), tuples2, cal))
158+
graph.add(FactorClass(EdgeKey(a, c), EdgeKey(a, b), tuples3, cal))
159+
elif method == "Essential+K":
160+
graph.add(FactorClass(EdgeKey(a, c), EdgeKey(b, c), K(0), tuples1))
161+
graph.add(FactorClass(EdgeKey(a, b), EdgeKey(b, c), K(0), tuples2))
162+
graph.add(FactorClass(EdgeKey(a, c), EdgeKey(a, b), K(0), tuples3))
163+
else:
164+
graph.add(FactorClass(EdgeKey(a, c), EdgeKey(b, c), tuples1))
165+
graph.add(FactorClass(EdgeKey(a, b), EdgeKey(b, c), tuples2))
166+
graph.add(FactorClass(EdgeKey(a, c), EdgeKey(a, b), tuples3))
145167

146168
return graph
147169

@@ -159,22 +181,23 @@ def get_initial_estimate(method, num_cameras, ground_truth, cal):
159181
initialEstimate.insert(EdgeKey(a, b).key(), F1)
160182
initialEstimate.insert(EdgeKey(a, c).key(), F2)
161183
total_dimension += F1.dim() + F2.dim()
162-
elif method in ["Essential+Ks", "Calibrated"]:
184+
elif method in ["Essential+Ks", "Essential+K", "Binary+Ks", "Binary+K", "Calibrated"]:
163185
E1, E2 = ground_truth
164186
for a in range(num_cameras):
165-
b = (a + 1) % num_cameras # Next camera
166-
c = (a + 2) % num_cameras # Camera after next
187+
b = (a + 1) % num_cameras
188+
c = (a + 2) % num_cameras
167189
initialEstimate.insert(EdgeKey(a, b).key(), E1)
168190
initialEstimate.insert(EdgeKey(a, c).key(), E2)
169191
total_dimension += E1.dim() + E2.dim()
170-
else:
171-
raise ValueError(f"Unknown method {method}")
172192

173-
if method == "Essential+Ks":
174-
# Insert initial calibrations
193+
# Insert initial calibrations
194+
if method in ["Essential+Ks", "Binary+Ks"]:
175195
for i in range(num_cameras):
176196
initialEstimate.insert(K(i), cal)
177197
total_dimension += cal.dim()
198+
elif method in ["Essential+K", "Binary+K"]:
199+
initialEstimate.insert(K(0), cal)
200+
total_dimension += cal.dim()
178201

179202
print(f"Total dimension of the problem: {total_dimension}")
180203
return initialEstimate
@@ -205,7 +228,7 @@ def compute_distances(method, result, ground_truth, num_cameras, cal):
205228
key_ab = EdgeKey(a, b).key()
206229
key_ac = EdgeKey(a, c).key()
207230

208-
if method in ["Essential+Ks", "Calibrated"]:
231+
if method in ["Essential+Ks", "Essential+K", "Binary+Ks", "Binary+K", "Calibrated"]:
209232
E_est_ab = result.atEssentialMatrix(key_ab)
210233
E_est_ac = result.atEssentialMatrix(key_ac)
211234

@@ -218,15 +241,18 @@ def compute_distances(method, result, ground_truth, num_cameras, cal):
218241
SF_est_ac = result.atSimpleFundamentalMatrix(key_ac).matrix()
219242
F_est_ab = FundamentalMatrix(SF_est_ab)
220243
F_est_ac = FundamentalMatrix(SF_est_ac)
221-
elif method == "Essential+Ks":
222-
# Retrieve calibrations from result:
244+
elif method in ["Essential+Ks", "Binary+Ks"]:
245+
# Retrieve calibrations from result for each camera
223246
cal_a = result.atCal3f(K(a))
224247
cal_b = result.atCal3f(K(b))
225248
cal_c = result.atCal3f(K(c))
226-
227-
# Convert estimated EssentialMatrices to FundamentalMatrices
228249
F_est_ab = FundamentalMatrix(cal_a.K(), E_est_ab, cal_b.K())
229250
F_est_ac = FundamentalMatrix(cal_a.K(), E_est_ac, cal_c.K())
251+
elif method in ["Essential+K", "Binary+K"]:
252+
# Use single shared calibration
253+
cal_shared = result.atCal3f(K(0))
254+
F_est_ab = FundamentalMatrix(cal_shared.K(), E_est_ab, cal_shared.K())
255+
F_est_ac = FundamentalMatrix(cal_shared.K(), E_est_ac, cal_shared.K())
230256
elif method == "Calibrated":
231257
# Use ground truth calibration
232258
F_est_ab = FundamentalMatrix(cal.K(), E_est_ab, cal.K())

0 commit comments

Comments
 (0)