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
