Function save_trajectory_tum_format

eval_code/recons/relpose/evo_utils.py:432–441

(traj, filename, verbose=False)

def save_trajectory_tum_format(traj, filename, verbose=False):
    traj = make_traj(traj)
    tostr = lambda a: " ".join(map(str, a))
    with Path(filename).open("w") as f:
        for i in range(traj.num_poses):
            f.write(
                f"{traj.timestamps[i]} {tostr(traj.positions_xyz[i])} {tostr(traj.orientations_quat_wxyz[i][[0,1,2,3]])}\n"
            )
    if verbose:
        print(f"Saved trajectory to {filename}")


def extract_metrics(file_path):