From 394e19b012cb9264feaec582948fa7ac8bff901c Mon Sep 17 00:00:00 2001 From: Chuyan Zhang Date: Mon, 15 Jan 2024 15:49:41 -0800 Subject: init commit --- utils.py | 36 ++++++++++++++++++++++++++++++++++++ 1 file changed, 36 insertions(+) create mode 100644 utils.py (limited to 'utils.py') 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 -- cgit v1.2.3-70-g09d2