def load_data(self, nm_gt, nm_est, nm_matches):
"""
Loads the trajectory data. The resuls {p_es, q_es, p_gt, q_gt} is
synchronized and has the same length.
"""
if not os.path.exists(os.path.join(self.data_dir, nm_gt)) or \
not os.path.exists(os.path.join(self.data_dir, nm_est)):
print(Fore.RED + "Either groundtruth or estimate does not exist")
return False
print(Fore.RED + 'Loading trajectory data...')
# only timestamped pose series is supported
self.t_es, self.p_es, self.q_es, self.t_gt, self.p_gt, self.q_gt =\
traj_loading.load_stamped_dataset(
self.data_dir, nm_gt, nm_est,
os.path.join(Trajectory.saved_res_dir_nm, self.est_type,
nm_matches),
start_t_sec=self.start_time_sec, end_t_sec=self.end_time_sec)
self.t_gt_raw, self.p_gt_raw, self.q_gt_raw =\
traj_loading.load_raw_groundtruth(self.data_dir, nm_gt,
start_t_sec=self.start_time_sec,
end_t_sec=self.end_time_sec)
if self.p_es.size == 0:
print(Fore.RED + "Empty estimate file.")
return False
self.accum_distances = traj_utils.get_distance_from_start(
self.p_gt_raw)
self.traj_length = self.accum_distances[-1]
self.accum_distances = traj_utils.get_distance_from_start(self.p_gt)
if os.path.isfile(self.cached_rel_err_fn):
print('Loading cached relative (odometry) errors from ' +
self.cached_rel_err_fn)
with open(self.cached_rel_err_fn, "rb") as f:
self.rel_errors = pickle.load(f)
print("Loaded odometry error calcualted at {0}".format(
self.rel_errors.keys()))
print(Fore.GREEN + '...done.')
return True
def compute_accumulate_errors(self):
self.accum_distances_est = traj_utils.get_distance_from_start(
self.p_es)
assert len(self.accum_distances_est) == len(
self.accum_distances
), "estimation and groundtruth should have the same length"
accum_distance_error = np.abs(self.accum_distances -
self.accum_distances_est)
accum_distances_error_ratio = accum_distance_error[
-1] / self.traj_length
self.accum_errors['accum_distance_error'] = accum_distance_error
self.accum_errors['accum_distance'] = self.accum_distances
self.accum_errors[
'accum_distance_error_ratio'] = accum_distances_error_ratio
def compute_relative_error(p_es, q_es, p_gt, q_gt, T_cm, dist, max_dist_diff,
accum_distances=[],
scale=1.0):
if len(accum_distances) == 0:
accum_distances = tu.get_distance_from_start(p_gt)
comparisons = tu.compute_comparison_indices_length(
accum_distances, dist, max_dist_diff)
print('number of samples = '+str(len(comparisons)))
T_mc = np.linalg.inv(T_cm)
errors = []
for idx, c in enumerate(comparisons):
if not c == -1:
T_c1 = tu.get_rigid_body_trafo(q_es[idx, :], p_es[idx, :])
T_c2 = tu.get_rigid_body_trafo(q_es[c, :], p_es[c, :])
T_c1_c2 = np.dot(np.linalg.inv(T_c1), T_c2)
T_c1_c2[:3, 3] *= scale
T_m1 = tu.get_rigid_body_trafo(q_gt[idx, :], p_gt[idx, :])
T_m2 = tu.get_rigid_body_trafo(q_gt[c, :], p_gt[c, :])
T_m1_m2 = np.dot(np.linalg.inv(T_m1), T_m2)
T_m1_m2_in_c1 = np.dot(T_cm, np.dot(T_m1_m2, T_mc))
T_error_in_c2 = np.dot(np.linalg.inv(T_m1_m2_in_c1), T_c1_c2)
T_c2_rot = np.eye(4)
T_c2_rot[0:3, 0:3] = T_c2[0:3, 0:3]
T_error_in_w = np.dot(T_c2_rot, np.dot(
T_error_in_c2, np.linalg.inv(T_c2_rot)))
errors.append(T_error_in_w)
error_trans_norm = []
error_trans_perc = []
error_yaw = []
error_gravity = []
e_rot = []
for e in errors:
tn = np.linalg.norm(e[0:3, 3])
error_trans_norm.append(tn)
error_trans_perc.append(tn / dist * 100)
ypr_angles = tf.euler_from_matrix(e, 'rzyx')
e_rot.append(tu.compute_angle(e))
error_yaw.append(abs(ypr_angles[0])*180.0/np.pi)
error_gravity.append(
np.sqrt(ypr_angles[1]**2+ypr_angles[2]**2)*180.0/np.pi)
return errors, np.array(error_trans_norm), np.array(error_trans_perc),\
np.array(error_yaw), np.array(error_gravity), np.array(e_rot)
def load_data(self):
"""
Loads the trajectory data. The resuls {p_es, q_es, p_gt, q_gt} is
synchronized and has the same length.
"""
if self.data_loaded == True:
print("Data already loaded")
return
print('Loading trajectory data...')
# associate the estimation and groundtruth
matches = associate_timestamps.read_files_and_associate(
self.est_traj_file, self.gt_traj_file, self.time_offset,
self.max_diff)
dict_matches = dict(matches)
data_es = np.loadtxt(self.est_traj_file)
data_gt = np.loadtxt(self.gt_traj_file)
self.p_es = []
self.p_gt = []
self.q_es = []
self.q_gt = []
self.t_gt = []
for es_id, es in enumerate(data_es):
if es_id in dict_matches:
gt = data_gt[dict_matches[es_id]]
self.p_es.append(es[1:4])
self.p_gt.append(gt[1:4])
self.q_es.append(es[4:8])
self.q_gt.append(gt[4:8])
self.t_gt.append(gt[0])
self.p_es = np.array(self.p_es)
self.p_gt = np.array(self.p_gt)
self.q_es = np.array(self.q_es)
self.q_gt = np.array(self.q_gt)
self.t_gt = np.array(self.t_gt)
self.accum_distances = traj_utils.get_distance_from_start(self.p_gt)
self.traj_length = self.accum_distances[-1]
self.data_loaded = True
print('...done.')
def load_data(self):
"""
Loads the trajectory data. The resuls {p_es, q_es, p_gt, q_gt} is
synchronized and has the same length.
"""
print('Loading trajectory data...')
# only timestamped pose series is supported
self.t_es, self.p_es, self.q_es, self.t_gt, self.p_gt, self.q_gt =\
traj_loading.load_stamped_dataset(self.data_dir)
self.accum_distances = traj_utils.get_distance_from_start(self.p_gt)
self.traj_length = self.accum_distances[-1]
if os.path.isfile(self.cached_rel_err_fn):
print('Loading cached relative (odometry) errors from ' +
self.cached_rel_err_fn)
with open(self.cached_rel_err_fn) as f:
self.rel_errors = pickle.load(f)
print("Loaded odometry error calcualted at {0}".format(
self.rel_errors.keys()))
self.data_loaded = True
print('...done.')