def get_kalman_error(self, use_groundtruth=True):
""" Return error of the Kalman in filter in the map frame
use_groundtruth: if False compare Kalman filter performance with radar images GPS
"""
if hasattr(self.reader, "groundtruth") and use_groundtruth:
error_pos = np.array([
self.kalman.mapdata.attitude.apply(
self.get_position(ts) -
self.reader.get_groundtruth_pos(ts))
for ts in self.get_timestamps(0, np.inf)
])
error_att = np.array([
rotation_proj(self.reader.get_groundtruth_att(ts),
self.get_attitude(ts)).as_euler('zxy')[0]
for ts in self.get_timestamps(0, np.inf)
])
else:
error_pos = np.array([
self.kalman.mapdata.attitude.apply(
self.get_position(ts) - self.reader.get_gps_pos(ts))
for ts in self.get_timestamps(0, np.inf)
])
error_att = np.array([
rotation_proj(self.reader.get_gps_att(ts),
self.get_attitude(ts)).as_euler('zxy')[0]
for ts in self.get_timestamps(0, np.inf)
])
return error_pos, error_att
def plot_attitude(self,
cv2=False,
cv2_corrected=False,
projection="ENU",
car_position=None):
""" Plot the orientation in the map frame given by the GPS and after fusion
cv2: if True plot CV2 measurements integrated alone
cv2_corrected: if True plot CV2 measurements with bias and altitude correction
projection: projection in given 2D plane
car_position: calculate position of the car if True, if False use position of top-left corner, if not given choose the most appropriate
"""
(pos0, att0), _ = get_origin(self, projection, car_position)
plt.figure()
reader = define_reader(self)
q = rot.from_dcm([[0, -1, 0], [-1, 0, 0], [0, 0, -1]])
if hasattr(reader, "groundtruth"):
plt.plot(reader.get_timestamps(),
np.unwrap([
rotation_proj(att0, q * r).as_euler('zxy')[0]
for r in reader.get_groundtruth_att()
]),
'black',
label="Groundtruth")
plt.plot(reader.get_timestamps(),
np.unwrap([
rotation_proj(att0, q * r).as_euler('zxy')[0]
for r in reader.get_gps_att()
]),
'green',
label="GPS")
if cv2:
plt.plot(reader.get_timestamps(),
np.unwrap([
rotation_proj(att0, q * r).as_euler('zxy')[0]
for r in self.get_measured_attitudes()
]),
'red',
label="CV2")
if cv2_corrected:
plt.plot(reader.get_timestamps(),
np.unwrap([
rotation_proj(att0, q * r).as_euler('zxy')[0]
for r in self.get_measured_attitudes(cv2_corrected)
]),
'r--',
label="CV2 corrected")
if not is_reader(self) and len(self.get_attitudes()) != 0:
plt.plot(self.get_timestamps(0, np.inf),
np.unwrap([
rotation_proj(att0, q * r).as_euler('zxy')[0]
for r in self.get_attitudes()
]),
'cornflowerblue',
label="Output")
plt.legend()
plt.title("Yaw")
plt.xlabel("Times (s)")
plt.ylabel("Yaw (rad)")
def plot_attitude(self, gps_only=False, cv2_corrected=False):
""" Plot the orientation in the map frame given by the GPS and after fusion
gps_only: if True plot only GPS data from dataset
cv2_corrected: if True add a line with bias correction on CV2 measurements
"""
plt.figure()
reader = define_reader(self)
pos0, att0 = get_plot_origin(self)
q = rot.from_dcm([[0, -1, 0], [-1, 0, 0], [0, 0, -1]])
if hasattr(reader, "groundtruth"):
plt.plot(reader.get_timestamps(),
np.unwrap([
rotation_proj(att0, q * r).as_euler('zxy')[0]
for r in reader.get_groundtruth_att()
]),
'black',
label="Groundtruth")
plt.plot(reader.get_timestamps(),
np.unwrap([
rotation_proj(att0, q * r).as_euler('zxy')[0]
for r in reader.get_gps_att()
]),
'green',
label="GPS")
if not gps_only:
plt.plot(reader.get_timestamps(),
np.unwrap([
rotation_proj(att0, q * r).as_euler('zxy')[0]
for r in self.get_measured_attitudes()
]),
'red',
label="CV2")
if cv2_corrected and not gps_only:
plt.plot(reader.get_timestamps(),
np.unwrap([
rotation_proj(att0, q * r).as_euler('zxy')[0]
for r in self.get_measured_attitudes(cv2_corrected)
]),
'r--',
label="CV2 corrected")
if hasattr(self, "get_attitude") and len(self.get_attitude()) != 0:
plt.plot(self.get_timestamps(0, np.inf),
np.unwrap([
rotation_proj(att0, q * r).as_euler('zxy')[0]
for r in self.get_attitude()
]),
'cornflowerblue',
label="Output")
plt.legend()
plt.title("Yaw")
plt.xlabel("Times (s)")
plt.ylabel("Yaw (rad)")
def extract_from_map(self, gps_pos, attitude, shape, scale=1):
""" Return an image from the map for a given position and attitude and with a given shape """
data_temp = RadarData(
0,
np.ones((int(np.ceil(shape[0] / scale)),
int(np.ceil(shape[1] / scale)))), gps_pos, attitude,
self.precision)
img1, cov_img1, new_origin = self.build_partial_map(data_temp)
P_start = self.attitude.apply(new_origin -
gps_pos)[0:2] / self.precision
R = rotation_proj(self.attitude, attitude).inv().as_dcm()[:2, :2]
M2 = np.concatenate((R, R.dot(np.array([[P_start[0]], [P_start[1]]]))),
axis=1)
M2 = scale * np.eye(2).dot(M2)
img2 = cv2.warpAffine(img1,
M2, (shape[1], shape[0]),
flags=cv2.INTER_LINEAR,
borderValue=0)
cov_img2 = cv2.warpAffine(cov_img1,
M2, (shape[1], shape[0]),
flags=cv2.INTER_LINEAR,
borderValue=0)
return img2, cov_img2
def update(self, new_data):
""" Update the state X thanks to GPS information """
pos2D = self.mapdata.attitude.apply(new_data.gps_pos -
self.mapdata.gps_pos)[0:2]
att2D = rotation_proj(self.mapdata.attitude,
new_data.attitude).as_euler("zxy")[0]
Y = np.append(pos2D, att2D)
Yhat = np.append(self.pos2D, self.att2D)
if self.bias_estimation:
H = np.block([np.eye(3), np.zeros((3, 3))])
else:
H = np.eye(3)
S = H.dot(self.P).dot(H.T) + self.R
K = self.P.dot(H.T).dot(np.linalg.inv(S))
#Z = stat_test(Y, Yhat, S, 0.99)*(Y - Yhat)
Z = (Y - Yhat)
e = K.dot(Z)
self.pos2D = self.pos2D + e[0:2]
self.att2D = self.att2D + e[2]
if self.bias_estimation:
self.bias = self.bias + e[3:6]
self.P = (np.eye(len(self.P)) - K.dot(H)).dot(self.P)
self.innovation = (Z, S)
def predict(self, new_data):
""" Based on image transformation measurement, predict new state X """
trans, rotation = new_data.image_transformation_from(self.last_data)
R = np.array([[np.cos(self.att2D), -np.sin(self.att2D)],
[np.sin(self.att2D),
np.cos(self.att2D)]])
if not np.any(np.isnan(trans)):
self.pos2D = self.pos2D + R.dot(trans[0:2] - self.bias[0:2])
self.att2D = self.att2D + rotation.as_euler(
'zxy')[0] - self.bias[2]
else:
self.pos2D = self.mapdata.attitude.apply(new_data.gps_pos -
self.mapdata.gps_pos)[0:2]
self.att2D = rotation_proj(self.mapdata.attitude,
new_data.attitude).as_euler("zxy")[0]
F = np.array([[
1, 0, -np.sin(self.att2D) * (trans[0] - self.bias[0]) -
np.cos(self.att2D) * (trans[1] - self.bias[1])
],
[
0, 1,
np.cos(self.att2D) * (trans[0] - self.bias[0]) -
np.sin(self.att2D) * (trans[1] - self.bias[1])
], [0, 0, 1]])
M = np.block([[R, np.zeros((2, 1))], [np.zeros(2), 1]])
if self.bias_estimation:
F = np.block(
[[F, np.block([[-R, np.zeros((2, 1))], [np.zeros(2), -1]])],
[np.zeros((3, 3)), np.eye(3)]])
M = np.block([[M], [np.zeros((3, 3))]])
self.P = F.dot(self.P).dot(F.T) + M.dot(self.Q).dot(M.T)
else:
self.P = F.dot(self.P).dot(F.T) + M.dot(self.Q).dot(M.T)
def predict_image(self, gps_pos, attitude, shape=None):
""" Give the prediction of an observation in a different position based on actual radar image """
exp_rot_matrix = rotation_proj(attitude,
self.attitude).as_dcm()[:2, :2]
exp_trans = attitude.apply(gps_pos -
self.gps_pos)[0:2] / self.precision
if shape is None:
shape = (np.shape(self.img)[1], np.shape(self.img)[0])
else:
shape = (shape[1], shape[0])
warp_matrix = np.concatenate(
(exp_rot_matrix, np.array([[-exp_trans[0]], [-exp_trans[1]]])),
axis=1)
predict_img = cv2.warpAffine(self.img,
warp_matrix,
shape,
flags=cv2.INTER_LINEAR,
borderValue=0)
mask = cv2.warpAffine(np.ones(np.shape(self.img)),
warp_matrix,
shape,
flags=cv2.INTER_LINEAR,
borderValue=0)
diff = mask - np.ones((shape[1], shape[0]))
diff[diff != -1] = 0
diff[diff == -1] = np.nan
prediction = diff + predict_img
return prediction
def get_kalman_error(self, t_ini=None, t_final=None, use_groundtruth=True):
""" Return error of the Kalman in filter in the map frame
use_groundtruth: if False compare Kalman filter performance with radar images GPS
"""
times = self.get_timestamps(t_ini, t_final)
if not t_ini is None and t_final is None:
if hasattr(self.reader, "groundtruth") and use_groundtruth:
error_pos = self.kalman.mapdata.attitude.apply(
self.get_positions(times) -
self.reader.get_groundtruth_pos(times))[0:2]
error_att = rotation_proj(
self.reader.get_groundtruth_att(times),
self.get_attitudes(times)).as_euler('zxy')[0]
else:
error_pos = self.kalman.mapdata.attitude.apply(
self.get_positions(times) -
self.reader.get_gps_pos(times))[0:2]
error_att = rotation_proj(
self.reader.get_gps_att(times),
self.get_attitudes(times)).as_euler('zxy')[0]
else:
if hasattr(self.reader, "groundtruth") and use_groundtruth:
error_pos = np.array([
self.kalman.mapdata.attitude.apply(
self.get_positions(ts) -
self.reader.get_groundtruth_pos(ts))[0:2]
for ts in times
])
error_att = np.array([
rotation_proj(self.reader.get_groundtruth_att(ts),
self.get_attitudes(ts)).as_euler('zxy')[0]
for ts in times
])
else:
error_pos = np.array([
self.kalman.mapdata.attitude.apply(
self.get_positions(ts) -
self.reader.get_gps_pos(ts))[0:2] for ts in times
])
error_att = np.array([
rotation_proj(self.reader.get_gps_att(ts),
self.get_attitudes(ts)).as_euler('zxy')[0]
for ts in times
])
return error_pos, error_att
def predict(self, new_data):
""" Based on GPS pos of the new data, predict new state X """
pos2D = self.mapdata.attitude.apply(new_data.gps_pos -
self.mapdata.gps_pos)[0:2]
att2D = rotation_proj(self.mapdata.attitude,
new_data.attitude).as_euler('zxy')[0]
F1 = np.array([[
1, 0, -self.trans[0] * np.sin(self.prev_att2D) -
self.trans[1] * np.cos(self.prev_att2D),
np.cos(self.prev_att2D), -np.sin(self.prev_att2D), 0
],
[
0, 1, self.trans[0] * np.cos(self.prev_att2D) -
self.trans[1] * np.sin(self.prev_att2D),
np.sin(self.prev_att2D),
np.cos(self.prev_att2D), 0
], [0, 0, 1, 0, 0, 1]])
#F2 = np.array([[0, 0, -(pos2D-self.prev_pos2D)[0]*np.sin(self.prev_att2D+self.rot)+(pos2D-self.prev_pos2D)[0]*np.cos(self.prev_att2D+self.rot), 0, 0, -(pos2D-self.prev_pos2D)[0]*np.sin(self.prev_att2D+self.rot)+(pos2D-self.prev_pos2D)[0]*np.cos(self.prev_att2D+self.rot)],
# [0, 0, -(pos2D-self.prev_pos2D)[1]*np.cos(self.prev_att2D+self.rot)-(pos2D-self.prev_pos2D)[1]*np.sin(self.prev_att2D+self.rot), 0, 0, -(pos2D-self.prev_pos2D)[1]*np.cos(self.prev_att2D+self.rot)-(pos2D-self.prev_pos2D)[1]*np.sin(self.prev_att2D+self.rot)],
# [0, 0, -1, 0, 0, 0]])
F2 = np.array([[
-np.cos(self.prev_att2D + self.rot),
-np.sin(self.prev_att2D + self.rot),
-(pos2D - self.prev_pos2D)[0] * np.sin(self.prev_att2D + self.rot)
+
(pos2D - self.prev_pos2D)[0] * np.cos(self.prev_att2D + self.rot),
-np.cos(self.rot), -np.sin(self.rot),
-(pos2D - self.prev_pos2D)[0] * np.sin(self.prev_att2D + self.rot)
+
(pos2D - self.prev_pos2D)[0] * np.cos(self.prev_att2D + self.rot) +
self.trans[0] * np.sin(self.rot) - self.trans[1] * np.cos(self.rot)
],
[
np.sin(self.prev_att2D + self.rot),
-np.cos(self.prev_att2D + self.rot),
-(pos2D - self.prev_pos2D)[1] *
np.cos(self.prev_att2D + self.rot) -
(pos2D - self.prev_pos2D)[1] *
np.sin(self.prev_att2D + self.rot),
np.sin(self.rot), -np.cos(self.rot),
-(pos2D - self.prev_pos2D)[1] *
np.cos(self.prev_att2D + self.rot) -
(pos2D - self.prev_pos2D)[1] *
np.sin(self.prev_att2D + self.rot) +
self.trans[0] * np.cos(self.rot) +
self.trans[1] * np.sin(self.rot)
], [0, 0, -1, 0, 0, -1]])
F = np.block([[F1], [F2 - F1]])
self.prev_pos2D = self.prev_pos2D + R(-self.prev_att2D).dot(self.trans)
self.prev_att2D = self.prev_att2D + self.rot
self.trans = R(self.prev_att2D).dot(pos2D - self.prev_pos2D)
self.rot = att2D - self.prev_att2D
self.P = F.dot(self.P).dot(F.T) + self.Q
def get_gps_measurements(self):
""" Return GPS measurements between two consecutive images """
if self.gps_rot is None or self.gps_trans is None:
print("Retreiving GPS measurements... ")
times = self.get_timestamps(0, np.inf)
self.gps_trans = np.zeros((len(times)-1,2))
self.gps_rot = np.zeros(len(times)-1)
for i in range(1,len(times)):
self.gps_rot[i-1] = rotation_proj(self.get_gps_att(times[i-1]), self.get_gps_att(times[i])).as_euler('zxy')[0]
self.gps_trans[i-1] = self.heatmaps[times[i-1]].earth2rbd(self.get_gps_pos(times[i]) - self.get_gps_pos(times[i-1]))[0:2]
return self.gps_trans, self.gps_rot
def image_position_from(self, otherdata):
""" Return the actual position and attitude based on radar images comparison """
translation, rotation = self.image_transformation_from(otherdata)
if not np.any(np.isnan(translation)):
gps_pos = otherdata.gps_pos + otherdata.earth2rbd(translation,True)
attitude = rotation.inv()*otherdata.attitude
else:
print("No cv2 measurement, use GPS instead")
trans = otherdata.earth2rbd(self.gps_pos-otherdata.gps_pos)
trans[2] = 0
gps_pos = otherdata.gps_pos + otherdata.earth2rbd(trans, True)
attitude = rotation_proj(otherdata.attitude, self.attitude).inv()*otherdata.attitude
return gps_pos, attitude
def add_data(self, otherdata):
""" Fusionning a new radardata with part of the map """
if self.gps_pos is None:
self.init_map(otherdata)
img1, cov_img1, new_origin = self.build_partial_map(otherdata)
shape = np.shape(img1)
v2 = self.attitude.apply(otherdata.gps_pos -
new_origin)[0:2] / self.precision
M2 = np.concatenate(
(rotation_proj(self.attitude, otherdata.attitude).as_dcm()[:2, :2],
np.array([[v2[0]], [v2[1]]])),
axis=1)
img2 = cv2.warpAffine(otherdata.img,
M2, (shape[1], shape[0]),
flags=cv2.INTER_LINEAR,
borderValue=0)
mask = cv2.warpAffine(np.ones(np.shape(otherdata.img)),
M2, (shape[1], shape[0]),
flags=cv2.INTER_LINEAR,
borderValue=0)
diff = mask - np.ones(shape)
diff[diff != 0] = np.nan
img2 = diff + img2
cov_img2 = cv2.warpAffine(self.img_cov *
np.ones(np.shape(otherdata.img)),
M2, (shape[1], shape[0]),
flags=cv2.INTER_LINEAR,
borderValue=0)
cov_img2 = diff + cov_img2
img, cov_img = merge_img(img1, img2, cov_img1, cov_img2)
self.update_map(img, cov_img, new_origin)
return img1, img2, v2
def build_partial_map(self, otherdata):
""" Build partial map of the chunks that are needed to contain the new data """
hdf5 = h5py.File('maps/' + self.map_name + '.h5', 'a')
try:
map_hdf5 = hdf5["map"]
cov_map_hdf5 = hdf5["covariance"]
q = rotation_proj(self.attitude, otherdata.attitude)
P5 = self.attitude.apply(otherdata.gps_pos - self.gps_pos)[0:2]
P6 = P5 + q.apply(np.array([otherdata.width(), 0, 0]))[0:2]
P7 = P5 + q.apply(
np.array([otherdata.width(),
otherdata.height(), 0]))[0:2]
P8 = P5 + q.apply(np.array([0, otherdata.height(), 0]))[0:2]
P9 = np.array([
min(P5[0], P6[0], P7[0], P8[0]),
min(P5[1], P6[1], P7[1], P8[1])
])
P10 = np.array([
max(P5[0], P6[0], P7[0], P8[0]),
max(P5[1], P6[1], P7[1], P8[1])
])
chunk_1 = np.flip(
np.floor(
(P9 / self.precision) / self.chunk_size).astype(np.int))
chunk_2 = np.flip(
np.floor(
(P10 / self.precision) / self.chunk_size).astype(np.int))
img = np.nan * np.ones(
(self.chunk_size *
(1 + chunk_2[0] - chunk_1[0]), self.chunk_size *
(1 + chunk_2[1] - chunk_1[1])))
cov_img = np.nan * np.ones(
(self.chunk_size *
(1 + chunk_2[0] - chunk_1[0]), self.chunk_size *
(1 + chunk_2[1] - chunk_1[1])))
for i in range(chunk_1[0], chunk_2[0] + 1):
for j in range(chunk_1[1], chunk_2[1] + 1):
if not str(i) + "/" + str(j) in map_hdf5:
map_hdf5.create_dataset(
str(i) + "/" + str(j),
data=np.nan * np.ones(
(self.chunk_size, self.chunk_size)),
shape=(self.chunk_size, self.chunk_size))
cov_map_hdf5.create_dataset(
str(i) + "/" + str(j),
data=np.nan * np.ones(
(self.chunk_size, self.chunk_size)),
shape=(self.chunk_size, self.chunk_size))
img[(i - chunk_1[0]) *
self.chunk_size:(i - chunk_1[0] + 1) * self.chunk_size,
(j - chunk_1[1]) *
self.chunk_size:(j - chunk_1[1] + 1) *
self.chunk_size] = map_hdf5[str(i) + "/" + str(j)]
cov_img[(i - chunk_1[0]) *
self.chunk_size:(i - chunk_1[0] + 1) *
self.chunk_size, (j - chunk_1[1]) *
self.chunk_size:(j - chunk_1[1] + 1) *
self.chunk_size] = cov_map_hdf5[str(i) + "/" +
str(j)]
gps_pos = self.gps_pos + self.attitude.inv().apply(
np.array([
chunk_1[1] * self.chunk_size * self.precision,
chunk_1[0] * self.chunk_size * self.precision, 0
]))
finally:
hdf5.close()
return img, cov_img, gps_pos
