def get_box_yaw_angle_in_camera_coords(box: Box):
"""
Calculate the heading angle, using the convention in KITTI labels.
:param box: bouding box
:return:
"""
box_corners = box.corners()
v = box_corners[:, 0] - box_corners[:, 4]
heading_angle = np.arctan2(-v[2], v[0])
return heading_angle
def get_box_yaw_angle_in_world_coords(box: Box):
"""
Calculate the heading angle, using world coordinates.
:param box: bouding box
:return:
"""
box_corners = box.corners()
v = box_corners[:, 0] - box_corners[:, 4]
heading_angle = np.arctan2(v[1], v[0])
return heading_angle
def project_kitti_box_to_image(
box: Box, p_left: np.ndarray,
imsize: Tuple[int, int]) -> Union[None, Tuple[int, int, int, int]]:
"""Projects 3D box into KITTI image FOV.
Args:
box: 3D box in KITTI reference frame.
p_left: <np.float: 3, 4>. Projection matrix.
imsize: (width, height). Image size.
Returns: (xmin, ymin, xmax, ymax). Bounding box in image plane or None if box is not in the image.
"""
# Create a new box.
box = box.copy()
# KITTI defines the box center as the bottom center of the object.
# We use the true center, so we need to adjust half height in negative y direction.
box.translate(np.array([0, -box.wlh[2] / 2, 0]))
# Check that some corners are inside the image.
corners = np.array(
[corner for corner in box.corners().T if corner[2] > 0]).T
if len(corners) == 0:
return None
# Project corners that are in front of the camera to 2d to get bbox in pixel coords.
imcorners = view_points(corners, p_left, normalize=True)[:2]
bbox = (np.min(imcorners[0]), np.min(imcorners[1]),
np.max(imcorners[0]), np.max(imcorners[1]))
# Crop bbox to prevent it extending outside image.
bbox_crop = tuple(max(0, b) for b in bbox)
bbox_crop = (
min(imsize[0], bbox_crop[0]),
min(imsize[0], bbox_crop[1]),
min(imsize[0], bbox_crop[2]),
min(imsize[1], bbox_crop[3]),
)
# Detect if a cropped box is empty.
if bbox_crop[0] >= bbox_crop[2] or bbox_crop[1] >= bbox_crop[3]:
return None
return bbox_crop
def get_box_corners(transformed_box: Box,
cam_intrinsic_mtx: np.array,
frustum_pointnet_convention=True):
box_corners_on_cam_coord = transformed_box.corners()
# Regarrange to conform Frustum-pointnet's convention
if frustum_pointnet_convention:
rearranged_idx = [0, 3, 7, 4, 1, 2, 6, 5]
box_corners_on_cam_coord = box_corners_on_cam_coord[:, rearranged_idx]
assert np.allclose((box_corners_on_cam_coord[:, 0] + box_corners_on_cam_coord[:, 6]) / 2,
np.array(transformed_box.center))
# For perspective transformation, the normalization should set to be True
box_corners_on_image = view_points(box_corners_on_cam_coord, view=cam_intrinsic_mtx, normalize=True)
return box_corners_on_image
