-
Notifications
You must be signed in to change notification settings - Fork 0
/
P3P.py
executable file
·133 lines (106 loc) · 4.21 KB
/
P3P.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
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
import numpy as np
import PnP
def P3P(Pc, Pw, K=np.eye(3)):
"""
Solve Perspective-3-Point problem, given correspondence and intrinsic
Input:
Pc: 4x2 numpy array of pixel coordinate of the April tag corners in (x,y) format
Pw: 4x3 numpy array of world coordinate of the April tag corners in (x,y,z) format
K: 3x3 numpy array for camera intrisic matrix (given in run_P3P.py)
Returns:
R: 3x3 numpy array describing camera orientation in the world (R_wc)
t: 3x1 numpy array describing camera translation in the world (t_wc)
"""
##### STUDENT CODE START #####
R = np.eye(3)
t = np.zeros([3])
p1 = Pw[1,:]
p2 = Pw[2,:]
p3 = Pw[3,:]
f = (K[0][0] + K[1][1]) / 2
offset = np.array([K[0][2], K[1][2]])
uv1 = Pc[1,:] - offset
uv2 = Pc[2,:] - offset
uv3 = Pc[3,:] - offset
uv1 = np.append(uv1, f)
uv2 = np.append(uv2, f)
uv3 = np.append(uv3, f)
# define a,b,c,alpha,beta,gamma
a = np.linalg.norm(p2-p3)
b = np.linalg.norm(p1-p3)
c = np.linalg.norm(p1-p2)
j1 = uv1 / np.linalg.norm(uv1)
j2 = uv2 / np.linalg.norm(uv2)
j3 = uv3 / np.linalg.norm(uv3)
calpha = np.dot(j2, j3)
cbata = np.dot(j1, j3)
cgamma = np.dot(j1, j2)
# define coefficients of the 4th degree polynomial
co1 = (np.power(a, 2) - np.power(c, 2)) / np.power(b, 2) # (a^2 - c^2) / b^2
co2 = (np.power(a, 2) + np.power(c, 2)) / np.power(b, 2) # (a^2 + c^2) / b^2
co3 = (np.power(b, 2) - np.power(c, 2)) / np.power(b, 2) # (b^2 - c^2) / b^2
co4 = (np.power(b, 2) - np.power(a, 2)) / np.power(b, 2) # (b^2 - a^2) / b^2
A4 = np.power(co1 - 1, 2) - 4 * np.power(c / b, 2) * np.power(calpha, 2)
A3 = 4 * (co1 * (1 - co1) * cbata - (1 - co2) * calpha * cgamma + 2 * np.power(c / b, 2) * np.power(calpha, 2) * cbata)
A2 = 2 * (np.power(co1, 2) - 1 + 2 * np.power(co1, 2) * np.power(cbata, 2) + 2 * co3 * np.power(calpha, 2) - 4 * co2 * calpha * cbata * cgamma + 2 * co4 * np.power(cgamma, 2))
A1 = 4 * (-co1 * (1 + co1) * cbata + 2 * np.power(a / b, 2) * np.power(cgamma, 2) * cbata - (1 - co2) * calpha * cgamma)
A0 = np.power((1 + co1), 2) - 4 * np.power(a / b, 2) * np.power(cgamma, 2)
# calculate real roots u and v
coeff = np.array([A4, A3, A2, A1, A0])
v = np.roots(coeff)
real = []
for number in v:
if np.isreal(number):
real.append(number.real)
v = np.array(real)
u = [0, 0]
u[0] = ((-1 + co1) * np.power(v[0], 2) - 2 * co1 * cbata * v[0] + 1 + co1) / (2 * (cgamma - calpha * v[0]))
u[1] = ((-1 + co1) * np.power(v[1], 2) - 2 * co1 * cbata * v[1] + 1 + co1) / (2 * (cgamma - calpha * v[1]))
# check for valid distances
s1 = np.power(b, 2) / (1 + np.power(v, 2) - 2 * v * cbata)
s1 = np.sqrt(s1)
s2 = u * s1
s3 = v * s1
s = []
if(s1[1] > 0 and s2[1] > 0 and s3[1] > 0):
s = np.array([s1[0], s2[0], s3[0]])
elif(s1[0] > 0 and s2[0] > 0 and s3[0] > 0):
s = np.array([s1[1], s2[1], s3[1]])
else:
print("Wrong!!!!!!!!!!!!")
# calculate 3D coordinates in Camera frame
p1cal = s[0] * j1
p2cal = s[1] * j2
p3cal = s[2] * j3
Xc = np.array([p1cal, p2cal, p3cal])
# Calculate R,t using Procrustes
R, t = Procrustes(np.array(Xc), Pw[1:])
##### STUDENT CODE END #####
return R, t
def Procrustes(X, Y):
"""
Solve Procrustes: Y = RX + t
Input:
X: Nx3 numpy array of N points in camera coordinate (returned by your P3P)
Y: Nx3 numpy array of N points in world coordinate
Returns:
R: 3x3 numpy array describing camera orientation in the world (R_wc)
t: 1x3 numpy array describing camera translation in the world (t_wc)
"""
##### STUDENT CODE START #####
R = np.eye(3)
t = np.zeros([3])
Xhat = np.mean(X, axis = 0)
Yhat = np.mean(Y, axis = 0)
X = (X - Xhat).T
Y = (Y - Yhat).T
U, S, Vh = np.linalg.svd(X @ Y.T)
V = np.transpose(Vh)
S = np.eye(3)
print(V @ np.transpose(U))
S[2][2] = np.linalg.det(V @ np.transpose(U))
R = V @ S @ np.transpose(U)
print(np.linalg.det(R))
t = Yhat - R @ Xhat
##### STUDENT CODE END #####
return R, t