def plot_gt(Y_origin_data, pose_folder, preprocessed_folder, data_seqs, 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 seq in data_seqs:
        end_idx += seq_sizes[seq]
        origin_poses = np.zeros((Y_origin_data[start_idx:end_idx].shape[0], 4, 4), dtype=np.float64)
        for (idx, row) in enumerate(Y_origin_data[start_idx:end_idx]):
            new_pose = np.array(list(map(float, row.strip().split(' '))), dtype=np.float64)
            new_pose = np.concatenate((new_pose, additional_row))
            new_pose = new_pose.reshape(4, 4)
            origin_poses[idx] = new_pose
        fig = plt.figure(figsize=(10, 10))
        if (dim == '2d'):
            plt.scatter(origin_poses[:, 0, 3], origin_poses[:, 1, 3], c=origin_poses[:, 2, 3], s=20, alpha=0.5)
        else:
            ax = fig.add_subplot(111, projection='3d')
            ax.scatter(origin_poses[:, 0, 3], origin_poses[:, 1, 3], origin_poses[:, 2, 3], c=origin_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'gt_{seq}_{dim}.png'))
        start_idx += seq_sizes[seq]
def plot_gt(Y_origin_data, pose_folder, preprocessed_folder, data_seqs, 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 seq in data_seqs:
        end_idx += seq_sizes[seq]
        origin_poses = np.zeros((Y_origin_data[start_idx:end_idx].shape[0], 4, 4), dtype=np.float64)
        for (idx, row) in enumerate(Y_origin_data[start_idx:end_idx]):
            new_pose = np.array(list(map(float, row.strip().split(' '))), dtype=np.float64)
            new_pose = np.concatenate((new_pose, additional_row))
            new_pose = new_pose.reshape(4, 4)
            origin_poses[idx] = new_pose
        fig = plt.figure(figsize=(10, 10))
        if (dim == '2d'):
            plt.scatter(origin_poses[:, 0, 3], origin_poses[:, 1, 3], c=origin_poses[:, 2, 3], s=20, alpha=0.5)
        else:
            ax = fig.add_subplot(111, projection='3d')
            ax.scatter(origin_poses[:, 0, 3], origin_poses[:, 1, 3], origin_poses[:, 2, 3], c=origin_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'gt_{seq}_{dim}.png'))
        start_idx += seq_sizes[seq]