def plotSequenceRelative(expDir, seq, seqLength, trajectory, dataDir, cmd, epoch): T = np.eye(4); estimatedCameraTraj = np.empty([seqLength,3]) # Extract the camera centres from all the frames # First frame as the world origin estimatedCameraTraj[0] = np.zeros([1,3]); for frame in range(seqLength-1): # Output is pose of frame i+1 with respect to frame i relativePose = trajectory[frame,:] if cmd.outputParameterization == 'default': R = axisAngle_to_rotMat(np.transpose(relativePose[:3])) t = np.reshape(relativePose[3:],(3,1)) elif cmd.outputParameterization == 'quaternion': R = quat_to_rotMat(relativePose[:4]) t = np.reshape(relativePose[4:],(3,1)) elif cmd.outputParameterization == 'euler': R = np.asarray(euler_to_rotMat(x = relativePose[0], y = relativePose[1], \ z = relativePose[3], seq = 'xyz'), dtype = np.float32) t = (np.asarray(relativePose[3:], dtype = np.float32)).reshape((3,1)) T_r = np.concatenate( ( np.concatenate([R,t],axis=1) , [[0.0,0.0,0.0,1.0]] ) , axis = 0 ) # With respect to the first frame T_abs = np.dot(T,T_r); # Update the T matrix till now. T = T_abs # Get the origin of the frame (i+1), ie the camera center estimatedCameraTraj[frame+1] = np.transpose(T[0:3,3]) # Get the ground truth camera trajectory gtCameraTraj = getGroundTruthTrajectory(seq, seqLength, dataDir); # Plot the estimated and groundtruth trajectories x_gt = gtCameraTraj[:,0] z_gt = gtCameraTraj[:,2] x_est = estimatedCameraTraj[:,0] z_est = estimatedCameraTraj[:,2] # Save plot # path = os.path.join(expDir, 'plots', 'traj', str(seq).zfill(2)) # currNumPlots = len(glob.glob1(path,"*.png")) # assert (currNumPlots%2==0) fig,ax = plt.subplots(1) ax.plot(x_gt,z_gt, 'c', label = "ground truth") ax.plot(x_est,z_est, 'm', label= "estimated") ax.legend() # fig.savefig(path + "/" + str(currNumPlots/2 + 1)) fig.savefig(os.path.join(expDir, 'plots', 'traj', str(seq).zfill(2), 'traj_' + str(epoch).zfill(3)))
def plotSequenceAbsolute(expDir, seq, seqLength, trajectory, dataDir, cmd, epoch): T = np.eye(4) estimatedCameraTraj = np.empty([seqLength, 3]) # Extract the camera centres from all the frames for frame in range(seqLength - 1): # Output is pose of frame i+1 with respect to frame i absolutePose = trajectory[frame, :] if cmd.outputParameterization == 'default': R = axisAngle_to_rotMat(np.transpose(absolutePose[:3])) t = np.reshape(absolutePose[3:], (3, 1)) elif cmd.outputParameterization == 'quaternion': R = quat_to_rotMat(absolutePose[:4]) t = np.reshape(absolutePose[4:], (3, 1)) elif cmd.outputParameterization == 'euler': R = np.asarray(euler_to_rotMat(x = absolutePose[0], y = absolutePose[1], \ z = absolutePose[3], seq = 'xyz'), dtype = np.float32) t = (np.asarray(absolutePose[3:], dtype=np.float32)).reshape( (3, 1)) T_r = np.concatenate( (np.concatenate([R, t], axis=1), [[0.0, 0.0, 0.0, 1.0]]), axis=0) # Get the origin of the frame (i+1), ie the camera center estimatedCameraTraj[frame + 1] = np.transpose(T_r[0:3, 3]) # Get the ground truth camera trajectory gtCameraTraj = getGroundTruthTrajectory(seq, seqLength, dataDir) # Plot the estimated and groundtruth trajectories x_gt = gtCameraTraj[:, 0] z_gt = gtCameraTraj[:, 2] x_est = estimatedCameraTraj[:, 0] z_est = estimatedCameraTraj[:, 2] # assert (currNumPlots%2==0) fig, ax = plt.subplots(1) ax.plot(x_gt, z_gt, 'c', label="ground truth") ax.plot(x_est, z_est, 'm', label="estimated") ax.legend() fig.savefig( os.path.join(expDir, 'plots', 'traj', str(seq).zfill(2), 'traj_' + str(epoch).zfill(3)))