def tf_box_4c_to_box_3d(boxes_4c, ground_plane):
"""Vectorized box_4c to box_3d conversion
Args:
boxes_4c: Tensor of boxes_4c (N, 10)
ground_plane: Tensor of ground plane coefficients (4,)
Returns:
Tensor of boxes_3d (N, 7)
"""
format_checker.check_box_4c_format(boxes_4c)
# Extract corners
corners = tf.reshape(boxes_4c[:, 0:8], [-1, 2, 4])
p1 = corners[:, :, 0]
p2 = corners[:, :, 1]
p3 = corners[:, :, 2]
p4 = corners[:, :, 3]
# Get line midpoints
midpoint_12 = (p1 + p2) / 2.0
midpoint_23 = (p2 + p3) / 2.0
midpoint_34 = (p3 + p4) / 2.0
midpoint_14 = (p1 + p4) / 2.0
# Check which direction is longer
vec_34_12 = midpoint_12 - midpoint_34
vec_34_12_mag = tf.norm(vec_34_12, axis=1)
vec_23_14 = midpoint_14 - midpoint_23
vec_23_14_mag = tf.norm(vec_23_14, axis=1)
# Calculate both possibilities (vec_34_12_mag or vec_23_14_mag),
# then mask out the values from the shorter direction
# vec_34_12_mag longer
vec_34_12_centroid, vec_34_12_length, vec_34_12_width, vec_34_12_ry = \
calculate_box_3d_info(vec_34_12, vec_34_12_mag,
p1, p2, p3, p4, midpoint=midpoint_34)
# vec_23_14_mag longer
vec_23_14_centroid, vec_23_14_length, vec_23_14_width, vec_23_14_ry = \
calculate_box_3d_info(vec_23_14, vec_23_14_mag,
p1, p2, p3, p4, midpoint=midpoint_23)
vec_34_12_mask = tf.greater(vec_34_12_mag, vec_23_14_mag)
vec_23_14_mask = tf.logical_not(vec_34_12_mask)
vec_34_12_float_mask = tf.reshape(
tf.cast(vec_34_12_mask, tf.float32), [-1, 1])
vec_23_14_float_mask = tf.reshape(
tf.cast(vec_23_14_mask, tf.float32), [-1, 1])
centroid_xz = vec_34_12_centroid * vec_34_12_float_mask + \
vec_23_14_centroid * vec_23_14_float_mask
length_out = vec_34_12_length * vec_34_12_float_mask + \
vec_23_14_length * vec_23_14_float_mask
width_out = vec_34_12_width * vec_34_12_float_mask + \
vec_23_14_width * vec_23_14_float_mask
ry_out = vec_34_12_ry * vec_34_12_float_mask + \
vec_23_14_ry * vec_23_14_float_mask
# Find new centroid y
a = ground_plane[0]
b = ground_plane[1]
c = ground_plane[2]
d = ground_plane[3]
h1 = boxes_4c[:, 8]
h2 = boxes_4c[:, 9]
centroid_x = centroid_xz[:, 0]
centroid_z = centroid_xz[:, 1]
# Squeeze to single dimension for stacking
length_out = tf.squeeze(length_out)
width_out = tf.squeeze(width_out)
ry_out = tf.squeeze(ry_out)
ground_y = -(a * centroid_x + c * centroid_z + d) / b
# h1 and h2 are along the -y axis
centroid_y = ground_y - h1
height_out = h2 - h1
box_3d_out = tf.stack([centroid_x, centroid_y, centroid_z,
length_out, width_out, height_out, ry_out], axis=1)
return box_3d_out
def np_box_4c_to_box_3d(box_4c, ground_plane):
"""Converts a single box_4c to box_3d. The longest midpoint-midpoint
length is used to calculate orientation. Points are projected onto the
orientation vector and the orthogonal vector to get the bounding box_3d.
The centroid is calculated by adding a vector of half the projected length
along the midpoint-midpoint vector, and a vector of the width
differences along the normal.
Args:
box_4c: box_4c to convert (10,)
ground_plane: ground plane coefficients (4,)
Returns:
box_3d (7,)
"""
format_checker.check_box_4c_format(box_4c)
# Extract corners
corners = box_4c[0:8].reshape(2, 4)
p1 = corners[:, 0]
p2 = corners[:, 1]
p3 = corners[:, 2]
p4 = corners[:, 3]
# Check for longest axis
midpoint_12 = (p1 + p2) / 2.0
midpoint_23 = (p2 + p3) / 2.0
midpoint_34 = (p3 + p4) / 2.0
midpoint_14 = (p1 + p4) / 2.0
vec_34_12 = midpoint_12 - midpoint_34
vec_34_12_mag = np.linalg.norm(vec_34_12)
vec_23_14 = midpoint_14 - midpoint_23
vec_23_14_mag = np.linalg.norm(vec_23_14)
# Check which midpoint -> midpoint vector is longer
if vec_34_12_mag > vec_23_14_mag:
# vec_34_12_mag longer
vec_34_12_norm = vec_34_12 / vec_34_12_mag
vec_mid_34_p1 = p1 - midpoint_34
vec_mid_34_p2 = p2 - midpoint_34
vec_mid_34_p3 = p3 - midpoint_34
vec_mid_34_p4 = p4 - midpoint_34
l1 = np.dot(vec_mid_34_p1, vec_34_12_norm)
l2 = np.dot(vec_mid_34_p2, vec_34_12_norm)
l3 = np.dot(vec_mid_34_p3, vec_34_12_norm)
l4 = np.dot(vec_mid_34_p4, vec_34_12_norm)
all_lengths = [l1, l2, l3, l4]
min_l = np.amin(all_lengths)
max_l = np.amax(all_lengths)
length_out = max_l - min_l
ortho_norm = np.asarray([-vec_34_12_norm[1], vec_34_12_norm[0]])
w1 = np.dot(vec_mid_34_p1, ortho_norm)
w2 = np.dot(vec_mid_34_p2, ortho_norm)
w3 = np.dot(vec_mid_34_p3, ortho_norm)
w4 = np.dot(vec_mid_34_p4, ortho_norm)
all_widths = [w1, w2, w3, w4]
min_w = np.amin(all_widths)
max_w = np.amax(all_widths)
w_diff = max_w + min_w
width_out = max_w - min_w
ry_out = -np.arctan2(vec_34_12[1], vec_34_12[0])
# New centroid
centroid = midpoint_34 + vec_34_12_norm * (min_l + max_l) / 2.0 + \
ortho_norm * w_diff
else:
# vec_23_14_mag longer
vec_23_14_norm = vec_23_14 / vec_23_14_mag
vec_mid_23_p1 = p1 - midpoint_23
vec_mid_23_p2 = p2 - midpoint_23
vec_mid_23_p3 = p3 - midpoint_23
vec_mid_23_p4 = p4 - midpoint_23
l1 = np.dot(vec_mid_23_p1, vec_23_14_norm)
l2 = np.dot(vec_mid_23_p2, vec_23_14_norm)
l3 = np.dot(vec_mid_23_p3, vec_23_14_norm)
l4 = np.dot(vec_mid_23_p4, vec_23_14_norm)
all_lengths = [l1, l2, l3, l4]
min_l = np.amin(all_lengths)
max_l = np.amax(all_lengths)
length_out = max_l - min_l
ortho_norm = np.asarray([-vec_23_14_norm[1], vec_23_14_norm[0]])
w1 = np.dot(vec_mid_23_p1, ortho_norm)
w2 = np.dot(vec_mid_23_p2, ortho_norm)
w3 = np.dot(vec_mid_23_p3, ortho_norm)
w4 = np.dot(vec_mid_23_p4, ortho_norm)
all_widths = [w1, w2, w3, w4]
min_w = np.amin(all_widths)
max_w = np.amax(all_widths)
w_diff = max_w + min_w
width_out = max_w - min_w
ry_out = -np.arctan2(vec_23_14[1], vec_23_14[0])
# New centroid
centroid = midpoint_23 + vec_23_14_norm * (min_l + max_l) / 2.0 + \
ortho_norm * w_diff
# Find new centroid y
a = ground_plane[0]
b = ground_plane[1]
c = ground_plane[2]
d = ground_plane[3]
h1 = box_4c[8]
h2 = box_4c[9]
centroid_x = centroid[0]
centroid_z = centroid[1]
ground_y = -(a * centroid_x + c * centroid_z + d) / b
# h1 and h2 are along the -y axis
centroid_y = ground_y - h1
height_out = h2 - h1
box_3d_out = np.stack([centroid_x, centroid_y, centroid_z,
length_out, width_out, height_out, ry_out])
return box_3d_out