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))