-
Notifications
You must be signed in to change notification settings - Fork 5
/
tools.py
126 lines (92 loc) · 3.55 KB
/
tools.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 sin, cos, sqrt
def cartesian_to_polar(x, y, vx, vy, yaw, dyaw, THRESH = 0.0001):
"""
Converts 2d cartesian position and velocity coordinates to polar coordinates
Args:
x, y, vx, vy : floats - position and velocity components in cartesian respectively
THRESH : float - minimum value of rho to return non-zero values
Returns:
rho, drho : floats - radius and velocity magnitude respectively
phi : float - angle in radians
"""
rho = sqrt(x * x + y * y)
phi = np.arctan2(y, x)
if rho < THRESH:
print("WARNING: in cartesian_to_polar(): d_squared < THRESH")
rho, phi, drho = 0, 0, 0
else:
drho = (x * vx + y * vy) / rho
return rho, phi, drho
def polar_to_cartesian(rho, phi, drho):
"""
Converts 2D polar coordinates into cartesian coordinates
Args:
rho. drho : floats - radius magnitude and velocity magnitudes respectively
phi : float - angle in radians
Returns:
x, y, vx, vy : floats - position and velocity components in cartesian respectively
"""
x, y = rho * cos(phi), rho * sin(phi)
vx, vy = drho * cos(phi) , drho * sin(phi)
return x, y, vx, vy
def time_difference(t1, t2):
"""
Computes the time difference in microseconds (type: float) of two epoch times values in seconds (type: int)
Args:
t1 : int - previous epoch time in seconds
t2 : int - current epoch time in seconds
Returns: a float - the time difference in seconds
"""
return (t2 - t1) / 1000000.0
def get_RMSE(predictions, truths):
"""
Computes the root mean square errors (RMSE) of the attributes of two lists of DataPoint() instances
Args:
predictions - a list of DataPoint() instances
truths - a list of DataPoint() instances
Returns:
px, py, vx, vy - The RMSE of each respective DataPoint() attributes (type: float)
"""
pxs, pys, vxs, vys, yaws, dyaws = [], [], [], [], [], []
for p, t in zip(predictions, truths):
ppx, ppy, pvx, pvy, pyaw, pdyaw = p.get()
tpx, tpy, tvx, tvy, tyaw, tdyaw = t.get()
pxs += [(ppx - tpx) * (ppx - tpx)]
pys += [(ppy - tpy) * (ppy - tpy)]
vxs += [(pvx - tvx) * (pvx - tvx)]
vys += [(pvy - tvy) * (pvy - tvy)]
yaws += [(pyaw - tyaw) * (pyaw - tyaw)]
dyaws += [(pdyaw - tdyaw) * (pdyaw - tdyaw)]
px, py = sqrt(np.mean(pxs)), sqrt(np.mean(pys))
vx, vy = sqrt(np.mean(vxs)), sqrt(np.mean(vys))
yaw, dyaw = sqrt(np.mean(yaws)), sqrt(np.mean(dyaws))
return px, py, vx, vy, yaw, dyaw
def calculate_jacobian(px, py, vx, vy, yaw, dyaw, THRESH = 0.0001, ZERO_REPLACEMENT = 0.0001):
"""
Calculates the Jacobian given for four state variables
Args:
px, py, vx, vy : floats - four state variables in the system
THRESH - minimum value of squared distance to return a non-zero matrix
ZERO_REPLACEMENT - value to replace zero to avoid division by zero error
Returns:
H : the jacobian matrix expressed as a 3 x 6 numpy matrix with float values
"""
d_squared = px * px + py * py
d = sqrt(d_squared)
d_cubed = d_squared * d
if d_squared < THRESH:
print("WARNING: in calculate_jacobian(): d_squared < THRESH")
H = np.matrix(np.zeros([3, 6]))
else:
r11 = px / d
r12 = py / d
r21 = -py / d_squared
r22 = px / d_squared
r31 = py * (vx * py - vy * px) / d_cubed
r32 = px * (vy * px - vx * py) / d_cubed
r34 = (px * np.cos(yaw) + py * np.sin(yaw)) / d
H = np.matrix([[r11, r12, 0, 0, 0, 0],
[r21, r22, 0, 0, 0, 0],
[r31, r32, r11, r12, r34, 0]])
return H