def filter_objs_to_roi(instances: np.ndarray, avm: ArgoverseMap, city_SE3_egovehicle: SE3, city_name: str) -> np.ndarray: """Filter objects to the region of interest (5 meter dilation of driveable area). We ignore instances outside of region of interest (ROI) during evaluation. Args: instances: Numpy array of shape (N,) with ObjectLabelRecord entries avm: Argoverse map object city_SE3_egovehicle: pose of egovehicle within city map at time of sweep city_name: name of city where log was captured Returns: instances_roi: objects with any of 4 cuboid corners located within ROI """ # for each cuboid, get its 4 corners in the egovehicle frame corners_egoframe = np.vstack([dt.as_2d_bbox() for dt in instances]) corners_cityframe = city_SE3_egovehicle.transform_point_cloud( corners_egoframe) corner_within_roi = avm.get_raster_layer_points_boolean( corners_cityframe, city_name, "roi") # check for each cuboid if any of its 4 corners lies within the ROI is_within_roi = corner_within_roi.reshape(-1, 4).any(axis=1) instances_roi = instances[is_within_roi] return instances_roi
def transform_xyz(xyz: np.ndarray, pose1: SE3, pose2: SE3) -> np.ndarray: # transform xyz from pose1 to pose2 # convert to city coordinate city_coord = pose1.transform_point_cloud(xyz) return pose2.inverse_transform_point_cloud(city_coord)
def get_B_SE2_A(B_SE3_A: SE3): """ Can take city_SE3_egovehicle -> city_SE2_egovehicle Can take egovehicle_SE3_object -> egovehicle_SE2_object Doesn't matter if we stretch square by h,w,l since triangles will be similar regardless Args: - B_SE3_A Returns: - B_SE2_A - B_yaw_A """ x_corners = np.array([1, 1, 1, 1, -1, -1, -1, -1]) y_corners = np.array([1, -1, -1, 1, 1, -1, -1, 1]) z_corners = np.array([1, 1, -1, -1, 1, 1, -1, -1]) corners_A_frame = np.vstack((x_corners, y_corners, z_corners)).T corners_B_frame = B_SE3_A.transform_point_cloud(corners_A_frame) p1 = corners_B_frame[1] p5 = corners_B_frame[5] dy = p1[1] - p5[1] dx = p1[0] - p5[0] # the orientation angle of the car B_yaw_A = np.arctan2(dy, dx) t = B_SE3_A.transform_matrix[:2, 3] # get x,y only B_SE2_A = SE2(rotation=rotmat2d(B_yaw_A), translation=t) return B_SE2_A, B_yaw_A
def draw_ground_pts_in_image( sdb: SynchronizationDB, lidar_points: np.ndarray, city_to_egovehicle_se3: SE3, dataset_map: ArgoverseMap, log_id: str, lidar_timestamp: int, city_name: str, dataset_dir: str, experiment_prefix: str, plot_ground: bool = True, motion_compensate: bool = False, camera: Optional[str] = None, ) -> Union[None, np.ndarray]: """Write an image to disk with rendered ground points for every camera. Args: sdb: instance of SynchronizationDB lidar_points: Numpy array of shape (N,3) in egovehicle frame city_to_egovehicle_se3: SE3 instance which takes a point in egovehicle frame and brings it into city frame dataset_map: Map dataset instance log_id: ID of the log city_name: A city's name (e.g. 'MIA' or 'PIT') motion_compensate: Whether to bring lidar points from world frame @ lidar timestamp, to world frame @ camera timestamp camera: camera name, if specified will return image of that specific camera, if None, will save all camera to disk and return None """ # put into city coords, then prune away ground and non-RoI points lidar_points = city_to_egovehicle_se3.transform_point_cloud(lidar_points) lidar_points = dataset_map.remove_non_driveable_area_points( lidar_points, city_name) _, not_ground_logicals = dataset_map.remove_ground_surface( copy.deepcopy(lidar_points), city_name, return_logicals=True) lidar_points = lidar_points[np.logical_not(not_ground_logicals) if plot_ground else not_ground_logicals] # put back into ego-vehicle coords lidar_points = city_to_egovehicle_se3.inverse_transform_point_cloud( lidar_points) calib_fpath = f"{dataset_dir}/{log_id}/vehicle_calibration_info.json" calib_data = read_json_file(calib_fpath) # repeat green to red colormap every 50 m. colors_arr = np.array([ [color_obj.rgb] for color_obj in Color("red").range_to(Color("green"), NUM_RANGE_BINS) ]).squeeze() np.fliplr(colors_arr) for cam_idx, camera_name in enumerate(RING_CAMERA_LIST + STEREO_CAMERA_LIST): im_dir = f"{dataset_dir}/{log_id}/{camera_name}" # load images, e.g. 'image_raw_ring_front_center_000000486.jpg' cam_timestamp = sdb.get_closest_cam_channel_timestamp( lidar_timestamp, camera_name, log_id) if cam_timestamp is None: continue im_fname = f"{camera_name}_{cam_timestamp}.jpg" im_fpath = f"{im_dir}/{im_fname}" # Swap channel order as OpenCV expects it -- BGR not RGB # must make a copy to make memory contiguous img = imageio.imread(im_fpath)[:, :, ::-1].copy() points_h = point_cloud_to_homogeneous(copy.deepcopy(lidar_points)).T if motion_compensate: uv, uv_cam, valid_pts_bool = project_lidar_to_img_motion_compensated( points_h, # these are recorded at lidar_time copy.deepcopy(calib_data), camera_name, cam_timestamp, lidar_timestamp, dataset_dir, log_id, False, ) else: uv, uv_cam, valid_pts_bool = project_lidar_to_img( points_h, copy.deepcopy(calib_data), camera_name, False) if valid_pts_bool is None or uv is None or uv_cam is None: continue if valid_pts_bool.sum() == 0: continue uv = np.round(uv[valid_pts_bool]).astype(np.int32) uv_cam = uv_cam.T[valid_pts_bool] pt_ranges = np.linalg.norm(uv_cam[:, :3], axis=1) rgb_bins = np.round(pt_ranges).astype(np.int32) # account for moving past 100 meters, loop around again rgb_bins = rgb_bins % NUM_RANGE_BINS uv_colors = (255 * colors_arr[rgb_bins]).astype(np.int32) img = draw_point_cloud_in_img_cv2(img, uv, np.fliplr(uv_colors)) if not Path(f"{experiment_prefix}_ground_viz/{log_id}/{camera_name}" ).exists(): os.makedirs( f"{experiment_prefix}_ground_viz/{log_id}/{camera_name}") save_dir = f"{experiment_prefix}_ground_viz/{log_id}/{camera_name}" cv2.imwrite(f"{save_dir}/{camera_name}_{lidar_timestamp}.jpg", img) if camera == camera_name: return cv2.cvtColor(img, cv2.COLOR_BGR2RGB) return None