-
Notifications
You must be signed in to change notification settings - Fork 2
/
utils.py
132 lines (112 loc) · 3.7 KB
/
utils.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
"""
Check_solutions is copied from [calculate_homography](https://github.com/kevin-george/cv_tools/blob/master/calculate_homography.py)
The reference can be found at [](http://igt.ip.uca.fr/~ab/Classes/DIKU-3DCV2/Handouts/Lecture16.pdf)
"""
import numpy as np
import cv2
def check_solutions(fp, sp, K, R1, R2, t):
"""Using cv2.triangulatePoints to calculate position
in real-world co-ordinate system. If z value for both
points are positive, then that is our solution.
More details can be found in the paper,
David Nister, An efficient solution to the five-point relative pose problem.
TODO: Need to take care of points that are far away
Args:
fp: Set of matched points from first image
sp: Set of matched points from second image
K: Calibration matrix
R1: First rotation matrix
R2: Second rotation matrix
t: Translation vector
Returns:
R: Correct rotation matrix
"""
dist = 50.0
P0 = np.eye(3,4)
P1 = np.dot(K, np.concatenate((R1, t.reshape(3,1)), axis=1))
Q = cv2.triangulatePoints(P0, P1, fp.T, sp.T)
mask1 = (Q[2, :] * Q[3, :]) > 0
Q[0, :] /= Q[3, :]
Q[1, :] /= Q[3, :]
Q[2, :] /= Q[3, :]
Q[3, :] /= Q[3, :]
mask1 &= (Q[2,:] < dist)
Q = np.dot(P1, Q)
mask1 &= (Q[2,:] > 0)
mask1 &= (Q[2,:] < dist)
P2 = np.dot(K, np.concatenate((R2, t.reshape(3, 1)), axis=1))
Q = cv2.triangulatePoints(P0, P2, fp.T, sp.T)
mask2 = (Q[2, :] * Q[3, :]) > 0
Q[0, :] /= Q[3, :]
Q[1, :] /= Q[3, :]
Q[2, :] /= Q[3, :]
Q[3, :] /= Q[3, :]
mask2 &= (Q[2, :] < dist)
Q = np.dot(P1, Q)
mask2 &= (Q[2, :] > 0)
mask2 &= (Q[2, :] < dist)
P3 = np.dot(K, np.concatenate((R1, -t.reshape(3, 1)), axis=1))
Q = cv2.triangulatePoints(P0, P3, fp.T, sp.T)
mask3 = (Q[2, :] * Q[3, :]) > 0
Q[0, :] /= Q[3, :]
Q[1, :] /= Q[3, :]
Q[2, :] /= Q[3, :]
Q[3, :] /= Q[3, :]
mask3 &= (Q[2, :] < dist)
Q = np.dot(P1, Q)
mask3 &= (Q[2, :] > 0)
mask3 &= (Q[2, :] < dist)
P4 = np.dot(K, np.concatenate((R2, -t.reshape(3, 1)), axis=1))
Q = cv2.triangulatePoints(P0, P4, fp.T, sp.T)
mask4 = (Q[2, :] * Q[3, :]) > 0
Q[0, :] /= Q[3, :]
Q[1, :] /= Q[3, :]
Q[2, :] /= Q[3, :]
Q[3, :] /= Q[3, :]
mask4 &= (Q[2, :] < dist)
Q = np.dot(P1, Q)
mask4 &= (Q[2, :] > 0)
mask4 &= (Q[2, :] < dist)
good1 = np.count_nonzero(mask1)
good2 = np.count_nonzero(mask2)
good3 = np.count_nonzero(mask3)
good4 = np.count_nonzero(mask4)
print("The four good:{},{},{},{}".format(good1, good2, good3, good4))
max_count = max(good1, good2, good3, good4)
if max_count == good1:
return R1, t
elif max_count == good2:
return R2, t
elif max_count == good3:
return R1, -t
elif max_count == good4:
return R2, -t
return None
def rotate_angle_no_output(R):
"""
http://www.cnblogs.com/singlex/p/RotateMatrix2Euler.html
:param R:
:return: thetaz, thetay, thetax
"""
r11 = R[0][0]
r21 = R[1][0]
r31 = R[2][0]
r32 = R[2][1]
r33 = R[2][2]
from math import pi, atan2, sqrt
z = atan2(r21, r11)/pi*180
y = atan2(-r31, sqrt(r32*r32 + r33*r33))/pi*180
x = atan2(r32, r33)/pi*180
return np.array([z, y, x]).reshape(3,1) # to make it the same as t
def rotate_angle(R):
z, y, x = rotate_angle_no_output(R)
print("rotate_angle:z:{},y:{},x:{}".format(z, y, x))
def cv2plt(im):
dim = len(im.shape)
assert dim >= 2
if dim == 2: # Gray
return cv2.cvtColor(im, cv2.COLOR_GRAY2RGB)
elif dim == 3: # BGR
return cv2.cvtColor(im, cv2.COLOR_BGR2RGB)
elif dim == 4: # BGRA
return cv2.cvtColor(im, cv2.COLOR_BGRA2RGBA)