def plot_results(Y_origin_data, Y_estimated_data, data_seqs, rnn_size, seq_sizes, dim='2d', save_graph=True, dataset='KITTI'):
    start_idx = 0
    end_idx = 0
    additional_row = np.array([0, 0, 0, 1], dtype=np.float64)
    for (i, seq) in enumerate(data_seqs):
        end_idx += seq_sizes[seq]
        poses = np.zeros((Y_origin_data[start_idx:end_idx].shape[0], 4, 4), dtype=np.float64)
        for idx in range(rnn_size):
            current_pose = np.array(list(map(float, Y_origin_data[(start_idx + idx)].strip().split(' '))), dtype=np.float64)
            current_pose = np.concatenate((current_pose, additional_row))
            current_pose = current_pose.reshape(4, 4)
            poses[idx] = current_pose
        for (idx, relative_pose) in enumerate(Y_estimated_data[(start_idx - (i * rnn_size)):(end_idx - ((i + 1) * rnn_size))]):
            rot_mat = common.euler_to_rot_mat(relative_pose[5], relative_pose[4], relative_pose[3])
            trans_mat = np.identity(4)
            trans_mat[:3, :3] = rot_mat
            trans_mat[(0, 3)] = relative_pose[0]
            trans_mat[(1, 3)] = relative_pose[1]
            trans_mat[(2, 3)] = relative_pose[2]
            current_pose = np.dot(current_pose, trans_mat)
            poses[(idx + rnn_size)] = current_pose
        fig = plt.figure(figsize=(10, 10))
        if (dim == '2d'):
            plt.scatter(poses[:, 0, 3], poses[:, 1, 3], c=poses[:, 2, 3], s=20, alpha=0.5)
        else:
            ax = fig.add_subplot(111, projection='3d')
            ax.scatter(poses[:, 0, 3], poses[:, 1, 3], poses[:, 2, 3], c=poses[:, 1, 3], s=20, alpha=0.5)
        if save_graph:
            graph_folder = os.path.join('result', dataset, 'graph')
            os.makedirs(graph_folder, exist_ok=True)
            plt.savefig(os.path.join(graph_folder, f'est_{seq}_{dim}.png'))
        start_idx += seq_sizes[seq]
def plot_results(Y_origin_data, Y_estimated_data, data_seqs, rnn_size, seq_sizes, dim='2d', save_graph=True, dataset='KITTI'):
    start_idx = 0
    end_idx = 0
    additional_row = np.array([0, 0, 0, 1], dtype=np.float64)
    for (i, seq) in enumerate(data_seqs):
        end_idx += seq_sizes[seq]
        poses = np.zeros((Y_origin_data[start_idx:end_idx].shape[0], 4, 4), dtype=np.float64)
        for idx in range(rnn_size):
            current_pose = np.array(list(map(float, Y_origin_data[(start_idx + idx)].strip().split(' '))), dtype=np.float64)
            current_pose = np.concatenate((current_pose, additional_row))
            current_pose = current_pose.reshape(4, 4)
            poses[idx] = current_pose
        for (idx, relative_pose) in enumerate(Y_estimated_data[(start_idx - (i * rnn_size)):(end_idx - ((i + 1) * rnn_size))]):
            rot_mat = common.euler_to_rot_mat(relative_pose[5], relative_pose[4], relative_pose[3])
            trans_mat = np.identity(4)
            trans_mat[:3, :3] = rot_mat
            trans_mat[(0, 3)] = relative_pose[0]
            trans_mat[(1, 3)] = relative_pose[1]
            trans_mat[(2, 3)] = relative_pose[2]
            current_pose = np.dot(current_pose, trans_mat)
            poses[(idx + rnn_size)] = current_pose
        fig = plt.figure(figsize=(10, 10))
        if (dim == '2d'):
            plt.scatter(poses[:, 0, 3], poses[:, 1, 3], c=poses[:, 2, 3], s=20, alpha=0.5)
        else:
            ax = fig.add_subplot(111, projection='3d')
            ax.scatter(poses[:, 0, 3], poses[:, 1, 3], poses[:, 2, 3], c=poses[:, 1, 3], s=20, alpha=0.5)
        if save_graph:
            graph_folder = os.path.join('result', dataset, 'graph')
            os.makedirs(graph_folder, exist_ok=True)
            plt.savefig(os.path.join(graph_folder, f'est_{seq}_{dim}.png'))
        start_idx += seq_sizes[seq]