fixed Y calculation #939

lardemua · May 13, 2024 · 04cd41a · 04cd41a
1 parent 4efce55
commit 04cd41a
Showing 1 changed file with 5 additions and 2 deletions.
diff --git a/atom_evaluation/scripts/other_calibrations/ali_eye_in_hand.py b/atom_evaluation/scripts/other_calibrations/ali_eye_in_hand.py
@@ -101,11 +101,14 @@ def li_calib(AA,BB):
         X = u @ np.diag([1,1,-1]) @ v.T
     X = np.append(X, x[18:21], axis=1)
     X = np.append(X, np.array([[0,0,0,1]]), axis=0)
-    print(X)
 
     # Get Y
-    Y = x[9:18].reshape((3,3)).T
+    Y = x[9:18].reshape((3,3))
     [u,s,v] = np.linalg.svd(Y)
+    s = np.array([[s[0], 0, 0],
+                  [0, s[1], 0],
+                  [0, 0, s[2]]])
+    v = v.T
     Y = u @ v.T
     if (np.linalg.det(Y) < 0):
         Y = u @ np.diag([1,1,-1]) @ v.T