-
Notifications
You must be signed in to change notification settings - Fork 0
/
horn.py
88 lines (76 loc) · 2.69 KB
/
horn.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
import numpy as np
import transformations as tf
import kin
def absoluteOrientation(A, B):
centroid_a = np.mean(A, axis=0)
centroid_b = np.mean(B, axis=0)
A_wrt_centroid = A - centroid_a
B_wrt_centroid = B - centroid_b
M = 0
for a_, b_ in zip(A_wrt_centroid, B_wrt_centroid):
M_ = np.dot(np.expand_dims(a_, axis=1), np.expand_dims(b_, axis=0))
M = M + M_
Sxx = M[0, 0]
Syx = M[1, 0]
Szx = M[2, 0]
Sxy = M[0, 1]
Syy = M[1, 1]
Szy = M[2, 1]
Sxz = M[0, 2]
Syz = M[1, 2]
Szz = M[2, 2]
N = np.array([[Sxx + Syy + Szz, Syz - Szy, Szx - Sxz, Sxy - Syx],
[Syz - Szy, Sxx - Syy - Szz, Sxy + Syx, Szx + Sxz],
[Szx - Sxz, Sxy + Syx, -Sxx + Syy - Szz, Syz + Szy],
[Sxy - Syx, Szx + Sxz, Syz + Szy, -Sxx - Syy + Szz]])
eig_val, eig_vec = np.linalg.eig(N)
q = eig_vec[:, np.argmax(eig_val)]
# print('eig_val', eig_val)
# print('max eig val', max(eig_val))
# print('eig vec', eig_vec)
# print('eig vec corresponding to max eig val', eig_vec[:, np.argmax(eig_val)])
# print('norm ', np.linalg.norm(q))
# print('q', q)
qw = q[0]
qx = q[1]
qy = q[2]
qz = q[3]
rot1 = np.array([[1 - 2 * qy ** 2 - 2 * qz ** 2, 2 * qx * qy - 2 * qz * qw, 2 * qx * qz + 2 * qy * qw],
[2 * qx * qy + 2 * qz * qw, 1 - 2 * qx ** 2 - 2 * qz ** 2, 2 * qy * qz - 2 * qx * qw],
[2 * qx * qz - 2 * qy * qw, 2 * qy * qz + 2 * qx * qw, 1 - 2 * qx ** 2 - 2 * qy ** 2]])
rot2 = tf.quaternion_matrix(q)
rot2 = kin.rotationFromHmgMatrix(rot2)
v = q[1:4]
v_out = np.outer(v, v)
Z = np.array([[qw, -qz, qy],
[qz, qw, -qx],
[-qy, qx, qw]])
rot3 = v_out + np.dot(Z, Z)
rot = rot1
t = centroid_b - np.dot(rot, centroid_a)
return rot, t
if __name__ == '__main__':
A = kin.generatePointSet(4, 3, 0, 10)
T_ab = kin.homogeneousMatrix(0, 50, 30, 0, 90, 0, True)
# A = np.array([[2, 6, 6, 0, 8],
# [5, 9, 2, 6, 9],
# [9, 8, 7, 7, 10]])
# A = A.transpose()
# T_ab = np.array([[0, -1, 0, 10],
# [1, 0, 0, 20],
# [0, 0, 1, 30],
# [0, 0, 0, 1]])
B = kin.transform(A, T_ab)
# print('A', A)
# f_A = np.flip(A, 0)
# print('T_ab', T_ab)
# print('B', B)
# f_B = np.flip(B, 0)
# print('flip B', f_B)
R, t = absoluteOrientation(A, B)
R_original = kin.rotationFromHmgMatrix(T_ab)
t_original = kin.translationFromHmgMatrix(T_ab)
e_ori = R_original - R
e_pos = t_original - t
print('e_ori', np.linalg.norm(e_ori))
print('e_pos', np.linalg.norm(e_pos))