def transform_world_frame(sequences, gts, auto_scale):
seq = min(sequences.keys())
traj0 = sequences[seq]['traj']
gt0 = gts[seq]['traj']
matches, ref_traj_interpolated = associate.associate_with_interpolation(gt0, traj0, 0, 0.5)
if len(matches) < 1:
sequences[seq]['ate'] = {}
sequences[seq]['ate_rmse'] = float('nan')
sequences[seq]['ate_num'] = 0
sequences[seq]['oe'] = {}
sequences[seq]['aoe_rmse'] = float('nan')
return
first_xyz = np.matrix([[float(value) for value in ref_traj_interpolated[a][0:3]] for a,b in matches]).transpose()
second_xyz = np.matrix([[float(value) for value in traj0[b][0:3]] for a,b in matches]).transpose()
if auto_scale:
rot, trans, scale, ate = evaluate_ate.umeyama_align(second_xyz, first_xyz)
else:
rot, trans, ate = evaluate_ate.align(second_xyz, first_xyz)
scale = 1
sequences[seq]['ate'] = {m[1]: e for m, e in zip(matches, ate)}
#print sequences[seq]['ate']
sequences[seq]['ate_rmse'] = np.sqrt(np.dot(ate, ate) / len(ate))
sequences[seq]['ate_num'] = len(ate)
tf_matrix = evaluate_ate.compose_transform_matrix(rot, trans, scale)
for seq in sequences:
traj_trans = {t: tf_matrix_to_values(np.dot(tf_matrix, tf_values_to_matrix(p))) for t,p in sequences[seq]['traj'].items()}
sequences[seq]['traj'] = traj_trans
seq = min(sequences.keys())
sequences[seq]['oe'] = {b: angle_diff_from_quaternions(ref_traj_interpolated[a][3:], sequences[seq]['traj'][b][3:]) for a,b in matches}
aoe = np.abs(np.array(sequences[seq]['oe'].values()))
sequences[seq]['aoe_rmse'] = np.sqrt(np.dot(aoe, aoe) / len(aoe)) if len(aoe) > 0 else float('nan')
def align_traj(ref_traj, target_traj, interpolation=True):
""" Note: return positions only!!! """
from tum_evaluate_tools import associate
from tum_evaluate_tools import evaluate_ate
import numpy
if interpolation:
matches, ref_traj_interpolated = associate.associate_with_interpolation(ref_traj, target_traj, 0, 0.03)
else:
ref_traj_interpolated = ref_traj
matches = associate.associate(ref_traj_interpolated, target_traj, 0, 0.005)
first_xyz = numpy.matrix([[float(value) for value in ref_traj_interpolated[a][0:3]] for a,b in matches]).transpose()
second_xyz = numpy.matrix([[float(value) for value in target_traj[b][0:3]] for a,b in matches]).transpose()
rot,trans,trans_error = evaluate_ate.align(second_xyz, first_xyz)
ate = numpy.sqrt(numpy.dot(trans_error,trans_error) / len(trans_error))
numpy.set_printoptions(suppress=True, precision=12)
print (rot)
print (trans)
print ('ATE: %f (%d matches)' % (ate, len(matches)))
target_traj_trans = {t: (numpy.dot(rot, numpy.matrix(p[:3]).transpose()) + trans).ravel().tolist()[0] for t,p in target_traj.items()}
return target_traj_trans
def search_time_diff(ref_traj, target_traj, t_min, t_max, precision, ax_ate,
ax_traj, plot_axis):
"""
return offset, match_count, ate
"""
results = {} # format: { offset: (ate, match_count, rot, trans) }
while t_max - t_min > precision:
step = (t_max - t_min) / 4
for offset in range(t_min, t_max + step, step):
if offset not in results:
if args.interpolation:
matches, ref_traj_interpolated = associate.associate_with_interpolation(
ref_traj, target_traj, offset, 30000000)
else:
ref_traj_interpolated = ref_traj
matches = associate.associate(ref_traj_interpolated,
target_traj, offset, 5000000)
if len(matches) < 2:
print("Couldn't find matching timestamp pairs!")
t1max = max(ref_traj.keys()
) #, key=(lambda k: float(ref_traj[k])))
t1min = min(ref_traj.keys()
) #, key=(lambda k: float(ref_traj[k])))
t2max = max(target_traj.keys()
) #, key=(lambda k: float(ref_traj[k])))
t2min = min(target_traj.keys()
) #, key=(lambda k: float(ref_traj[k])))
print('traj 1: %19d -> %19d (%d msgs)' %
(t1min, t1max, len(ref_traj)))
print('traj 2: %19d -> %19d (%d msgs)' %
(t2min, t2max, len(target_traj)))
exit()
first_xyz = numpy.matrix(
[[float(value) for value in ref_traj_interpolated[a][0:3]]
for a, b in matches]).transpose()
second_xyz = numpy.matrix(
[[float(value) for value in target_traj[b][0:3]]
for a, b in matches]).transpose()
rot, trans, trans_error = evaluate_ate.align(
second_xyz, first_xyz)
ate = numpy.sqrt(
numpy.dot(trans_error, trans_error) / len(trans_error))
results[offset] = (ate, len(matches), rot, trans)
print("ATE for offset %f (ms): %f (%d matches)" %
(offset * 1e-6, ate, len(matches)))
t_optimal = sorted(results.items(), key=lambda x: x[1][0])[0][0]
offsets = sorted(results)
opt_id = offsets.index(t_optimal)
t_min = t_optimal - step * 4 if opt_id == 0 else offsets[opt_id - 1]
t_max = t_optimal + step * 4 if opt_id == len(
offsets) - 1 else offsets[opt_id + 1]
print('optimal offset range %f - %f - %f (ms)' %
(t_min * 1e-6, t_optimal * 1e-6, t_max * 1e-6))
#traj = { t : (rot * target_traj[t] + trans) for t in target_traj}
ax_ate.plot([t * 1e-6 for t in sorted(results)],
[results[offset][0] for offset in sorted(results)], '.-')
stamps = sorted(target_traj.keys())
traj = rot * numpy.matrix([[float(value) for value in target_traj[t][0:3]]
for t in stamps]).transpose() + trans
ax_traj.plot(list((numpy.array(stamps) + t_optimal) * 1e-9),
list(traj.A[plot_axis]), '-')
return t_optimal, results[t_optimal][1], results[t_optimal][0]