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 render_bev_labels_mpl_tmp( self, city_name: str, ax: plt.Axes, axis: str, lidar_pts: np.ndarray, local_lane_polygons: np.ndarray, local_das: np.ndarray, 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) 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)
def get_future_traj(self, axes: Axes, log_id: str, timestamp_ind: int, city_to_egovehicle_se3: SE3): traj = np.zeros((FUTURE_TIME_STEP, 2)) for ind, i in enumerate( range(timestamp_ind + 1, timestamp_ind + FUTURE_TIME_STEP + 1)): timestamp = self.timestamps[i] pose_city_to_ego = self.log_egopose_dict[log_id][timestamp] relative_pose = city_to_egovehicle_se3.inverse_transform_point_cloud( pose_city_to_ego["translation"]) traj[ind, :] = relative_pose[0:2] # axes.scatter(traj[:, 0].tolist(), traj[:, 1].tolist(), s=5, c='g') return traj
def render_ego_past_traj( self, axes: Axes, log_id: str, timestamp_ind: int, city_to_egovehicle_se3: SE3, ) -> None: x = [0] y = [0] for i in range(timestamp_ind - EGO_TIME_STEP, timestamp_ind): timestamp = self.timestamps[i] pose_city_to_ego = self.log_egopose_dict[log_id][timestamp] relative_pose = city_to_egovehicle_se3.inverse_transform_point_cloud( pose_city_to_ego["translation"]) x.append(relative_pose[0]) y.append(relative_pose[1]) axes.scatter(x, y, s=5, c='b', alpha=1, zorder=2)
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 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
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, timestamp_ind: 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)) local_das[da_idx] = local_da 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) # ) local_lane_polygons[lane_idx] = local_lane_polygon draw_lane_polygons(ax, local_lane_polygons, color="tab:gray") draw_lane_polygons(ax, local_das, color=(1, 0, 1)) # render ego vehicle bbox_ego = np.array([[-1.0, -1.0, 0.0], [3.0, -1.0, 0.0], [-1.0, 1.0, 0.0], [3.0, 1.0, 0.0]]) # bbox_ego = rotate_polygon_about_pt(bbox_ego, city_to_egovehicle_se3.rotation, np.zeros((3,))) plot_bbox_2D(ax, bbox_ego, 'g') # render surrounding vehicle and pedestrians objects = self.log_timestamp_dict[log_id][timestamp] hist_objects = {} all_occluded = True for frame_rec in objects: if frame_rec.occlusion_val != IS_OCCLUDED_FLAG: all_occluded = False hist_objects[frame_rec.track_uuid] = [ None for i in range(0, SURR_TIME_STEP + 1) ] hist_objects[ frame_rec.track_uuid][SURR_TIME_STEP] = frame_rec.bbox_city_fr if not all_occluded: for ind, i in enumerate( range(timestamp_ind - SURR_TIME_STEP, timestamp_ind)): objects = self.log_timestamp_dict[log_id][self.timestamps[i]] for frame_rec in objects: if frame_rec.track_uuid in hist_objects: hist_objects[ frame_rec.track_uuid][ind] = frame_rec.bbox_city_fr # render color with decreasing lightness color_lightness = np.linspace(1, 0, SURR_TIME_STEP + 2)[1:].tolist() for track_id in hist_objects: for ind_color, bbox_city_fr in enumerate( hist_objects[track_id]): if bbox_city_fr is None: continue bbox_relative = city_to_egovehicle_se3.inverse_transform_point_cloud( bbox_city_fr) x = bbox_relative[:, 0].tolist() y = bbox_relative[:, 1].tolist() x[1], x[2], x[3], y[1], y[2], y[3] = x[2], x[3], x[1], y[ 2], y[3], y[1] ax.fill(x, y, c=(1, 1, color_lightness[ind_color]), alpha=1, zorder=1) 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)