-
Notifications
You must be signed in to change notification settings - Fork 96
/
Math.py
192 lines (151 loc) · 5.61 KB
/
Math.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
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
import numpy as np
# using this math: https://en.wikipedia.org/wiki/Rotation_matrix
def rotation_matrix(yaw, pitch=0, roll=0):
tx = roll
ty = yaw
tz = pitch
Rx = np.array([[1,0,0], [0, np.cos(tx), -np.sin(tx)], [0, np.sin(tx), np.cos(tx)]])
Ry = np.array([[np.cos(ty), 0, np.sin(ty)], [0, 1, 0], [-np.sin(ty), 0, np.cos(ty)]])
Rz = np.array([[np.cos(tz), -np.sin(tz), 0], [np.sin(tz), np.cos(tz), 0], [0,0,1]])
return Ry.reshape([3,3])
# return np.dot(np.dot(Rz,Ry), Rx)
# option to rotate and shift (for label info)
def create_corners(dimension, location=None, R=None):
dx = dimension[2] / 2
dy = dimension[0] / 2
dz = dimension[1] / 2
x_corners = []
y_corners = []
z_corners = []
for i in [1, -1]:
for j in [1,-1]:
for k in [1,-1]:
x_corners.append(dx*i)
y_corners.append(dy*j)
z_corners.append(dz*k)
corners = [x_corners, y_corners, z_corners]
# rotate if R is passed in
if R is not None:
corners = np.dot(R, corners)
# shift if location is passed in
if location is not None:
for i,loc in enumerate(location):
corners[i,:] = corners[i,:] + loc
final_corners = []
for i in range(8):
final_corners.append([corners[0][i], corners[1][i], corners[2][i]])
return final_corners
# this is based on the paper. Math!
# calib is a 3x4 matrix, box_2d is [(xmin, ymin), (xmax, ymax)]
# Math help: http://ywpkwon.github.io/pdf/bbox3d-study.pdf
def calc_location(dimension, proj_matrix, box_2d, alpha, theta_ray):
#global orientation
orient = alpha + theta_ray
R = rotation_matrix(orient)
# format 2d corners
xmin = box_2d[0][0]
ymin = box_2d[0][1]
xmax = box_2d[1][0]
ymax = box_2d[1][1]
# left top right bottom
box_corners = [xmin, ymin, xmax, ymax]
# get the point constraints
constraints = []
left_constraints = []
right_constraints = []
top_constraints = []
bottom_constraints = []
# using a different coord system
dx = dimension[2] / 2
dy = dimension[0] / 2
dz = dimension[1] / 2
# below is very much based on trial and error
# based on the relative angle, a different configuration occurs
# negative is back of car, positive is front
left_mult = 1
right_mult = -1
# about straight on but opposite way
if alpha < np.deg2rad(92) and alpha > np.deg2rad(88):
left_mult = 1
right_mult = 1
# about straight on and same way
elif alpha < np.deg2rad(-88) and alpha > np.deg2rad(-92):
left_mult = -1
right_mult = -1
# this works but doesnt make much sense
elif alpha < np.deg2rad(90) and alpha > -np.deg2rad(90):
left_mult = -1
right_mult = 1
# if the car is facing the oppositeway, switch left and right
switch_mult = -1
if alpha > 0:
switch_mult = 1
# left and right could either be the front of the car ot the back of the car
# careful to use left and right based on image, no of actual car's left and right
for i in (-1,1):
left_constraints.append([left_mult * dx, i*dy, -switch_mult * dz])
for i in (-1,1):
right_constraints.append([right_mult * dx, i*dy, switch_mult * dz])
# top and bottom are easy, just the top and bottom of car
for i in (-1,1):
for j in (-1,1):
top_constraints.append([i*dx, -dy, j*dz])
for i in (-1,1):
for j in (-1,1):
bottom_constraints.append([i*dx, dy, j*dz])
# now, 64 combinations
for left in left_constraints:
for top in top_constraints:
for right in right_constraints:
for bottom in bottom_constraints:
constraints.append([left, top, right, bottom])
# filter out the ones with repeats
constraints = filter(lambda x: len(x) == len(set(tuple(i) for i in x)), constraints)
# create pre M (the term with I and the R*X)
pre_M = np.zeros([4,4])
# 1's down diagonal
for i in range(0,4):
pre_M[i][i] = 1
best_loc = None
best_error = [1e09]
best_X = None
# loop through each possible constraint, hold on to the best guess
# constraint will be 64 sets of 4 corners
count = 0
for constraint in constraints:
# each corner
Xa = constraint[0]
Xb = constraint[1]
Xc = constraint[2]
Xd = constraint[3]
X_array = [Xa, Xb, Xc, Xd]
# M: all 1's down diagonal, and upper 3x1 is Rotation_matrix * [x, y, z]
Ma = np.copy(pre_M)
Mb = np.copy(pre_M)
Mc = np.copy(pre_M)
Md = np.copy(pre_M)
M_array = [Ma, Mb, Mc, Md]
# create A, b
A = np.zeros([4,3], dtype=np.float)
b = np.zeros([4,1])
indicies = [0,1,0,1]
for row, index in enumerate(indicies):
X = X_array[row]
M = M_array[row]
# create M for corner Xx
RX = np.dot(R, X)
M[:3,3] = RX.reshape(3)
M = np.dot(proj_matrix, M)
A[row, :] = M[index,:3] - box_corners[row] * M[2,:3]
b[row] = box_corners[row] * M[2,3] - M[index,3]
# solve here with least squares, since over fit will get some error
loc, error, rank, s = np.linalg.lstsq(A, b, rcond=None)
# found a better estimation
if error < best_error:
count += 1 # for debugging
best_loc = loc
best_error = error
best_X = X_array
# return best_loc, [left_constraints, right_constraints] # for debugging
best_loc = [best_loc[0][0], best_loc[1][0], best_loc[2][0]]
return best_loc, best_X