init commitHEADmaster

1 files changed, 36 insertions, 0 deletions

diff --git a/utils.py b/utils.py
new file mode 100644
index 0000000..95a1c31
--- /dev/null
+++ b/utils.py
@@ -0,0 +1,36 @@
+import numpy as np
+
+def quat2rot(q):
+    r, x, y, z = q[0], q[1], q[2], q[3]
+    return np.array([
+        [1. - 2. * (y * y + z * z), 2. * (x * y - r * z), 2. * (x * z + r * y)],
+		[2. * (x * y + r * z), 1. - 2. * (x * x + z * z), 2. * (y * z - r * x)],
+		[2. * (x * z - r * y), 2. * (y * z + r * x), 1. - 2. * (x * x + y * y)]
+    ], dtype=np.float32)
+
+def rot2quat(r):
+    if r[2][2] < 0:
+        if r[0][0] > r[1][1]:
+            t = 1 + r[0][0] - r[1][1] - r[2][2]
+            q = np.array([t, r[0][1] + r[1][0], r[0][2] + r[2][0], r[1][2] - r[2][1]])
+        else:
+            t = 1 - r[0][0] + r[1][1] - r[2][2]
+            q = np.array([r[0][1] + r[1][0], t, r[1][2] + r[2][1], r[2][0] - r[0][2]])
+    else:
+        if r[0][0] < -r[1][1]:
+            t = 1 - r[0][0] - r[1][1] + r[2][2]
+            q = np.array([r[2][0] + r[0][2], r[1][2] + r[2][1], t, r[0][1] - r[1][0]])
+        else:
+            t = 1 + r[0][0] + r[1][1] + r[2][2]
+            q = np.array([r[1][2] - r[2][1], r[2][0] + r[0][2], r[0][1] - r[1][0], t])
+    return q * 0.5 / np.sqrt(t)
+
+def sq2cov(s, q):
+    S = np.diag(s)
+    R = quat2rot(q)
+    M = S @ R
+    return M.T @ M
+
+def mat_log(m):
+    eigenvalues, eigenvectors = np.linalg.eig(m)
+    return eigenvectors @ np.diag(np.log(eigenvalues)) @ np.linalg.inv(eigenvectors)
\ No newline at end of file