def compute_velocity(self):
#print("pos diff: " , np.diff(self.p_gt,axis=0))
#print("t diff: " , np.diff(self.t_gt,axis=0))
pos_diff = np.diff(self.p_gt,axis=0)
q_diff = np.diff(self.q_es_aligned,axis=0)
t_diff = np.diff(self.t_gt,axis=0)
lin_vel = np.zeros(np.shape(pos_diff))
lin_vel_norm = np.zeros(np.shape(pos_diff)[0])
angle_ypr = np.zeros(np.shape(self.p_gt))
angle_vel = np.zeros(np.shape(pos_diff))
angle_vel_norm = np.zeros(np.shape(pos_diff)[0])
for i in range(np.shape(pos_diff)[0]):
lin_vel[i,:] = pos_diff[i,:]/t_diff[i]
lin_vel_norm[i] = np.linalg.norm(lin_vel[i,:])
angle_ypr[i,:] = tf.euler_from_quaternion(self.q_es_aligned[i, :])
angle_ypr_diff = np.diff(angle_ypr,axis=0)
for i in range(np.shape(pos_diff)[0]):
angle_vel[i,:] = angle_ypr_diff[i,:]/t_diff[i]
angle_vel_norm[i] = np.linalg.norm(angle_vel[i,:])
stats_lin_vel = res_writer.compute_statistics(lin_vel_norm)
stats_ang_vel = res_writer.compute_statistics(angle_vel_norm)
self.velocity['lin_vel'] = stats_lin_vel
self.velocity['ang_vel'] = stats_ang_vel
def compute_relative_error_at_subtraj_len(self, subtraj_len,
max_dist_diff=-1):
if max_dist_diff < 0:
max_dist_diff = 0.2 * subtraj_len
if self.rel_errors and (subtraj_len in self.rel_errors):
print("Relative error at sub-trajectory length {0} is already "
"computed or loaded from cache.".format(subtraj_len))
else:
print("Computing relative error at sub-trajectory "
"length {0}".format(subtraj_len))
Tcm = np.identity(4)
_, e_trans, e_trans_perc, e_yaw, e_gravity, e_rot =\
traj_err.compute_relative_error(
self.p_es, self.q_es, self.p_gt, self.q_gt, Tcm,
subtraj_len, max_dist_diff, self.accum_distances,
self.scale)
dist_rel_err = {'rel_trans': e_trans,
'rel_trans_stats':
res_writer.compute_statistics(e_trans),
'rel_trans_perc': e_trans_perc,
'rel_trans_perc_stats':
res_writer.compute_statistics(e_trans_perc),
'rel_rot': e_rot,
'rel_rot_stats':
res_writer.compute_statistics(e_rot),
'rel_yaw': e_yaw,
'rel_yaw_stats':
res_writer.compute_statistics(e_yaw),
'rel_gravity': e_gravity,
'rel_gravity_stats':
res_writer.compute_statistics(e_gravity)}
self.rel_errors[subtraj_len] = dist_rel_err
def compute_absolute_error(self):
if self.abs_errors:
print("Absolute errors already calculated")
else:
print('Calculating RMSE...')
# align trajectory if necessary
self.align_trajectory()
e_trans, e_trans_vec, e_rot, e_ypr, e_scale_perc =\
traj_err.compute_absolute_error(self.p_es_aligned,
self.q_es_aligned,
self.p_gt,
self.q_gt)
stats_trans = res_writer.compute_statistics(e_trans)
stats_rot = res_writer.compute_statistics(e_rot)
stats_scale = res_writer.compute_statistics(e_scale_perc)
self.abs_errors['abs_e_trans'] = e_trans
self.abs_errors['abs_e_trans_stats'] = stats_trans
self.abs_errors['abs_e_trans_vec'] = e_trans_vec
self.abs_errors['abs_e_rot'] = e_rot
self.abs_errors['abs_e_rot_stats'] = stats_rot
self.abs_errors['abs_e_ypr'] = e_ypr
self.abs_errors['abs_e_scale_perc'] = e_scale_perc
self.abs_errors['abs_e_scale_stats'] = stats_scale
print('...RMSE calculated.')
return
def updateStatistics(self):
if self.n_traj == 0:
return
for et in self.kAbsMetrics:
self.abs_errors[et + '_stats'] = rw.compute_statistics(
np.array(self.abs_errors[et]))
self.overall_rel_errors = {}
for et in kRelMetrics:
values = []
for d in self.rel_errors:
self.rel_errors[d][et + '_stats'] = rw.compute_statistics(
self.rel_errors[d][et])
values.extend(self.rel_errors[d][et].tolist())
self.overall_rel_errors[et] = rw.compute_statistics(values)
rel_e_distances, plot_settings,
output_dir)
else:
print(
"Skip plotting overall odometry error since datasets are evaluated at"
" different distances.")
rel_err_table = {}
rel_err_table['values'] = []
for alg in algorithms:
alg_aver_rel = []
for et in rel_err_names:
cur_errors = []
for _, v in all_odo_err[et][alg].items():
cur_errors.extend(v)
alg_aver_rel.append('{0:.3f}'.format(
res_writer.compute_statistics(cur_errors)['mean']))
rel_err_table['values'].append(alg_aver_rel)
rel_err_table['cols'] = rel_err_labels
rel_err_table['rows'] = algorithms
res_writer.write_tex_table(
rel_err_table['values'], rel_err_table['rows'],
rel_err_table['cols'],
os.path.join(output_dir,
args.platform + '_rel_err_' + eval_uid + '.txt'))
if args.plot_trajectories:
print(Fore.MAGENTA +
'--- Plotting trajectory top and side view ... ---')
plot_trajectories(dataset_trajectories_list,
datasets,
algorithms,