plot_traj_open3d.py

def plot_traj_open3d(ref: PoseTrajectory3D, est: PoseTrajectory3D, axis_len=0.2, skip=5):
    geometries = []

    # Reference trajectory (blue frames)
    for i, T in enumerate(ref.poses_se3):
        if i % skip != 0:
            continue
        mesh = o3d.geometry.TriangleMesh.create_coordinate_frame(size=axis_len)
        mesh.transform(T)
        geometries.append(mesh)

    # Estimated trajectory (orange frames)
    for i, T in enumerate(est.poses_se3):
        if i % skip != 0:
            continue
        mesh = o3d.geometry.TriangleMesh.create_coordinate_frame(size=axis_len)        
        mesh.transform(T)
        geometries.append(mesh)

    o3d.visualization.draw_geometries(geometries)