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 add(self, new_data, fusion=True):
""" Add a new radar data on the map
fusion: if false, add raw data to the map
"""
if self.last_data is None:
# use position/attitude custom initialisation
if not (self.position is None):
new_data = RadarData(new_data.id, new_data.img, self.position,
new_data.attitude)
if not (self.attitude is None):
new_data = RadarData(new_data.id, new_data.img,
new_data.gps_pos, self.attitude)
if self.mapping:
self.mapdata.add_data(new_data)
else:
self.mapdata = deepcopy(new_data)
self.last_data = deepcopy(new_data)
self.position = deepcopy(self.mapdata.gps_pos)
self.attitude = deepcopy(self.mapdata.attitude)
else:
if fusion:
self.predict(new_data)
self.update(new_data)
self.position = self.process_position(new_data)
self.attitude = self.process_attitude(new_data)
else:
self.position = new_data.gps_pos
self.attitude = new_data.attitude
self.last_data = RadarData(new_data.id, new_data.img,
self.position, self.attitude)
if self.mapping:
self.mapdata.add_data(self.last_data)
return deepcopy(self.position), deepcopy(self.attitude)
def localize(self, new_data, gps_guess=False):
""" Find the position of a image thanks to the map """
if gps_guess:
mapping_img, _ = self.mapdata.extract_from_map(
new_data.gps_pos, new_data.attitude, np.shape(new_data.img))
gps_pos, attitude = projection(self.mapdata.gps_pos,
self.mapdata.attitude,
new_data.gps_pos, new_data.attitude)
mapping_data = RadarData(None, mapping_img, gps_pos, attitude)
else:
mapping_img, _ = self.mapdata.extract_from_map(
self.position, self.attitude, np.shape(new_data.img))
mapping_data = RadarData(None, mapping_img, self.position,
self.attitude)
self.position, self.attitude = new_data.image_position_from(
mapping_data)
self.last_data = RadarData(new_data.id, new_data.img, self.position,
self.attitude)
if self.mapping:
self.mapdata.add_data(self.last_data)
return deepcopy(self.position), deepcopy(self.attitude)
def load_heatmaps(self, t_ini=0, t_final=np.inf):
""" Function load radar data magnitude from HDF5 between t_ini and t_final"""
hdf5 = h5py.File(self.src,'r+')
cv2_transformations = SqliteDict('cv2_transformations.db', autocommit=True)
cv2_transformations['use_dataset'] = self.src
cv2_transformations.close()
try:
aperture = hdf5['radar']['broad01']['aperture2D']
except:
raise Exception("The images should be in radar/broad01/aperture2D directory")
if not(('preprocessed' in aperture.attrs) and aperture.attrs['preprocessed']):
hdf5.close()
raise Exception("The dataset should be preprocessed before with Preprocessor")
try:
times = list(aperture.keys())
# Importing radar images from dataset
t0 = float(times[0])
times = [times[i] for i in range(len(times)) if float(times[i])-t0>=t_ini and float(times[i])-t0<=t_final]
prev_perc = -1
for i, t in enumerate(times):
if np.floor(i/(len(times)-1)*10) != prev_perc:
print("Loading data: "+str(np.floor(i/(len(times)-1)*10)*10)+"%")
prev_perc = np.floor(i/(len(times)-1)*10)
heatmap = aperture[t][...];
if not np.sum(heatmap) == 0:
gps_pos = np.array(list(aperture[t].attrs['POSITION'][0]))
att = np.array(list(aperture[t].attrs['ATTITUDE'][0]))
self.heatmaps[float(t)-t0] = RadarData(float(t), np.array(Image.fromarray(heatmap, 'L')), gps_pos, rot.from_quat(att))
self.tracklog_translation = -aperture.attrs['tracklog_translation']
# Importing groundtruth GPS information if available
aperture_gt = hdf5['radar']['broad01']
groundtruth = ("groundtruth" in aperture_gt)
if groundtruth:
print("Loading groundtruth")
aperture_gt = hdf5['radar']['broad01']['groundtruth']
self.groundtruth = dict()
times_gt = list(aperture_gt.keys())
times_gt = [times_gt[i] for i in range(len(times_gt)) if (t_ini <= float(times_gt[i])-t0 < t_final) or (i+1<len(times_gt) and t_ini <= float(times_gt[i+1])-t0 < t_final) or (i-1>=0 and t_ini <= float(times_gt[i-1])-t0 < t_final)]
gt_att, gt_pos, gt_time = [], [], []
for i, t in enumerate(times_gt):
gt_att.append(rot.from_quat(np.array(list(aperture_gt[t].attrs['ATTITUDE'][0]))))
gt_pos.append(np.array(list(aperture_gt[t].attrs['POSITION'][0])))
gt_time.append(float(t)-t0)
gt_pos = rbd_translate(gt_pos, gt_att, self.tracklog_translation - (-aperture_gt.attrs['tracklog_translation']))
# Interpolating groundtruth positions to make them match with radar images positions
if times_gt[0]> times[0] or times_gt[-1]<times[-1]:
for t in times:
if times_gt[0] > t or times_gt[-1] < t:
self.heatmaps.pop(float(t)-t0)
times = list(self.heatmaps.keys())
slerp = Slerp(gt_time, rot.from_quat([r.as_quat() for r in gt_att]))
gt_att = [rot.from_quat(q) for q in slerp(times).as_quat()]
gt_pos = np.array([np.interp(times, gt_time, np.array(gt_pos)[:,i]) for i in range(0,3)]).T
for i in range(len(times)):
self.groundtruth[times[i]] = {'POSITION': gt_pos[i], 'ATTITUDE': gt_att[i]}
else:
print("No groundtruth data found")
hdf5.close()
print("Data loaded")
except:
hdf5.close()
raise Exception("A problem occured when importing data")