velocity of ego agent

[Draupadin]

Hi, I'm trying to retrieve the velocity of the ego vehicle with the function get_ego_as_agent(). However, this only implements the controid, yaw and extend. Is there any way to get the velocity integraded into this function aswell? Or can you hint me to any other way to get the true velocity? I can see from the centroid position, that the vehicle should be moving and I could calculate the approximate speed with use of the timestamps, but I guess that won't be very exact.

Could you please help me with getting the ego speed? Please also let me know, if you need any additional info.

[thversfelt]

This is how I calculate the true (global) velocity:

from l5kit.geometry.transform import transform_point

# The timestep during this frame.
timestep = 0.1

# Get the ego reference system to world reference system transformation matrix.
world_from_ego = data_batch["world_from_agent"][scene_index].cpu().numpy()

# Get the current local position of the ego in the scene.
local_position = data_batch["history_positions"][scene_index][0].cpu().numpy()

# Transform the position to the world reference system.
position = transform_point(local_position, world_from_ego)

# Get the previous position of the ego in the scene.
previous_local_position = data_batch["history_positions"][scene_index][1].cpu().numpy()

# Transform the position to the world reference system.
previous_position = transform_point(previous_local_position, world_from_ego)

# Calculate the ego's velocity.
velocity = (position - previous_position) / timestep

Hope it helps :)

[thversfelt]

Also, if you want to get the speed from the velocity:

# The magnitude of the velocity vector is the speed.
speed = np.linalg.norm(velocity)