def np_box_3d_to_box_8co(box_3d):
"""Computes the 3D bounding box corner positions from Box3D format.
The order of corners are preserved during this conversion.
Args:
box_3d: 1 x 7 ndarray of box_3d in the format
[x, y, z, l, w, h, ry]
Returns:
corners_3d: An ndarray or a tensor of shape (3 x 8) representing
the box as corners in the following format ->
[[x1,...,x8], [y1...,y8], [z1,...,z8]].
"""
format_checker.check_box_3d_format(box_3d)
ry = box_3d[6]
# Compute transform matrix
# This includes rotation and translation
rot = np.array([[np.cos(ry), 0, np.sin(ry), box_3d[0]],
[0, 1, 0, box_3d[1]],
[-np.sin(ry), 0, np.cos(ry), box_3d[2]]])
length = box_3d[3]
width = box_3d[4]
height = box_3d[5]
# 3D BB corners
x_corners = np.array([length / 2, length / 2,
-length / 2, -length / 2,
length / 2, length / 2,
-length / 2, -length / 2])
y_corners = np.array([0.0, 0.0, 0.0, 0.0,
-height, -height, -height, -height])
z_corners = np.array([width / 2, -width / 2,
-width / 2, width / 2,
width / 2, -width / 2,
-width / 2, width / 2])
# Create a ones column
ones_col = np.ones(x_corners.shape)
# Append the column of ones to be able to multiply
box_8c = np.dot(rot, np.array([x_corners,
y_corners,
z_corners,
ones_col]))
# Ignore the fourth column
box_8c = box_8c[0:3]
return box_8c
def test_check_box_3d_format(self):
# Case 1, invalid type
test_var = [0, 0, 0, 0, 0, 0, 0]
np.testing.assert_raises(TypeError, fc.check_box_3d_format, test_var)
# Case 2, invalid shape
test_var = np.ones([1, 5])
np.testing.assert_raises(TypeError, fc.check_box_3d_format, test_var)
test_var = np.ones([5, 6])
np.testing.assert_raises(TypeError, fc.check_box_3d_format, test_var)
test_var = np.ones([1, 7])
fc.check_box_3d_format(test_var)
test_var = np.ones([10, 7])
fc.check_box_3d_format(test_var)
test_var = tf.ones([5, 7])
fc.check_box_3d_format(test_var)
test_var = tf.ones([5, 3])
np.testing.assert_raises(TypeError, fc.check_box_3d_format, test_var)
def tf_box_3d_to_box_4c(boxes_3d, ground_plane):
"""Vectorized conversion of box_3d to box_4c tensors
Args:
boxes_3d: Tensor of boxes_3d (N, 7)
ground_plane: Tensor ground plane coefficients (4,)
Returns:
Tensor of boxes_4c (N, 10)
"""
format_checker.check_box_3d_format(boxes_3d)
anchors = box_3d_encoder.tf_box_3d_to_anchor(boxes_3d)
centroid_x = anchors[:, 0]
centroid_y = anchors[:, 1]
centroid_z = anchors[:, 2]
dim_x = anchors[:, 3]
dim_y = anchors[:, 4]
dim_z = anchors[:, 5]
# Create temporary box at (0, 0) for rotation
half_dim_x = dim_x / 2
half_dim_z = dim_z / 2
# Box corners
x_corners = tf.stack([half_dim_x, half_dim_x,
-half_dim_x, -half_dim_x], axis=1)
z_corners = tf.stack([half_dim_z, -half_dim_z,
-half_dim_z, half_dim_z], axis=1)
# Rotations from boxes_3d
all_rys = boxes_3d[:, 6]
# Find nearest 90 degree
half_pi = np.pi / 2
ortho_rys = tf.round(all_rys / half_pi) * half_pi
# Get rys and 0/1 padding
ry_diffs = all_rys - ortho_rys
zeros = tf.zeros_like(ry_diffs, dtype=tf.float32)
ones = tf.ones_like(ry_diffs, dtype=tf.float32)
# Create transformation matrix, including rotation and translation
tr_mat = tf.stack(
[tf.stack([tf.cos(ry_diffs), tf.sin(ry_diffs), centroid_x], axis=1),
tf.stack([-tf.sin(ry_diffs), tf.cos(ry_diffs), centroid_z], axis=1),
tf.stack([zeros, zeros, ones], axis=1)],
axis=2)
# Create a ones row
ones_row = tf.ones_like(x_corners)
# Append the column of ones to be able to multiply
points_stacked = tf.stack([x_corners, z_corners, ones_row], axis=1)
corners = tf.matmul(tr_mat, points_stacked,
transpose_a=True,
transpose_b=False)
# Discard the last row (ones)
corners = corners[:, 0:2]
flat_corners = tf.reshape(corners, [-1, 8])
# Get ground plane coefficients
a = ground_plane[0]
b = ground_plane[1]
c = ground_plane[2]
d = ground_plane[3]
# Calculate heights off ground plane
ground_y = -(a * centroid_x + c * centroid_z + d) / b
h1 = ground_y - centroid_y
h2 = h1 + dim_y
batched_h1 = tf.reshape(h1, [-1, 1])
batched_h2 = tf.reshape(h2, [-1, 1])
# Stack into (?, 10)
box_4c = tf.concat([flat_corners, batched_h1, batched_h2], axis=1)
return box_4c
def np_box_3d_to_box_4c(box_3d, ground_plane):
"""Converts a single box_3d to box_4c
Args:
box_3d: box_3d (6,)
ground_plane: ground plane coefficients (4,)
Returns:
box_4c (10,)
"""
format_checker.check_box_3d_format(box_3d)
anchor = box_3d_encoder.box_3d_to_anchor(box_3d, ortho_rotate=True)[0]
centroid_x = anchor[0]
centroid_y = anchor[1]
centroid_z = anchor[2]
dim_x = anchor[3]
dim_y = anchor[4]
dim_z = anchor[5]
# Create temporary box at (0, 0) for rotation
half_dim_x = dim_x / 2
half_dim_z = dim_z / 2
# Box corners
x_corners = np.asarray([half_dim_x, half_dim_x,
-half_dim_x, -half_dim_x])
z_corners = np.array([half_dim_z, -half_dim_z,
-half_dim_z, half_dim_z])
ry = box_3d[6]
# Find nearest 90 degree
half_pi = np.pi / 2
ortho_ry = np.round(ry / half_pi) * half_pi
# Find rotation to make the box ortho aligned
ry_diff = ry - ortho_ry
# Create transformation matrix, including rotation and translation
tr_mat = np.array([[np.cos(ry_diff), np.sin(ry_diff), centroid_x],
[-np.sin(ry_diff), np.cos(ry_diff), centroid_z],
[0, 0, 1]])
# Create a ones row
ones_row = np.ones(x_corners.shape)
# Append the column of ones to be able to multiply
points_stacked = np.vstack([x_corners, z_corners, ones_row])
corners = np.matmul(tr_mat, points_stacked)
# Discard the last row (ones)
corners = corners[0:2]
# Calculate height off ground plane
ground_y = geometry_utils.calculate_plane_point(
ground_plane, [centroid_x, None, centroid_z])[1]
h1 = ground_y - centroid_y
h2 = h1 + dim_y
# Stack into (10,) ndarray
box_4c = np.hstack([corners.flatten(), h1, h2])
return box_4c
def tf_box_3d_to_box_8co(boxes_3d):
"""Computes the 3D bounding box corner positions from Box3D format.
The order of corners are preserved during this conversion.
Args:
boxes_3d: N x 7 tensor of box_3d in the format
[x, y, z, l, w, h, ry]
Returns:
corners_3d: An ndarray or a tensor of shape (N x 3 x 8) representing
the box as corners in following format -> [[[x1,...,x8],[y1...,y8],
[z1,...,z8]]].
"""
format_checker.check_box_3d_format(boxes_3d)
all_rys = boxes_3d[:, 6]
ry_sin = tf.sin(all_rys)
ry_cos = tf.cos(all_rys)
zeros = tf.zeros_like(all_rys, dtype=tf.float32)
ones = tf.ones_like(all_rys, dtype=tf.float32)
# Rotation matrix
rot_mats = tf.stack([tf.stack([ry_cos, zeros, ry_sin], axis=1),
tf.stack([zeros, ones, zeros], axis=1),
tf.stack([-ry_sin, zeros, ry_cos], axis=1)],
axis=2)
length = boxes_3d[:, 3]
width = boxes_3d[:, 4]
height = boxes_3d[:, 5]
half_length = length / 2
half_width = width / 2
x_corners = tf.stack([half_length, half_length,
-half_length, -half_length,
half_length, half_length,
-half_length, -half_length], axis=1)
y_corners = tf.stack([zeros, zeros, zeros, zeros,
-height, -height, -height, -height], axis=1)
z_corners = tf.stack([half_width, -half_width,
-half_width, half_width,
half_width, -half_width,
-half_width, half_width], axis=1)
corners = tf.stack([x_corners,
y_corners,
z_corners], axis=1)
boxes_8c = tf.matmul(rot_mats, corners,
transpose_a=True,
transpose_b=False)
# Translate the corners
corners_3d_x = boxes_8c[:, 0] + tf.reshape(boxes_3d[:, 0], (-1, 1))
corners_3d_y = boxes_8c[:, 1] + tf.reshape(boxes_3d[:, 1], (-1, 1))
corners_3d_z = boxes_8c[:, 2] + tf.reshape(boxes_3d[:, 2], (-1, 1))
boxes_8c = tf.stack([corners_3d_x,
corners_3d_y,
corners_3d_z], axis=1)
return boxes_8c
def tf_box_3d_to_box_8c(boxes_3d):
"""Computes the 3D bounding box corner positions from box_3d format.
This function does not preserve corners order during conversion from
box_3d -> box_8c. Instead of using the box_3d's orientation, 'ry',
nearest 90 degree angle is selected to create an axis-aligned box.
This helps in calculating the closest corner to corner when comparing
the corners to the ground-truth boxes.
Args:
boxes_3d: N x 7 tensor of box_3d in the format
[x, y, z, l, w, h, ry]
Returns:
corners_3d: A tensor of shape (N x 3 x 8) representing
the box as corners in following format -> [[[x1,...,x8],[y1...,y8],
[z1,...,z8]]].
"""
format_checker.check_box_3d_format(boxes_3d)
anchors = box_3d_encoder.tf_box_3d_to_anchor(boxes_3d)
centroid_x = anchors[:, 0]
centroid_y = anchors[:, 1]
centroid_z = anchors[:, 2]
dim_x = anchors[:, 3]
dim_y = anchors[:, 4]
dim_z = anchors[:, 5]
all_rys = boxes_3d[:, 6]
# Find nearest 90 degree
half_pi = np.pi / 2
ortho_rys = tf.round(all_rys / half_pi) * half_pi
ry_diff = all_rys - ortho_rys
ry_sin = tf.sin(ry_diff)
ry_cos = tf.cos(ry_diff)
zeros = tf.zeros_like(ry_diff, dtype=tf.float32)
ones = tf.ones_like(ry_diff, dtype=tf.float32)
# Rotation matrix
rot_mats = tf.stack([tf.stack([ry_cos, zeros, ry_sin], axis=1),
tf.stack([zeros, ones, zeros], axis=1),
tf.stack([-ry_sin, zeros, ry_cos], axis=1)],
axis=2)
half_dim_x = dim_x / 2
half_dim_z = dim_z / 2
x_corners = tf.stack([half_dim_x, half_dim_x,
-half_dim_x, -half_dim_x,
half_dim_x, half_dim_x,
-half_dim_x, -half_dim_x], axis=1)
y_corners = tf.stack([zeros, zeros, zeros, zeros,
-dim_y, -dim_y, -dim_y, -dim_y], axis=1)
z_corners = tf.stack([half_dim_z, -half_dim_z,
-half_dim_z, half_dim_z,
half_dim_z, -half_dim_z,
-half_dim_z, half_dim_z], axis=1)
corners = tf.stack([x_corners,
y_corners,
z_corners], axis=1)
boxes_8c = tf.matmul(rot_mats, corners,
transpose_a=True,
transpose_b=False)
# Translate the corners
corners_3d_x = boxes_8c[:, 0] + tf.reshape(centroid_x, (-1, 1))
corners_3d_y = boxes_8c[:, 1] + tf.reshape(centroid_y, (-1, 1))
corners_3d_z = boxes_8c[:, 2] + tf.reshape(centroid_z, (-1, 1))
boxes_8c = tf.stack([corners_3d_x,
corners_3d_y,
corners_3d_z], axis=1)
return boxes_8c
def np_box_3d_to_box_8c(box_3d):
"""Computes the 3D bounding box corner positions from box_3d format.
This function does not preserve corners order but rather the corners
are rotated to the nearest 90 degree angle. This helps in calculating
the closest corner to corner when comparing the corners to the ground-
truth boxes.
Args:
box_3d: ndarray of size (7,) representing box_3d in the format
[x, y, z, l, w, h, ry]
Returns:
corners_3d: An ndarray or a tensor of shape (3 x 8) representing
the box as corners in following format -> [[x1,...,x8],[y1...,y8],
[z1,...,z8]].
"""
format_checker.check_box_3d_format(box_3d)
# This function is vectorized and returns an ndarray
anchor = box_3d_encoder.box_3d_to_anchor(box_3d, ortho_rotate=True)[0]
centroid_x = anchor[0]
centroid_y = anchor[1]
centroid_z = anchor[2]
dim_x = anchor[3]
dim_y = anchor[4]
dim_z = anchor[5]
half_dim_x = dim_x / 2
half_dim_z = dim_z / 2
# 3D BB corners
x_corners = np.array([half_dim_x, half_dim_x,
-half_dim_x, -half_dim_x,
half_dim_x, half_dim_x,
-half_dim_x, -half_dim_x])
y_corners = np.array([0.0, 0.0, 0.0, 0.0,
-dim_y, -dim_y, -dim_y, -dim_y])
z_corners = np.array([half_dim_z, -half_dim_z,
-half_dim_z, half_dim_z,
half_dim_z, -half_dim_z,
-half_dim_z, half_dim_z])
ry = box_3d[6]
# Find nearest 90 degree
half_pi = np.pi / 2
ortho_ry = np.round(ry / half_pi) * half_pi
# Find rotation to make the box ortho aligned
ry_diff = ry - ortho_ry
# Compute transform matrix
# This includes rotation and translation
rot = np.array([[np.cos(ry_diff), 0, np.sin(ry_diff), centroid_x],
[0, 1, 0, centroid_y],
[-np.sin(ry_diff), 0, np.cos(ry_diff), centroid_z]])
# Create a ones column
ones_col = np.ones(x_corners.shape)
# Append the column of ones to be able to multiply
box_8c = np.dot(rot, np.array([x_corners,
y_corners,
z_corners,
ones_col]))
# Ignore the fourth column
box_8c = box_8c[0:3]
return box_8c
def project_to_image_space(box_3d, calib_p2,
truncate=False, image_size=None,
discard_before_truncation=True, distortion=None):
""" Projects a box_3d into image space
Args:
box_3d: single box_3d to project
calib_p2: stereo calibration p2 matrix
truncate: if True, 2D projections are truncated to be inside the image
image_size: [w, h] must be provided if truncate is True,
used for truncation
discard_before_truncation: If True, discard boxes that are larger than
80% of the image in width OR height BEFORE truncation. If False,
discard boxes that are larger than 80% of the width AND
height AFTER truncation.
Returns:
Projected box in image space [x1, y1, x2, y2]
Returns None if box is not inside the image
"""
format_checker.check_box_3d_format(box_3d)
obj_label = box_3d_encoder.box_3d_to_object_label(box_3d)
corners_3d = obj_panoptic_utils.compute_box_corners_3d(obj_label)
projected = calib_panoptic_utils.project_to_image(corners_3d, calib_p2, dist=distortion)
x1 = np.amin(projected[0])
y1 = np.amin(projected[1])
x2 = np.amax(projected[0])
y2 = np.amax(projected[1])
img_box = np.array([x1, y1, x2, y2])
# print('img_box = ', img_box)
if truncate:
if not image_size:
raise ValueError('Image size must be provided')
image_w = image_size[0]
image_h = image_size[1]
# Discard invalid boxes (outside image space)
# Watch this code carefully. It is NOT deleting the boxes larger than the image.
# It is just deleting invalid boxes!
if img_box[0] > image_w or \
img_box[1] > image_h or \
img_box[2] < 0 or \
img_box[3] < 0:
print('Discard invalid boxes (outside image space)')
return None
# Discard boxes that are larger than 80% of the image width OR height
# (This part does not work well with the Panoptic dataset! Apparenly,chances are very high that one box height is larger than 80% of the image height)
# if discard_before_truncation:
# img_box_w = img_box[2] - img_box[0]
# img_box_h = img_box[3] - img_box[1]
# if img_box_w > (image_w * 0.8) or img_box_h > (image_h * 0.8):
# print('Discard boxes that are larger than 80% of the image width OR height')
# return None
# Truncate remaining boxes into image space
if img_box[0] < 0:
img_box[0] = 0
if img_box[1] < 0:
img_box[1] = 0
if img_box[2] > image_w:
img_box[2] = image_w
if img_box[3] > image_h:
img_box[3] = image_h
# Discard boxes that are covering the the whole image after truncation
# (This part does not work well with the Panoptic dataset! Apparenly,chances are very high that one box height is larger than 80% of the image height)
# if not discard_before_truncation:
# img_box_w = img_box[2] - img_box[0]
# img_box_h = img_box[3] - img_box[1]
# if img_box_w > (image_w * 0.8) and img_box_h > (image_h * 0.8):
# print('Discard boxes that are covering the the whole image after truncation')
# return None
return img_box
def project_to_bev(boxes_3d, bev_extents):
"""
Projects an array of 3D boxes into bird's eye view
Args:
boxes_3d: list of 3d boxes in the format:
N x [x, y, z, l, w, h, ry]
bev_extents: xz extents of the 3d area
[[min_x, max_x], [min_z, max_z]]
Returns:
box_points: counter-clockwise order box points in bev map space
N x [[x0, y0], ... [x3, y3]] - (N x 4 x 2)
box_points_norm: points normalized as a percentage of the map size
N x [[x0, y0], ... [x3, y3]] - (N x 4 x 2)
"""
format_checker.check_box_3d_format(boxes_3d)
boxes_3d = np.array(boxes_3d, dtype=np.float32)
x = boxes_3d[:, 0]
z = boxes_3d[:, 2]
l = boxes_3d[:, 3]
w = boxes_3d[:, 4]
ry = boxes_3d[:, 6]
# 1|0 2D corners
# 2|3
l_2 = l / 2.0
w_2 = w / 2.0
p0 = np.array([l_2, w_2])
p1 = np.array([-l_2, w_2])
p2 = np.array([-l_2, -w_2])
p3 = np.array([l_2, -w_2])
box_points = np.empty((len(boxes_3d), 4, 2))
for box_idx in range(len(boxes_3d)):
rot = ry[box_idx]
rot_mat = np.reshape([[np.cos(rot), np.sin(rot)],
[-np.sin(rot), np.cos(rot)]],
(2, 2))
box_x = x[box_idx]
box_z = z[box_idx]
box_xz = [box_x, box_z]
box_p0 = np.dot(rot_mat, p0[:, box_idx]) + box_xz
box_p1 = np.dot(rot_mat, p1[:, box_idx]) + box_xz
box_p2 = np.dot(rot_mat, p2[:, box_idx]) + box_xz
box_p3 = np.dot(rot_mat, p3[:, box_idx]) + box_xz
box_points[box_idx] = np.array([box_p0, box_p1, box_p2, box_p3])
# Calculate normalized box corners for ROI pooling
x_extents_min = bev_extents[0][0]
z_extents_min = bev_extents[1][1] # z axis is reversed
points_shifted = box_points - [x_extents_min, z_extents_min]
x_extents_range = bev_extents[0][1] - bev_extents[0][0]
z_extents_range = bev_extents[1][0] - bev_extents[1][1]
box_points_norm = points_shifted / [x_extents_range, z_extents_range]
box_points = np.asarray(box_points, dtype=np.float32)
box_points_norm = np.asarray(box_points_norm, dtype=np.float32)
return box_points, box_points_norm
def project_to_image_space(box_3d,
calib_p2,
truncate=False,
image_size=None,
discard_before_truncation=True):
""" Projects a box_3d into image space
Args:
box_3d: single box_3d to project
calib_p2: stereo calibration p2 matrix
truncate: if True, 2D projections are truncated to be inside the image
image_size: [w, h] must be provided if truncate is True,
used for truncation
discard_before_truncation: If True, discard boxes that are larger than
80% of the image in width OR height BEFORE truncation. If False,
discard boxes that are larger than 80% of the width AND
height AFTER truncation.
Returns:
Projected box in image space [x1, y1, x2, y2]
Returns None if box is not inside the image
"""
format_checker.check_box_3d_format(box_3d)
obj_label = box_3d_encoder.box_3d_to_object_label(box_3d)
corners_3d = obj_utils.compute_box_corners_3d(obj_label)
projected = calib_utils.project_to_image(corners_3d, calib_p2)
x1 = np.amin(projected[0])
y1 = np.amin(projected[1])
x2 = np.amax(projected[0])
y2 = np.amax(projected[1])
img_box = np.array([x1, y1, x2, y2])
if truncate:
if not image_size:
raise ValueError('Image size must be provided')
image_w = image_size[0]
image_h = image_size[1]
# Discard invalid boxes (outside image space)
if img_box[0] > image_w or \
img_box[1] > image_h or \
img_box[2] < 0 or \
img_box[3] < 0:
return None
# Discard boxes that are larger than 80% of the image width OR height
if discard_before_truncation:
img_box_w = img_box[2] - img_box[0]
img_box_h = img_box[3] - img_box[1]
if img_box_w > (image_w * 0.8) or img_box_h > (image_h * 0.8):
return None
# Truncate remaining boxes into image space
if img_box[0] < 0:
img_box[0] = 0
if img_box[1] < 0:
img_box[1] = 0
if img_box[2] > image_w:
img_box[2] = image_w
if img_box[3] > image_h:
img_box[3] = image_h
# Discard boxes that are covering the the whole image after truncation
if not discard_before_truncation:
img_box_w = img_box[2] - img_box[0]
img_box_h = img_box[3] - img_box[1]
if img_box_w > (image_w * 0.8) and img_box_h > (image_h * 0.8):
return None
return img_box
