def get_empty_anchor_filter_2d(anchors, voxel_grid_2d, density_threshold=1):
""" Returns a filter for empty anchors from the given 2D anchor list
Args:
anchors: list of 3d anchors in the format
N x [x, y, z, dim_x, dim_y, dim_z]
voxel_grid_2d: a VoxelGrid object containing a 2D voxel grid of
point cloud used to filter the anchors
density_threshold: minimum number of points in voxel to keep the anchor
Returns:
anchor filter: N Boolean mask
"""
format_checker.check_anchor_format(anchors)
# Remove y dimensions from anchors to project into BEV
anchors_2d = anchors[:, [0, 2, 3, 5]]
# Get Integral image of the voxel, add 1 since filled = 0, empty is -1
leaf_layout = voxel_grid_2d.leaf_layout_2d + 1
leaf_layout = np.squeeze(leaf_layout)
integral_image = IntegralImage2D(leaf_layout)
# Make anchor container
anchor_container = np.zeros([len(anchors_2d), 4]).astype(np.uint32)
num_anchors = len(anchors_2d)
# Set up objects containing corners of anchors
top_left_up = np.zeros([num_anchors, 2]).astype(np.float32)
bot_right_down = np.zeros([num_anchors, 2]).astype(np.float32)
# Calculate minimum corner
top_left_up[:, 0] = anchors_2d[:, 0] - (anchors_2d[:, 2] / 2.)
top_left_up[:, 1] = anchors_2d[:, 1] - (anchors_2d[:, 3] / 2.)
# Calculate maximum corner
bot_right_down[:, 0] = anchors_2d[:, 0] + (anchors_2d[:, 2] / 2.)
bot_right_down[:, 1] = anchors_2d[:, 1] + (anchors_2d[:, 3] / 2.)
# map_to_index() expects N x 2 points
anchor_container[:, :2] = voxel_grid_2d.map_to_index(top_left_up)
anchor_container[:, 2:] = voxel_grid_2d.map_to_index(bot_right_down)
# Transpose to pass into query()
anchor_container = anchor_container.T
# Get point density score for each anchor
point_density_score = integral_image.query(anchor_container)
# Create the filter
anchor_filter = point_density_score >= density_threshold
return anchor_filter
def get_empty_anchor_filter(anchors, voxel_grid_3d, density_threshold=1):
""" Returns a filter for empty boxes from the given 3D anchor list
Args:
anchors: list of 3d anchors in the format
N x [x, y, z, dim_x, dim_y, dim_z]
voxel_grid_3d: a VoxelGrid object containing a 3D voxel grid of
pointcloud used to filter the anchors
density_threshold: minimum number of points in voxel to keep the anchor
Returns:
anchor filter: N Boolean mask
"""
format_checker.check_anchor_format(anchors)
# Get Integral image of the voxel, add 1 since filled = 0, empty is -1
integral_image = IntegralImage(voxel_grid_3d.leaf_layout + 1)
# Make cuboid container
cuboid_container = np.zeros([len(anchors), 6]).astype(np.uint32)
top_left_up = np.zeros([len(anchors), 3]).astype(np.float32)
bot_right_down = np.zeros([len(anchors), 3]).astype(np.float32)
# Calculate minimum corner
top_left_up[:, 0] = anchors[:, 0] - (anchors[:, 3] / 2.)
top_left_up[:, 1] = anchors[:, 1] - (anchors[:, 4])
top_left_up[:, 2] = anchors[:, 2] - (anchors[:, 5] / 2.)
# Calculate maximum corner
bot_right_down[:, 0] = anchors[:, 0] + (anchors[:, 3] / 2.)
bot_right_down[:, 1] = anchors[:, 1]
bot_right_down[:, 2] = anchors[:, 2] + (anchors[:, 5] / 2.)
# map_to_index() expects N x 3 points
cuboid_container[:, :3] = voxel_grid_3d.map_to_index(top_left_up)
cuboid_container[:, 3:] = voxel_grid_3d.map_to_index(bot_right_down)
# Transpose to pass into query()
cuboid_container = cuboid_container.T
# Get point density score for each cuboid
point_density_score = integral_image.query(cuboid_container)
# Create the filter
anchor_filter = point_density_score >= density_threshold
# Flatten into shape (N,)
anchor_filter = anchor_filter.flatten()
return anchor_filter
def offset_to_anchor(anchors, offsets):
"""Decodes the anchor regression predictions with the
anchor.
Args:
anchors: A numpy array or a tensor of shape [N, 6]
representing the generated anchors.
offsets: A numpy array or a tensor of shape
[N, 6] containing the predicted offsets in the
anchor format [x, y, z, dim_x, dim_y, dim_z].
Returns:
anchors: A numpy array of shape [N, 6]
representing the predicted anchor boxes.
"""
fc.check_anchor_format(anchors)
fc.check_anchor_format(offsets)
# x = dx * dim_x + x_anch
x_pred = (offsets[:, 0] * anchors[:, 3]) + anchors[:, 0]
# y = dy * dim_y + x_anch
y_pred = (offsets[:, 1] * anchors[:, 4]) + anchors[:, 1]
# z = dz * dim_z + z_anch
z_pred = (offsets[:, 2] * anchors[:, 5]) + anchors[:, 2]
tensor_format = isinstance(anchors, tf.Tensor)
if tensor_format:
# dim_x = exp(log(dim_x) + dx)
dx_pred = tf.exp(tf.log(anchors[:, 3]) + offsets[:, 3])
# dim_y = exp(log(dim_y) + dy)
dy_pred = tf.exp(tf.log(anchors[:, 4]) + offsets[:, 4])
# dim_z = exp(log(dim_z) + dz)
dz_pred = tf.exp(tf.log(anchors[:, 5]) + offsets[:, 5])
anchors = tf.stack((x_pred, y_pred, z_pred, dx_pred, dy_pred, dz_pred),
axis=1)
else:
dx_pred = np.exp(np.log(anchors[:, 3]) + offsets[:, 3])
dy_pred = np.exp(np.log(anchors[:, 4]) + offsets[:, 4])
dz_pred = np.exp(np.log(anchors[:, 5]) + offsets[:, 5])
anchors = np.stack((x_pred, y_pred, z_pred, dx_pred, dy_pred, dz_pred),
axis=1)
return anchors
def tf_anchor_to_offset(anchors, ground_truth):
"""Encodes the anchor regression predictions with the
ground truth.
This function assumes the ground_truth tensor has been arranged
in a way that each corresponding row in ground_truth, is matched
with that anchor according to the highest IoU.
For instance, the ground_truth might be a matrix of shape (256, 6)
of repeated entries for the original ground truth of shape (x, 6),
where each entry has been selected as the highest IoU match with that
anchor. This is different from the same function in numpy format, where
we loop through all the ground truth anchors, and calculate IoUs for
each and then select the match with the highest IoU.
Args:
anchors: A tensor of shape (N, 6) representing
the generated anchors.
ground_truth: A tensor of shape (N, 6) containing
the label boxes in the anchor format. Each ground-truth entry
has been matched with the anchor in the same entry as having
the highest IoU.
Returns:
anchor_offsets: A tensor of shape (N, 6)
encoded/normalized with the ground-truth, representing the
offsets.
"""
fc.check_anchor_format(anchors)
# Make sure anchors and anchor_gts have the same shape
dim_cond = tf.equal(tf.shape(anchors), tf.shape(ground_truth))
with tf.control_dependencies([dim_cond]):
t_x_gt = (ground_truth[:, 0] - anchors[:, 0]) / anchors[:, 3]
t_y_gt = (ground_truth[:, 1] - anchors[:, 1]) / anchors[:, 4]
t_z_gt = (ground_truth[:, 2] - anchors[:, 2]) / anchors[:, 5]
t_dx_gt = tf.log(ground_truth[:, 3] / anchors[:, 3])
t_dy_gt = tf.log(ground_truth[:, 4] / anchors[:, 4])
t_dz_gt = tf.log(ground_truth[:, 5] / anchors[:, 5])
anchor_offsets = tf.stack(
(t_x_gt, t_y_gt, t_z_gt, t_dx_gt, t_dy_gt, t_dz_gt), axis=1)
return anchor_offsets
def anchor_to_offset(anchors, ground_truth):
"""Encodes the anchor regression predictions with the
ground truth.
Args:
anchors: A numpy array of shape (N, 6) representing
the generated anchors.
ground_truth: A numpy array of shape (6,) containing
the label boxes in the anchor format.
Returns:
anchor_offsets: A numpy array of shape (N, 6)
encoded/normalized with the ground-truth, representing the
offsets.
"""
fc.check_anchor_format(anchors)
anchors = np.asarray(anchors).reshape(-1, 6)
ground_truth = np.reshape(ground_truth, (6, ))
# 这里的归一化不是根据grid size来做的，反而是根据anchor size来做的好特别。。。
# t_x_gt = (x_gt - x_anch)/dim_x_anch
t_x_gt = (ground_truth[0] - anchors[:, 0]) / anchors[:, 3]
# t_y_gt = (y_gt - y_anch)/dim_y_anch
t_y_gt = (ground_truth[1] - anchors[:, 1]) / anchors[:, 4]
# t_z_gt = (z_gt - z_anch)/dim_z_anch
t_z_gt = (ground_truth[2] - anchors[:, 2]) / anchors[:, 5]
# 长宽高可以是负无限。。。这个encoding很有问题呀
# t_dx_gt = log(dim_x_gt/dim_x_anch)
t_dx_gt = np.log(ground_truth[3] / anchors[:, 3])
# t_dy_gt = log(dim_y_gt/dim_y_anch)
t_dy_gt = np.log(ground_truth[4] / anchors[:, 4])
# t_dz_gt = log(dim_z_gt/dim_z_anch)
t_dz_gt = np.log(ground_truth[5] / anchors[:, 5])
anchor_offsets = np.stack(
(t_x_gt, t_y_gt, t_z_gt, t_dx_gt, t_dy_gt, t_dz_gt), axis=1)
return anchor_offsets
def test_check_anchor_format(self):
# Case 1, invalid type
test_var = [0, 0, 0, 0, 0, 0]
np.testing.assert_raises(TypeError, fc.check_anchor_format, test_var)
# Case 2, invalid shape
test_var = np.ones([1, 5])
np.testing.assert_raises(TypeError, fc.check_anchor_format, test_var)
test_var = np.ones([1, 6])
fc.check_anchor_format(test_var)
test_var = np.ones([5, 6])
fc.check_anchor_format(test_var)
test_var = tf.ones([5, 6])
fc.check_anchor_format(test_var)
test_var = tf.ones([5, 4])
np.testing.assert_raises(TypeError, fc.check_anchor_format, test_var)
def anchors_to_box_3d(anchors, fix_lw=False):
"""Converts an anchor form [x, y, z, dim_x, dim_y, dim_z]
to 3d box format of [x, y, z, l, w, h, ry]
Note: In this conversion, if the flag 'fix_lw' is set to true,
the box_3d 'length' will be the longer of dim_x and dim_z, and 'width'
will be the shorter dimension. All ry values are set to 0.
Args:
anchors: N x 6 ndarray of anchors in 'anchor' form
fix_lw: A boolean flag to switch width and length in the case
where width is longer than length.
Returns:
N x 7 ndarray of box_3d
"""
tensor_format = isinstance(anchors, tf.Tensor)
if tensor_format:
box_3d_x = anchors[:, 0]
box_3d_y = anchors[:, 1]
box_3d_z = anchors[:, 2]
box_3d_l = anchors[:, 3]
box_3d_w = anchors[:, 5]
box_3d_h = anchors[:, 4]
# This needs to be of shape N, so any of the box_3d elements
# will do.
box_3d_ry = tf.zeros_like(box_3d_x, dtype=tf.float32)
if fix_lw:
# Swap the width and length
swapped_indices = box_3d_w[:] > box_3d_l[:]
swap_idx_booleans = tf.cast(swapped_indices, dtype=tf.float32)
swap_idx_booleans_neg = tf.cast(tf.logical_not(swapped_indices),
dtype=tf.float32)
masked_dimx_p = tf.multiply(box_3d_l, swap_idx_booleans)
masked_dimx_n = tf.multiply(box_3d_w, swap_idx_booleans_neg)
masked_dimx = tf.add(masked_dimx_p, masked_dimx_n)
masked_dimz_n = tf.multiply(box_3d_l, swap_idx_booleans_neg)
masked_dimz_p = tf.multiply(box_3d_w, swap_idx_booleans)
masked_dimz = tf.add(masked_dimz_n, masked_dimz_p)
new_orientation = tf.ones(tf.shape(box_3d_ry), dtype=tf.float32)
# Assign 90 degrees orienation
new_orientation = tf.multiply(
new_orientation, tf.constant(-np.pi / 2, dtype=tf.float32))
masked_new_ry = tf.multiply(new_orientation, swap_idx_booleans)
masked_original_ry = tf.multiply(box_3d_ry, swap_idx_booleans)
masked_orientation = tf.add(masked_new_ry, masked_original_ry)
box_3d_l = tf.squeeze(masked_dimz)
box_3d_w = tf.squeeze(masked_dimx)
box_3d_ry = tf.squeeze(masked_orientation)
box_3d = tf.stack([
box_3d_x, box_3d_y, box_3d_z, box_3d_l, box_3d_w, box_3d_h,
box_3d_ry
],
axis=1)
else:
fc.check_anchor_format(anchors)
anchors = np.asarray(anchors)
box_3d = np.zeros((len(anchors), 7))
# Set x, y, z
box_3d[:, 0:3] = anchors[:, 0:3]
# Set length to dim_x
box_3d[:, 3] = anchors[:, 3]
# Set width to dim_z
box_3d[:, 4] = anchors[:, 5]
# Set height to dim_y
box_3d[:, 5] = anchors[:, 4]
box_3d[:, 6] = 0
if fix_lw:
swapped_indices = box_3d[:, 4] > box_3d[:, 3]
modified_box_3d = np.copy(box_3d)
modified_box_3d[swapped_indices, 3] = box_3d[swapped_indices, 4]
modified_box_3d[swapped_indices, 4] = box_3d[swapped_indices, 3]
modified_box_3d[swapped_indices, 6] = -np.pi / 2
return modified_box_3d
return box_3d