def target_to_cam(nusc: NuScenes, pointsensor_token: str, annotation_token: str, pointsensor_channel: str = 'LIDAR_TOP'): """ Given an annotation token (3d detection in world coordinate frame) and pointsensor sample_data token, transform the label from world-coordinate frame to LiDAR. :param nusc: NuScenes instance. :param pointsensor_token: Lidar/radar sample_data token. :param annotation_token: Camera sample_annotation token. :param pointsensor_channel: Laser channel name, e.g. 'LIDAR_TOP'. :return box with the labels for the 3d detection task in the LiDAR channel frame. """ # Point LiDAR sample point_data = nusc.get('sample_data', pointsensor_token) # Sample LiDAR info # From LiDAR to ego cs_rec = nusc.get('calibrated_sensor', point_data['calibrated_sensor_token']) # Transformation metadata from ego to world coordinate frame pose_rec = nusc.get('ego_pose', point_data['ego_pose_token']) # Obtain the annotation from the token annotation_metadata = nusc.get('sample_annotation', annotation_token) # Obtain box parameters box = nusc.get_box(annotation_metadata['token']) # Move them to the ego-pose frame. box.translate(-np.array(pose_rec['translation'])) box.rotate(Quaternion(pose_rec['rotation']).inverse) # Move them to the calibrated sensor frame. box.translate(-np.array(cs_rec['translation'])) box.rotate(Quaternion(cs_rec['rotation']).inverse) return box
def bbox_3d_to_2d(nusc: NuScenes, camera_token: str, annotation_token: str, visualize: bool = False) -> List: """ Get the 2D annotation bounding box for a given `sample_data_token`. return None if no intersection (bounding box). :param nusc: NuScenes instance. :param camera_token: Camera sample_data token. :param annotation_token: Sample data token belonging to a camera keyframe. :param visualize: bool to plot the resulting bounding box. :return: List of 2D annotation record that belongs to the input `sample_data_token` """ # Obtain camera sample_data cam_data = nusc.get('sample_data', camera_token) # Get the calibrated sensor and ego pose record to get the transformation matrices. # From camera to ego cs_rec = nusc.get('calibrated_sensor', cam_data['calibrated_sensor_token']) # From ego to world coordinate frame pose_rec = nusc.get('ego_pose', cam_data['ego_pose_token']) # Camera intrinsic parameters camera_intrinsic = np.array(cs_rec['camera_intrinsic']) # Obtain the annotation from the token annotation_metadata = nusc.get('sample_annotation', annotation_token) # Get the box in global coordinates from sample ann token box = nusc.get_box(annotation_metadata['token']) # Mapping the box from world coordinate-frame to camera sensor # Move them to the ego-pose frame. box.translate(-np.array(pose_rec['translation'])) box.rotate(Quaternion(pose_rec['rotation']).inverse) # Move them to the calibrated sensor frame. box.translate(-np.array(cs_rec['translation'])) box.rotate(Quaternion(cs_rec['rotation']).inverse) # Filter out the corners that are not in front of the calibrated sensor. corners_3d = box.corners() # 8 corners of the 3d bounding box in_front = np.argwhere(corners_3d[2, :] > 0).flatten( ) # corners that are behind the sensor are removed corners_3d = corners_3d[:, in_front] # Project 3d box to 2d. corner_coords = view_points(corners_3d, camera_intrinsic, True).T[:, :2].tolist() # Filter points that are outside the image final_coords = post_process_coords(corner_coords) if final_coords is None: return None min_x, min_y, max_x, max_y = [int(coord) for coord in final_coords] if visualize: # Load image from dataroot img_path = osp.join(nusc.dataroot, cam_data['filename']) img = cv2.imread(img_path, 1) # Draw rectangle on image with coords img_r = cv2.rectangle(img, (min_x, min_y), (max_x, max_y), (255, 165, 0), 3) img_r = img_r[:, :, ::-1] plt.figure(figsize=(12, 4), dpi=100) plt.imshow(img_r) plt.show() return final_coords