def get_min_max_3d_box_corners(boxes, name=None):
"""Given a set of upright boxes, return its 2 min and max corner points
Args:
boxes: tf Tensor [N, 7]. The inner dims are [center{x,y,z}, length, width,
height, heading].
name: the name scope.
Returns:
corners: tf Tensor [N, 8, 3].
"""
with tf.compat.v1.name_scope(name, 'GetMinMax3dBoxCorners', [boxes]):
center_x, center_y, center_z, length, width, height, heading = tf.unstack(
boxes, axis=-1)
# [N, 3, 3]
rotation = transform_utils.get_yaw_rotation(heading)
# [N, 3]
translation = tf.stack([center_x, center_y, center_z], axis=-1)
l2 = length * 0.5
w2 = width * 0.5
h2 = height * 0.5
# [N, 2, 3]
corners = tf.reshape(tf.stack([-l2, -w2, -h2, l2, w2, h2], axis=-1),
[-1, 2, 3])
# [N, 2, 3]
corners = tf.einsum('nij,nkj->nki', rotation,
corners) + tf.expand_dims(translation, axis=-2)
return corners
def is_within_box_3d(point, box, name=None):
center = box[:, 0:3] # xc,yc,zc
dim = box[:, 3:6] # L,W,H
heading = box[:, 6]
# [M, 3, 3]
rotation = transform_utils.get_yaw_rotation(heading) # rotation matrix
# [M, 4, 4]
transform = transform_utils.get_transform(rotation,
center) # transform matrix
# [M, 4, 4]
transform = tf.linalg.inv(transform)
# [M, 3, 3]
rotation = transform[:, 0:3, 0:3]
# [M, 3]
translation = transform[:, 0:3, 3] # translation matrix
# [N, M, 3]
point_in_box_frame = tf.einsum('nj,mij->nmi', point,
rotation) + translation #
# [N, M, 3]
point_in_box = tf.logical_and(point_in_box_frame <= dim * 0.5,
point_in_box_frame >= -dim * 0.5)
# [N, M]
point_in_box = tf.cast(tf.reduce_prod(input_tensor=tf.cast(point_in_box,
dtype=tf.uint8),
axis=-1),
dtype=tf.bool)
return point_in_box
def test_transform_point_among_frames(self):
p = tf.constant([[1.0, 0, 0]], dtype=tf.float32)
from_frame_pose = transform_utils.get_transform(
transform_utils.get_yaw_rotation(math.pi * 0.5),
tf.constant([0.0, 0.0, 2.0], dtype=tf.float32))
to_frame_pose = transform_utils.get_transform(
transform_utils.get_yaw_rotation(math.pi * 0.1),
tf.constant([0.0, 0.0, 0.0], dtype=tf.float32))
p = p[tf.newaxis, tf.newaxis, tf.newaxis, ...]
from_frame_pose = from_frame_pose[tf.newaxis, tf.newaxis, tf.newaxis,
...]
to_frame_pose = to_frame_pose[tf.newaxis, tf.newaxis, tf.newaxis, ...]
pp = box_utils.transform_point(p, from_frame_pose, to_frame_pose)
with self.test_session():
self.assertAllClose(
pp[0, 0, 0, ...].eval(),
[[math.cos(math.pi * 0.4),
math.sin(math.pi * 0.4), 2.0]])
def test_transform_box_among_frames(self):
b = tf.constant([[1.0, 0, 0, 2.0, 2.0, 2.0, math.pi * 0.1]],
dtype=tf.float32)
from_frame_pose = transform_utils.get_transform(
transform_utils.get_yaw_rotation(math.pi * 0.5),
tf.constant([0.0, 0.0, 1.0], dtype=tf.float32))
to_frame_pose = transform_utils.get_transform(
transform_utils.get_yaw_rotation(math.pi * 0.25),
tf.constant([0.0, 0.0, 0.0], dtype=tf.float32))
b = b[tf.newaxis, tf.newaxis, tf.newaxis, ...]
from_frame_pose = from_frame_pose[tf.newaxis, tf.newaxis, tf.newaxis,
...]
to_frame_pose = to_frame_pose[tf.newaxis, tf.newaxis, tf.newaxis, ...]
bb = box_utils.transform_box(b, from_frame_pose, to_frame_pose)
with self.test_session():
self.assertAllClose(bb[0, 0, 0, ...].eval(), [[
math.cos(math.pi * 0.25),
math.sin(math.pi * 0.25), 1.0, 2.0, 2.0, 2.0, math.pi * 0.35
]])
def is_within_box_3d(point, box, name=None):
"""Checks whether a point is in a 3d box given a set of points and boxes.
Args:
point: [N, 3] tensor. Inner dims are: [x, y, z].
box: [M, 7] tensor. Inner dims are: [center_x, center_y, center_z, length,
width, height, heading].
name: tf name scope.
Returns:
point_in_box; [N, M] boolean tensor.
"""
with tf.compat.v1.name_scope(name, 'IsWithinBox3D', [point, box]):
center = box[:, 0:3]
dim = box[:, 3:6]
heading = box[:, 6]
# [M, 3, 3]
rotation = transform_utils.get_yaw_rotation(heading)
# [M, 4, 4]
transform = transform_utils.get_transform(rotation, center)
# [M, 4, 4]
transform = tf.linalg.inv(transform)
# [M, 3, 3]
rotation = transform[:, 0:3, 0:3]
# [M, 3]
translation = transform[:, 0:3, 3]
# [N, M, 3]
point_in_box_frame = tf.einsum('nj,mij->nmi', point,
rotation) + translation
# [N, M, 3]
point_in_box = tf.logical_and(
tf.logical_and(point_in_box_frame <= dim * 0.5,
point_in_box_frame >= -dim * 0.5),
tf.reduce_all(tf.not_equal(dim, 0), axis=-1, keepdims=True))
# [N, M]
point_in_box = tf.cast(tf.reduce_prod(input_tensor=tf.cast(
point_in_box, dtype=tf.uint8),
axis=-1),
dtype=tf.bool)
return point_in_box
def get_upright_3d_box_corners(boxes, name=None):
"""Given a set of upright boxes, return its 8 corners.
Given a set of boxes, returns its 8 corners. The corners are ordered layers
(bottom, top) first and then counter-clockwise within each layer.
Args:
boxes: tf Tensor [N, 7]. The inner dims are [center{x,y,z}, length, width,
height, heading].
name: the name scope.
Returns:
corners: tf Tensor [N, 8, 3].
"""
with tf.compat.v1.name_scope(name, 'GetUpright3dBoxCorners', [boxes]):
center_x, center_y, center_z, length, width, height, heading = tf.unstack(
boxes, axis=-1)
# [N, 3, 3]
rotation = transform_utils.get_yaw_rotation(heading)
# [N, 3]
translation = tf.stack([center_x, center_y, center_z], axis=-1)
l2 = length * 0.5
w2 = width * 0.5
h2 = height * 0.5
# [N, 8, 3]
corners = tf.reshape(
tf.stack([
l2, w2, -h2, -l2, w2, -h2, -l2, -w2, -h2, l2, -w2, -h2, l2, w2,
h2, -l2, w2, h2, -l2, -w2, h2, l2, -w2, h2
],
axis=-1), [-1, 8, 3])
# [N, 8, 3]
corners = tf.einsum('nij,nkj->nki', rotation,
corners) + tf.expand_dims(translation, axis=-2)
return corners
def get_min_max_3d_box_corners(boxes,
vehicleToWorldTransform,
worldToReferencePointTransform,
useGlobalPoseTransform,
name=None):
"""Given a set of upright boxes, return its 2 min and max corner points
in world (global) space frame
Args:
boxes: tf Tensor [N, 7]. The inner dims are [center{x,y,z}, length, width,
height, heading].
vehicleToWorldTransform: None or a 4x4 transformation if needed from local to global space
name: the name scope.
Returns:
corners: tf Tensor [N, 3, 2].
"""
with tf.compat.v1.name_scope(name, 'GetMinMax3dBoxCorners', [boxes]):
center_x, center_y, center_z, length, width, height, heading = tf.unstack(
boxes, axis=-1)
if useGlobalPoseTransform:
vehicleToWorldRotation = vehicleToWorldTransform[0:3, 0:3]
vehicleToWorldTranslation = vehicleToWorldTransform[0:3, 3]
worldToReferenceRotation = worldToReferencePointTransform[0:3, 0:3]
worldToReferenceTranslation = worldToReferencePointTransform[0:3,
3]
# Step 1: get the box corners in local space
# [N, 3, 3]
rotation = transform_utils.get_yaw_rotation(heading)
# [N, 3]
translation = tf.stack([center_x, center_y, center_z], axis=-1)
l2 = length * 0.5
w2 = width * 0.5
h2 = height * 0.5
# [N, 8, 3]
corners = tf.reshape(
tf.stack([
l2, w2, -h2, -l2, w2, -h2, -l2, -w2, -h2, l2, -w2, -h2, l2, w2,
h2, -l2, w2, h2, -l2, -w2, h2, l2, -w2, h2
],
axis=-1), [-1, 8, 3])
# [N, 8, 3]
# Step 2: transform the box corners from AABB to OOBB in local space
corners = tf.einsum('nij,nkj->nki', rotation,
corners) + tf.expand_dims(translation, axis=-2)
if useGlobalPoseTransform:
# [N, 8, 3]
# Step 3: transform the box corners from OOBB in local space to OOBB in world space
corners = tf.einsum('jk,nik->nij', vehicleToWorldRotation,
corners) + tf.expand_dims(
vehicleToWorldTranslation, axis=-2)
# Step 4: transform from 00BB in world space to OOBB in reference frame (i.e. relative to the first reference vehicle position)
corners = tf.einsum('jk,nik->nij', worldToReferenceRotation,
corners) + tf.expand_dims(
worldToReferenceTranslation, axis=-2)
corners = corners.numpy()
#print(corners)
min_corners = corners.min(axis=1)
max_corners = corners.max(axis=1)
#print(min_corners)
#print(max_corners)
minMaxCorners = np.stack([min_corners, max_corners],
axis=1).transpose(0, 2, 1)
return minMaxCorners
