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)