Fix key error

borglab · Oct 15, 2024 · eda3662 · eda3662
1 parent 6ba0b13
commit eda3662
Showing 1 changed file with 18 additions and 17 deletions.
diff --git 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()