File size: 2,314 Bytes
663494c |
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 |
"""Utility functions related to object rotation and orientation computation"""
import math
from typing import List
import numpy as np
import pyquaternion
from numba import jit
def get_yaw_vector(rotation: np.ndarray) -> np.ndarray:
"""Convert yaw from rotation to a vector"""
yaw_deg = rotation[1]
yaw_rad = np.radians(yaw_deg)
return np.array([np.cos(yaw_rad), np.sin(yaw_rad)])
def forward_vec2rot(forward_vector: np.ndarray) -> np.ndarray:
"""Convert a forward vector to rotation vector in degree
Note: only yaw is considered now
Args
forward_vector: (3, ) or (2, ) with z ignored
"""
yaw: float = np.rad2deg(math.atan2(forward_vector[1], forward_vector[0]))
return np.array([0, yaw, 0])
def compute_heading_diff(yaw_vec1: np.ndarray, yaw_vec2: np.ndarray) -> float:
"""Compute the difference of two headings, e.g., ego and obj
Returns:
heading_diff_deg: float in degree, range [-180, 180] degree
"""
# make sure that the input dot product is within [-1, 1]
dot_product: float = yaw_vec1.dot(yaw_vec2)
dot_product: float = min(1.0, max(-1.0, dot_product))
heading_diff_rad = np.arccos(dot_product)
# convert to degrees within [-180, 180]
heading_diff_deg = np.degrees(heading_diff_rad)
heading_diff_deg = min(heading_diff_deg, 360.0 - heading_diff_deg)
return heading_diff_deg
def convert_mat2qua(rotation: np.ndarray) -> np.ndarray:
"""Convert a 3x3 rotation matrix to a (4, ) np array for quaternion"""
rotation: np.ndarray = pyquaternion.Quaternion(matrix=rotation).elements # (4, )
return rotation
# @jit
def rotx(t):
"""Rotation about the x-axis."""
c = np.cos(t)
s = np.sin(t)
return np.array([[1, 0, 0], [0, c, -s], [0, s, c]])
# @jit
def roty(t):
"""Rotation about the y-axis."""
c = np.cos(t)
s = np.sin(t)
return np.array([[c, 0, s], [0, 1, 0], [-s, 0, c]])
# @jit
def rotz(t):
"""Rotation about the z-axis."""
c = np.cos(t)
s = np.sin(t)
return np.array([[c, -s, 0], [s, c, 0], [0, 0, 1]])
# @jit
def transform_from_rot_trans(R, t):
"""Transforation matrix from rotation matrix and translation vector."""
R = R.reshape(3, 3)
t = t.reshape(3, 1)
return np.vstack((np.hstack([R, t]), [0, 0, 0, 1]))
|