-
Notifications
You must be signed in to change notification settings - Fork 36
/
correspondece_constraint.py
177 lines (141 loc) · 5.48 KB
/
correspondece_constraint.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
import numpy as np
from data_processing import KITTI_dataloader
def recover_angle(bin_anchor, bin_confidence, bin_num):
# select anchor from bins
max_anc = np.argmax(bin_confidence)
anchors = bin_anchor[max_anc]
# compute the angle offset
if anchors[1] > 0:
angle_offset = np.arccos(anchors[0])
else:
angle_offset = -np.arccos(anchors[0])
# add the angle offset to the center ray of each bin to obtain the local orientation
wedge = 2 * np.pi / bin_num
angle = angle_offset + max_anc * wedge
# angle - 2pi, if exceed 2pi
angle_l = angle % (2 * np.pi)
# change to ray back to [-pi, pi]
angle = angle_l - np.pi / 2
if angle > np.pi:
angle -= 2 * np.pi
angle = round(angle, 2)
return angle
def compute_orientaion(P2, obj):
x = (obj.xmax + obj.xmin) / 2
# compute camera orientation
u_distance = x - P2[0, 2]
focal_length = P2[0, 0]
rot_ray = np.arctan(u_distance / focal_length)
# global = alpha + ray
rot_global = obj.alpha + rot_ray
# local orientation, [0, 2 * pi]
# rot_local = obj.alpha + np.pi / 2
rot_local = KITTI_dataloader.get_new_alpha(obj.alpha)
rot_global = round(rot_global, 2)
return rot_global, rot_local
def translation_constraints(P2, obj, rot_local):
bbox = [obj.xmin, obj.ymin, obj.xmax, obj.ymax]
# rotation matrix
R = np.array([[ np.cos(obj.rot_global), 0, np.sin(obj.rot_global)],
[ 0, 1, 0 ],
[-np.sin(obj.rot_global), 0, np.cos(obj.rot_global)]])
A = np.zeros((4, 3))
b = np.zeros((4, 1))
I = np.identity(3)
xmin_candi, xmax_candi, ymin_candi, ymax_candi = obj.box3d_candidate(rot_local, soft_range=8)
X = np.bmat([xmin_candi, xmax_candi,
ymin_candi, ymax_candi])
# X: [x, y, z] in object coordinate
X = X.reshape(4,3).T
# construct equation (4, 3)
for i in range(4):
matrice = np.bmat([[I, np.matmul(R, X[:,i])], [np.zeros((1,3)), np.ones((1,1))]])
M = np.matmul(P2, matrice)
if i % 2 == 0:
A[i, :] = M[0, 0:3] - bbox[i] * M[2, 0:3]
b[i, :] = M[2, 3] * bbox[i] - M[0, 3]
else:
A[i, :] = M[1, 0:3] - bbox[i] * M[2, 0:3]
b[i, :] = M[2, 3] * bbox[i] - M[1, 3]
# solve x, y, z, using method of least square
Tran = np.matmul(np.linalg.pinv(A), b)
tx, ty, tz = [float(np.around(tran, 2)) for tran in Tran]
return tx, ty, tz
class detectionInfo(object):
def __init__(self, line):
self.name = line[0]
self.truncation = float(line[1])
self.occlusion = int(line[2])
# local orientation = alpha + pi/2
self.alpha = float(line[3])
# in pixel coordinate
self.xmin = float(line[4])
self.ymin = float(line[5])
self.xmax = float(line[6])
self.ymax = float(line[7])
# height, weigh, length in object coordinate, meter
self.h = float(line[8])
self.w = float(line[9])
self.l = float(line[10])
# x, y, z in camera coordinate, meter
self.tx = float(line[11])
self.ty = float(line[12])
self.tz = float(line[13])
# global orientation [-pi, pi]
self.rot_global = float(line[14])
def member_to_list(self):
output_line = []
for name, value in vars(self).items():
output_line.append(value)
return output_line
def box3d_candidate(self, rot_local, soft_range):
x_corners = [self.l, self.l, self.l, self.l, 0, 0, 0, 0]
y_corners = [self.h, 0, self.h, 0, self.h, 0, self.h, 0]
z_corners = [0, 0, self.w, self.w, self.w, self.w, 0, 0]
x_corners = [i - self.l / 2 for i in x_corners]
y_corners = [i - self.h for i in y_corners]
z_corners = [i - self.w / 2 for i in z_corners]
corners_3d = np.transpose(np.array([x_corners, y_corners, z_corners]))
point1 = corners_3d[0, :]
point2 = corners_3d[1, :]
point3 = corners_3d[2, :]
point4 = corners_3d[3, :]
point5 = corners_3d[6, :]
point6 = corners_3d[7, :]
point7 = corners_3d[4, :]
point8 = corners_3d[5, :]
# set up projection relation based on local orientation
xmin_candi = xmax_candi = ymin_candi = ymax_candi = 0
if 0 < rot_local < np.pi / 2:
xmin_candi = point8
xmax_candi = point2
ymin_candi = point2
ymax_candi = point5
if np.pi / 2 <= rot_local <= np.pi:
xmin_candi = point6
xmax_candi = point4
ymin_candi = point4
ymax_candi = point1
if np.pi < rot_local <= 3 / 2 * np.pi:
xmin_candi = point2
xmax_candi = point8
ymin_candi = point8
ymax_candi = point1
if 3 * np.pi / 2 <= rot_local <= 2 * np.pi:
xmin_candi = point4
xmax_candi = point6
ymin_candi = point6
ymax_candi = point5
# soft constraint
div = soft_range * np.pi / 180
if 0 < rot_local < div or 2*np.pi-div < rot_local < 2*np.pi:
xmin_candi = point8
xmax_candi = point6
ymin_candi = point6
ymax_candi = point5
if np.pi - div < rot_local < np.pi + div:
xmin_candi = point2
xmax_candi = point4
ymin_candi = point8
ymax_candi = point1
return xmin_candi, xmax_candi, ymin_candi, ymax_candi