def test_2d_cuboid_interior_test1() -> None: """Test 2d version of the 3d projection above. This is a similar test except it slices away the z dimension. """ pc_raw, bbox_3d, gt_segment, _ = get_scenario_1() pc_raw_2d = np.unique(to_2d(pc_raw), axis=0) bbox_2d = to_2d(bbox_3d)[(0, 1, 4, 5), :] gt_segment_2d = np.unique(to_2d(gt_segment), axis=0) filtered_points, _ = filter_point_cloud_to_bbox_2D_vectorized(bbox_2d, pc_raw_2d) assert np.array_equal(filtered_points, gt_segment_2d) filtered_points = filter_point_cloud_to_bbox(bbox_2d, pc_raw_2d) assert np.array_equal(filtered_points, gt_segment_2d)
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)