def project_kitti_box_to_image(box: Box, p_left: np.ndarray, imsize: Tuple[int, int]) \
-> Union[None, Tuple[int, int, int, int]]:
"""
Projects 3D box into image FOV.
:param box: 3D box in camera coordinate
:param p_left: <np.float: 3, 4>. Projection matrix.
:param imsize: (width, height). Image size.
:return: (xmin, ymin, xmax, ymax). Bounding box in image plane or None if box is not in the image.
"""
# Check that some corners are inside the image.
corners = np.array([corner for corner in box.corners().T
if corner[2] > 0]).T
if len(corners) == 0:
return None
# Project corners that are in front of the camera to 2d to get bbox in pixel coords.
imcorners = view_points(corners, p_left, normalize=True)[:2]
bbox = (np.min(imcorners[0]), np.min(imcorners[1]), np.max(imcorners[0]),
np.max(imcorners[1]))
# Crop bbox to prevent it extending outside image.
bbox_crop = tuple(max(0, b) for b in bbox)
bbox_crop = (min(imsize[0], bbox_crop[0]), min(imsize[0], bbox_crop[1]),
min(imsize[0], bbox_crop[2]), min(imsize[1], bbox_crop[3]))
# Detect if a cropped box is empty.
if bbox_crop[0] >= bbox_crop[2] or bbox_crop[1] >= bbox_crop[3]:
return None
return bbox_crop
def project_kitti_box_to_image(
box: Box, p_left: np.ndarray,
imsize: Tuple[int, int]) -> Tuple[int, int, int, int]:
"""
Projects 3D box into KITTI image FOV.
:param box: 3D box in KITTI reference frame.
:param p_left: <np.float: 3, 4>. Projection matrix.
:param imsize: (width , height). Image size.
:return: (xmin, ymin, xmax, ymax). Bounding box in image plane.
"""
# Create a new box.
box = box.copy()
# KITTI defines the box center as the bottom center of the object.
# We use the true center, so we need to adjust half height in negative y direction.
box.translate(np.array([0, -box.wlh[2] / 2, 0]))
# Project corners to 2d to get bbox in pixel coords.
corners = np.array(
[corner for corner in box.corners().T if corner[2] > 0]).T
imcorners = view_points(corners, p_left, normalize=True)[:2]
bbox = (np.min(imcorners[0]), np.min(imcorners[1]),
np.max(imcorners[0]), np.max(imcorners[1]))
# Crop bbox to prevent it extending outside image
bbox_crop = (max(0, bbox[0]), max(0, bbox[1]), min(imsize[0], bbox[2]),
min(imsize[1], bbox[3]))
return bbox_crop
def cropimg(img: Image, bbox: Box, camera_intrinsic: np.ndarray) -> Image:
bbox_img = view_points(bbox.corners(), view=camera_intrinsic, normalize=True)
# in case the annotation runs off the edge of the page
bbox_img[bbox_img < 0] = 0
bbox_img[0, bbox_img[0, :] > img.size[0]] = img.size[0]-1
bbox_img[1, bbox_img[1, :] > img.size[1]] = img.size[1]-1
# crop
return img.crop((min(bbox_img[0]), min(bbox_img[1]), max(bbox_img[0]), max(bbox_img[1])))
def bev_iou(self, det1, det2, dist_thresh=2):
ious = []
for det in det2:
dist = np.linalg.norm(
np.array(det1['translation'][:2]) -
np.array(det['translation'][:2]))
if dist > dist_thresh:
ious.append(0)
continue
box1 = Box(det1['translation'], det1['size'],
Quaternion(det1['rotation']))
box2 = Box(det['translation'], det['size'],
Quaternion(det['rotation']))
iou, iou_2d = iou3d_global(box1.corners(), box2.corners())
ious.append(iou_2d)
return ious
def project_kitti_box_to_image(
box: Box, p_left: np.ndarray,
imsize: Tuple[int, int]) -> Union[None, Tuple[int, int, int, int]]:
"""
Projects 3D box into KITTI image FOV.
:param box: 3D box in KITTI reference frame.
:param p_left: <np.float: 3, 4>. Projection matrix.
:param imsize: (width, height). Image size.
:return: (xmin, ymin, xmax, ymax). Bounding box in image plane or None if box is not in the image.
"""
# Create a new box.
# box = box.copy()
# KITTI defines the box center as the bottom center of the object.
# We use the true center, so we need to adjust half height in negative y direction.
box.translate(np.array([0, -box.wlh[2] / 2, 0]))
# Check that some corners are inside the image.
corners = np.array(
[corner for corner in box.corners().T if corner[2] > 0]).T
if len(corners) == 0:
return None
# Project corners that are in front of the camera to 2d to get bbox in pixel coords.
imcorners = view_points(corners, p_left, normalize=True)[:2]
bbox = (np.min(imcorners[0]), np.min(imcorners[1]),
np.max(imcorners[0]), np.max(imcorners[1]))
# Crop bbox to prevent it extending outside image.
bbox_crop = tuple(max(0, b) for b in bbox)
bbox_crop = (
min(imsize[0], bbox_crop[0]),
min(imsize[0], bbox_crop[1]),
min(imsize[0], bbox_crop[2]),
min(imsize[1], bbox_crop[3]),
)
# Detect if a cropped box is empty.
if bbox_crop[0] >= bbox_crop[2] or bbox_crop[1] >= bbox_crop[3]:
return None
return bbox_crop
def render(self, events: DataFrame, timestamp: int, frame_gt: List[TrackingBox], frame_pred: List[TrackingBox], points=None, pose_record=None, cs_record=None, ifplotgt=False,\
threshold=0.1, ifpltsco=False, outscen_class=False,nusc=None, ifplthis=False, pltve=False) \
-> None:
"""
Render function for a given scene timestamp
:param events: motmetrics events for that particular
:param timestamp: timestamp for the rendering
:param frame_gt: list of ground truth boxes
:param frame_pred: list of prediction boxes
"""
# Init.
#print('Rendering {}'.format(timestamp))
switches = events[events.Type == 'SWITCH']
switch_ids = switches.HId.values #对应frame_pred的tracking_id
switch_gt_ids = switches.OId.values #对应GT的tracking_id
FN = events[events.Type == 'MISS']
#FN = FN.HId.values
FN = FN.OId.values #对应GT的tracking_id
FP = events[events.Type == 'FP']
FP = FP.HId.values #对应frame_pred的tracking_id
fig, ax = plt.subplots()
#plt.style.use('dark_background') # 黑 背景颜色
plt.style.use('classic') # 白 背景颜色
points.render_height(ax,
view=np.eye(4),
x_lim=(-50, 50),
y_lim=(-50, 50),
marker_size=0.1) #BEV
#points.render_height(ax, view=np.eye(4) )#BEV
#points = points.rotate(Quaternion( pose_record["rotation"]).inverse)
sam_anno = []
if len(frame_gt) > 0:
sample = nusc.get('sample', frame_gt[0].sample_token)
sample_annotation_tokens = sample['anns'] #标注
for anno_token in sample_annotation_tokens:
sam_anno.append(
nusc.get('sample_annotation',
anno_token)["instance_token"])
vislev = {'v0-40': 0, 'v40-60': 1, 'v60-80': 2, 'v80-100': 3}
#points.render_intensity(ax)
# Plot GT boxes.
if ifplotgt:
for i, b in enumerate(frame_gt):
color = 'k'
#qua = tuple(self.list_add(list(b.rotation),[0.924,0.0,0.0,0.383]))
box = Box(np.array(b.ego_translation, dtype=float),
b.size,
Quaternion(b.rotation),
name=b.tracking_name,
token=b.tracking_id)
#qua = tuple(self.list_add(list(pose_record["rotation"]),[ 0.831,0.0,0.0,0.556]))
#box.translate(-np.array(pose_record["translation"]))
box.rotate(Quaternion(pose_record["rotation"]).inverse)
# move box to sensor coord system
box.translate(-np.array(cs_record["translation"]))
box.rotate(Quaternion(cs_record["rotation"]).inverse)
if outscen_class:
#####TrackingRenderer.gt_ptsnumrange = {'0-5nums':0, '5-10nums':0, '10-50nums':0, '50-200nums':0, '>200nums': 0 } #car lidar点云数
#####TrackingRenderer.gt_ptsnumrange = {'0-5nums':0, '5-10nums':0, '10-20nums':0, '20-30nums':0, '>30nums': 0 } #ped lidar点云数
num_pts = frame_gt[i].num_pts
#####car
if TrackingRenderer.outscen == 'car':
if num_pts > 0 and num_pts <= 5:
TrackingRenderer.gt_ptsnumrange['0-5nums'] += 1
elif num_pts > 5 and num_pts <= 10:
TrackingRenderer.gt_ptsnumrange['5-10nums'] += 1
elif num_pts > 10 and num_pts <= 50:
TrackingRenderer.gt_ptsnumrange['10-50nums'] += 1
elif num_pts > 50 and num_pts <= 200:
TrackingRenderer.gt_ptsnumrange['50-200nums'] += 1
elif num_pts > 200:
TrackingRenderer.gt_ptsnumrange['>200nums'] += 1
else:
####ped
if num_pts > 0 and num_pts <= 5:
TrackingRenderer.gt_ptsnumrange['0-5nums'] += 1
elif num_pts > 5 and num_pts <= 10:
TrackingRenderer.gt_ptsnumrange['5-10nums'] += 1
elif num_pts > 10 and num_pts <= 20:
TrackingRenderer.gt_ptsnumrange['10-20nums'] += 1
elif num_pts > 20 and num_pts <= 30:
TrackingRenderer.gt_ptsnumrange['20-30nums'] += 1
elif num_pts > 30:
TrackingRenderer.gt_ptsnumrange['>30nums'] += 1
if box.token in FN:
#####distance
dis = math.sqrt(box.center[0]**2 + box.center[1]**2)
if dis > 0 and dis <= 15:
TrackingRenderer.fn_disrange[
'<15m'] = TrackingRenderer.fn_disrange[
'<15m'] + 1
elif dis > 15 and dis <= 30:
TrackingRenderer.fn_disrange[
'15-30m'] = TrackingRenderer.fn_disrange[
'15-30m'] + 1
elif dis > 30 and dis <= 45:
TrackingRenderer.fn_disrange[
'30-45m'] = TrackingRenderer.fn_disrange[
'30-45m'] + 1
elif dis > 45 and dis <= 54:
TrackingRenderer.fn_disrange[
'45-54m'] = TrackingRenderer.fn_disrange[
'45-54m'] + 1
else:
TrackingRenderer.fn_disrange[
'-1'] = TrackingRenderer.fn_disrange['-1'] + 1
#####ve TrackingRenderer.fn_verange = {'0-0.1m/s':0, '0.1-2.5m/s':0, '2.5-5m/s':0, '5-10m/s':0, '>10m/s': 0 } #car 绝对速度
##### TrackingRenderer.fn_verange = {'0-0.1m/s':0, '0.1-1.0m/s':0, '1.0-1.5m/s':0, '1.5-2m/s':0, '>2m/s': 0 } #ped 绝对速度
ve = math.sqrt(frame_gt[i].velocity[0]**2 +
frame_gt[i].velocity[1]**2)
if TrackingRenderer.outscen == 'car':
if ve > 0 and ve <= 0.1:
TrackingRenderer.fn_verange['0-0.1m/s'] += 1
elif ve > 0.1 and ve <= 2.5:
TrackingRenderer.fn_verange['0.1-2.5m/s'] += 1
elif ve > 2.5 and ve <= 5:
TrackingRenderer.fn_verange['2.5-5m/s'] += 1
elif ve > 5 and ve <= 10:
TrackingRenderer.fn_verange['5-10m/s'] += 1
else:
TrackingRenderer.fn_verange['>10m/s'] += 1
else:
if ve > 0 and ve <= 0.1:
TrackingRenderer.fn_verange['0-0.1m/s'] += 1
elif ve > 0.1 and ve <= 1.0:
TrackingRenderer.fn_verange['0.1-1.0m/s'] += 1
elif ve > 1.0 and ve <= 1.5:
TrackingRenderer.fn_verange['1.0-1.5m/s'] += 1
elif ve > 1.5 and ve <= 2:
TrackingRenderer.fn_verange['1.5-2m/s'] += 1
else:
TrackingRenderer.fn_verange['>2m/s'] += 1
#####num_pts TrackingRenderer.fn_ptsnumrange = {'0-5nums':0, '5-10nums':0, '10-50nums':0, '50-200nums':0, '>200nums': 0 } #car lidar点云数 抽样参考比例：0.21 0.23 0.26，0.2，0.1
#############TrackingRenderer.fn_ptsnumrange = {'0-5nums':0, '5-10nums':0, '10-20nums':0, '20-30nums':0, '>30nums': 0 } #ped lidar点云数
num_pts = frame_gt[i].num_pts
if TrackingRenderer.outscen == 'car':
if num_pts > 0 and num_pts <= 5:
TrackingRenderer.fn_ptsnumrange['0-5nums'] += 1
elif num_pts > 5 and num_pts <= 10:
TrackingRenderer.fn_ptsnumrange[
'5-10nums'] += 1
elif num_pts > 10 and num_pts <= 50:
TrackingRenderer.fn_ptsnumrange[
'10-50nums'] += 1
elif num_pts > 50 and num_pts <= 200:
TrackingRenderer.fn_ptsnumrange[
'50-200nums'] += 1
elif num_pts > 200:
TrackingRenderer.fn_ptsnumrange[
'>200nums'] += 1
else:
TrackingRenderer.fn_ptsnumrange['-1'] += 1
else:
if num_pts > 0 and num_pts <= 5:
TrackingRenderer.fn_ptsnumrange['0-5nums'] += 1
elif num_pts > 5 and num_pts <= 10:
TrackingRenderer.fn_ptsnumrange[
'5-10nums'] += 1
elif num_pts > 10 and num_pts <= 20:
TrackingRenderer.fn_ptsnumrange[
'10-20nums'] += 1
elif num_pts > 20 and num_pts <= 30:
TrackingRenderer.fn_ptsnumrange[
'20-30nums'] += 1
elif num_pts > 200:
TrackingRenderer.fn_ptsnumrange['>30nums'] += 1
else:
TrackingRenderer.fn_ptsnumrange['-1'] += 1
######读取
#sample = nusc.get('sample', frame_gt[i].sample_token)
#sample_annotation_tokens = sample['anns'] #标注
try:
ind = sam_anno.index(b.tracking_id)
sample_annotation = nusc.get(
'sample_annotation',
sample_annotation_tokens[ind])
####TrackingRenderer.vis_ratio = {'0-0.4':0, '0.4-0.6':0, '0.6-0.8':0, '0.8-1.0':0} #相机视角 0-40%, 40-60%, 60-80% and 80-100% The visibility of an instance is the fraction of annotation visible in all 6 images.
visibility = nusc.get(
'visibility',
sample_annotation['visibility_token'])
vis_level = vislev[visibility["level"]]
if vis_level == 0:
TrackingRenderer.vis_ratio['0-0.4'] += 1
elif vis_level == 1:
TrackingRenderer.vis_ratio['0.4-0.6'] += 1
elif vis_level == 2:
TrackingRenderer.vis_ratio['0.6-0.8'] += 1
elif vis_level == 3:
TrackingRenderer.vis_ratio['0.8-1.0'] += 1
####TrackingRenderer.gt_ratio = {'ang_muta':0, 've_muta':0, 'aug_other':0, 've_other':0, 'firfn_trk':0, 'nonfirfn_trk':0} #仅包含与上一帧持续追踪的样本，角度和速度突变分别与other相并
pre_token = sample_annotation['prev']
if pre_token == '': #仅作为first_FN
TrackingRenderer.gt_ratio['firfn_trk'] += 1
else:
TrackingRenderer.gt_ratio['nonfirfn_trk'] += 1
pre_annotation = nusc.get(
'sample_annotation', pre_token)
vari_ang = abs(
R.from_quat(list(frame_gt[i].rotation)).
as_euler('zxy', degrees=False)[0] -
R.from_quat(
list(pre_annotation['rotation'])
).as_euler('zxy', degrees=False)[0])
if vari_ang > 0.52: #30度
TrackingRenderer.gt_ratio['angvar>30'] += 1
elif 0.52 > vari_ang and vari_ang > 0.35:
TrackingRenderer.gt_ratio[
'30>angvar>20'] += 1
elif 0.35 > vari_ang and vari_ang > 0.17:
TrackingRenderer.gt_ratio[
'20>angvar>10'] += 1
elif vari_ang < 0.17:
TrackingRenderer.gt_ratio['10>angvar'] += 1
else:
pass
pre_ve = nusc.box_velocity(
pre_annotation['token'])
ve_varity = abs(ve - math.sqrt(pre_ve[0]**2 +
pre_ve[1]**2))
if ve_varity > TrackingRenderer.mutave_thr[0]:
TrackingRenderer.gt_ratio[
'vevari>%s' %
(TrackingRenderer.mutave_thr[0])] += 1
elif ve_varity < TrackingRenderer.mutave_thr[
0] and ve_varity >= TrackingRenderer.mutave_thr[
1]:
TrackingRenderer.gt_ratio[
'%s<vevari<%s' %
(TrackingRenderer.mutave_thr[1],
TrackingRenderer.mutave_thr[0])] += 1
elif ve_varity < TrackingRenderer.mutave_thr[
1] and ve_varity >= TrackingRenderer.mutave_thr[
2]:
TrackingRenderer.gt_ratio[
'%s<vevari<%s' %
(TrackingRenderer.mutave_thr[2],
TrackingRenderer.mutave_thr[1])] += 1
else:
TrackingRenderer.gt_ratio[
'vevari<%s' %
TrackingRenderer.mutave_thr[2]] += 1
except ValueError: #标注错误
TrackingRenderer.fault_datas += 1
box.render(ax,
view=np.eye(4),
colors=(color, color, color),
linewidth=1)
else:
pass
# Plot predicted boxes.
pred_trackid = []
for i, b in enumerate(frame_pred):
box = Box(
b.ego_translation,
b.size,
Quaternion(b.rotation),
name=b.tracking_name,
token=b.tracking_id,
score=b.tracking_score,
)
# move box to ego vehicle coord system before has done the translation
box.rotate(Quaternion(pose_record["rotation"]).inverse)
# move box to sensor coord system
box.translate(-np.array(cs_record["translation"]))
box.rotate(Quaternion(cs_record["rotation"]).inverse)
pred_trackid.append(b.tracking_id)
if outscen_class:
if b.tracking_id in FP:
#####distance TrackingRenderer.fp_disrange = {'<15m':0, '15-30m':0, '30-45m':0, '45-54m':0}
dis = math.sqrt(box.center[0]**2 + box.center[1]**2)
if dis > 0 and dis <= 15:
TrackingRenderer.fp_disrange[
'<15m'] = TrackingRenderer.fp_disrange['<15m'] + 1
elif dis > 15 and dis <= 30:
TrackingRenderer.fp_disrange[
'15-30m'] = TrackingRenderer.fp_disrange[
'15-30m'] + 1
elif dis > 30 and dis <= 45:
TrackingRenderer.fp_disrange[
'30-45m'] = TrackingRenderer.fp_disrange[
'30-45m'] + 1
elif dis > 45 and dis <= 54:
TrackingRenderer.fp_disrange[
'45-54m'] = TrackingRenderer.fp_disrange[
'45-54m'] + 1
else:
TrackingRenderer.fp_disrange[
'-1'] = TrackingRenderer.fp_disrange['-1'] + 1
#####ve TrackingRenderer.fp_verange = {'0-0.1m/s':0, '0.1-2.5m/s':0, '2.5-5m/s':0, '5-10m/s':0, '>10m/s': 0 } #car 绝对速度
#####TrackingRenderer.fp_verange = {'0-0.1m/s':0, '0.1-1.0m/s':0, '1.0-1.5m/s':0, '1.5-2m/s':0, '>2m/s': 0 } #ped 绝对速度
ve = math.sqrt(frame_pred[i].velocity[0]**2 +
frame_pred[i].velocity[1]**2)
if TrackingRenderer.outscen == 'car':
if ve > 0 and ve <= 0.1:
TrackingRenderer.fp_verange['0-0.1m/s'] += 1
elif ve > 0.1 and ve <= 2.5:
TrackingRenderer.fp_verange['0.1-2.5m/s'] += 1
elif ve > 2.5 and ve <= 5:
TrackingRenderer.fp_verange['2.5-5m/s'] += 1
elif ve > 5 and ve <= 10:
TrackingRenderer.fp_verange['5-10m/s'] += 1
else:
TrackingRenderer.fp_verange['>10m/s'] += 1
else:
if ve > 0 and ve <= 0.1:
TrackingRenderer.fp_verange['0-0.1m/s'] += 1
elif ve > 0.1 and ve <= 1.0:
TrackingRenderer.fp_verange['0.1-1.0m/s'] += 1
elif ve > 1.0 and ve <= 1.5:
TrackingRenderer.fp_verange['1.0-1.5m/s'] += 1
elif ve > 1.5 and ve <= 2:
TrackingRenderer.fp_verange['1.5-2m/s'] += 1
else:
TrackingRenderer.fp_verange['>2m/s'] += 1
#####num_pts TrackingRenderer.fp_ptsnumrange = {'0-5nums':0, '5-10nums':0, '10-50nums':0, '50-200nums':0, '>200nums': 0 } #car lidar点云数 抽样参考比例：0.21 0.23 0.26，0.2，0.1
########## TrackingRenderer.fp_ptsnumrange = {'0-5nums':0, '5-10nums':0, '10-20nums':0, '20-30nums':0, '>30nums': 0 } #ped lidar点云数
points_xyzr = np.stack(points.points, 1)
points_xyz = points_xyzr[:, :3]
points_mask = points_in_box(box, points.points[:3])
mask_indx = np.arange(points_mask.shape[0])
mask_indx = mask_indx[points_mask]
num_pts = mask_indx.shape[0]
if TrackingRenderer.outscen == 'car':
if num_pts > 0 and num_pts <= 5:
TrackingRenderer.fp_ptsnumrange['0-5nums'] += 1
elif num_pts > 5 and num_pts <= 10:
TrackingRenderer.fp_ptsnumrange['5-10nums'] += 1
elif num_pts > 10 and num_pts <= 50:
TrackingRenderer.fp_ptsnumrange['10-50nums'] += 1
elif num_pts > 50 and num_pts <= 200:
TrackingRenderer.fp_ptsnumrange['50-200nums'] += 1
elif num_pts > 200:
TrackingRenderer.fp_ptsnumrange['>200nums'] += 1
else:
if num_pts > 0 and num_pts <= 5:
TrackingRenderer.fp_ptsnumrange['0-5nums'] += 1
elif num_pts > 5 and num_pts <= 10:
TrackingRenderer.fp_ptsnumrange['5-10nums'] += 1
elif num_pts > 10 and num_pts <= 20:
TrackingRenderer.fp_ptsnumrange['10-20nums'] += 1
elif num_pts > 20 and num_pts <= 30:
TrackingRenderer.fp_ptsnumrange['20-30nums'] += 1
elif num_pts > 30:
TrackingRenderer.fp_ptsnumrange['>30nums'] += 1
#####TrackingRenderer.fpscorrange = {'0-0.1':0, '0.2-0.4':0, '0.4-0.6':0,'0.6-1.0':0}
score = box.score
if score >= 0 and score <= 0.1:
TrackingRenderer.fpscorrange['0-0.1'] += 1
if score >= 0.2 and score <= 0.4:
TrackingRenderer.fpscorrange['0.2-0.4'] += 1
if score >= 0.4 and score <= 0.6:
TrackingRenderer.fpscorrange['0.4-0.6'] += 1
if score >= 0.6 and score <= 1.0:
TrackingRenderer.fpscorrange['0.6-1.0'] += 1
#####TrackingRenderer.trk_ratio = {'ang_muta':0, 've_muta':0, 'aug_other':0, 've_other':0} #仅包含与上一帧持续追踪的样本，角度和速度突变分别与other相并
if box.token in TrackingRenderer.his_trackid:
pre_box = TrackingRenderer.his_track[
TrackingRenderer.his_trackid.index(box.token)]
vari_ang = abs(
(R.from_quat(list(frame_pred[i].rotation)).
as_euler('zxy', degrees=False)[0]) -
(R.from_quat(list(pre_box.rotation)).as_euler(
'zxy', degrees=False)[0]))
if vari_ang > 0.52: #30度
TrackingRenderer.trk_ratio['angvar>30'] += 1
elif 0.52 > vari_ang > 0.35:
TrackingRenderer.trk_ratio['30>angvar>20'] += 1
elif 0.35 > vari_ang > 0.17:
TrackingRenderer.trk_ratio['20>angvar>10'] += 1
elif vari_ang < 0.17:
TrackingRenderer.trk_ratio['10>angvar'] += 1
pre_ve = pre_box.velocity
ve = frame_pred[i].velocity
ve_varity = abs(
math.sqrt(ve[0]**2 + ve[1]**2) -
math.sqrt(pre_ve[0]**2 + pre_ve[1]**2))
if ve_varity > TrackingRenderer.mutave_thr[
0]: # car均匀加速度为2.778m/s^2 3*0.5s=1.5
TrackingRenderer.trk_ratio[
'vevari>%s' %
TrackingRenderer.mutave_thr[0]] += 1
elif ve_varity < TrackingRenderer.mutave_thr[
0] and ve_varity >= TrackingRenderer.mutave_thr[
1]:
TrackingRenderer.trk_ratio[
'%s<vevari<%s' %
(TrackingRenderer.mutave_thr[1],
TrackingRenderer.mutave_thr[0])] += 1
elif ve_varity < TrackingRenderer.mutave_thr[
1] and ve_varity >= TrackingRenderer.mutave_thr[
2]:
TrackingRenderer.trk_ratio[
'%s<vevari<%s' %
(TrackingRenderer.mutave_thr[2],
TrackingRenderer.mutave_thr[1])] += 1
else:
TrackingRenderer.trk_ratio[
'vevari<%s' %
TrackingRenderer.mutave_thr[2]] += 1
# Determine color for this tracking id.
if b.tracking_id not in self.id2color.keys():
self.id2color[b.tracking_id] = (
float(hash(b.tracking_id + 'r') % 256) / 255,
float(hash(b.tracking_id + 'g') % 256) / 255,
float(hash(b.tracking_id + 'b') % 256) / 255)
if ifplthis:
box_for_path = copy.deepcopy(box)
box_for_path.rotate(Quaternion(cs_record["rotation"]))
box_for_path.translate(np.array(cs_record["translation"]))
box_for_path.rotate(Quaternion(pose_record["rotation"]))
box_for_path.translate(np.array(
pose_record["translation"])) #到全局
# 记录轨迹坐标
if b.tracking_id in self.track_path.keys():
self.track_path[b.tracking_id].append(box_for_path)
else:
self.track_path[b.tracking_id] = [box_for_path]
# Render box. Highlight identity switches in red.
if b.tracking_id in switch_ids:
color = self.id2color[b.tracking_id]
box.render(ax, view=np.eye(4), colors=('r', 'r', color))
if outscen_class:
###TrackingRenderer.ids_verange = {'0-0.1m/s':0, '0.1-2.5m/s':0, '2.5-5m/s':0, '5-10m/s':0, '>10m/s': 0 } #car 绝对速度
###TrackingRenderer.ids_verange = {'0-0.1m/s':0, '0.1-1.0m/s':0, '1.0-1.5m/s':0, '1.5-2m/s':0, '>2m/s': 0 } #ped 绝对速度
ve = math.sqrt(frame_pred[i].velocity[0]**2 +
frame_pred[i].velocity[1]**2)
if TrackingRenderer.outscen == 'car':
if ve > 0 and ve <= 0.1:
TrackingRenderer.ids_verange['0-0.1m/s'] += 1
elif ve > 0.1 and ve <= 2.5:
TrackingRenderer.ids_verange['0.1-2.5m/s'] += 1
elif ve > 2.5 and ve <= 5:
TrackingRenderer.ids_verange['2.5-5m/s'] += 1
elif ve > 5 and ve <= 10:
TrackingRenderer.ids_verange['5-10m/s'] += 1
else:
TrackingRenderer.ids_verange['>10m/s'] += 1
else:
if ve > 0 and ve <= 0.1:
TrackingRenderer.ids_verange['0-0.1m/s'] += 1
elif ve > 0.1 and ve <= 1.0:
TrackingRenderer.ids_verange['0.1-1.0m/s'] += 1
elif ve > 1.0 and ve <= 1.5:
TrackingRenderer.ids_verange['1.0-1.5m/s'] += 1
elif ve > 1.5 and ve <= 2:
TrackingRenderer.ids_verange['1.5-2m/s'] += 1
else:
TrackingRenderer.ids_verange['>2m/s'] += 1
####TrackingRenderer.ids_factratio = {'delay_trk':0, 'del_oth_trk':0, 'reappear':0, 'reapother':0, 've_muta':0, 've_other':0, 'reapdeltrk':0 }
indx = np.where(switch_ids == b.tracking_id)
gt_id = switch_gt_ids[indx]
try:
x = sam_anno.index(gt_id)
#sample = nusc.get('sample', frame_gt[x].sample_token)
#sample_annotation_tokens = sample['anns'] #标注
sample_annotation = nusc.get(
'sample_annotation', sample_annotation_tokens[x])
if sample_annotation['prev'] == '': #参考意义不大
TrackingRenderer.ids_factratio['del_oth_trk'] += 1
else:
TrackingRenderer.ids_factratio['delay_trk'] += 1
visibility = nusc.get(
'visibility',
sample_annotation['visibility_token'])
vis_level = vislev[visibility["level"]]
pre_annotation = nusc.get(
"sample_annotation", sample_annotation['prev'])
pre_vis = nusc.get(
'visibility',
pre_annotation['visibility_token'])
pre_vislevel = vislev[pre_vis["level"]]
if vis_level > pre_vislevel or (vis_level
== pre_vislevel
and vis_level < 3):
TrackingRenderer.ids_factratio['reappear'] += 1
elif vis_level == pre_vislevel:
TrackingRenderer.ids_factratio[
'reapdeltrk'] += 1
else:
TrackingRenderer.ids_factratio[
'reapother'] += 1
pre_ve = nusc.box_velocity(pre_annotation['token'])
ve = nusc.box_velocity(sample_annotation['token'])
ve_varity = abs(
math.sqrt(ve[0]**2 + ve[1]**2) -
math.sqrt(pre_ve[0]**2 + pre_ve[1]**2))
if ve_varity > TrackingRenderer.mutave_thr[
0]: # car均匀加速度为2.778m/s^2 3*0.5s=1.5
TrackingRenderer.ids_factratio[
'vevari>%s' %
(TrackingRenderer.mutave_thr[0])] += 1
elif ve_varity < TrackingRenderer.mutave_thr[
0] and ve_varity >= TrackingRenderer.mutave_thr[
1]:
TrackingRenderer.ids_factratio[
'%s<vevari<%s' %
(TrackingRenderer.mutave_thr[1],
TrackingRenderer.mutave_thr[0])] += 1
elif ve_varity < TrackingRenderer.mutave_thr[
1] and ve_varity >= TrackingRenderer.mutave_thr[
2]:
TrackingRenderer.ids_factratio[
'%s<vevari<%s' %
(TrackingRenderer.mutave_thr[2],
TrackingRenderer.mutave_thr[1])] += 1
else:
TrackingRenderer.ids_factratio[
'vevari<%s' %
TrackingRenderer.mutave_thr[2]] += 1
except: #标注错误
pass
else:
color = self.id2color[b.tracking_id]
box.render(ax, view=np.eye(4), colors=(color, color, color))
# Render other infos
if ifpltsco:
corners = view_points(box.corners(),
view=np.eye(4),
normalize=False)[:2, :]
# ax.text(4,5,"hhaa0.8", fontsize=5)
# ax.text(4,5,"%.2f\n%.2f,%.2f"%(b.tracking_score,b.velocity[0],b.velocity[1]), fontsize=5)
ax.text(box.center[0],
box.center[1],
"%.2f\nvx=%.2f,vy=%.2f" %
(b.tracking_score, b.velocity[0], b.velocity[1]),
fontdict={
'size': '6',
'color': 'b'
})
#ax.text(box.center[0],box.center[1],"%.2f\n%.2f,%.2f"%(b.tracking_score,b.velocity[0],b.velocity[1]), fontsize=5)
#if pltve:
#删去当前帧多余轨迹线
keys = list(self.track_path.keys())
for key in keys:
if key not in pred_trackid:
self.track_path.pop(key)
# 画历史轨迹线：
if ifplthis:
for id in self.track_path.keys():
color = self.id2color[id]
for box_path in self.track_path[id]:
#转到当前帧局部
# move box to ego vehicle coord system before has done the translation
box_path.translate(-np.array(pose_record["translation"]))
box_path.rotate(
Quaternion(pose_record["rotation"]).inverse)
# move box to sensor coord system
box_path.translate(-np.array(cs_record["translation"]))
box_path.rotate(Quaternion(cs_record["rotation"]).inverse)
ax.scatter(box_path.center[0], box_path.center[1], 10,
color)
#转回全局
box_path.rotate(Quaternion(cs_record["rotation"]))
box_path.translate(np.array(cs_record["translation"]))
box_path.rotate(Quaternion(pose_record["rotation"]))
box_path.translate(np.array(pose_record["translation"]))
TrackingRenderer.his_track = frame_pred
TrackingRenderer.his_trackid = pred_trackid
# Plot MOTA metrics. ly
# 目标位置 左上角，距离 Y 轴 0.01 倍距离，距离 X 轴 0.95倍距离
self.cursumfp += len(FP) #当前场景累积值
self.cursumfn += len(FN)
#print("FN=%d,FP=%d,switch_ids=%d,cursumfp=%d,cursumfn=%d" % ( len(FN), len(FP), len(switch_ids), self.cursumfp, self.cursumfn ))
#ax.text(0.01, 0.95, "IDS:%d\nFP:%d\nFN:%d\ncur_sce sumfp:%d sumfn:%d\nthreshold:%f"%(len(switch_ids),len(FP),len(FN),self.cursumfp,self.cursumfn,threshold), transform=ax.transAxes, fontdict={'size': '10', 'color': 'b'})
# Plot ego pose.
plt.scatter(0, 0, s=96, facecolors='none', edgecolors='k', marker='o')
plt.xlim(-50, 50)
plt.ylim(-50, 50)
plt.axis('off')
# Save to disk and close figure.
fig.savefig(os.path.join(self.save_path, '{}.png'.format(timestamp)))
plt.close(fig)
# Test iou3d (camera coord)
box_1 = compute_box_3d(dim=[1.599275, 1.9106505, 4.5931444],
location=[7.1180778, 2.1364648, 41.784885],
rotation_y=-1.1312813047259618)
box_2 = compute_box_3d(dim=[1.599275, 1.9106505, 4.5931444],
location=[7.1180778, 2.1364648, 41.784885],
rotation_y=-1.1312813047259618)
iou = iou3d(box_1, box_2)
print("Results should be almost 1.0: ", iou)
# # Test iou3d (global coord)
translation1 = [634.7540893554688, 1620.952880859375, 0.4360223412513733]
size1 = [1.9073231220245361, 4.5971598625183105, 1.5940513610839844]
rotation1 = [
-0.6379619591303222, 0.6256341359192967, -0.320485847319929,
0.31444441216651253
]
translation2 = [634.7540893554688, 1620.952880859375, 0.4360223412513733]
size2 = [1.9073231220245361, 4.5971598625183105, 1.5940513610839844]
rotation2 = [
-0.6379619591303222, 0.6256341359192967, -0.320485847319929,
0.31444441216651253
]
box_1 = Box(translation1, size1, Quaternion(rotation1))
box_2 = Box(translation2, size2, Quaternion(rotation2))
iou, iou_2d = iou3d_global(box_1.corners(), box_2.corners())
print(iou, iou_2d)
my_sample = nusc.get('sample', my_sample['next'])
img = get_image(my_sample, root_dir)
sample_data = nusc.get_sample_data(my_sample['data']['CAM_FRONT'])
labels = sample_data[1]
# this label is according to camera, what we have is lidar
# converts top lidar
intrinsic = sample_data[2]
for label in labels:
c2d = project_cam_coords_to_pixel([label.center], intrinsic)[0]
cv2.circle(img, (int(c2d[0]), int(c2d[1])), 3, (0, 255, 255), -1)
# convert center and wlh to 3d box
box = Box(center=label.center,
size=label.wlh,
orientation=label.orientation)
corners3d = box.corners()
corners3d_2d = project_cam_coords_to_pixel(corners3d, intrinsic)
idx = all_category_db.index(label.name)
c = get_unique_color_by_id(idx)
draw_3d_box(corners3d_2d, img, c)
if len(corners3d_2d) > 4:
cv2.putText(img, '{0}'.format(all_category[idx]),
(int(corners3d_2d[1][0]), int(corners3d_2d[1][1])),
cv2.FONT_HERSHEY_PLAIN, .9, (255, 255, 255))
# show some image and lidar points
cv2.imshow('aa', img)
cv2.imwrite('results/{}.png'.format(i), img)
i += 1
cv2.waitKey(0)
