def render_nusc_result(nusc, results, sample_token):
from nuscenes.utils.data_classes import Box
from pyquaternion import Quaternion
annos = results[sample_token]
sample = nusc.get("sample", sample_token)
sd_rec = nusc.get('sample_data', sample['data']["LIDAR_TOP"])
cs_record = nusc.get('calibrated_sensor',
sd_rec['calibrated_sensor_token'])
pose_record = nusc.get('ego_pose', sd_rec['ego_pose_token'])
boxes = []
for anno in annos:
rot = Quaternion(anno["rotation"])
box = Box(anno["translation"],
anno["size"],
rot,
name=anno["detection_name"])
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)
boxes.append(box)
nusc.explorer.render_sample_data(sample["data"]["LIDAR_TOP"],
extern_boxes=boxes,
nsweeps=10)
nusc.explorer.render_sample_data(sample["data"]["LIDAR_TOP"], nsweeps=10)
def box_nuscenes_to_kitti(box: Box, velo_to_cam_rot: Quaternion,
velo_to_cam_trans: np.ndarray,
r0_rect: Quaternion,
kitti_to_nu_lidar_inv: Quaternion = Quaternion(axis=(0, 0, 1), angle=np.pi / 2).inverse) \
-> Box:
"""
Transform from nuScenes lidar frame to KITTI reference frame.
:param box: Instance in nuScenes lidar frame.
:param velo_to_cam_rot: Quaternion to rotate from lidar to camera frame.
:param velo_to_cam_trans: <np.float: 3>. Translate from lidar to camera frame.
:param r0_rect: Quaternion to rectify camera frame.
:param kitti_to_nu_lidar_inv: Quaternion to rotate nuScenes to KITTI LIDAR.
:return: Box instance in KITTI reference frame.
"""
# Copy box to avoid side-effects.
box = box.copy()
# Rotate to KITTI lidar.
box.rotate(kitti_to_nu_lidar_inv)
# Transform to KITTI camera.
box.rotate(velo_to_cam_rot)
box.translate(velo_to_cam_trans)
# Rotate to KITTI rectified camera.
box.rotate(r0_rect)
# 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 y direction.
box.translate(np.array([0, box.wlh[2] / 2, 0]))
return box
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 get_binimg(self, rec):
egopose = self.nusc.get(
'ego_pose',
self.nusc.get('sample_data',
rec['data']['LIDAR_TOP'])['ego_pose_token'])
trans = -np.array(egopose['translation'])
rot = Quaternion(egopose['rotation']).inverse
img = np.zeros((self.nx[0], self.nx[1]))
for tok in rec['anns']:
inst = self.nusc.get('sample_annotation', tok)
# add category for lyft
if not inst['category_name'].split('.')[0] == 'vehicle':
continue
box = Box(inst['translation'], inst['size'],
Quaternion(inst['rotation']))
box.translate(trans)
box.rotate(rot)
pts = box.bottom_corners()[:2].T
pts = np.round((pts - self.bx[:2] + self.dx[:2] / 2.) /
self.dx[:2]).astype(np.int32)
pts[:, [1, 0]] = pts[:, [0, 1]]
cv2.fillPoly(img, [pts], 1.0)
return torch.Tensor(img).unsqueeze(0)
def _get_poly_region_in_image(self, instance_annotation, ego_translation,
ego_rotation):
box = Box(instance_annotation['translation'],
instance_annotation['size'],
Quaternion(instance_annotation['rotation']))
box.translate(ego_translation)
box.rotate(ego_rotation)
pts = box.bottom_corners()[:2].T
pts = np.round(
(pts - self.bev_start_position[:2] + self.bev_resolution[:2] / 2.0)
/ self.bev_resolution[:2]).astype(np.int32)
pts[:, [1, 0]] = pts[:, [0, 1]]
z = box.bottom_corners()[2, 0]
return pts, z
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 get_boxes(nusc, rec):
"""Simplified version of nusc.get_boxes"""
egopose = nusc.get(
'ego_pose',
nusc.get('sample_data', rec['data']['LIDAR_TOP'])['ego_pose_token'])
trans = -np.array(egopose['translation'])
rot = Quaternion(egopose['rotation']).inverse
boxes = []
for tok in rec['anns']:
inst = nusc.get('sample_annotation', tok)
box = Box(inst['translation'],
inst['size'],
Quaternion(inst['rotation']),
name=inst['category_name'])
box.translate(trans)
box.rotate(rot)
boxes.append(box)
return boxes
def boxes_to_sensor(boxes: List[Dict], pose_record: Dict, cs_record: Dict):
"""
Map boxes from global coordinates to the vehicle's sensor coordinate system.
:param boxes: The boxes in global coordinates.
:param pose_record: The pose record of the vehicle at the current timestamp.
:param cs_record: The calibrated sensor record of the sensor.
:return: The transformed boxes.
"""
boxes_out = []
for box in boxes:
# Create Box instance.
box = Box(box['translation'], box['size'], Quaternion(box['rotation']))
# Move box to ego vehicle coord system.
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)
boxes_out.append(box)
return boxes_out
def convert_eval_format(self, results):
ret = {
'meta': {
'use_camera': True,
'use_lidar': False,
'use_radar': self.opt.pointcloud,
'use_map': False,
'use_external': False
},
'results': {}
}
print('Converting nuscenes format...')
for image_id in self.images:
if not (image_id in results):
continue
image_info = self.coco.loadImgs(ids=[image_id])[0]
sample_token = image_info['sample_token']
trans_matrix = np.array(image_info['trans_matrix'], np.float32)
velocity_mat = np.array(image_info['velocity_trans_matrix'],
np.float32)
sensor_id = image_info['sensor_id']
sample_results = []
for item in results[image_id]:
class_name = self.class_name[int(item['class'] - 1)] \
if not ('detection_name' in item) else item['detection_name']
if self.opt.tracking and class_name in self._tracking_ignored_class:
continue
score = float(item['score']) if not (
'detection_score' in item) else item['detection_score']
if 'size' in item:
size = item['size']
else:
size = [float(item['dim'][1]), float(item['dim'][2]), \
float(item['dim'][0])]
if 'translation' in item:
translation = item['translation']
else:
translation = np.dot(
trans_matrix,
np.array([
item['loc'][0], item['loc'][1] - size[2],
item['loc'][2], 1
], np.float32))
det_id = item['det_id'] if 'det_id' in item else -1
tracking_id = item[
'tracking_id'] if 'tracking_id' in item else 1
if not ('rotation' in item):
rot_cam = Quaternion(axis=[0, 1, 0], angle=item['rot_y'])
loc = np.array(
[item['loc'][0], item['loc'][1], item['loc'][2]],
np.float32)
box = Box(loc, size, rot_cam, name='2', token='1')
box.translate(np.array([0, -box.wlh[2] / 2, 0]))
box.rotate(Quaternion(image_info['cs_record_rot']))
box.translate(np.array(image_info['cs_record_trans']))
box.rotate(Quaternion(image_info['pose_record_rot']))
box.translate(np.array(image_info['pose_record_trans']))
rotation = box.orientation
rotation = [float(rotation.w), float(rotation.x), \
float(rotation.y), float(rotation.z)]
else:
rotation = item['rotation']
nuscenes_att = np.array(item['nuscenes_att'], np.float32) \
if 'nuscenes_att' in item else np.zeros(8, np.float32)
att = ''
if class_name in self._cycles:
att = self.id_to_attribute[np.argmax(nuscenes_att[0:2]) +
1]
elif class_name in self._pedestrians:
att = self.id_to_attribute[np.argmax(nuscenes_att[2:5]) +
3]
elif class_name in self._vehicles:
att = self.id_to_attribute[np.argmax(nuscenes_att[5:8]) +
6]
if 'velocity' in item and len(item['velocity']) == 2:
velocity = item['velocity']
else:
velocity = item['velocity'] if 'velocity' in item else [
0, 0, 0
]
velocity = np.dot(
velocity_mat,
np.array([velocity[0], velocity[1], velocity[2], 0],
np.float32))
# velocity = np.dot(trans_matrix, np.array(
# [velocity[0], velocity[1], velocity[2], 0], np.float32))
velocity = [float(velocity[0]), float(velocity[1])]
result = {
'sample_token': sample_token,
'translation': [float(translation[0]), float(translation[1]), \
float(translation[2])],
'size': size,
'rotation': rotation,
'velocity': velocity,
'detection_name': class_name,
'attribute_name': att if not ('attribute_name' in item) else item['attribute_name'],
'detection_score': score,
'tracking_name': class_name,
'tracking_score': score,
'tracking_id': tracking_id,
'sensor_id': sensor_id,
'det_id': det_id}
sample_results.append(result)
if sample_token in ret['results']:
ret['results'][sample_token] = ret['results'][
sample_token] + sample_results
else:
ret['results'][sample_token] = sample_results
for sample_token in ret['results'].keys():
confs = sorted([
(-d['detection_score'], ind)
for ind, d in enumerate(ret['results'][sample_token])
])
ret['results'][sample_token] = [
ret['results'][sample_token][ind]
for _, ind in confs[:min(500, len(confs))]
]
if self.opt.iou_thresh > 0:
print("Applying BEV NMS...")
n_removed = 0
for sample_token, dets in tqdm(ret['results'].items()):
ret['results'][sample_token], n = self.apply_bev_nms(
dets, self.opt.iou_thresh, dist_thresh=2)
n_removed += n
print("Removed {} detections with IOU > {}".format(
n_removed, self.opt.iou_thresh))
return ret
def run(self, image_or_path_or_tensor, meta={}, image_info=None):
load_time, pre_time, net_time, dec_time, post_time = 0, 0, 0, 0, 0
merge_time, track_time, tot_time, display_time = 0, 0, 0, 0
self.debugger.clear()
start_time = time.time()
# read image
pre_processed = False
if isinstance(image_or_path_or_tensor, np.ndarray):
image = image_or_path_or_tensor
elif type(image_or_path_or_tensor) == type(""):
image = cv2.imread(image_or_path_or_tensor)
else:
image = image_or_path_or_tensor["image"][0].numpy()
pre_processed_images = image_or_path_or_tensor
pre_processed = True
loaded_time = time.time()
load_time += loaded_time - start_time
detections = []
# for multi-scale testing
for scale in self.opt.test_scales:
scale_start_time = time.time()
if not pre_processed:
# not prefetch testing or demo
images, meta = self.pre_process(image, scale, meta)
else:
# prefetch testing
images = pre_processed_images["images"][scale][0]
meta = pre_processed_images["meta"][scale]
meta = {k: v.numpy()[0] for k, v in meta.items()}
if "pre_dets" in pre_processed_images["meta"]:
meta["pre_dets"] = pre_processed_images["meta"]["pre_dets"]
if "cur_dets" in pre_processed_images["meta"]:
meta["cur_dets"] = pre_processed_images["meta"]["cur_dets"]
images = images.to(self.opt.device,
non_blocking=self.opt.non_block_test)
# initializing tracker
pre_hms, pre_inds = None, None
pre_process_time = time.time()
pre_time += pre_process_time - scale_start_time
# run the network
# output: the output feature maps, only used for visualizing
# dets: output tensors after extracting peaks
output, dets, forward_time, FeatureMaps = self.process(
images, self.pre_images, pre_hms, pre_inds, return_time=True)
net_time += forward_time - pre_process_time
decode_time = time.time()
dec_time += decode_time - forward_time
# convert the cropped and 4x downsampled output coordinate system
# back to the input image coordinate system
result = self.post_process(dets, meta, scale)
post_process_time = time.time()
post_time += post_process_time - decode_time
detections.append(result)
if self.opt.debug >= 2:
self.debug(
self.debugger,
images,
result,
output,
scale,
pre_images=self.pre_images
if not self.opt.no_pre_img else None,
pre_hms=pre_hms,
)
# merge multi-scale testing results
results = self.merge_outputs(detections)
torch.cuda.synchronize()
end_time = time.time()
merge_time += end_time - post_process_time
# public detection mode in MOT challenge
if self.opt.public_det:
results = (pre_processed_images["meta"]["cur_dets"]
if self.opt.public_det else None)
if self.dataset == "nuscenes":
trans_matrix = np.array(image_info["trans_matrix"], np.float32)
results_by_class = {}
ddd_boxes_by_class = {}
depths_by_class = {}
ddd_boxes_by_class2 = {}
ddd_org_boxes_by_class = {}
ddd_box_submission1 = {}
ddd_box_submission2 = {}
for class_name in NUSCENES_TRACKING_NAMES:
results_by_class[class_name] = []
ddd_boxes_by_class2[class_name] = []
ddd_boxes_by_class[class_name] = []
depths_by_class[class_name] = []
ddd_org_boxes_by_class[class_name] = []
ddd_box_submission1[class_name] = []
ddd_box_submission2[class_name] = []
for det in results:
cls_id = int(det["class"])
class_name = nuscenes_class_name[cls_id - 1]
if class_name not in NUSCENES_TRACKING_NAMES:
continue
if det["score"] < 0.3:
continue
if class_name == "pedestrian" and det["score"] < 0.35:
continue
results_by_class[class_name].append(det["bbox"].tolist() +
[det["score"]])
size = [
float(det["dim"][1]),
float(det["dim"][2]),
float(det["dim"][0]),
]
rot_cam = Quaternion(axis=[0, 1, 0], angle=det["rot_y"])
translation_submission1 = np.dot(
trans_matrix,
np.array(
[
det["loc"][0], det["loc"][1] - size[2],
det["loc"][2], 1
],
np.float32,
),
).copy()
loc = np.array([det["loc"][0], det["loc"][1], det["loc"][2]],
np.float32)
depths_by_class[class_name].append([float(det["loc"][2])
].copy())
trans = [det["loc"][0], det["loc"][1], det["loc"][2]]
ddd_org_boxes_by_class[class_name].append([
float(det["dim"][0]),
float(det["dim"][1]),
float(det["dim"][2])
] + trans + [det["rot_y"]])
box = Box(loc, size, rot_cam, name="2", token="1")
box.translate(np.array([0, -box.wlh[2] / 2, 0]))
box.rotate(Quaternion(image_info["cs_record_rot"]))
box.translate(np.array(image_info["cs_record_trans"]))
box.rotate(Quaternion(image_info["pose_record_rot"]))
box.translate(np.array(image_info["pose_record_trans"]))
rotation = box.orientation
rotation = [
float(rotation.w),
float(rotation.x),
float(rotation.y),
float(rotation.z),
]
ddd_box_submission1[class_name].append([
float(translation_submission1[0]),
float(translation_submission1[1]),
float(translation_submission1[2]),
].copy() + size.copy() + rotation.copy())
q = Quaternion(rotation)
angle = q.angle if q.axis[2] > 0 else -q.angle
ddd_boxes_by_class[class_name].append([
size[2],
size[0],
size[1],
box.center[0],
box.center[1],
box.center[2],
angle,
].copy())
online_targets = []
for class_name in NUSCENES_TRACKING_NAMES:
if len(results_by_class[class_name]) > 0 and NMS:
boxess = torch.from_numpy(
np.array(results_by_class[class_name])[:, :4])
scoress = torch.from_numpy(
np.array(results_by_class[class_name])[:, -1])
if class_name == "bus" or class_name == "truck":
ovrlp = 0.7
else:
ovrlp = 0.8
keep, count = nms(boxess, scoress, overlap=ovrlp)
keep = keep.data.numpy().tolist()
keep = sorted(set(keep))
results_by_class[class_name] = np.array(
results_by_class[class_name])[keep]
ddd_boxes_by_class[class_name] = np.array(
ddd_boxes_by_class[class_name])[keep]
depths_by_class[class_name] = np.array(
depths_by_class[class_name])[keep]
ddd_org_boxes_by_class[class_name] = np.array(
ddd_org_boxes_by_class[class_name])[keep]
ddd_box_submission1[class_name] = np.array(
ddd_box_submission1[class_name])[keep]
online_targets += self.tracker[class_name].update(
results_by_class[class_name],
FeatureMaps,
ddd_boxes=ddd_boxes_by_class[class_name],
depths_by_class=depths_by_class[class_name],
ddd_org_boxes=ddd_org_boxes_by_class[class_name],
submission=ddd_box_submission1[class_name],
classe=class_name,
)
else:
online_targets = self.tracker.update(results, FeatureMaps)
return online_targets
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)
def convert_eval_format(self, results):
from nuscenes.utils.data_classes import Box
ret = {
"meta": {
"use_camera": True,
"use_lidar": False,
"use_radar": False,
"use_map": False,
"use_external": False,
},
"results": {},
}
print("Converting nuscenes format...")
for image_id in self.images:
if not (image_id in results):
continue
image_info = self.coco.loadImgs(ids=[image_id])[0]
sample_token = image_info["sample_token"]
trans_matrix = np.array(image_info["trans_matrix"], np.float32)
sensor_id = image_info["sensor_id"]
sample_results = []
for item in results[image_id]:
class_name = (
self.class_name[int(item["class"] - 1)]
if not ("detection_name" in item)
else item["detection_name"]
)
if self.opt.tracking and class_name in self._tracking_ignored_class:
continue
score = (
float(item["score"])
if not ("detection_score" in item)
else item["detection_score"]
)
if "size" in item:
size = item["size"]
else:
size = [
float(item["dim"][1]),
float(item["dim"][2]),
float(item["dim"][0]),
]
if "translation" in item:
translation = item["translation"]
else:
translation = np.dot(
trans_matrix,
np.array(
[
item["loc"][0],
item["loc"][1] - size[2],
item["loc"][2],
1,
],
np.float32,
),
)
det_id = item["det_id"] if "det_id" in item else -1
tracking_id = item["tracking_id"] if "tracking_id" in item else 1
if not ("rotation" in item):
rot_cam = Quaternion(axis=[0, 1, 0], angle=item["rot_y"])
loc = np.array(
[item["loc"][0], item["loc"][1], item["loc"][2]], np.float32
)
box = Box(loc, size, rot_cam, name="2", token="1")
box.translate(np.array([0, -box.wlh[2] / 2, 0]))
box.rotate(Quaternion(image_info["cs_record_rot"]))
box.translate(np.array(image_info["cs_record_trans"]))
box.rotate(Quaternion(image_info["pose_record_rot"]))
box.translate(np.array(image_info["pose_record_trans"]))
rotation = box.orientation
rotation = [
float(rotation.w),
float(rotation.x),
float(rotation.y),
float(rotation.z),
]
else:
rotation = item["rotation"]
nuscenes_att = (
np.array(item["nuscenes_att"], np.float32)
if "nuscenes_att" in item
else np.zeros(8, np.float32)
)
att = ""
if class_name in self._cycles:
att = self.id_to_attribute[np.argmax(nuscenes_att[0:2]) + 1]
elif class_name in self._pedestrians:
att = self.id_to_attribute[np.argmax(nuscenes_att[2:5]) + 3]
elif class_name in self._vehicles:
att = self.id_to_attribute[np.argmax(nuscenes_att[5:8]) + 6]
if "velocity" in item and len(item["velocity"]) == 2:
velocity = item["velocity"]
else:
velocity = item["velocity"] if "velocity" in item else [0, 0, 0]
velocity = np.dot(
trans_matrix,
np.array(
[velocity[0], velocity[1], velocity[2], 0], np.float32
),
)
velocity = [float(velocity[0]), float(velocity[1])]
result = {
"sample_token": sample_token,
"translation": [
float(translation[0]),
float(translation[1]),
float(translation[2]),
],
"size": size,
"rotation": rotation,
"velocity": velocity,
"detection_name": class_name,
"attribute_name": att
if not ("attribute_name" in item)
else item["attribute_name"],
"detection_score": score,
"tracking_name": class_name,
"tracking_score": score,
"tracking_id": tracking_id,
"sensor_id": sensor_id,
"det_id": det_id,
}
sample_results.append(result)
if sample_token in ret["results"]:
ret["results"][sample_token] = (
ret["results"][sample_token] + sample_results
)
else:
ret["results"][sample_token] = sample_results
for sample_token in ret["results"].keys():
confs = sorted(
[
(-d["detection_score"], ind)
for ind, d in enumerate(ret["results"][sample_token])
]
)
ret["results"][sample_token] = [
ret["results"][sample_token][ind]
for _, ind in confs[: min(500, len(confs))]
]
return ret
def get_boxes(self,
token: str,
filter_classes: List[str] = None,
max_dist: float = None) -> List[Box]:
"""
Load up all the boxes associated with a sample.
Boxes are in nuScenes lidar frame.
:param token: KittiDB unique id.
:param filter_classes: List of Kitti classes to use or None to use all.
:param max_dist: Maximum distance in m to still draw a box.
:return: Boxes in nuScenes lidar reference frame.
"""
# Get transforms for this sample
transforms = self.get_transforms(token, root=self.root)
boxes = []
if token.startswith('test_'):
# No boxes to return for the test set.
return boxes
with open(KittiDB.get_filepath(token, 'label_2', root=self.root),
'r') as f:
for line in f:
# Parse this line into box information.
parsed_line = self.parse_label_line(line)
if parsed_line['name'] in {'DontCare', 'Misc'}:
continue
center = parsed_line['xyz_camera']
wlh = parsed_line['wlh']
yaw_camera = parsed_line['yaw_camera']
name = parsed_line['name']
score = parsed_line['score']
# Optional: Filter classes.
if filter_classes is not None and name not in filter_classes:
continue
# The Box class coord system is oriented the same way as as KITTI LIDAR: x forward, y left, z up.
# For orientation confer: http://www.cvlibs.net/datasets/kitti/setup.php.
# 1: Create box in Box coordinate system with center at origin.
# The second quaternion in yaw_box transforms the coordinate frame from the object frame
# to KITTI camera frame. The equivalent cannot be naively done afterwards, as it's a rotation
# around the local object coordinate frame, rather than the camera frame.
quat_box = Quaternion(axis=(0, 1, 0),
angle=yaw_camera) * Quaternion(
axis=(1, 0, 0), angle=np.pi / 2)
box = Box([0.0, 0.0, 0.0], wlh, quat_box, name=name)
# 2: Translate: KITTI defines the box center as the bottom center of the vehicle. We use true center,
# so we need to add half height in negative y direction, (since y points downwards), to adjust. The
# center is already given in camera coord system.
box.translate(center + np.array([0, -wlh[2] / 2, 0]))
# 3: Transform to KITTI LIDAR coord system. First transform from rectified camera to camera, then
# camera to KITTI lidar.
box.rotate(Quaternion(matrix=transforms['r0_rect']).inverse)
box.translate(-transforms['velo_to_cam']['T'])
box.rotate(
Quaternion(matrix=transforms['velo_to_cam']['R']).inverse)
# 4: Transform to nuScenes LIDAR coord system.
box.rotate(self.kitti_to_nu_lidar)
# Set score or NaN.
box.score = score
# Set dummy velocity.
box.velocity = np.array((0.0, 0.0, 0.0))
# Optional: Filter by max_dist
if max_dist is not None:
dist = np.sqrt(np.sum(box.center[:2]**2))
if dist > max_dist:
continue
boxes.append(box)
return boxes
def get_binimg(self, rec):
egopose = self.nusc.get(
'ego_pose',
self.nusc.get('sample_data',
rec['data']['LIDAR_TOP'])['ego_pose_token'])
trans = -np.array(egopose['translation'])
rot = Quaternion(egopose['rotation']).inverse
#vehicle label
img_vehicle = np.zeros((self.nx[0], self.nx[1]))
for tok in rec['anns']:
inst = self.nusc.get('sample_annotation', tok)
# add category for lyft
if not inst['category_name'].split('.')[0] == 'vehicle':
continue
box = Box(inst['translation'], inst['size'],
Quaternion(inst['rotation']))
box.translate(trans)
box.rotate(rot)
pts = box.bottom_corners()[:2].T
pts = np.round((pts - self.bx[:2] + self.dx[:2] / 2.) /
self.dx[:2]).astype(np.int32)
pts[:, [1, 0]] = pts[:, [0, 1]]
cv2.fillPoly(img_vehicle, [pts], 1.0)
# cv2.imwrite('./output/vehicle{}.png'.format(rec['timestamp']),img_vehicle*255)
#map label
map_name = self.scene2map[self.nusc.get('scene',
rec['scene_token'])['name']]
rot_map = Quaternion(egopose['rotation']).rotation_matrix
rot_map = np.arctan2(rot_map[1, 0], rot_map[0, 0])
center = np.array([
egopose['translation'][0], egopose['translation'][1],
np.cos(rot_map),
np.sin(rot_map)
])
lmap = get_local_map(self.nusc_maps[map_name], center, 50.0,
['road_segment', 'lane'],
['lane_divider', 'road_divider'])
#road_segment
img_road_segment = np.zeros((self.nx[0], self.nx[1]))
arr_pts = []
for pts in lmap['road_segment']:
pts = np.round((pts - self.bx[:2] + self.dx[:2] / 2.) /
self.dx[:2]).astype(np.int32)
pts[:, [1, 0]] = pts[:, [0, 1]]
arr_pts.append(pts)
cv2.fillPoly(img_road_segment, arr_pts, 1.0)
#lane
#lane = np.zeros((self.nx[0], self.nx[1]))
arr_pts = []
for pts in lmap['lane']:
pts = np.round((pts - self.bx[:2] + self.dx[:2] / 2.) /
self.dx[:2]).astype(np.int32)
pts[:, [1, 0]] = pts[:, [0, 1]]
arr_pts.append(pts)
# cv2.fillPoly(lane,arr_pts,1.0)
cv2.fillPoly(img_road_segment, arr_pts, 1.0)
#road_divider
# img_road_divider = np.zeros((self.nx[0], self.nx[1]))
# arr_pts=[]
# for pts in lmap['road_divider']:
# pts = np.round(
# (pts - self.bx[:2] + self.dx[:2]/2.) / self.dx[:2]
# ).astype(np.int32)
# pts[:, [1, 0]] = pts[:, [0, 1]]
# arr_pts.append(pts)
# cv2.polylines(img_road_divider,arr_pts,False,1.0,1)
#lane_divider
img_lane_divider = np.zeros((self.nx[0], self.nx[1]))
arr_pts = []
for pts in lmap['lane_divider']:
pts = np.round((pts - self.bx[:2] + self.dx[:2] / 2.) /
self.dx[:2]).astype(np.int32)
pts[:, [1, 0]] = pts[:, [0, 1]]
arr_pts.append(pts)
cv2.polylines(img_lane_divider, arr_pts, False, 1.0, 2)
# cv2.imwrite('./output/lane_divider{}.png'.format(rec['timestamp']),img_lane_divider*255)
#plt.plot(pts[:, 1], pts[:, 0], c=(159./255., 0.0, 1.0), alpha=0.5)
return torch.Tensor(
np.stack([img_vehicle, img_road_segment, img_lane_divider]))
def evaluation(self, det_annos, class_names, **kwargs):
eval_det_annos = copy.deepcopy(det_annos)
# Create NuScenes JSON output file
nusc_annos = {}
for sample in eval_det_annos:
try:
sample_idx = sample['sample_idx'][0]
except:
continue
sample_results = []
calib = self.get_calib(sample_idx)
sample['boxes_lidar'] = np.array(sample['boxes_lidar'])
positions = sample['boxes_lidar'][:, :3]
dimensions = sample['boxes_lidar'][:, 3:6]
rotations = sample['boxes_lidar'][:, 6]
for center, dimension, yaw, label, score in zip(
positions, dimensions, rotations, sample['name'],
sample['score']):
quaternion = Quaternion(axis=[0, 0, 1], radians=yaw)
box = Box(center, dimension, quaternion)
# Move box to ego vehicle coord system
box.rotate(Quaternion(calib.lidar_calibrated['rotation']))
box.translate(np.array(calib.lidar_calibrated['translation']))
# Move box to global coord system
box.rotate(Quaternion(calib.ego_pose['rotation']))
box.translate(np.array(calib.ego_pose['translation']))
if (float(score) < 0):
score = 0
if (float(score) > 1):
score = 1
if (label == 'Cyclist'):
label = 'bicycle'
sample_results.append({
"sample_token":
sample_idx,
"translation":
box.center.tolist(),
"size":
box.wlh.tolist(),
"rotation":
box.orientation.elements.tolist(),
"lidar_yaw":
float(yaw),
"velocity": (0, 0),
"detection_name":
label.lower(),
"detection_score":
float(score),
"attribute_name":
self.DefaultAttribute[label.lower()],
})
nusc_annos[sample_idx] = sample_results
for sample_id in self.sample_id_list:
if sample_id not in nusc_annos:
nusc_annos[sample_id] = []
nusc_submission = {
"meta": {
"use_camera": False,
"use_lidar": True,
"use_radar": False,
"use_map": False,
"use_external": False,
},
"results": nusc_annos,
}
eval_file = os.path.join(kwargs['output_dir'], 'nusc_results.json')
with open(eval_file, "w") as f:
json.dump(nusc_submission, f, indent=2)
# Call NuScenes evaluation
cfg = config_factory('detection_cvpr_2019')
nusc_eval = DetectionEval(self.nusc,
config=cfg,
result_path=eval_file,
eval_set=self.split,
output_dir=kwargs['output_dir'],
verbose=True)
metric_summary = nusc_eval.main(plot_examples=10, render_curves=True)
# Reformat the metrics summary a bit for the tensorboard logger
err_name_mapping = {
'trans_err': 'mATE',
'scale_err': 'mASE',
'orient_err': 'mAOE',
'vel_err': 'mAVE',
'attr_err': 'mAAE'
}
result = {}
result['mean_ap'] = metric_summary['mean_ap']
for tp_name, tp_val in metric_summary['tp_errors'].items():
result[tp_name] = tp_val
class_aps = metric_summary['mean_dist_aps']
class_tps = metric_summary['label_tp_errors']
for class_name in class_aps.keys():
result['mAP_' + class_name] = class_aps[class_name]
for key, val in err_name_mapping.items():
result[val + '_' + class_name] = class_tps[class_name][key]
return str(result), result
def _get_bboxes(self, anns, image_info=None):
bboxes, track_ids, classes = [], [], []
if self.dataset == "nuscenes":
for ann in anns:
trans_matrix = np.array(image_info["trans_matrix"], np.float32)
cls_id = int(self.cat_ids[ann["category_id"]])
class_name = self.class_name[cls_id - 1]
if class_name not in NUSCENES_TRACKING_NAMES:
continue
center_location = ann["location"]
wlh = ann["dim"]
rotation = ann["rotation_y"]
size = [float(wlh[1]), float(wlh[2]), float(wlh[0])]
rot_cam = Quaternion(axis=[0, 1, 0], angle=rotation)
loc = np.array(
[
center_location[0], center_location[1],
center_location[2]
],
np.float32,
).copy()
translation = np.dot(
trans_matrix,
np.array(
[
center_location[0],
center_location[1] - size[2] / 2,
center_location[2],
1,
],
np.float32,
),
).copy()
box = Box(loc, size, rot_cam, name="2", token="1")
box.translate(np.array([0, -box.wlh[2] / 2, 0]))
box.rotate(Quaternion(image_info["cs_record_rot"]))
box.translate(np.array(image_info["cs_record_trans"]))
box.rotate(Quaternion(image_info["pose_record_rot"]))
box.translate(np.array(image_info["pose_record_trans"]))
rotation = box.orientation
rotation = [
float(rotation.w),
float(rotation.x),
float(rotation.y),
float(rotation.z),
]
q = Quaternion(rotation)
angle = q.angle if q.axis[2] > 0 else -q.angle
bboxes.append([
size[2],
size[0],
size[1],
box.center[0],
box.center[1],
box.center[2],
angle,
].copy())
track_ids.append(ann["track_id"])
classes.append(class_name)
else:
for ann in anns:
cls_id = int(self.cat_ids[ann["category_id"]])
if (cls_id > self.opt.num_classes or cls_id <= -99
or ("iscrowd" in ann and ann["iscrowd"] > 0)):
continue
bbox = self._coco_box_to_bbox(ann["bbox"])
h, w = bbox[3] - bbox[1], bbox[2] - bbox[0]
if h > 0 and w > 0:
bboxes.append([bbox[0], bbox[1], bbox[2], bbox[3]].copy())
track_ids.append(ann["track_id"] if "track_id" in
ann else -1)
return bboxes, track_ids, classes