/
SFM.py
126 lines (96 loc) · 4.18 KB
/
SFM.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
import numpy as np
from math import sqrt
from dataclasses import dataclass
@dataclass
class Line:
m: float
n: float
def calc_TFL_dist(prev_container, curr_container, focal, pp):
norm_prev_pts, norm_curr_pts, R, foe, tZ = prepare_3D_data(prev_container, curr_container, focal, pp)
if abs(tZ) < 10e-6:
print('tz = ', tZ)
elif norm_prev_pts.size == 0:
print('no prev points')
elif norm_prev_pts.size == 0:
print('no curr points')
else:
curr_container.corresponding_ind, curr_container.traffic_lights_3d_location, curr_container.valid = calc_3D_data(
norm_prev_pts, norm_curr_pts, R, foe, tZ)
return curr_container
def prepare_3D_data(prev_container, curr_container, focal, pp):
norm_prev_pts = normalize(prev_container.traffic_light, focal, pp)
norm_curr_pts = normalize(curr_container.traffic_light, focal, pp)
R, foe, tZ = decompose((curr_container))
return norm_prev_pts, norm_curr_pts, R, foe, tZ
def calc_3D_data(norm_prev_pts, norm_curr_pts, R, foe, tZ):
norm_rot_pts = rotate(norm_prev_pts, R)
pts_3D = []
corresponding_ind = []
validVec = []
for p_curr in norm_curr_pts:
corresponding_p_ind, corresponding_p_rot = find_corresponding_points(p_curr, norm_rot_pts, foe)
Z = calc_dist(p_curr, corresponding_p_rot, foe, tZ)
valid = (Z > 0)
if not valid:
Z = 0
validVec.append(valid)
P = Z * np.array([p_curr[0], p_curr[1], 1])
pts_3D.append((P[0], P[1], P[2]))
corresponding_ind.append(corresponding_p_ind)
return corresponding_ind, np.array(pts_3D), validVec
def normalize(pts, focal, pp):
# transform pixels into normalized pixels using the focal length and principle point
norm_pts = []
for tfl in pts:
norm_pts.append([(tfl[0] - pp[0]) / focal, (tfl[1] - pp[1]) / focal])
return np.array(norm_pts)
def unnormalize(pts, focal, pp):
unnorm_pts = []
for tfl in pts:
unnorm_pts.append([(tfl[0] * focal) + pp[0], (tfl[1] * focal) + pp[1]])
return np.array(unnorm_pts)
# transform normalized pixels into pixels using the focal length and principle point
def decompose(EM):
# extract R, foe and tZ from the Ego Motion
R = EM.EM[:3, :3]
t = EM.EM[:, 3]
foe = [t[0] / t[2], t[1] / t[2]]
return R, foe, t[2]
def rotate(pts, R):
norm_rotate = []
for tfl in pts:
p_vec = np.array([tfl[0], tfl[1], 1])
result = R.dot(p_vec)
norm_rotate.append([result[0] / result[2], result[1] / result[2]])
return np.array(norm_rotate)
# rotate the points - pts using R
def distance_point_to_line(point, line: Line):
return abs((line.m * point[0] + line.n - point[1]) / sqrt(line.m ** 2 + 1))
def find_corresponding_points(p, norm_pts_rot, foe):
m = (foe[1] - p[1]) / (foe[0] - p[0])
n = (p[1] * foe[0] - foe[1] * p[0]) / (foe[0] - p[0])
distances = [distance_point_to_line(point, Line(m, n))
for point in norm_pts_rot]
return distances.index(min(distances)), norm_pts_rot[distances.index(min(distances))]
# compute the epipolar line between p and foe
# run over all norm_pts_rot and find the one closest to the epipolar line
# return the closest point and its index
def calc_dist(p_curr, p_rot, foe, tZ):
# calculate the distance of p_curr using x_curr, x_rot, foe_x and tZ
# calculate the distance of p_curr using y_curr, y_rot, foe_y and tZ
# combine the two estimations and return estimated Z
return calc_z(foe, p_curr, p_rot, tZ, choose_index(p_curr, p_rot))
def choose_index(p_curr, p_rot):
if p_curr[0] - p_rot[0] > p_curr[1] - p_rot[1]:
z_index = 0
else:
z_index = 1
return z_index
def calc_z(foe, p_curr, p_rot, tZ, z_index):
return (tZ * (foe[z_index] - p_rot[z_index])) / (p_curr[z_index] - p_rot[z_index])
def get_foe_rotate(prev_container, curr_container, focal, pp):
norm_prev_pts, norm_curr_pts, R, norm_foe, tZ = prepare_3D_data(prev_container, curr_container, focal, pp)
norm_rot_pts = rotate(norm_prev_pts, R)
rot_pts = unnormalize(norm_rot_pts, focal, pp)
foe = np.squeeze(unnormalize(np.array([norm_foe]), focal, pp))
return foe, rot_pts