def get_lane_direction(pos, city, am):
if am is None:
from argoverse.map_representation.map_api import ArgoverseMap
am = ArgoverseMap()
else:
pass
drct_conf = np.array([np.append(*am.get_lane_direction(p[:2], city)) for p in pos])
return drct_conf
def get_batch_lane_direction(pos, city, am):
if am is None:
from argoverse.map_representation.map_api import ArgoverseMap
am = ArgoverseMap()
else:
pass
drct_conf = list()
for ps, c in zip(pos, city):
drct_conf.append(np.array([np.append(*am.get_lane_direction(p[:2], c)) for p in ps]))
return drct_conf
def render_bev_labels_mpl(
self,
city_name: str,
ax: plt.Axes,
axis: str,
lidar_pts: np.ndarray,
local_lane_polygons: np.ndarray,
local_das: np.ndarray,
log_id: str,
timestamp: int,
city_to_egovehicle_se3: SE3,
avm: ArgoverseMap,
) -> None:
"""Plot nearby lane polygons and nearby driveable areas (da) on the Matplotlib axes.
Args:
city_name: The name of a city, e.g. `"PIT"`
ax: Matplotlib axes
axis: string, either 'ego_axis' or 'city_axis' to demonstrate the
lidar_pts: Numpy array of shape (N,3)
local_lane_polygons: Polygons representing the local lane set
local_das: Numpy array of objects of shape (N,) where each object is of shape (M,3)
log_id: The ID of a log
timestamp: In nanoseconds
city_to_egovehicle_se3: Transformation from egovehicle frame to city frame
avm: ArgoverseMap instance
"""
if axis is not "city_axis":
# rendering instead in the egovehicle reference frame
for da_idx, local_da in enumerate(local_das):
local_da = city_to_egovehicle_se3.inverse_transform_point_cloud(
local_da)
local_das[da_idx] = rotate_polygon_about_pt(
local_da, city_to_egovehicle_se3.rotation, np.zeros(3))
for lane_idx, local_lane_polygon in enumerate(local_lane_polygons):
local_lane_polygon = city_to_egovehicle_se3.inverse_transform_point_cloud(
local_lane_polygon)
local_lane_polygons[lane_idx] = rotate_polygon_about_pt(
local_lane_polygon, city_to_egovehicle_se3.rotation,
np.zeros(3))
draw_lane_polygons(ax, local_lane_polygons)
draw_lane_polygons(ax, local_das, color="tab:pink")
if axis is not "city_axis":
lidar_pts = rotate_polygon_about_pt(
lidar_pts, city_to_egovehicle_se3.rotation, np.zeros((3, )))
draw_point_cloud_bev(ax, lidar_pts)
objects = self.log_timestamp_dict[log_id][timestamp]
all_occluded = True
for frame_rec in objects:
if frame_rec.occlusion_val != IS_OCCLUDED_FLAG:
all_occluded = False
if not all_occluded:
for i, frame_rec in enumerate(objects):
bbox_city_fr = frame_rec.bbox_city_fr
bbox_ego_frame = frame_rec.bbox_ego_frame
color = frame_rec.color
if frame_rec.occlusion_val != IS_OCCLUDED_FLAG:
bbox_ego_frame = rotate_polygon_about_pt(
bbox_ego_frame, city_to_egovehicle_se3.rotation,
np.zeros((3, )))
if axis is "city_axis":
plot_bbox_2D(ax, bbox_city_fr, color)
if self.plot_lane_tangent_arrows:
bbox_center = np.mean(bbox_city_fr, axis=0)
tangent_xy, conf = avm.get_lane_direction(
query_xy_city_coords=bbox_center[:2],
city_name=city_name)
dx = tangent_xy[0] * LANE_TANGENT_VECTOR_SCALING
dy = tangent_xy[1] * LANE_TANGENT_VECTOR_SCALING
ax.arrow(bbox_center[0],
bbox_center[1],
dx,
dy,
color="r",
width=0.5,
zorder=2)
else:
plot_bbox_2D(ax, bbox_ego_frame, color)
cuboid_lidar_pts, _ = filter_point_cloud_to_bbox_2D_vectorized(
bbox_ego_frame[:, :2], copy.deepcopy(lidar_pts))
if cuboid_lidar_pts is not None:
draw_point_cloud_bev(ax, cuboid_lidar_pts, color)
else:
logger.info(f"all occluded at {timestamp}")
xcenter = city_to_egovehicle_se3.translation[0]
ycenter = city_to_egovehicle_se3.translation[1]
ax.text(xcenter - 146,
ycenter + 50,
"ALL OBJECTS OCCLUDED",
fontsize=30)
def verify_lane_tangent_vector() -> None:
"""Debug low confidence lane tangent predictions.
I noticed that the confidence score of lane direction is
pretty low (almost zero) in some logs
"""
POSE_FILE_DIR = "../debug_lane_tangent"
# both of these are Pittsburgh logs
log_ids = [
"033669d3-3d6b-3d3d-bd93-7985d86653ea",
"028d5cb1-f74d-366c-85ad-84fde69b0fd3",
]
avm = ArgoverseMap()
city_name = "PIT"
for log_id in log_ids:
print(f"On {log_id}")
pose_fpaths = glob.glob(
f"{POSE_FILE_DIR}/{log_id}/poses/city_SE3_egovehicle_*.json")
num_poses = len(pose_fpaths)
egovehicle_xy_arr = np.zeros((num_poses, 2))
for i, pose_fpath in enumerate(pose_fpaths):
json_data = read_json_file(pose_fpath)
egovehicle_xy_arr[i, 0] = json_data["translation"][0]
egovehicle_xy_arr[i, 1] = json_data["translation"][1]
for i, query_xy_city_coords in enumerate(egovehicle_xy_arr[::10, :]):
query_xy_city_coords = np.array(
[3116.8282170094944, 1817.1269613456188])
query_xy_city_coords = np.array(
[3304.7072308190845, 1993.1670162837597])
# start = time.time()
lane_dir_vector, confidence = avm.get_lane_direction(
query_xy_city_coords, city_name, visualize=False)
# end = time.time()
# duration = end - start
# print(f'query took {duration} s')
# if confidence < 0.5:
print(f"\t{i}: {confidence}")
# if confidence == 0.:
# pdb.set_trace()
# This was an absolute failure case!
# lane_dir_vector, confidence = avm.get_lane_direction(query_xy_city_coords, city_name, visualize=True)
visualize = True
if visualize:
fig = plt.figure(figsize=(22.5, 8))
ax = fig.add_subplot(111)
dx = lane_dir_vector[0] * 20
dy = lane_dir_vector[1] * 20
plt.arrow(
query_xy_city_coords[0],
query_xy_city_coords[1],
dx,
dy,
color="r",
width=0.3,
zorder=2,
)
query_x, query_y = query_xy_city_coords
ax.scatter([query_x], [query_y], 100, color="k", marker=".")
# make another plot now!
plot_nearby_halluc_lanes(ax, city_name, avm, query_x, query_y)
ax.axis("equal")
plt.show()
plt.close("all")