forked from commaai/speedchallenge
-
Notifications
You must be signed in to change notification settings - Fork 1
/
test_warp.py
116 lines (97 loc) · 3.44 KB
/
test_warp.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
import cv2
import numpy as np
f = 910
rotXval = 90
rotYval = 90
rotZval = 90
distXval = 500
distYval = 500
distZval = 500
def onFchange(val):
global f
f = val
def onRotXChange(val):
global rotXval
rotXval = val
def onRotYChange(val):
global rotYval
rotYval = val
def onRotZChange(val):
global rotZval
rotZval = val
def onDistXChange(val):
global distXval
distXval = val
def onDistYChange(val):
global distYval
distYval = val
def onDistZChange(val):
global distZval
distZval = val
if __name__ == '__main__':
#Read input image, and create output image
src = cv2.imread('test1.jpg')
dst = np.zeros_like(src)
h, w = src.shape[:2]
#Create user interface with trackbars that will allow to modify the parameters of the transformation
wndname1 = "Source:"
wndname2 = "WarpPerspective: "
cv2.namedWindow(wndname1, 1)
cv2.namedWindow(wndname2, 1)
cv2.createTrackbar("f", wndname2, f, 1000, onFchange)
cv2.createTrackbar("Rotation X", wndname2, rotXval, 180, onRotXChange)
cv2.createTrackbar("Rotation Y", wndname2, rotYval, 180, onRotYChange)
cv2.createTrackbar("Rotation Z", wndname2, rotZval, 180, onRotZChange)
cv2.createTrackbar("Distance X", wndname2, distXval, 1000, onDistXChange)
cv2.createTrackbar("Distance Y", wndname2, distYval, 1000, onDistYChange)
cv2.createTrackbar("Distance Z", wndname2, distZval, 1000, onDistZChange)
#Show original image
cv2.imshow(wndname1, src)
k = -1
while k != 27:
if f <= 0: f = 1
rotX = (rotXval - 90)*np.pi/180
rotY = (rotYval - 90)*np.pi/180
rotZ = (rotZval - 90)*np.pi/180
distX = distXval - 500
distY = distYval - 500
distZ = distZval - 500
# Camera intrinsic matrix
K = np.array([[f, 0, w/2, 0],
[0, f, h/2, 0],
[0, 0, 1, 0]])
# Camera intrinsic matrix
K = np.array([[f, 0, w/2, 0],
[0, f, h/2, 0],
[0, 0, 1, 0]])
# K inverse
Kinv = np.zeros((4,3))
Kinv[:3,:3] = np.linalg.inv(K[:3,:3])*f
Kinv[-1,:] = [0, 0, 1]
# Rotation matrices around the X,Y,Z axis
RX = np.array([[1, 0, 0, 0],
[0,np.cos(rotX),-np.sin(rotX), 0],
[0,np.sin(rotX),np.cos(rotX) , 0],
[0, 0, 0, 1]])
RY = np.array([[ np.cos(rotY), 0, np.sin(rotY), 0],
[ 0, 1, 0, 0],
[ -np.sin(rotY), 0, np.cos(rotY), 0],
[ 0, 0, 0, 1]])
RZ = np.array([[ np.cos(rotZ), -np.sin(rotZ), 0, 0],
[ np.sin(rotZ), np.cos(rotZ), 0, 0],
[ 0, 0, 1, 0],
[ 0, 0, 0, 1]])
# Composed rotation matrix with (RX,RY,RZ)
R = np.linalg.multi_dot([ RX , RY , RZ ])
# Translation matrix
T = np.array([[1,0,0,distX],
[0,1,0,distY],
[0,0,1,distZ],
[0,0,0,1]])
# Overall homography matrix
H = np.linalg.multi_dot([K, R, T, Kinv])
# Apply matrix transformation
cv2.warpPerspective(src, H, (w, h), dst, cv2.INTER_NEAREST, cv2.BORDER_CONSTANT, 0)
# Show the image
cv2.imshow(wndname2, dst)
k = cv2.waitKey(1)