Пример #1
0
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')
Пример #2
0
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")
Пример #3
0
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))