def _transform_with_convert(
self, transform: Transform,
transformer: skimage.transform.AffineTransform):
position = transform.position.as_numpy()[:2]
position = position.reshape(-1, 2)
orientation = transform.orientation.as_numpy()
orientation = orientation.reshape(-1, 2)
position, orientation = self.transform(position, orientation,
transformer)
position = Vector2.from_numpy(position.squeeze()).to_vector3(0)
orientation = Vector2.from_numpy(orientation.squeeze())
return Transform(position, orientation)
def ngsim_to_carla(self, ngsim_transform: Transform, z: float, rear_axle_offset: float) -> Transform:
p = ngsim_transform.position.as_vector2()
p = self._transformation_matrix @ np.array([p.x, p.y, 1])
p = Vector2.from_numpy(p)
direction = Vector2.from_yaw_radian(ngsim_transform.orientation.yaw_radians - self._rotation)
p -= direction * rear_axle_offset # in ngsim origin point is in the center of rear axle
return Transform(p.to_vector3(z), direction)
def __init__(self, ngsim_origin: np.ndarray, carla_origin: np.ndarray):
assert ngsim_origin.shape == (2,), "Expected 2d point"
assert carla_origin.shape == (2,), "Expected 2d point"
ngsim_base_vectors = np.array([[1, 0], [0, 1]])
scale = PIXELS_TO_METERS
carla_base_vectors = np.array([[1, 0], [0, 1]]) * scale
self._transformation_matrix = cv2.getAffineTransform(
src=np.float32([ngsim_origin, *(ngsim_origin + ngsim_base_vectors)]),
dst=np.float32([carla_origin, *(carla_origin + carla_base_vectors)]),
)
NO_TRANSLATION = 0
self._rotation = Vector2.from_numpy(
self._transformation_matrix @ np.array([1, 0, NO_TRANSLATION])
).yaw_radians
def get_velocity(self) -> Vector2:
direction = Vector2.from_numpy(self._direction)
return direction * self._speed
def get_transform(self) -> Transform:
return Transform(
Vector2.from_numpy(self.front).to_vector3(0),
Vector2.from_numpy(self._direction),
)