How can we know agent's angle in a top-view map?

I’m trying to draw agent’s trajectory and the final position&rotation, just like this:

I understand that sim.get_agent_state().rotation gives its rotation in quaternion. But still struggling with converting that to an angle in top-view map.

Is there any utility that does this conversion in this repository?

[mathfac]

In your TASK config specify TOP_DOWN_MAP measure and HEADING_SENSOR sensor. Here is an example of creating the video with top down map:

config=habitat.get_config("./configs/tasks/pointnav.yaml")
config.defrost()
config.TASK.SENSORS.append('HEADING_SENSOR')
config.TASK.MEASUREMENTS.append('TOP_DOWN_MAP')
config.freeze()
env = habitat.Env(config)
observations = env.reset()
images = []
while not env.episode_over:
     observations = env.step(env.action_space.sample())
     im = observations["rgb"]
     top_down_map = [draw_top_down_map](https://github.com/facebookresearch/habitat-api/blob/61af1b666bf082000280e407870fd42aa73da6a9/examples/shortest_path_follower_example.py#L37)(
         env.get_metrics(), observations["heading"], im.shape[0]
     )
     output_im = np.concatenate((im, top_down_map), axis=1)
     images.append(output_im)
habitat.utils.visualizations.utils.images_to_video(images, "./", "trajectory")

It doesn’t include trajectory.

[erikwijmans]

There isn’t one that I know of, but

from habitat.tasks.utils import quaternion_rotate_vector, cartesian_to_polar

heading_vector = quaternion_rotate_vector(
    agent_state.rotation.inverse(), np.array([0, 0, -1])
)

phi = cartesian_to_polar(-heading_vector[2], heading_vector[0])[1]
top_down_map_angle = phi - np.pi / 2

should do it

EDIT: @mathfac has a better solution bellow 😃