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