Returns the position of the vehicle matrix in the ego coordinate system. :param ego_matrix: ndarray 4x4 Matrix of the ego vehicle in global coordinates :param vehicle_matrix: ndarray 4x4 Matrix of another actor in global coordinates :return: ndarray position of the other veh
(ego_matrix, vehicle_matrix)
| 158 | return normalized * 1000 |
| 159 | |
| 160 | def get_relative_transform(ego_matrix, vehicle_matrix): |
| 161 | """ |
| 162 | Returns the position of the vehicle matrix in the ego coordinate system. |
| 163 | :param ego_matrix: ndarray 4x4 Matrix of the ego vehicle in global |
| 164 | coordinates |
| 165 | :param vehicle_matrix: ndarray 4x4 Matrix of another actor in global |
| 166 | coordinates |
| 167 | :return: ndarray position of the other vehicle in the ego coordinate system |
| 168 | """ |
| 169 | relative_pos = vehicle_matrix[:3, 3] - ego_matrix[:3, 3] |
| 170 | rot = ego_matrix[:3, :3].T |
| 171 | relative_pos = rot @ relative_pos |
| 172 | |
| 173 | return relative_pos |
| 174 | |
| 175 | def normalize_angle(x): |
| 176 | x = x % (2 * np.pi) # force in range [0, 2 pi) |