def analyse_trajectory(results_dir, n_align_frames=200, use_hand_eye_calib=True):
params = yaml.load(open(os.path.join(results_dir, "dataset_params.yaml"), "r"))
if params["dataset_is_blender"]:
analyse_synthetic_trajectory(results_dir)
elif use_hand_eye_calib:
analyse_optitrack_trajectory_with_hand_eye_calib(results_dir, params, n_align_frames)
else:
t_es, p_es, q_es, t_gt, p_gt, q_gt = load_dataset(results_dir, params["cam_delay"])
scale, rot, trans = align_trajectory.align_sim3(p_gt[0:n_align_frames, :], p_es[0:n_align_frames, :])
p_es_aligned = np.zeros(np.shape(p_es))
for i in range(np.shape(p_es)[0]):
p_es_aligned[i, :] = scale * rot.dot(p_es[i, :]) + trans
# plot position error (drift)
translation_error = p_gt - p_es_aligned
plot_translation_error(t_es, translation_error, results_dir)
def analyse_trajectory(results_dir, n_align_frames = 200, use_hand_eye_calib = True):
params = yaml.load(open(os.path.join(results_dir, 'dataset_params.yaml'),'r'))
if params['dataset_is_blender']:
analyse_synthetic_trajectory(results_dir)
elif use_hand_eye_calib:
analyse_optitrack_trajectory_with_hand_eye_calib(results_dir, params, n_align_frames)
else:
t_es, p_es, q_es, t_gt, p_gt, q_gt = load_dataset(results_dir, params['cam_delay'])
scale,rot,trans = align_trajectory.align_sim3(p_gt[0:n_align_frames,:], p_es[0:n_align_frames,:])
p_es_aligned = np.zeros(np.shape(p_es))
for i in range(np.shape(p_es)[0]):
p_es_aligned[i,:] = scale*rot.dot(p_es[i,:]) + trans
# plot position error (drift)
translation_error = (p_gt-p_es_aligned)
plot_translation_error(t_es, translation_error, results_dir)
def analyse_optitrack_trajectory_with_hand_eye_calib(results_dir, params, n_align_frames=200):
print ("loading hand-eye-calib")
T_cm_quat = np.array(
[
params["hand_eye_calib"]["Tcm_qx"],
params["hand_eye_calib"]["Tcm_qy"],
params["hand_eye_calib"]["Tcm_qz"],
params["hand_eye_calib"]["Tcm_qw"],
]
)
T_cm_tran = np.array(
[params["hand_eye_calib"]["Tcm_tx"], params["hand_eye_calib"]["Tcm_ty"], params["hand_eye_calib"]["Tcm_tz"]]
)
T_cm = get_rigid_body_trafo(T_cm_quat, T_cm_tran)
T_mc = transformations.inverse_matrix(T_cm)
t_es, p_es, q_es, t_gt, p_gt, q_gt = load_dataset(results_dir, params["cam_delay"])
# align Sim3 to get scale
print ("align Sim3 using " + str(n_align_frames) + " first frames.")
scale, rot, trans = align_trajectory.align_sim3(p_gt[0:n_align_frames, :], p_es[0:n_align_frames, :])
print "scale = " + str(scale)
# get trafo between (v)ision and (o)ptitrack frame
print q_gt[0, :]
print p_gt[0, :]
T_om = get_rigid_body_trafo(q_gt[0, :], p_gt[0, :])
T_vc = get_rigid_body_trafo(q_es[0, :], scale * p_es[0, :])
T_cv = transformations.inverse_matrix(T_vc)
T_ov = np.dot(T_om, np.dot(T_mc, T_cv))
print "T_ov = " + str(T_ov)
# apply transformation to estimated trajectory
q_es_aligned = np.zeros(np.shape(q_es))
rpy_es_aligned = np.zeros(np.shape(p_es))
rpy_gt = np.zeros(np.shape(p_es))
p_es_aligned = np.zeros(np.shape(p_es))
for i in range(np.shape(p_es)[0]):
T_vc = get_rigid_body_trafo(q_es[i, :], p_es[i, :])
T_vc[0:3, 3] *= scale
T_om = np.dot(T_ov, np.dot(T_vc, T_cm))
p_es_aligned[i, :] = T_om[0:3, 3]
q_es_aligned[i, :] = transformations.quaternion_from_matrix(T_om)
rpy_es_aligned[i, :] = transformations.euler_from_quaternion(q_es_aligned[i, :], "rzyx")
rpy_gt[i, :] = transformations.euler_from_quaternion(q_gt[i, :], "rzyx")
# plot position error (drift)
translation_error = p_gt - p_es_aligned
plot_translation_error(t_es, translation_error, results_dir)
# plot orientation error (drift)
orientation_error = rpy_gt - rpy_es_aligned
plot_rotation_error(t_es, orientation_error, results_dir)
# plot scale drift
motion_gt = np.diff(p_gt, 0)
motion_es = np.diff(p_es_aligned, 0)
dist_gt = np.sqrt(np.sum(np.multiply(motion_gt, motion_gt), 1))
dist_es = np.sqrt(np.sum(np.multiply(motion_es, motion_es), 1))
fig = plt.figure(figsize=(8, 2.5))
ax = fig.add_subplot(111, xlabel="time [s]", ylabel="scale change [\%]", xlim=[0, t_es[-1] + 4])
scale_drift = np.divide(dist_es, dist_gt) * 100 - 100
ax.plot(t_es, scale_drift, "b-")
fig.tight_layout()
fig.savefig(results_dir + "/scale_drift.pdf")
# plot trajectory
fig = plt.figure()
ax = fig.add_subplot(111, title="trajectory", aspect="equal", xlabel="x [m]", ylabel="y [m]")
ax.plot(p_es_aligned[:, 0], p_es_aligned[:, 1], "b-", label="estimate")
ax.plot(p_gt[:, 0], p_gt[:, 1], "r-", label="groundtruth")
ax.plot(p_es_aligned[0:n_align_frames, 0], p_es_aligned[0:n_align_frames, 1], "g-", linewidth=2, label="aligned")
ax.legend()
fig.tight_layout()
fig.savefig(results_dir + "/trajectory.pdf")
def analyse_optitrack_trajectory_with_hand_eye_calib(results_dir, params, n_align_frames = 200):
print('loading hand-eye-calib')
T_cm_quat = np.array([params['hand_eye_calib']['Tcm_qx'],
params['hand_eye_calib']['Tcm_qy'],
params['hand_eye_calib']['Tcm_qz'],
params['hand_eye_calib']['Tcm_qw']])
T_cm_tran = np.array([params['hand_eye_calib']['Tcm_tx'],
params['hand_eye_calib']['Tcm_ty'],
params['hand_eye_calib']['Tcm_tz']])
T_cm = get_rigid_body_trafo(T_cm_quat, T_cm_tran)
T_mc = transformations.inverse_matrix(T_cm)
t_es, p_es, q_es, t_gt, p_gt, q_gt = load_dataset(results_dir, params['cam_delay'])
# align Sim3 to get scale
print('align Sim3 using '+str(n_align_frames)+' first frames.')
scale,rot,trans = align_trajectory.align_sim3(p_gt[0:n_align_frames,:], p_es[0:n_align_frames,:])
print 'scale = '+str(scale)
# get trafo between (v)ision and (o)ptitrack frame
print q_gt[0,:]
print p_gt[0,:]
T_om = get_rigid_body_trafo(q_gt[0,:], p_gt[0,:])
T_vc = get_rigid_body_trafo(q_es[0,:], scale*p_es[0,:])
T_cv = transformations.inverse_matrix(T_vc)
T_ov = np.dot(T_om, np.dot(T_mc, T_cv))
print 'T_ov = ' + str(T_ov)
# apply transformation to estimated trajectory
q_es_aligned = np.zeros(np.shape(q_es))
rpy_es_aligned = np.zeros(np.shape(p_es))
rpy_gt = np.zeros(np.shape(p_es))
p_es_aligned = np.zeros(np.shape(p_es))
for i in range(np.shape(p_es)[0]):
T_vc = get_rigid_body_trafo(q_es[i,:],p_es[i,:])
T_vc[0:3,3] *= scale
T_om = np.dot(T_ov, np.dot(T_vc, T_cm))
p_es_aligned[i,:] = T_om[0:3,3]
q_es_aligned[i,:] = transformations.quaternion_from_matrix(T_om)
rpy_es_aligned[i,:] = transformations.euler_from_quaternion(q_es_aligned[i,:], 'rzyx')
rpy_gt[i,:] = transformations.euler_from_quaternion(q_gt[i,:], 'rzyx')
# plot position error (drift)
translation_error = (p_gt-p_es_aligned)
plot_translation_error(t_es, translation_error, results_dir)
# plot orientation error (drift)
orientation_error = (rpy_gt - rpy_es_aligned)
plot_rotation_error(t_es, orientation_error, results_dir)
# plot scale drift
motion_gt = np.diff(p_gt, 0)
motion_es = np.diff(p_es_aligned, 0)
dist_gt = np.sqrt(np.sum(np.multiply(motion_gt,motion_gt),1))
dist_es = np.sqrt(np.sum(np.multiply(motion_es,motion_es),1))
fig = plt.figure(figsize=(8,2.5))
ax = fig.add_subplot(111, xlabel='time [s]', ylabel='scale change [\%]', xlim=[0,t_es[-1]+4])
scale_drift = np.divide(dist_es,dist_gt)*100-100
ax.plot(t_es, scale_drift, 'b-')
fig.tight_layout()
fig.savefig(results_dir+'/scale_drift.pdf')
# plot trajectory
fig = plt.figure()
ax = fig.add_subplot(111, title='trajectory', aspect='equal', xlabel='x [m]', ylabel='y [m]')
ax.plot(p_es_aligned[:,0], p_es_aligned[:,1], 'b-', label='estimate')
ax.plot(p_gt[:,0], p_gt[:,1], 'r-', label='groundtruth')
ax.plot(p_es_aligned[0:n_align_frames,0], p_es_aligned[0:n_align_frames,1], 'g-', linewidth=2, label='aligned')
ax.legend()
fig.tight_layout()
fig.savefig(results_dir+'/trajectory.pdf')
data_gt = associate.read_file_list(traj_groundtruth)
data_es = associate.read_file_list(traj_estimate)
# select matches
offset = -params['cam_delay']
print ('offset = '+str(offset))
matches = associate.associate(data_gt, data_es, offset, 0.02)
#matches = matches[500:]
p_gt = np.array([[float(value) for value in data_gt[a][0:3]] for a,b in matches])
q_gt = np.array([[float(value) for value in data_gt[a][3:7]] for a,b in matches])
p_es = np.array([[float(value) for value in data_es[b][0:3]] for a,b in matches])
q_es = np.array([[float(value) for value in data_es[b][3:7]] for a,b in matches])
# --------------------------------------------------------------------------------
# align Sim3 to get scale
scale,rot,trans = align_trajectory.align_sim3(p_gt[0:n_align_sim3,:], p_es[0:n_align_sim3,:])
#model_aligned = s * R * model + t
#alignment_error = model_aligned - data
#t_error = np.sqrt(np.sum(np.multiply(alignment_error,alignment_error),0)).A[0]
p_es_aligned = np.transpose(scale*np.dot(rot,np.transpose(p_es)))+trans
p_es = scale*p_es
print 's='+str(scale)
print 't='+str(trans)
print 'R='+str(rot)
# plot sim3 aligned trajectory
fig = plt.figure()
