-
Notifications
You must be signed in to change notification settings - Fork 0
/
euler_quat_transform.py
64 lines (56 loc) · 1.6 KB
/
euler_quat_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
import numpy as np
from tf.transformations import euler_from_quaternion, quaternion_from_euler,unit_vector
from math import ceil
def quat_to_euler_point(data):
'''
data: a point
'''
quat = data[3:7]
pos = data[0:3]
euler = euler_from_quaternion(quat)
stack = np.hstack((pos,euler))
return stack
def quat_to_euler_list(data):
'''
data: numpy matrix
'''
data = filter_static_points(data)
quat_list = data[:,3:7]
p_list = data[:,0:3]
euler_list = []
for rows in quat_list:
#rows = [ round(rows, 4) for elem in rows]
rows = unit_vector(rows)
tmp = euler_from_quaternion(rows)
euler_list.append(tmp)
stack = np.hstack((p_list,euler_list))
return stack
def euler_to_quat_list(data):
euler_list = data[:,3:6]
p_list = data[:,0:3]
quat_list = []
for rows in euler_list:
tmp = quaternion_from_euler(rows[0],rows[1],rows[2])
# tmp = [ round(elem, 4) for elem in tmp ]
tmp = unit_vector(tmp)
quat_list.append(tmp)
stack = np.hstack((p_list,quat_list))
return stack
def filter_static_points(mat):
last = mat[0]
new_mat = [last]
for idx in range(mat.shape[0]):
if np.linalg.norm(mat[idx]-last) < 0.01:
pass
else:
new_mat.append(mat[idx])
last = mat[idx]
return np.array(new_mat)
def main():
data =np.load("home_to_pre_pick.npy")
euler_data = quat_to_euler(data)
# print euler_data
quat_data = euler_to_quat(euler_data)
print quat_data
if __name__ == "__main__":
main()