def test_find_all_polygon_bboxes_overlapping_query_bbox(
polygons_and_gt_bboxes):
"""Test for correctness of """
poly_bboxes = np.array(
[compute_point_cloud_bbox(poly) for poly in polygons_and_gt_bboxes[0]])
query_bbox = np.array([-1.5, 0.5, 1.5, 1.5])
overlap_indxs = find_all_polygon_bboxes_overlapping_query_bbox(
poly_bboxes, query_bbox)
gt_overlap_bool = np.array([True, True, False, True, True])
gt_overlap_indxs = np.where(gt_overlap_bool)[0]
assert np.allclose(overlap_indxs, gt_overlap_indxs)
def test_find_all_polygon_bboxes_overlapping_query_bbox(
polygons_and_gt_bboxes: Tuple[List[np.ndarray],
List[np.ndarray]]) -> None:
"""Test for correctness of finding polygons which overlap with the query bbox."""
poly_bboxes = np.array(
[compute_point_cloud_bbox(poly) for poly in polygons_and_gt_bboxes[0]])
query_bbox = np.array([-1.5, 0.5, 1.5, 1.5])
overlap_indxs = find_all_polygon_bboxes_overlapping_query_bbox(
poly_bboxes, query_bbox)
gt_overlap_bool = np.array([True, True, False, True, True])
gt_overlap_indxs = np.where(gt_overlap_bool)[0]
assert np.allclose(overlap_indxs, gt_overlap_indxs)
def get_lane_ids_in_xy_bbox(
self,
query_x: float,
query_y: float,
city_name: str,
query_search_range_manhattan: float = 5.0,
) -> List[int]:
"""
Prune away all lane segments based on Manhattan distance. We vectorize this instead
of using a for-loop. Get all lane IDs within a bounding box in the xy plane.
This is a approximation of a bubble search for point-to-polygon distance.
The bounding boxes of small point clouds (lane centerline waypoints) are precomputed in the map.
We then can perform an efficient search based on manhattan distance search radius from a
given 2D query point.
We pre-assign lane segment IDs to indices inside a big lookup array, with precomputed
hallucinated lane polygon extents.
Args:
query_x: representing x coordinate of xy query location
query_y: representing y coordinate of xy query location
city_name: either 'MIA' for Miami or 'PIT' for Pittsburgh
query_search_range_manhattan: search radius along axes
Returns:
lane_ids: lane segment IDs that live within a bubble
"""
query_min_x = query_x - query_search_range_manhattan
query_max_x = query_x + query_search_range_manhattan
query_min_y = query_y - query_search_range_manhattan
query_max_y = query_y + query_search_range_manhattan
overlap_indxs = find_all_polygon_bboxes_overlapping_query_bbox(
self.city_halluc_bbox_table[city_name],
np.array([query_min_x, query_min_y, query_max_x, query_max_y]),
)
if len(overlap_indxs) == 0:
return []
neighborhood_lane_ids: List[int] = []
for overlap_idx in overlap_indxs:
lane_segment_id = self.city_halluc_tableidx_to_laneid_map[
city_name][str(overlap_idx)]
neighborhood_lane_ids.append(lane_segment_id)
return neighborhood_lane_ids
