def get_all_differences(all_sensor_data, all_ground_truths):
pxs, pys, vxs, vys, rhos, phis, drhos = [], [], [], [], [], [], []
for s, t in zip(all_sensor_data, all_ground_truths):
if s.get_name() == 'lidar':
spx, spy, _, _ = s.get()
tpx, tpy, _, _ = t.get()
pxs += [spx - tpx]
pys += [spy - tpy]
else:
spx, spy, svx, svy = s.get()
tpx, tpy, tvx, tvy = t.get()
srho, sphi, sdrho = s.get_raw()
trho, tphi, tdrho = cartesian_to_polar(tpx, tpy, tvx, tvy)
pxs += [spx - tpx]
pys += [spy - tpy]
vxs += [svx - tvx]
vys += [svy - tvy]
rhos += [srho - trho]
phis += [sphi - tphi]
drhos += [sdrho - tdrho]
return pxs, pys, vxs, vys, rhos, phis, drhos
def kalman(data, kalmanFilter, timestamp):
dt = time_difference(timestamp, data.get_timestamp())
timestamp = data.get_timestamp()
kalmanFilter.F[0, 2], kalmanFilter.F[1, 3] = dt, dt
kalmanFilter.Q = updateQ(dt)
kalmanFilter.predict()
z = np.matrix(data.get_raw()).T
x = kalmanFilter.x
if data.get_name() == 'radar':
px, py, vx, vy = x[0, 0], x[1, 0], x[2, 0], x[3, 0]
rho, phi, drho = cartesian_to_polar(px, py, vx, vy)
H = calculate_jacobian(px, py, vx, vy)
Hx = (np.matrix([[rho, phi, drho]])).T
R = radar_R
elif data.get_name() == 'lidar':
H = lidar_H
Hx = lidar_H * x
R = lidar_R
kalmanFilter.update(z, H, Hx, R)
# if data.get_name() == 'lidar':
# print x, kalmanFilter.x, "lidar"
# elif data.get_name() == 'radar':
# print x, kalmanFilter.x, "radar"
return timestamp
def update(self, data):
dt = time_difference(self.timestamp, data.get_timestamp())
self.timestamp = data.get_timestamp()
self.kalmanFilter.updateF(dt)
self.updateQ(dt)
self.kalmanFilter.predict()
z = np.matrix(data.get_raw()).T
x = self.kalmanFilter.getx()
if data.get_name() == 'radar':
px, py, vx, vy = x[0, 0], x[1, 0], x[2, 0], x[3, 0]
rho, phi, drho = cartesian_to_polar(px, py, vx, vy)
H = calculate_jacobian(px, py, vx, vy)
Hx = (np.matrix([[rho, phi, drho]])).T
R = self.radar_R
elif data.get_name() == 'lidar':
H = self.lidar_H
Hx = self.lidar_H * x
R = self.lidar_R
self.kalmanFilter.update(z, H, Hx, R)
def parse_data(file_path, ENCODE=False):
"""
Args:
file_path
- path to a text file with all data.
- each line should have the following format:
[SENSOR ID] [SENSOR RAW VALUES] [TIMESTAMP] [GROUND TRUTH VALUES]
Whereas radar has three measurements (rho, phi, rhodot), lidar has two measurements (x, y).
Specifically:
For a row containing radar data, the columns are:
sensor_type, rho_measured, phi_measured, rhodot_measured, timestamp, x_groundtruth, y_groundtruth, vx_groundtruth, vy_groundtruth
For a row containing lidar data, the columns are:
sensor_type, x_measured, y_measured, timestamp, x_groundtruth, y_groundtruth, vx_groundtruth, vy_groundtruth
Example 1:
line with three measurements from a radar sensor in polar coordinate
followed by a timestamp in unix time
followed by the the "ground truth" which is
actual real position and velocity in cartesian coordinates (four state variables)
R 8.46642 0.0287602 -3.04035 1477010443399637 8.6 0.25 -3.00029 0
(R) (rho) (phi) (drho) (timestamp) (real x) (real y) (real vx) (real vy)
Example 2:
line with two measurements from a lidar sensor in cartesian coordinates
followed by a timestamp in unix time
followed by the the "ground truth" which is
the actual real position and velocity in cartesian coordinates (four state variables)
L 8.44818 0.251553 1477010443449633 8.45 0.25 -3.00027 0
Returns:
all_sensor_data, all_ground_truths
- two lists of DataPoint() instances
"""
all_sensor_data = []
all_ground_truths = []
if (ENCODE):
message = 0
with open(file_path) as f:
for line in f:
data = line.split()
if data[0] == 'L':
data_read = {
'timestamp': int(data[3]),
'name': 'lidar',
'x': float(data[1]),
'y': float(data[2]),
'vx': 0.0,
'vy': 0.0
}
# print('helpers:parse_data: data read LiDAR', data_read['x'],data_read['y'],data_read['vx'],data_read['vx'])
sensor_data = DataPoint(data_read)
# print('helpers:parse_data: data read LiDAR', sensor_data.data[0],sensor_data.data[1],sensor_data.data[2],sensor_data.data[3] )
g = {
'timestamp': int(data[3]),
'name': 'state',
'x': float(data[4]),
'y': float(data[5]),
'vx': float(data[6]),
'vy': float(data[7])
}
ground_truth = DataPoint(g)
# print('helpers:parse_data: Ground truth LiDAR', ground_truth.data[0],ground_truth.data[1], ground_truth.data[2], ground_truth.data[3] )
elif data[0] == 'R':
#herer sensor_data is the output of the DataPoint class
if (ENCODE): #Here we implement the QIM if used as an option
#convert to cartesian
x, y, vx, vy = polar_to_cartesian(float(data[1]),
float(data[2]),
float(data[3]))
#embedd data - only modify x,y
x, y = QIM_encode_twobit(np.array([x, y]), message)
#print('helpers:parse_data: encoded data read raDAR', sensor_data.data[0],sensor_data.data[1],sensor_data.data[2],sensor_data.data[3] )
message += 1
message %= 4
#convert to polar
rhho, phhi, derho = cartesian_to_polar(x, y, vx, vy)
data_read = {
'timestamp': int(data[4]),
'name': 'radar',
'rho': float(rhho),
'phi': float(phhi),
'drho': float(derho)
}
else:
data_read = {
'timestamp': int(data[4]),
'name': 'radar',
'rho': float(data[1]),
'phi': float(data[2]),
'drho': float(data[3])
}
sensor_data = DataPoint(data_read)
#print('helpers:parse_data: data read raDAR', sensor_data.data[0],sensor_data.data[1],sensor_data.data[2],sensor_data.data[3] )
g = {
'timestamp': int(data[4]),
'name': 'state',
'x': float(data[5]),
'y': float(data[6]),
'vx': float(data[7]),
'vy': float(data[8])
}
ground_truth = DataPoint(g)
# print('helpers:parse_data: Ground truth raDAR',ground_truth.data[0],ground_truth.data[1], ground_truth.data[2], ground_truth.data[3] )
all_sensor_data.append(sensor_data)
all_ground_truths.append(ground_truth)
return all_sensor_data, all_ground_truths