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")
ax.plot(p_gt[:,0], 'r-')
ax.plot(p_gt[:,1], 'g-')
ax.plot(p_gt[:,2], 'b-')
ax.plot(p_es_aligned[:,0], 'r--')
ax.plot(p_es_aligned[:,1], 'g--')
ax.plot(p_es_aligned[:,2], 'b--')
# --------------------------------------------------------------------------------
# hand-eye-calib
# select random measurements
I = np.array(np.random.rand(n_measurements,1)*(np.shape(matches)[0]-delta), dtype=int)[:,0]
R,b = align_trajectory.hand_eye_calib(q_gt, q_es, p_gt, p_es, I, delta, True)
print 'quat = ' + str(transformations.quaternion_from_matrix(transformations.convert_3x3_to_4x4(R)))
print 'b = ' + str(b)
rpy_es = np.zeros([q_es.shape[0]-1, 3])
rpy_gt = np.zeros([q_gt.shape[0]-1, 3])
t_gt = np.zeros([q_es.shape[0]-1,3])
t_es = np.zeros([q_es.shape[0]-1,3])
for i in range(delta,np.shape(q_es)[0]):
A1 = transformations.quaternion_matrix(q_es[i-delta,:])[:3,:3]
A2 = transformations.quaternion_matrix(q_es[i,:])[:3,:3]
A = np.dot(A1.transpose(), A2)
B1 = transformations.quaternion_matrix(q_gt[i-delta,:])[:3,:3]
B2 = transformations.quaternion_matrix(q_gt[i,:])[:3,:3]
B = np.dot(B1.transpose(), B2)
B_es = np.dot(np.transpose(R), np.dot(A, R))
