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 box_3d_to_anchor(boxes_3d, ortho_rotate=False):
""" Converts a box_3d [x, y, z, l, w, h, ry]
into anchor form [x, y, z, dim_x, dim_y, dim_z]
Anchors in box_3d format should have an ry of 0 or 90 degrees.
l and w will be matched to dim_x or dim_z depending on the rotation,
while h will always correspond to dim_y
Args:
boxes_3d: N x 7 ndarray of box_3d
ortho_rotate: optional, if True the box is rotated to the
nearest 90 degree angle, or else the box is projected
onto the x and z axes
Returns:
N x 6 ndarray of anchors in 'anchor' form
"""
boxes_3d = np.asarray(boxes_3d).reshape(-1, 7)
fc.check_box_3d_format(boxes_3d)
num_anchors = len(boxes_3d)
anchors = np.zeros((num_anchors, 6))
# Set x, y, z
anchors[:, [0, 1, 2]] = boxes_3d[:, [0, 1, 2]]
# Dimensions along x, y, z
box_l = boxes_3d[:, [3]]
box_w = boxes_3d[:, [4]]
box_h = boxes_3d[:, [5]]
box_ry = boxes_3d[:, [6]]
# Rotate to nearest multiple of 90 degrees
if ortho_rotate:
half_pi = np.pi / 2
box_ry = np.round(box_ry / half_pi) * half_pi
cos_ry = np.abs(np.cos(box_ry))
sin_ry = np.abs(np.sin(box_ry))
# dim_x, dim_y, dim_z
anchors[:, [3]] = box_l * cos_ry + box_w * sin_ry
anchors[:, [4]] = box_h
anchors[:, [5]] = box_w * cos_ry + box_l * sin_ry
return anchors
def box_3d_to_3d_iou_format(boxes_3d):
""" Returns a numpy array of 3d box format for iou calculation
Args:
boxes_3d: list of 3d boxes
Returns:
new_anchor_list: numpy array of 3d box format for iou
"""
boxes_3d = np.asarray(boxes_3d)
fc.check_box_3d_format(boxes_3d)
iou_3d_boxes = np.zeros([len(boxes_3d), 7])
iou_3d_boxes[:, 4:7] = boxes_3d[:, 0:3]
iou_3d_boxes[:, 1] = boxes_3d[:, 3]
iou_3d_boxes[:, 2] = boxes_3d[:, 4]
iou_3d_boxes[:, 3] = boxes_3d[:, 5]
iou_3d_boxes[:, 0] = boxes_3d[:, 6]
return iou_3d_boxes
def box_3d_to_object_label(box_3d, obj_type='Car'):
"""Turns a box_3d into an ObjectLabel
Args:
box_3d: 3D box in the format [x, y, z, l, w, h, ry]
obj_type: Optional, the object type
Returns:
ObjectLabel with the location, size, and rotation filled out
"""
fc.check_box_3d_format(box_3d)
obj_label = obj_utils.ObjectLabel()
obj_label.type = obj_type
obj_label.t = box_3d.take((0, 1, 2))
obj_label.l = box_3d[3]
obj_label.w = box_3d[4]
obj_label.h = box_3d[5]
obj_label.ry = box_3d[6]
return obj_label
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 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
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_bev_box(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: rotated box(x_center, y_center, x_dimension, y_dimension, ry)
N x [x_bev, y_bev, w_bev, h_bev, ry] - (N x 5)
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]
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]
#bev image_x_axis = 3D x_axis, bev image_y_axis = -3D z_axis (y is down, y=0 corresponds to z=max_z)
box_points_norm = points_shifted / [x_extents_range, z_extents_range]
box_points_norm = np.asarray(box_points_norm, dtype=np.float32)
box = np.vstack([x, z, l, w, ry]).transpose()
box_norm = box.copy()
box_shifted = box_norm[:, :2] - [x_extents_min, z_extents_min]
box_norm[:, :2] = box_shifted / [x_extents_range, z_extents_range]
#size normalize, dont have to shift.
#[WRONG!]box_norm[:, 2:4] = box_norm[:, 2:4] / [x_extents_range, z_extents_range]
box_norm[:, 2] = np.sqrt((box_points_norm[:, 0, 0] - box_points_norm[:, 1, 0])**2 + \
(box_points_norm[:, 0, 1] - box_points_norm[:, 1, 1]) **2 )
box_norm[:, 3] = np.sqrt((box_points_norm[:, 1, 0] - box_points_norm[:, 2, 0])**2 + \
(box_points_norm[:, 1, 1] - box_points_norm[:, 2, 1]) **2 )
#box_dir = box_points_norm[:, 1] - box_points_norm[:, 0]
#box_norm[:, -1] = np.arctan2(box_dir[:, 1], box_dir[:, 0])
#rotate to corner first and then normalize != normalize first and then rotate to corner
box_norm = np.asarray(box_norm, dtype=np.float32)
#use to do transformation between BEV and 3D.
#norm_factor={'xmin':x_extents_min, 'zmin':z_extents_min, 'xrange':x_extents_range, 'zrange':z_extents_range}
#return box_norm, box_points_norm, norm_factor
return box_norm, box_points_norm
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
