Perturb

borglab · Oct 28, 2024 · dd6be3b · dd6be3b
1 parent f00477d
commit dd6be3b
Showing 1 changed file with 11 additions and 5 deletions.
diff --git a/python/gtsam/examples/ViewGraphComparison.py b/python/gtsam/examples/ViewGraphComparison.py
@@ -109,7 +109,7 @@ def build_factor_graph(method, num_cameras, measurements):
     return graph
 
 
-def get_initial_estimate(method, num_cameras, ground_truth, cal):
+def get_initial_estimate(method, num_cameras, ground_truth, cal, std=1e-6):
     """get initial estimate for method"""
     initialEstimate = Values()
     total_dimension = 0
@@ -119,16 +119,22 @@ def get_initial_estimate(method, num_cameras, ground_truth, cal):
         for a in range(num_cameras):
             b = (a + 1) % num_cameras  # Next camera
             c = (a + 2) % num_cameras  # Camera after next
-            initialEstimate.insert(EdgeKey(a, b).key(), F1)
-            initialEstimate.insert(EdgeKey(a, c).key(), F2)
+            # Retract with delta drawn from zero-mean normal
+            F1_perturbed = F1.retract(np.random.normal(0, std, F1.dim()))
+            F2_perturbed = F2.retract(np.random.normal(0, 1e-4, F2.dim()))
+            initialEstimate.insert(EdgeKey(a, b).key(), F1_perturbed)
+            initialEstimate.insert(EdgeKey(a, c).key(), F2_perturbed)
             total_dimension += F1.dim() + F2.dim()
     elif method == "EssentialMatrix":
         E1, E2 = ground_truth
         for a in range(num_cameras):
             b = (a + 1) % num_cameras  # Next camera
             c = (a + 2) % num_cameras  # Camera after next
-            initialEstimate.insert(EdgeKey(a, b).key(), E1)
-            initialEstimate.insert(EdgeKey(a, c).key(), E2)
+            # Retract with delta drawn from zero-mean normal
+            E1_perturbed = E1.retract(np.random.normal(0, std, E1.dim()))
+            E2_perturbed = E2.retract(np.random.normal(0, 1e-4, E2.dim()))
+            initialEstimate.insert(EdgeKey(a, b).key(), E1_perturbed)
+            initialEstimate.insert(EdgeKey(a, c).key(), E2_perturbed)
             total_dimension += E1.dim() + E2.dim()
         # Insert initial calibrations
         for i in range(num_cameras):