def get_lane_segments_containing_xy(self, query_x: float, query_y: float,
city_name: str) -> List[int]:
"""
Get the occupied lane ids, i.e. given (x,y), list those lane IDs whose hallucinated
lane polygon contains this (x,y) query point.
This function performs a "point-in-polygon" test.
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
Returns:
occupied_lane_ids: list of integers, representing lane segment IDs containing (x,y)
"""
neighborhood_lane_ids = self.get_lane_ids_in_xy_bbox(
query_x, query_y, city_name)
occupied_lane_ids: List[int] = []
if neighborhood_lane_ids is not None:
for lane_id in neighborhood_lane_ids:
lane_polygon = self.get_lane_segment_polygon(
lane_id, city_name)
inside = point_inside_polygon(lane_polygon.shape[0],
lane_polygon[:,
0], lane_polygon[:,
1],
query_x, query_y)
if inside:
occupied_lane_ids += [lane_id]
return occupied_lane_ids
def verify_manhattan_search_functionality():
"""
Minimal example where we
"""
adm = ArgoverseMap()
# query_x = 254.
# query_y = 1778.
ref_query_x = 422.0
ref_query_y = 1005.0
city_name = "PIT" # 'MIA'
for trial_idx in range(10):
query_x = ref_query_x + (np.random.rand() - 0.5) * 10
query_y = ref_query_y + (np.random.rand() - 0.5) * 10
# query_x,query_y = (3092.49845414,1798.55426805)
query_x, query_y = (3112.80160113, 1817.07585338)
lane_segment_ids = avm.get_lane_ids_in_xy_bbox(query_x, query_y,
city_name, 5000)
fig = plt.figure(figsize=(22.5, 8))
ax = fig.add_subplot(111)
# ax.scatter([query_x], [query_y], 500, color='k', marker='.')
plot_lane_segment_patch(pittsburgh_bounds, ax, color="m", alpha=0.1)
if len(lane_segment_ids) > 0:
for i, lane_segment_id in enumerate(lane_segment_ids):
patch_color = "y" # patch_colors[i % 4]
lane_centerline = avm.get_lane_segment_centerline(
lane_segment_id, city_name)
test_x, test_y = lane_centerline.mean(axis=0)
inside = point_inside_polygon(n_poly_vertices,
pittsburgh_bounds[:, 0],
pittsburgh_bounds[:, 1], test_x,
test_y)
if inside:
halluc_lane_polygon = avm.get_lane_segment_polygon(
lane_segment_id, city_name)
xmin, ymin, xmax, ymax = find_lane_segment_bounds_in_table(
adm, city_name, lane_segment_id)
add_lane_segment_to_ax(ax, lane_centerline,
halluc_lane_polygon, patch_color,
xmin, xmax, ymin, ymax)
ax.axis("equal")
plt.show()
datetime_str = generate_datetime_string()
plt.savefig(f"{trial_idx}_{datetime_str}.jpg")
plt.close("all")
def test_point_in_polygon_square() -> None:
"""
Ensure point barely inside square boundary is "inside".
"""
square = np.array([[2, 2], [2, -2], [-2, -2], [-2, 2]])
n_vertices = 4
poly_x_pts = square[:, 0]
poly_y_pts = square[:, 1]
test_x = 0
test_y = 1.99999
inside = point_inside_polygon(n_vertices, poly_x_pts, poly_y_pts, test_x,
test_y)
assert inside
assert inside == point_inside_polygon_interior_sanity_check(
n_vertices, poly_x_pts, poly_y_pts, test_x, test_y)
def test_point_in_polygon_shapely_vs_our_implementation() -> None:
"""
Using 20 points originally sampled in unit square (uniform random),
ensure that our implementation of point-in-polygon matches
shapely.geometry's implementation. Using non-convex polygon.
"""
poly_boundary = [(0, 0), (1, 2), (3, 2), (4, 0), (2, 1), (0, 0)]
n_vertices = len(poly_boundary) - 1
vert_x_pts, vert_y_pts = zip(*poly_boundary)
# skip the last vertex since it is repeating the zero'th vertex
vert_x_pts = np.array(vert_x_pts)[:-1]
vert_y_pts = np.array(vert_y_pts)[:-1]
test_pts = np.array([
[0.7906206529554805, 3.5247972478750755],
[2.796929714397516, 1.2869605710087773],
[2.13736772658553, 0.9354300802539841],
[0.5533843176857371, 3.5907793820406786],
[1.067522648741737, 2.107324516856971],
[0.18413436334503475, 1.999149713775295],
[1.021532057710976, 1.8409888453463181],
[1.5852780307128183, 3.3784534320811512],
[1.808454845296604, 2.2902639353983276],
[2.3004402727971534, 2.764413577543416],
[0.22177110806299005, 3.3753677368156096],
[0.7884126834996188, 3.105915665452909],
[0.20123469298750463, 2.735888039565455],
[2.485042055570448, 2.160836673437083],
[3.2481703626785148, 0.8704179207883942],
[3.884111027227804, 1.951093430056864],
[1.4627990780181208, 3.0282856766333315],
[1.5737052681266528, 1.5394901616575405],
[3.9923222020570623, 0.05764955338346489],
[0.19501433730945195, 3.0991165694473595],
])
gt_is_inside = np.array([
False,
True,
True,
False,
False,
False,
True,
False,
False,
False,
False,
False,
False,
False,
True,
False,
False,
True,
False,
False,
])
is_inside = np.zeros(test_pts.shape[0], dtype=bool)
for i in range(20):
test_x = test_pts[i, 0]
test_y = test_pts[i, 1]
inside = point_inside_polygon(n_vertices, vert_x_pts, vert_y_pts,
test_x, test_y)
assert inside == point_inside_polygon_interior_sanity_check(
n_vertices, vert_x_pts, vert_y_pts, test_x, test_y)
