def rotate_nms_cc(dets, trackers):
trackers_corners = box_np_ops.center_to_corner_box2d(
trackers[:, :2], trackers[:, 2:4], trackers[:, 4] * np.pi / 180)
dets_corners = box_np_ops.center_to_corner_box2d(dets[:, :2], dets[:, 2:4],
dets[:, 4] * np.pi / 180)
standup_iou, standup_iou_new = get_iou(dets_corners, trackers_corners, 1)
return standup_iou, standup_iou_new
def rotate_nms_cc(dets, trackers):
trackers_corners = box_np_ops.center_to_corner_box2d(
trackers[:, :2], trackers[:, 2:4], trackers[:, 4])
trackers_standup = box_np_ops.corner_to_standup_nd(trackers_corners)
dets_corners = box_np_ops.center_to_corner_box2d(dets[:, :2], dets[:, 2:4],
dets[:, 4])
dets_standup = box_np_ops.corner_to_standup_nd(dets_corners)
standup_iou = box_np_ops.iou_jit(dets_standup, trackers_standup, eps=0.0)
return standup_iou
def sample_class_v2(self, name, num, gt_boxes):
sampled = self._sampler_dict[name].sample(num)
sampled = copy.deepcopy(sampled)
num_gt = gt_boxes.shape[0]
num_sampled = len(sampled)
if num_gt > 0:
gt_boxes_bv = box_np_ops.center_to_corner_box2d(
gt_boxes[:, 0:2], gt_boxes[:, 3:5], gt_boxes[:, 6])
sp_boxes = np.stack([i["box3d_lidar"] for i in sampled], axis=0)
valid_mask = np.zeros([gt_boxes.shape[0]], dtype=np.bool_)
valid_mask = np.concatenate(
[valid_mask,
np.ones([sp_boxes.shape[0]], dtype=np.bool_)], axis=0)
if num_gt > 0:
boxes = np.concatenate([gt_boxes, sp_boxes], axis=0).copy()
else:
boxes = sp_boxes
if self._enable_global_rot:
# place samples to any place in a circle.
prep.noise_per_object_v3_(
boxes,
None,
valid_mask,
0,
0,
self._global_rot_range,
num_try=100)
sp_boxes_new = boxes[gt_boxes.shape[0]:]
sp_boxes_bv = box_np_ops.center_to_corner_box2d(
sp_boxes_new[:, 0:2], sp_boxes_new[:, 3:5], sp_boxes_new[:, 6])
if num_gt > 0:
total_bv = np.concatenate([gt_boxes_bv, sp_boxes_bv], axis=0)
else:
total_bv = sp_boxes_bv
# coll_mat = collision_test_allbox(total_bv)
coll_mat = prep.box_collision_test(total_bv, total_bv)
diag = np.arange(total_bv.shape[0])
coll_mat[diag, diag] = False
valid_samples = []
for i in range(num_gt, num_gt + num_sampled):
if coll_mat[i].any():
coll_mat[i] = False
coll_mat[:, i] = False
else:
if self._enable_global_rot:
sampled[i - num_gt]["box3d_lidar"][:2] = boxes[i, :2]
sampled[i - num_gt]["box3d_lidar"][6] = boxes[i, 6]
sampled[i - num_gt]["rot_transform"] = (
boxes[i, 6] - sp_boxes[i - num_gt, 6])
valid_samples.append(sampled[i - num_gt])
return valid_samples
def sample_class_v2(self, name, num, gt_boxes):
sampled = self._sampler_dict[name].sample(
num) # 调用类别对应的BatchSampler内的sample方法，返回用来采样对象的信息
sampled = copy.deepcopy(sampled)
num_gt = gt_boxes.shape[0] # 该帧数据下原始真值数量
num_sampled = len(sampled) # 该帧数据下采样真值数量
gt_boxes_bv = box_np_ops.center_to_corner_box2d(
gt_boxes[:, 0:2], gt_boxes[:, 3:5], gt_boxes[:, 6]) # 生成原始真值鸟瞰图
sp_boxes = np.stack([i["box3d_lidar"] for i in sampled],
axis=0) # 读取采样真值框，横向排列
valid_mask = np.zeros([gt_boxes.shape[0]], dtype=np.bool_) # 原始真值框标为0
valid_mask = np.concatenate(
[valid_mask,
np.ones([sp_boxes.shape[0]], dtype=np.bool_)],
axis=0) # 将原始真值框标志0与采样真值框标志1拼起来
boxes = np.concatenate([gt_boxes, sp_boxes],
axis=0).copy() # 拼接原始真值框与采样真值框
if self._enable_global_rot: # False
# place samples to any place in a circle.
prep.noise_per_object_v3_(boxes,
None,
valid_mask,
0,
0,
self._global_rot_range,
num_try=100)
sp_boxes_new = boxes[gt_boxes.shape[0]:] # 添加扰动后的新采样框
sp_boxes_bv = box_np_ops.center_to_corner_box2d(
sp_boxes_new[:, 0:2], sp_boxes_new[:, 3:5],
sp_boxes_new[:, 6]) # 新采样框的鸟瞰图
total_bv = np.concatenate([gt_boxes_bv, sp_boxes_bv], axis=0) # 总鸟瞰图
# coll_mat = collision_test_allbox(total_bv)
coll_mat = prep.box_collision_test(total_bv,
total_bv) # 根据鸟瞰图判断相互之间是否碰撞，(采样，原始)
diag = np.arange(total_bv.shape[0])
coll_mat[diag, diag] = False # 对角线上，自己和自己不发生碰撞
valid_samples = []
for i in range(num_gt, num_gt + num_sampled):
if coll_mat[i].any():
coll_mat[i] = False
coll_mat[:, i] = False
else:
if self._enable_global_rot: # False
sampled[i - num_gt]["box3d_lidar"][:2] = boxes[i, :2]
sampled[i - num_gt]["box3d_lidar"][-1] = boxes[i, -1]
sampled[i - num_gt]["rot_transform"] = (
boxes[i, -1] - sp_boxes[i - num_gt, -1])
valid_samples.append(sampled[i - num_gt]) # 保留不冲突的采样对象的信息
return valid_samples
def draw_box_in_bev(img,
coors_range,
boxes,
color,
thickness=1,
labels=None,
label_color=None):
"""
Args:
boxes: center format.
"""
coors_range = np.array(coors_range)
bev_corners = box_np_ops.center_to_corner_box2d(boxes[:, [0, 1]],
boxes[:, [3, 4]], boxes[:,
6])
bev_corners -= coors_range[:2]
bev_corners *= np.array(
img.shape[:2])[::-1] / (coors_range[3:5] - coors_range[:2])
standup = box_np_ops.corner_to_standup_nd(bev_corners)
text_center = standup[:, 2:]
text_center[:, 1] -= (standup[:, 3] - standup[:, 1]) / 2
bev_lines = np.concatenate(
[bev_corners[:, [0, 2, 3]], bev_corners[:, [1, 3, 0]]], axis=2)
bev_lines = bev_lines.reshape(-1, 4)
colors = np.tile(np.array(color).reshape(1, 3), [bev_lines.shape[0], 1])
colors = colors.astype(np.int32)
img = cv2_draw_lines(img, bev_lines, colors, thickness)
if boxes.shape[1] == 9:
# draw velocity arrows
for box in boxes:
velo = box[-2:]
# velo = np.array([-np.sin(box[6]), -np.cos(box[6])])
velo_unified = velo
if np.isnan(velo_unified[0]):
continue
velo_unified = velo_unified * np.array(
img.shape[:2])[::-1] / (coors_range[3:5] - coors_range[:2])
center = box[:2] - coors_range[:2]
center = center * np.array(
img.shape[:2])[::-1] / (coors_range[3:5] - coors_range[:2])
center = tuple(map(lambda x: int(x), center))
center2 = tuple(map(lambda x: int(x), center + velo_unified))
cv2.arrowedLine(img,
center,
center2,
color,
thickness,
tipLength=0.3)
if labels is not None:
if label_color is None:
label_color = colors
else:
label_color = np.tile(
np.array(label_color).reshape(1, 3), [bev_lines.shape[0], 1])
label_color = label_color.astype(np.int32)
img = cv2_draw_text(img, text_center, labels, label_color, thickness)
return img
def rotate_nms_cc(dets, thresh):
scores = dets[:, 5]
order = scores.argsort()[::-1].astype(np.int32) # highest->lowest
dets_corners = box_np_ops.center_to_corner_box2d(dets[:, :2], dets[:, 2:4],
dets[:, 4])
dets_standup = box_np_ops.corner_to_standup_nd(dets_corners)
standup_iou = box_np_ops.iou_jit(dets_standup, dets_standup, eps=0.0)
# print(dets_corners.shape, order.shape, standup_iou.shape)
return rotate_non_max_suppression_cpu(dets_corners, order, standup_iou,
thresh)
def get_min_max(trackers_cov, width_coe):
trackers_cov[:, 3] *= width_coe
trackers_cov_corner = box_np_ops.center_to_corner_box2d(
trackers_cov[:, :2], trackers_cov[:, 3:5],
trackers_cov[:, 6] * np.pi / 180)
# min_x_and_y_sta = np.min(trackers_cov_corner, axis=1)
# max_x_and_y_sta = np.max(trackers_cov_corner, axis=1)
# add_dimension_sta = np.concatenate((min_x_and_y_sta, max_x_and_y_sta), axis=1)
# trackers_cov = np.hstack((trackers_cov, add_dimension_sta))
trackers_cov[:, 3] = trackers_cov[:, 3] / width_coe
return trackers_cov, trackers_cov_corner
def rotate_nms_cc(dets, trackers, usespeed=0):
'''数据形式：x,y,l,w,vx,vy,theta'''
trackers_corners = box_np_ops.center_to_corner_box2d(
trackers[:, :2], trackers[:, 2:4], trackers[:, 6])
trackers_standup = box_np_ops.corner_to_standup_nd(trackers_corners)
dets_corners = box_np_ops.center_to_corner_box2d(dets[:, :2], dets[:, 2:4],
dets[:, 6])
dets_standup = box_np_ops.corner_to_standup_nd(dets_corners)
standup_iou = box_np_ops.iou_jit(dets_standup, trackers_standup,
eps=0.0) #位置iou
# print('standup_iou',standup_iou)
# 速度的iou
if usespeed == 1:
trackers_boxlist = IOU_Calculation.point_to_box(
trackers[:, 4:6], 0.4, 0.2) #vx,vy,vx_width,vy_width
dets_boxlist = IOU_Calculation.point_to_box(dets[:, 4:6], 0.4, 0.2)
speed_iou = IOU_Calculation.iou_matrix(dets_boxlist, trackers_boxlist)
# print('speed_iou', speed_iou)
IOU_matrix = standup_iou * 0.8 + speed_iou * 0.2 #分配权重
return IOU_matrix
else:
return standup_iou
def filter_gt_box_outside_range(gt_boxes, limit_range):
"""remove gtbox outside training range.
this function should be applied after other prep functions
Args:
gt_boxes ([type]): [description]
limit_range ([type]): [description]
"""
gt_boxes_bv = box_np_ops.center_to_corner_box2d(
gt_boxes[:, [0, 1]], gt_boxes[:, [3, 3 + 1]], gt_boxes[:, 6])
bounding_box = box_np_ops.minmax_to_corner_2d(
np.asarray(limit_range)[np.newaxis, ...])
ret = points_in_convex_polygon_jit(
gt_boxes_bv.reshape(-1, 2), bounding_box)
return np.any(ret.reshape(-1, 4), axis=1)
def _fine_sample_by_grd(sampled_cls, grd_gridmask, voxel_size, coord_range,
grid_size):
new_sampled_cls = []
for s in sampled_cls:
boxes = s['box3d_lidar'][np.newaxis, ...]
boxes_bv = box_np_ops.center_to_corner_box2d(boxes[:, 0:2],
boxes[:, 3:5], boxes[:,
6])
boxes_bv = np.reshape(boxes_bv, [-1, 2])
voxel_idx = _points_to_voxelidx_2d(boxes_bv, voxel_size[:2],
coord_range[:2], grid_size[:2])
if np.all(grd_gridmask[tuple(voxel_idx.T)]):
new_sampled_cls.append(s)
#print(len(sampled_cls), '->', len(new_sampled_cls))
return new_sampled_cls
def _riou3d_shapely(rbboxes1, rbboxes2):
N, K = rbboxes1.shape[0], rbboxes2.shape[0]
corners1 = box_np_ops.center_to_corner_box2d(rbboxes1[:, :2],
rbboxes1[:, 3:5], rbboxes1[:,
6])
corners2 = box_np_ops.center_to_corner_box2d(rbboxes2[:, :2],
rbboxes2[:, 3:5], rbboxes2[:,
6])
iou = np.zeros([N, K], dtype=np.float32)
for i in range(N):
for j in range(K):
iw = (min(rbboxes1[i, 2] + rbboxes1[i, 5], rbboxes2[j, 2] +
rbboxes2[j, 5]) - max(rbboxes1[i, 2], rbboxes2[j, 2]))
if iw > 0:
p1 = Polygon(corners1[i])
p2 = Polygon(corners2[j])
inc = p1.intersection(p2).area * iw
# inc = p1.intersection(p2).area
if inc > 0:
iou[i, j] = inc / (p1.area * rbboxes1[i, 5] +
p2.area * rbboxes2[j, 5] - inc)
# iou[i, j] = inc / (p1.area + p2.area - inc)
return iou
def rotate_nms_3d_cc(dets, thresh, flag):
assert dets.shape[1] == 8
scores = dets[:, -1]
ious_3d = boxes_iou_3d(dets[:, 0:7],
dets[:, 0:7],
aug_thickness=None,
criterion=-1,
flag=flag)
ious_3d = ious_3d.cpu().data.numpy()
order = scores.argsort().cpu().data.numpy().astype(
np.int32)[::-1] # highest->lowest
dets_np = dets.cpu().data.numpy()
dets_corners = box_np_ops.center_to_corner_box2d(dets_np[:, :2],
dets_np[:, 3:5],
dets_np[:, 6])
#dets_standup = box_np_ops.corner_to_standup_nd(dets_corners)
#standup_iou = box_np_ops.iou_jit(dets_standup, dets_standup, eps=0.0)
# print(dets_corners.shape, order.shape, standup_iou.shape)
indices = rotate_non_max_suppression_cpu(dets_corners, order, ious_3d,
thresh)
return indices
def draw_box_in_bev(img,
coors_range,
boxes,
color,
thickness=1,
labels=None,
label_color=None):
"""
Args:
boxes: center format.
"""
coors_range = np.array(coors_range)
bev_corners = box_np_ops.center_to_corner_box2d(boxes[:, [0, 1]],
boxes[:, [3, 4]], boxes[:,
6])
bev_corners -= coors_range[:2]
bev_corners *= np.array(
img.shape[:2])[::-1] / (coors_range[3:5] - coors_range[:2])
standup = box_np_ops.corner_to_standup_nd(bev_corners)
text_center = standup[:, 2:]
text_center[:, 1] -= (standup[:, 3] - standup[:, 1]) / 2
bev_lines = np.concatenate(
[bev_corners[:, [0, 2, 3]], bev_corners[:, [1, 3, 0]]], axis=2)
bev_lines = bev_lines.reshape(-1, 4)
colors = np.tile(np.array(color).reshape(1, 3), [bev_lines.shape[0], 1])
colors = colors.astype(np.int32)
img = cv2_draw_lines(img, bev_lines, colors, thickness)
if labels is not None:
if label_color is None:
label_color = colors
else:
label_color = np.tile(
np.array(label_color).reshape(1, 3), [bev_lines.shape[0], 1])
label_color = label_color.astype(np.int32)
img = cv2_draw_text(img, text_center, labels, label_color,
thickness * 2)
return img
def sample_group(self, name, num, gt_boxes, gt_group_ids):
sampled, group_num = self.sample(name, num)
sampled = copy.deepcopy(sampled)
# rewrite sampled group id to avoid duplicated with gt group ids
gid_map = {}
max_gt_gid = np.max(gt_group_ids)
sampled_gid = max_gt_gid + 1
for s in sampled:
gid = s["group_id"]
if gid in gid_map:
s["group_id"] = gid_map[gid]
else:
gid_map[gid] = sampled_gid
s["group_id"] = sampled_gid
sampled_gid += 1
num_gt = gt_boxes.shape[0]
gt_boxes_bv = box_np_ops.center_to_corner_box2d(
gt_boxes[:, 0:2], gt_boxes[:, 3:5], gt_boxes[:, 6])
sp_boxes = np.stack([i["box3d_lidar"] for i in sampled], axis=0)
sp_group_ids = np.stack([i["group_id"] for i in sampled], axis=0)
valid_mask = np.zeros([gt_boxes.shape[0]], dtype=np.bool_)
valid_mask = np.concatenate(
[valid_mask,
np.ones([sp_boxes.shape[0]], dtype=np.bool_)], axis=0)
boxes = np.concatenate([gt_boxes, sp_boxes], axis=0).copy()
group_ids = np.concatenate([gt_group_ids, sp_group_ids], axis=0)
if self._enable_global_rot:
# place samples to any place in a circle.
prep.noise_per_object_v3_(boxes,
None,
valid_mask,
0,
0,
self._global_rot_range,
group_ids=group_ids,
num_try=100)
sp_boxes_new = boxes[gt_boxes.shape[0]:]
sp_boxes_bv = box_np_ops.center_to_corner_box2d(
sp_boxes_new[:, 0:2], sp_boxes_new[:, 3:5], sp_boxes_new[:, 6])
total_bv = np.concatenate([gt_boxes_bv, sp_boxes_bv], axis=0)
# coll_mat = collision_test_allbox(total_bv)
coll_mat = prep.box_collision_test(total_bv, total_bv)
diag = np.arange(total_bv.shape[0])
coll_mat[diag, diag] = False
valid_samples = []
idx = num_gt
for num in group_num:
if coll_mat[idx:idx + num].any():
coll_mat[idx:idx + num] = False
coll_mat[:, idx:idx + num] = False
else:
for i in range(num):
if self._enable_global_rot:
sampled[idx - num_gt +
i]["box3d_lidar"][:2] = boxes[idx + i, :2]
sampled[idx - num_gt +
i]["box3d_lidar"][-1] = boxes[idx + i, -1]
sampled[idx - num_gt + i]["rot_transform"] = (
boxes[idx + i, -1] -
sp_boxes[idx + i - num_gt, -1])
valid_samples.append(sampled[idx - num_gt + i])
idx += num
return valid_samples
def rotate_nms_merge(dets, thresh):
'''
input dets_corners, order, standup_iou,thresh
return bbox_list ( not inds, cause we merges the bboxes, new boxes is not in the original boxlist)
we can use score or centerness as weight to get better merge method
parameter for merge nms
iou_thresh : > thresh hold , then we add those bboxes to same list, and merge them
intersect_bbox_nums_thresh : numbers of intersection bboxes > threshold , including itself, if the number is too small, we can ignore the bboxes
TODO:
score weight : with or without centerness.sigmoid
'''
scores = dets[:, 5]
order = scores.argsort()[::-1].astype(np.int32) # highest->lowest
dets_corners = box_np_ops.center_to_corner_box2d(dets[:, :2], dets[:, 2:4],
dets[:, 4])
dets_standup = box_np_ops.corner_to_standup_nd(dets_corners)
standup_iou = box_np_ops.iou_jit(dets_standup, dets_standup, eps=0.0)
unmerged_boxlist = []
while order.size > 0:
i = order[0]
merged_boxes_ind = np.where(standup_iou[i] > thresh)
unmerged_boxlist.append(merged_boxes_ind[0])
# filter out the merge inds in the order
for ele in merged_boxes_ind[0]:
if ele in order:
remove_inds = np.where(order == ele)[0]
order = np.delete(order, remove_inds[0])
return unmerged_boxlist
#depreciate : moving anchor to absord the bbox
merged_boxes_num = 0
rounds = 0
thresh = 0.1
intersect_bbox_nums_thresh = 1
start = 0
while order.size > 0:
print('we have {} bboxes in round {}'.format(order.size, rounds))
i = order[0]
merged_boxes_ind = np.where(standup_iou[i] >= thresh)
# we can set a thresh hold for intersetction number, if intersetion bbox num too small, we give up the bbox
if merged_boxes_ind[0].size > intersect_bbox_nums_thresh:
print('round {} have intersection{} nums'.format(
rounds, merged_boxes_ind[0].size))
if not merged_boxes_num in unmerged_boxlist:
unmerged_boxlist[merged_boxes_num] = merged_boxes_ind
unmerged_boxes_inds = np.where(standup_iou[i] < thresh)
else:
unmerged_boxlist[merged_boxes_num] = np.concatenate(
(unmerged_boxlist[merged_boxes_num], merged_boxes_ind[1:]),
axis=0)
unmerged_boxes_inds = np.where(standup_iou[i] < thresh)
else:
order = order[unmerged_boxes_inds + 1]
#rounds+=1
final_bboxes_list = []
total_box_check = 0
for key, bboxes in unmerged_boxlist.items():
final_bboxes_list.append(bboxes.mean(axis=0))
total_box_check += unmerged_boxlist[key].shape[0]
embed()
return final_bboxes_list
def draw_box_in_bev(img,
coors_range,
boxes,
color,
thickness=1,
labels=None,
label_color=None):
"""
Args:
boxes: center format.
"""
# img: (600, 1000, 3)
# coors_range: array([-50, -30, -3, 50, 30, 1])
# boxes: N, 7: x,y,z,w,l,h,theta
#import pdb; pdb.set_trace()
coors_range = np.array(coors_range)
bev_corners = box_np_ops.center_to_corner_box2d(
boxes[:, [0, 1]], boxes[:, [3, 4]], boxes[:, 6])
# [x, y], [w, l], [theta]
# bev_corners: N, 4, 2
bev_corners -= coors_range[:2]
bev_corners *= np.array(
img.shape[:2])[::-1] / (coors_range[3:5] - coors_range[:2])
#[1000, 600] / [100, 60] => multiply by 10 to all coords.
standup = box_np_ops.corner_to_standup_nd(bev_corners) # [30, 4]
text_center = standup[:, 2:]
text_center[:, 1] -= (standup[:, 3] - standup[:, 1]) / 2
bev_lines = np.concatenate(
[bev_corners[:, [0, 1, 2, 3]], bev_corners[:, [1, 2, 3, 0]]], axis=2)
# bev_lines.shape = N, 3, 4
bev_lines = bev_lines.reshape(-1, 4)
colors = np.tile(np.array(color).reshape(1, 3), [bev_lines.shape[0], 1])
colors = colors.astype(np.int32)
img = cv2_draw_lines(img, bev_lines, colors, thickness)
if boxes.shape[1] == 9:
# draw velocity arrows
for box in boxes:
velo = box[-2:]
# velo = np.array([-np.sin(box[6]), -np.cos(box[6])])
velo_unified = velo
if np.isnan(velo_unified[0]):
continue
velo_unified = velo_unified * np.array(
img.shape[:2])[::-1] / (coors_range[3:5] - coors_range[:2])
center = box[:2] - coors_range[:2]
center = center * np.array(
img.shape[:2])[::-1] / (coors_range[3:5] - coors_range[:2])
center = tuple(map(lambda x: int(x), center))
center2 = tuple(map(lambda x: int(x), center + velo_unified))
cv2.arrowedLine(img, center, center2, color, thickness, tipLength=0.3)
if labels is not None:
if label_color is None:
label_color = colors
else:
label_color = np.tile(
np.array(label_color).reshape(1, 3), [bev_lines.shape[0], 1])
label_color = label_color.astype(np.int32)
img = cv2_draw_text(img, text_center, labels, label_color,
thickness)
return img
