-
Notifications
You must be signed in to change notification settings - Fork 0
/
transform.py
113 lines (92 loc) · 2.65 KB
/
transform.py
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
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
import numpy as np
import math
def r_x(theta):
"""
Rotate about X axis
:param theta: angle in radians
:return: rotation (radians) about X axis
"""
ct = math.cos(theta)
st = math.sin(theta)
rot_x = np.array([[1., 0., 0.],
[0, ct, st],
[0, -st, ct]])
return rot_x
def r_y(theta):
"""
Rotate about Y axis
:param theta: angle in radians
:return: rotation (radians) about Y axis
"""
ct = math.cos(theta)
st = math.sin(theta)
rot_y = np.array([[ct, 0, -st],
[0., 1., 0.],
[st, 0, ct]])
return rot_y
def r_z(theta):
"""
Rotate about Z axis
:param theta: angle in radians
:return: rotation (radians) about Z axis
"""
ct = math.cos(theta)
st = math.sin(theta)
rot_z = np.array([[ct, st, 0],
[-st, ct, 0.],
[0., 0., 1.]])
return rot_z
def euler_to_rot(r):
"""
rotation matrix based on euler angles
R_xyz = R_z(GAMMA) * R_y(BETA) * R_x(ALPHA)
:param r: array containing rotation about each axis
:return: rotation matrix corresponding to rotation angles
"""
rx = r_x(r[0]) # R_x(alpha)
ry = r_y(r[1]) # R_y(beta)
rz = r_z(r[2]) # R_z(gamma)
temp = rz.dot(ry)
return temp.dot(rx)
def quat_to_rot(q):
"""
rotation matrix based on quaternions
:param q: array containing quaternions
:return: rotation matrix corresponding to quaternion values
"""
# (w, x, y, z) = q
row_1 = np.array([1-2*(q[2]**2 + q[3]**2) , 2*(q[1]*q[2] - q[0]*q[3]) , 2*(q[0]*q[2] + q[1]*q[3])])
row_2 = np.array([2*(q[1]*q[2]+q[0]*q[3]) , 1-2*(q[1]**2 + q[3]**2) , 2*(q[2]*q[3] - q[0]*q[1])])
row_3 = np.array([2*(q[1]*q[3] - q[0]*q[2]) , 2*(q[0]*q[1] + q[2]*q[3]) , 1-2*(q[1]**2 + q[2]**2)])
rotation = np.vstack((row_1, row_2, row_3))
return rotation
def rot_to_euler(rot):
"""
Extract euler angles from a rotation matrix
:param rot: rotation matrix
:return: array containing rotation angles
"""
euler = np.zeros(3)
r00 = rot[0, 0]
r01 = rot[0, 1]
r02 = rot[0, 2]
r10 = rot[1, 0]
r11 = rot[1, 1]
r12 = rot[1, 2]
r20 = rot[2, 0]
r21 = rot[2, 1]
r22 = rot[2, 2]
beta = math.atan2(r20, math.sqrt(r21**2 + r22**2))
cb = math.cos(beta)
# if cb < finfo(float).eps:
# euler[0] = 10
# euler[1] = 10
# euler[2] = 10
# return euler
# else:
gamma = math.atan2(r10/-cb, r00/cb)
alpha = math.atan2(r21/-cb, r22/cb)
euler[0] = alpha
euler[1] = beta
euler[2] = gamma
return euler