Transform rpy in carla.Rotation to rotation matrix in np.array :param carla_rotation: carla.Rotation :return: np.array rotation matrix
(carla_rotation)
| 51 | |
| 52 | |
| 53 | def carla_rot_to_mat(carla_rotation): |
| 54 | """ |
| 55 | Transform rpy in carla.Rotation to rotation matrix in np.array |
| 56 | |
| 57 | :param carla_rotation: carla.Rotation |
| 58 | :return: np.array rotation matrix |
| 59 | """ |
| 60 | roll = np.deg2rad(carla_rotation.roll) |
| 61 | pitch = np.deg2rad(carla_rotation.pitch) |
| 62 | yaw = np.deg2rad(carla_rotation.yaw) |
| 63 | |
| 64 | yaw_matrix = np.array([ |
| 65 | [np.cos(yaw), -np.sin(yaw), 0], |
| 66 | [np.sin(yaw), np.cos(yaw), 0], |
| 67 | [0, 0, 1] |
| 68 | ]) |
| 69 | pitch_matrix = np.array([ |
| 70 | [np.cos(pitch), 0, -np.sin(pitch)], |
| 71 | [0, 1, 0], |
| 72 | [np.sin(pitch), 0, np.cos(pitch)] |
| 73 | ]) |
| 74 | roll_matrix = np.array([ |
| 75 | [1, 0, 0], |
| 76 | [0, np.cos(roll), np.sin(roll)], |
| 77 | [0, -np.sin(roll), np.cos(roll)] |
| 78 | ]) |
| 79 | |
| 80 | rotation_matrix = yaw_matrix.dot(pitch_matrix).dot(roll_matrix) |
| 81 | return rotation_matrix |
| 82 | |
| 83 | def get_loc_rot_vel_in_ev(actor_list, ev_transform, get_acceleration = False, origin = False): |
| 84 | location, rotation, absolute_velocity = [], [], [] |
no test coverage detected