From eda3662b6ecb72f575536c0cb22828df99b37720 Mon Sep 17 00:00:00 2001 From: Travis Driver Date: Tue, 15 Oct 2024 18:00:34 -0400 Subject: [PATCH] Fix key error --- .../cycle_consistent_rotation_estimator.py | 35 ++++++++++--------- 1 file changed, 18 insertions(+), 17 deletions(-) diff --git a/gtsfm/view_graph_estimator/cycle_consistent_rotation_estimator.py b/gtsfm/view_graph_estimator/cycle_consistent_rotation_estimator.py index 0ed9117da..682527e9d 100644 --- a/gtsfm/view_graph_estimator/cycle_consistent_rotation_estimator.py +++ b/gtsfm/view_graph_estimator/cycle_consistent_rotation_estimator.py @@ -267,12 +267,12 @@ def optimize_essential_matrices( else: key = E(len(pair_to_key)) pair_to_key[pair] = key - for kp0, kp1 in zip(mkps0, mkps1): - graph.add( - gtsam.EssentialMatrixFactor( - key, calibrations[pair[1]].calibrate(kp1), calibrations[pair[0]].calibrate(kp0), noise_model + for kp0, kp1 in zip(mkps0, mkps1): + graph.add( + gtsam.EssentialMatrixFactor( + key, calibrations[pair[1]].calibrate(kp1), calibrations[pair[0]].calibrate(kp0), noise_model + ) ) - ) # i0 -> i2 pair = (i0, i2) @@ -283,12 +283,12 @@ def optimize_essential_matrices( else: key = E(len(pair_to_key)) pair_to_key[pair] = key - for kp0, kp1 in zip(mkps0, mkps1): - graph.add( - gtsam.EssentialMatrixFactor( - key, calibrations[pair[1]].calibrate(kp1), calibrations[pair[0]].calibrate(kp0), noise_model + for kp0, kp1 in zip(mkps0, mkps1): + graph.add( + gtsam.EssentialMatrixFactor( + key, calibrations[pair[1]].calibrate(kp1), calibrations[pair[0]].calibrate(kp0), noise_model + ) ) - ) # i1 -> i2 pair = (i1, i2) @@ -299,16 +299,17 @@ def optimize_essential_matrices( else: key = E(len(pair_to_key)) pair_to_key[pair] = key - for kp0, kp1 in zip(mkps0, mkps1): - graph.add( - gtsam.EssentialMatrixFactor( - key, calibrations[pair[1]].calibrate(kp1), calibrations[pair[0]].calibrate(kp0), noise_model + for kp0, kp1 in zip(mkps0, mkps1): + graph.add( + gtsam.EssentialMatrixFactor( + key, calibrations[pair[1]].calibrate(kp1), calibrations[pair[0]].calibrate(kp0), noise_model + ) ) - ) + print("Num edges in E opt:", len(pair_to_key)) # Create initial estimate. initial = gtsam.Values() - for key, pair in pair_to_key.items(): + for pair, key in pair_to_key.items(): initial.insert(key, gtsam.EssentialMatrix(i2Ri1_dict[pair], i2Ui1_dict[pair])) # Optimize! @@ -318,7 +319,7 @@ def optimize_essential_matrices( result = optimizer.optimize() # Add optimized rotations and translations to the dictionary. - for key, pair in pair_to_key.items(): + for pair, key in pair_to_key.items(): E_opt = result.atEssentialMatrix(key) i2Ri1_dict[pair] = E_opt.rotation() i2Ui1_dict[pair] = E_opt.direction()