1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
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)
|
