def in_angular_distance(yaw1: np.ndarray, yaw2: np.ndarray, th: float) -> bool:
"""
Check if the absolute distance in degrees is under the given threshold
"""
abs_angular_distance_degrees = abs(angular_distance(float(yaw2), float(yaw1))) * 180 / np.pi
return bool(abs_angular_distance_degrees < th)
def residuals(xyrv: np.ndarray) -> np.ndarray:
x, y, r, v = np.split(xyrv, 4)
x1, x2 = x[0:N - 1], x[1:N]
y1, y2 = y[0:N - 1], y[1:N]
r1, r2 = r[0:N - 1], r[1:N]
v1, v2 = v[0:N - 1], v[1:N]
return w * np.hstack([
x - gx,
y - gy,
angular_distance(r, gr),
v - gv,
np.append(x1 + np.cos(r1) * v1 - x2, 0),
np.append(y1 + np.sin(r1) * v1 - y2, 0),
np.append(angular_distance(r1, r2), 0),
np.append(v1 - v2, 0),
])
def residuals(steer_acc: np.ndarray) -> np.ndarray:
x, y, r, v = control2position(steer_acc)
return np.hstack([
wgx * (x - gx),
wgy * (y - gy),
wgr * angular_distance(r, gr),
wgv * (v - gv),
wsteer_acc * steer_acc,
])
def _create_targets_for_deep_prediction(
num_frames: int,
frames: np.ndarray,
selected_track_id:
int, # modified that ego car is -1, not None (None is used in l5kit)
agents: List[np.ndarray],
agent_from_world: np.ndarray,
current_agent_yaw: float,
agent_origin: np.ndarray,
) -> Tuple[np.ndarray, np.ndarray, np.ndarray]:
"""
Internal function that creates the targets and availability masks for deep prediction-type models.
The futures/history offset (in meters) are computed. When no info is available (e.g. agent not in frame)
a 0 is set in the availability array (1 otherwise).
Args:
num_frames (int): number of offset we want in the future/history
frames (np.ndarray): available frames. This may be less than num_frames
selected_track_id (Optional[int]): agent track_id or AV (-1)
agents (List[np.ndarray]): list of agents arrays (same len of frames)
agent_from_world (np.ndarray): local from world matrix
current_agent_yaw (float): angle of the agent at timestep 0
agent_origin (np.ndarray): (2,) xy coord
Returns:
Tuple[np.ndarray, np.ndarray, np.ndarray]: position offsets, angle offsets, availabilities
"""
# How much the coordinates differ from the current state in meters.
coords_offset = np.zeros((num_frames, 2), dtype=np.float32)
yaws_offset = np.zeros((num_frames, 1), dtype=np.float32)
availability = np.zeros((num_frames, ), dtype=np.float32)
for i, (frame, agents) in enumerate(zip(frames, agents)):
if selected_track_id == -1:
agent_centroid = frame["ego_translation"][:2]
agent_yaw = rotation33_as_yaw(frame["ego_rotation"])
else:
# it's not guaranteed the target will be in every frame
try:
agent = filter_agents_by_track_id(agents, selected_track_id)[0]
except IndexError:
availability[i] = 0.0 # keep track of invalid futures/history
continue
agent_centroid = agent["centroid"]
agent_yaw = agent["yaw"]
coords_offset[i] = transform_point(agent_centroid,
agent_from_world) - agent_origin
yaws_offset[i] = angular_distance(agent_yaw, current_agent_yaw)
availability[i] = 1.0
return coords_offset, yaws_offset, availability
def test_angular_distance() -> None:
# test easy cases
assert 30.0 == pytest.approx(degrees(angular_distance(radians(30.0), 0)), 1e-3)
assert -30.0 == pytest.approx(degrees(angular_distance(0, radians(30.0))), 1e-3)
assert 90.0 == pytest.approx(degrees(angular_distance(radians(90), 0)), 1e-3)
assert -90.0 == pytest.approx(degrees(angular_distance(0, radians(90.0))), 1e-3)
# test overflowing cases
assert 0.0 == pytest.approx(degrees(angular_distance(radians(180.0), radians(-180))), 1e-3)
assert 0.0 == pytest.approx(degrees(angular_distance(radians(180.0), radians(-180))), 1e-3)
# note this may seem counter-intuitive, but 170 - (-20) is in fact -170
assert -20 == pytest.approx(degrees(angular_distance(radians(170.0), radians(-170))), 1e-3)
# in the same way, -170 - 20 yields 170
assert 20 == pytest.approx(degrees(angular_distance(radians(-170.0), radians(170))), 1e-3)
assert -120.0 == pytest.approx(degrees(angular_distance(radians(150.0), radians(-90))), 1e-3)
assert 120.0 == pytest.approx(degrees(angular_distance(radians(-90.0), radians(150))), 1e-3)
# test > np.pi cases
assert -120.0 == pytest.approx(degrees(angular_distance(radians(150.0 + 360), radians(-90 - 360))), 1e-3)
assert 120.0 == pytest.approx(degrees(angular_distance(radians(-90 - 360), radians(150 + 360))), 1e-3)
assert -20 == pytest.approx(degrees(angular_distance(radians(170.0), radians(-170 - 3 * 360))), 1e-3)
assert -20 == pytest.approx(degrees(angular_distance(radians(170.0), radians(-170 + 3 * 360))), 1e-3)
assert -20 == pytest.approx(degrees(angular_distance(radians(170.0 + 5 * 360), radians(-170))), 1e-3)
assert -20 == pytest.approx(degrees(angular_distance(radians(170.0 - 5 * 360), radians(-170))), 1e-3)
