Yaw from Quaternion

Duli · August 14, 2019, 6:33am

Hi,

How would one calculate the yaw angle of the vehicle using its quaternion?

holger-motional · August 10, 2019, 3:47am

Please use this function: https://github.com/nutonomy/nuscenes-devkit/blob/master/python-sdk/nuscenes/eval/detection/utils.py#L162

chid_bot · May 14, 2021, 6:30am

Hello,

This link doesn’t provide the required function. It only includes “detection_name_to_rel_attributes” and “category_to_detection_name” functions.
Can you please check the link again?

Thanks and regards,

holger-motional · May 14, 2021, 10:58am

Apologies for the wrong link. Try:

github.com

nutonomy/nuscenes-devkit/blob/9b209638ef3dee6d0cdc5ac700c493747f5b35fe/python-sdk/nuscenes/eval/common/utils.py#L112


    min_wlh = np.minimum(sa_size, sr_size)
    volume_annotation = np.prod(sa_size)
    volume_result = np.prod(sr_size)
    intersection = np.prod(min_wlh)  # type: float
    union = volume_annotation + volume_result - intersection  # type: float
    iou = intersection / union


    return iou




def quaternion_yaw(q: Quaternion) -> float:
    """
    Calculate the yaw angle from a quaternion.
    Note that this only works for a quaternion that represents a box in lidar or global coordinate frame.
    It does not work for a box in the camera frame.
    :param q: Quaternion of interest.
    :return: Yaw angle in radians.
    """


    # Project into xy plane.
    v = np.dot(q.rotation_matrix, np.array([1, 0, 0]))

chid_bot · May 18, 2021, 7:28am

Thank you for the link. I was able to write my own function in the mean time. And if there is someone who wishes to get the yaw angle (heading angle) from a quaternion directly then please use the below function,

```
   import numpy as np
```
```
   quarternion = [w, x, y, z]
```

   numerator = 2 * (quaternion[0]*quaternion[3]+quaternion[1]*quaternion[2])

   denominator = 1 - (2*(np.square(quaternion[2])+np.square(quaternion[3])))

   yaw_angle = np.arctan2(numerator, denominator)

The yaw_angle will be between [-180, +180] radians

Please refer to Wiki link for the math
https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles