def render(self, events: DataFrame, timestamp: int, frame_gt: List[TrackingBox], frame_pred: List[TrackingBox]) \
-> 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
fig, ax = plt.subplots()
# Plot GT boxes.
for b in frame_gt:
color = 'k'
box = Box(b.ego_translation,
b.size,
Quaternion(b.rotation),
name=b.tracking_name,
token=b.tracking_id)
box.render(ax,
view=np.eye(4),
colors=(color, color, color),
linewidth=1)
# Plot predicted boxes.
for b in frame_pred:
box = Box(b.ego_translation,
b.size,
Quaternion(b.rotation),
name=b.tracking_name,
token=b.tracking_id)
# 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)
# 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))
else:
color = self.id2color[b.tracking_id]
box.render(ax, view=np.eye(4), colors=(color, color, color))
# Plot ego pose.
plt.scatter(0, 0, s=96, facecolors='none', edgecolors='k', marker='o')
plt.xlim(-50, 50)
plt.ylim(-50, 50)
# Save to disk and close figure.
fig.savefig(os.path.join(self.save_path, '{}.png'.format(timestamp)))
plt.close(fig)
def _second_det_to_nusc_box(detection):
box3d = detection["box3d_lidar"].detach().cpu().numpy()
scores = detection["scores"].detach().cpu().numpy()
labels = detection["label_preds"].detach().cpu().numpy()
# Note: direction is not converted second_format
# box3d[:, 6] = np.apply_along_axis(
# lambda r: -ds_utils.wrap_to_pi(r + np.pi / 2),
# axis=1,
# arr=box3d[:, 6:])
box_list = []
for i in range(box3d.shape[0]):
quat = Quaternion(axis=[0, 0, 1], radians=box3d[i, 6])
velocity = (np.nan, np.nan, np.nan)
if box3d.shape[1] == 9:
velocity = (*box3d[i, 7:9], 0.0)
# velo_val = np.linalg.norm(box3d[i, 7:9])
# velo_ori = box3d[i, 6]
# velocity = (velo_val * np.cos(velo_ori), velo_val * np.sin(velo_ori), 0.0)
box = Box(box3d[i, :3],
box3d[i, 3:6],
quat,
label=labels[i],
score=scores[i],
velocity=velocity)
box_list.append(box)
return box_list
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 _second_det_to_nusc_box(detection):
from nuscenes.utils.data_classes import Box
import pyquaternion
box3d = detection["box3d_lidar"].detach().cpu().numpy()
scores = detection["scores"].detach().cpu().numpy()
labels = detection["label_preds"].detach().cpu().numpy()
box3d[:, 6] = -box3d[:, 6] - np.pi / 2
box_list = []
for i in range(box3d.shape[0]):
quat = pyquaternion.Quaternion(axis=[0, 0, 1], radians=box3d[i, 6])
velocity = (np.nan, np.nan, np.nan)
if box3d.shape[1] == 9:
velocity = (*box3d[i, 7:9], 0.0)
# velo_val = np.linalg.norm(box3d[i, 7:9])
# velo_ori = box3d[i, 6]
# velocity = (velo_val * np.cos(velo_ori), velo_val * np.sin(velo_ori), 0.0)
box = Box(
box3d[i, :3],
box3d[i, 3:6],
quat,
label=labels[i],
score=scores[i],
velocity=velocity)
box_list.append(box)
return box_list
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 cast_kitti_format_to_nusc_box_3d(cur_boxes,
cur_score,
cur_class,
cur_attribute=None,
cur_velocity=None,
classes=None):
"""
cur_boxes: [-1, 7], kitti format box
cur_score: [-1]
cur_class: [-1]
cur_velocity: [-1, 3]
"""
cur_boxes_ctr = cur_boxes[:, :3]
cur_boxes_size = cur_boxes[:, 3:-1]
cur_boxes_angle = cur_boxes[:, -1]
cur_boxes_ctr[:, 1] -= cur_boxes_size[:, 1] / 2.
cur_boxes_ctr[:, 1] = -cur_boxes_ctr[:, 1] # l, h, w
cur_boxes_ctr = cur_boxes_ctr[:, [0, 2, 1]] # lwh
# from lhw to wlh
cur_boxes_size = cur_boxes_size[:, [2, 0, 1]]
# finally cast angle
cur_boxes_angle = -cur_boxes_angle
box_list = []
for i in range(cur_boxes.shape[0]):
quat = Quaternion(axis=[0, 0, 1], radians=cur_boxes_angle[i])
if cur_velocity is not None:
velocity = (*cur_velocity[i, :], 0.0)
else:
velocity = (np.nan, np.nan, np.nan)
if cur_attribute is not None:
attribute = cur_attribute[i] # 8
cur_class_name = classes[cur_class[i]]
if cur_class_name in [
'car', 'truck', 'bus', 'trailer', 'construction_vehicle'
]:
attribute = np.argmax(attribute[:3])
elif cur_class_name in ['pedestrian']:
attribute = np.argmax(attribute[5:])
elif cur_class_name in ['motorcycle', 'bicycle']:
attribute = np.argmax(attribute[3:5])
elif cur_class_name in ['traffic_cone', 'barrier']:
attribute = -1
else:
attribute = -1
box = Box(
cur_boxes_ctr[i],
cur_boxes_size[i],
quat,
label=cur_class[i],
score=cur_score[i],
velocity=velocity,
name=attribute,
)
box_list.append(box)
return box_list
def get_box(self, sample_annotation_token: str) -> Box:
"""
Instantiates a Box class from a sample annotation record.
:param sample_annotation_token: Unique sample_annotation identifier.
"""
record = self.get('sample_annotation', sample_annotation_token)
return Box(record['translation'], record['size'], Quaternion(record['rotation']),
name=record['category_name'], token=record['token'])
def get_boxes(self, sample_data_token: str) -> List[Box]:
"""
Instantiates Boxes for all annotation for a particular sample_data record. If the sample_data is a
keyframe, this returns the annotations for that sample. But if the sample_data is an intermediate
sample_data, a linear interpolation is applied to estimate the location of the boxes at the time the
sample_data was captured.
:param sample_data_token: Unique sample_data identifier.
"""
# Retrieve sensor & pose records
sd_record = self.get('sample_data', sample_data_token)
curr_sample_record = self.get('sample', sd_record['sample_token'])
if curr_sample_record['prev'] == "" or sd_record['is_key_frame']:
# If no previous annotations available, or if sample_data is keyframe just return the current ones.
boxes = list(map(self.get_box, curr_sample_record['anns']))
else:
prev_sample_record = self.get('sample', curr_sample_record['prev'])
curr_ann_recs = [self.get('sample_annotation', token) for token in curr_sample_record['anns']]
prev_ann_recs = [self.get('sample_annotation', token) for token in prev_sample_record['anns']]
# Maps instance tokens to prev_ann records
prev_inst_map = {entry['instance_token']: entry for entry in prev_ann_recs}
t0 = prev_sample_record['timestamp']
t1 = curr_sample_record['timestamp']
t = sd_record['timestamp']
# There are rare situations where the timestamps in the DB are off so ensure that t0 < t < t1.
t = max(t0, min(t1, t))
boxes = []
for curr_ann_rec in curr_ann_recs:
if curr_ann_rec['instance_token'] in prev_inst_map:
# If the annotated instance existed in the previous frame, interpolate center & orientation.
prev_ann_rec = prev_inst_map[curr_ann_rec['instance_token']]
# Interpolate center.
center = [np.interp(t, [t0, t1], [c0, c1]) for c0, c1 in zip(prev_ann_rec['translation'],
curr_ann_rec['translation'])]
# Interpolate orientation.
rotation = Quaternion.slerp(q0=Quaternion(prev_ann_rec['rotation']),
q1=Quaternion(curr_ann_rec['rotation']),
amount=(t - t0) / (t1 - t0))
box = Box(center, curr_ann_rec['size'], rotation, name=curr_ann_rec['category_name'],
token=curr_ann_rec['token'])
else:
# If not, simply grab the current annotation.
box = self.get_box(curr_ann_rec['token'])
boxes.append(box)
return boxes
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 test_points_in_box(self):
""" Test the box.in_box method. """
vel = (np.nan, np.nan, np.nan)
def qyaw(yaw):
return Quaternion(axis=(0, 0, 1), angle=yaw)
# Check points inside box
box = Box([0.0, 0.0, 0.0], [2.0, 2.0, 0.0], qyaw(0.0), 1, 2.0, vel)
points = np.array([[0.0, 0.0, 0.0], [0.5, 0.5, 0.0]]).transpose()
mask = points_in_box(box, points, wlh_factor=1.0)
self.assertEqual(mask.all(), True)
# Check points outside box
box = Box([0.0, 0.0, 0.0], [2.0, 2.0, 0.0], qyaw(0.0), 1, 2.0, vel)
points = np.array([[0.1, 0.0, 0.0], [0.5, -1.1, 0.0]]).transpose()
mask = points_in_box(box, points, wlh_factor=1.0)
self.assertEqual(mask.all(), False)
# Check corner cases
box = Box([0.0, 0.0, 0.0], [2.0, 2.0, 0.0], qyaw(0.0), 1, 2.0, vel)
points = np.array([[-1.0, -1.0, 0.0], [1.0, 1.0, 0.0]]).transpose()
mask = points_in_box(box, points, wlh_factor=1.0)
self.assertEqual(mask.all(), True)
# Check rotation (45 degs) and translation (by [1,1])
rot = 45
trans = [1.0, 1.0]
box = Box([0.0 + trans[0], 0.0 + trans[1], 0.0], [2.0, 2.0, 0.0],
qyaw(rot / 180.0 * np.pi), 1, 2.0, vel)
points = np.array([[0.70 + trans[0], 0.70 + trans[1], 0.0],
[0.71 + 1.0, 0.71 + 1.0, 0.0]]).transpose()
mask = points_in_box(box, points, wlh_factor=1.0)
self.assertEqual(mask[0], True)
self.assertEqual(mask[1], False)
# Check 3d box
box = Box([0.0, 0.0, 0.0], [2.0, 2.0, 2.0], qyaw(0.0), 1, 2.0, vel)
points = np.array([[0.0, 0.0, 0.0], [0.5, 0.5, 0.5]]).transpose()
mask = points_in_box(box, points, wlh_factor=1.0)
self.assertEqual(mask.all(), True)
# Check wlh factor
for wlh_factor in [0.5, 1.0, 1.5, 10.0]:
box = Box([0.0, 0.0, 0.0], [2.0, 2.0, 0.0], qyaw(0.0), 1, 2.0, vel)
points = np.array([[0.0, 0.0, 0.0], [0.5, 0.5, 0.0]]).transpose()
mask = points_in_box(box, points, wlh_factor=wlh_factor)
self.assertEqual(mask.all(), True)
for wlh_factor in [0.1, 0.49]:
box = Box([0.0, 0.0, 0.0], [2.0, 2.0, 0.0], qyaw(0.0), 1, 2.0, vel)
points = np.array([[0.0, 0.0, 0.0], [0.5, 0.5, 0.0]]).transpose()
mask = points_in_box(box, points, wlh_factor=wlh_factor)
self.assertEqual(mask[0], True)
self.assertEqual(mask[1], False)
def calculate_object_box(box_aligned, box_inclined, image_ground, image_zmax, object_class, score, p):
# Grid Map data
resolution = p['pointcloud_grid_map_interface']['grids']['cartesian']['resolution']['x']
if resolution - p['pointcloud_grid_map_interface']['grids']['cartesian']['resolution']['y'] > 0.001:
raise ValueError('Grid Map resolution in x and y direction need to be equal')
grid_map_data_origin_idx = np.array(
[p['pointcloud_grid_map_interface']['grids']['cartesian']['range']['x'] / 2 +
p['pointcloud_grid_map_interface']['grids']['cartesian']['offset']['x'],
p['pointcloud_grid_map_interface']['grids']['cartesian']['range']['y'] / 2])
image_to_velo = np.array([[0, -1, grid_map_data_origin_idx[0]], [-1, 0, grid_map_data_origin_idx[1]], [0, 0, 1]])
heigth_diff = (p['pointcloud_grid_map_interface']['z_max'] - p['pointcloud_grid_map_interface']['z_min'])
height_offset = p['pointcloud_grid_map_interface']['z_min']
# Convert box
height, width = image_zmax.shape
y_min = box_aligned[0] * height
x_min = box_aligned[1] * width
y_max = box_aligned[2] * height
x_max = box_aligned[3] * width
x_c = box_inclined[0] * width
y_c = box_inclined[1] * height
w_s = box_inclined[2]
h_s = box_inclined[3]
sin_angle = box_inclined[4]
cos_angle = box_inclined[5]
angle_rad = math.atan2(sin_angle, cos_angle) / 2
vec_s_x = math.cos(angle_rad)
vec_s_y = math.sin(angle_rad)
object_width = w_s * resolution / \
math.sqrt(vec_s_x * vec_s_x / (height * height) + vec_s_y * vec_s_y / (width * width))
object_length = h_s * resolution/ \
math.sqrt(vec_s_x * vec_s_x / (width * width) + vec_s_y * vec_s_y / (height * height))
image_ground_box = image_ground[int(y_min):math.ceil(y_max),
int(x_min):math.ceil(x_max)]
mean_ground = image_ground_box.mean()
image_height_max_box = image_zmax[int(y_min):math.ceil(y_max),
int(x_min):math.ceil(x_max)]
height_max = image_height_max_box.max()
height_max_m = heigth_diff * height_max / 255.0 + height_offset
mean_ground_m = heigth_diff * mean_ground / 255.0 + height_offset
if height_max_m - mean_ground_m > 0:
object_height = height_max_m - mean_ground_m
else:
object_height = 0.1
# Box output in box coordinate system
object_wlh = [object_width, object_length, object_height]
object_center = np.array([x_c * resolution, y_c * resolution])
object_center_velo = image_to_velo.dot(np.append(object_center, 1))
object_center_velo[2] = (height_max_m + mean_ground_m) / 2
object_quat = Quaternion(axis=(0, 0, 1), angle=angle_rad)
return Box(object_center_velo, object_wlh, object_quat, score=score,
velocity=(0, 0, 0), name=object_class)
def filter_eval_boxes(nusc: NuScenes,
eval_boxes: EvalBoxes,
max_dist: Dict[str, float],
verbose: bool = False) -> EvalBoxes:
"""
Applies filtering to boxes. Distance, bike-racks and points per box.
:param nusc: An instance of the NuScenes class.
:param eval_boxes: An instance of the EvalBoxes class.
:param max_dist: Maps the detection name to the eval distance threshold for that class.
:param verbose: Whether to print to stdout.
"""
# Accumulators for number of filtered boxes.
total, dist_filter, point_filter, bike_rack_filter = 0, 0, 0, 0
for ind, sample_token in enumerate(eval_boxes.sample_tokens):
# Filter on distance first
total += len(eval_boxes[sample_token])
eval_boxes.boxes[sample_token] = [box for box in eval_boxes[sample_token] if
box.ego_dist < max_dist[box.detection_name]]
dist_filter += len(eval_boxes[sample_token])
# Then remove boxes with zero points in them. Eval boxes have -1 points by default.
eval_boxes.boxes[sample_token] = [box for box in eval_boxes[sample_token] if not box.num_pts == 0]
point_filter += len(eval_boxes[sample_token])
# Perform bike-rack filtering
sample_anns = nusc.get('sample', sample_token)['anns']
bikerack_recs = [nusc.get('sample_annotation', ann) for ann in sample_anns if
nusc.get('sample_annotation', ann)['category_name'] == 'static_object.bicycle_rack']
bikerack_boxes = [Box(rec['translation'], rec['size'], Quaternion(rec['rotation'])) for rec in bikerack_recs]
filtered_boxes = []
for box in eval_boxes[sample_token]:
if box.detection_name in ['bicycle', 'motorcycle']:
in_a_bikerack = False
for bikerack_box in bikerack_boxes:
if np.sum(points_in_box(bikerack_box, np.expand_dims(np.array(box.translation), axis=1))) > 0:
in_a_bikerack = True
if not in_a_bikerack:
filtered_boxes.append(box)
else:
filtered_boxes.append(box)
eval_boxes.boxes[sample_token] = filtered_boxes
bike_rack_filter += len(eval_boxes.boxes[sample_token])
if verbose:
print("=> Original number of boxes: %d" % total)
print("=> After distance based filtering: %d" % dist_filter)
print("=> After LIDAR points based filtering: %d" % point_filter)
print("=> After bike rack filtering: %d" % bike_rack_filter)
return eval_boxes
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 boxes_lidar_to_nusenes(det_info):
boxes3d = det_info['boxes_lidar']
scores = det_info['score']
labels = det_info['pred_labels']
box_list = []
for k in range(boxes3d.shape[0]):
quat = Quaternion(axis=[0, 0, 1], radians=boxes3d[k, 6])
velocity = (*boxes3d[k, 7:9], 0.0) if boxes3d.shape[1] == 9 else (0.0, 0.0, 0.0)
box = Box(
boxes3d[k, :3],
boxes3d[k, [4, 3, 5]], # wlh
quat, label=labels[k], score=scores[k], velocity=velocity,
)
box_list.append(box)
return box_list
def _get_anns(self, ann_tokens, ref_pose_record):
"""
Get all annotations for a given sample
:param ann_tokens (list): list of annotation tokens
:param cam (dict): ca
"""
## TODO: add filtering based on num_radar/num_lidar points here
## TODO: for 'scene' samples, ref_pose_record stuff needs attention here
annotations = []
for i, token in enumerate(ann_tokens):
ann = self.get('sample_annotation', token)
this_ann = {}
try:
this_ann['category'] = self.cfg.CATEGORIES[
ann['category_name']]
except:
continue
this_ann['category_id'] = self.cfg.CAT_ID[this_ann['category']]
this_ann['num_lidar_pts'] = ann['num_lidar_pts']
this_ann['num_radar_pts'] = ann['num_radar_pts']
## Create Box object (boxes are in global coordinates)
box = Box(ann['translation'],
ann['size'],
Quaternion(ann['rotation']),
name=this_ann['category'],
token=ann['token'])
## Get distance to vehicle
box_dist = nsutils.get_box_dist(box, ref_pose_record)
this_ann['distance'] = box_dist
if self.cfg.MAX_BOX_DIST:
if box_dist > abs(self.cfg.MAX_BOX_DIST):
continue
## Take to the vehicle coordinate system
box = nsutils.global_to_vehicle(box, ref_pose_record)
## Calculate box velocity
if self.cfg.BOX_VELOCITY:
box.velocity = self.box_velocity(box.token)
this_ann['box_3d'] = box
annotations.append(this_ann)
return annotations
def _second_det_to_nusc_box(detection):
from nuscenes.utils.data_classes import Box
import pyquaternion
box3d = detection["box3d_lidar"].detach().cpu().numpy()
scores = detection["scores"].detach().cpu().numpy()
labels = detection["label_preds"].detach().cpu().numpy()
box3d[:, 6] = -box3d[:, 6] - np.pi / 2
box_list = []
for i in range(box3d.shape[0]):
quat = pyquaternion.Quaternion(axis=[0, 0, 1], radians=box3d[i, 6])
box = Box(box3d[i, :3],
box3d[i, 3:6],
quat,
label=labels[i],
score=scores[i])
box_list.append(box)
return box_list
def __load_commands__(self) -> [List, Dict]:
"""
:return:
"""
t2c = self.__load_t2c_table__(self.version)
cmnds = t2c["commands"]
self.scene_tokens = t2c["scene_tokens"]
ret = []
lookup = {}
max_length = 0
# Get data from DB
for c in cmnds:
scene_token = c["scene_token"]
if self.version == "train" or self.version == "val":
box = Box(
c["translation"],
c["size"],
Quaternion(c["rotation"]),
name=c["obj_name"],
)
else:
box = None
command_token = c["command_token"]
command_text = c["command"]
cmd = Command(
self,
scene_token,
c["sample_token"],
box,
command_text,
command_token,
box_token=c.get("box_token", None),
)
ret.append(cmd)
if len(command_text) > max_length:
max_length = len(command_text)
if scene_token not in lookup:
lookup[scene_token] = []
lookup[scene_token].append(cmd)
return ret, lookup, max_length
def get_sample_data(pred):
box_list = []
score_list = []
pred = pred.copy()
for item in pred:
box = Box(item['translation'],
item['size'],
Quaternion(item['rotation']),
name=item['detection_name'])
score_list.append(item['detection_score'])
box_list.append(box)
top_boxes = reorganize_boxes(box_list)
top_scores = np.array(score_list).reshape(-1)
return top_boxes, top_scores
def _second_det_to_nusc_box(detection):
box3d = detection["box3d_lidar"].detach().cpu().numpy()
scores = detection["scores"].detach().cpu().numpy()
labels = detection["label_preds"].detach().cpu().numpy()
box3d[:, -1] = -box3d[:, -1] - np.pi / 2
box_list = []
for i in range(box3d.shape[0]):
quat = Quaternion(axis=[0, 0, 1], radians=box3d[i, -1])
velocity = (*box3d[i, 6:8], 0.0)
box = Box(
box3d[i, :3],
box3d[i, 3:6],
quat,
label=labels[i],
score=scores[i],
velocity=velocity,
)
box_list.append(box)
return box_list
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 get_boxes(nusc, sample_data_token: str) -> List[Box]:
"""
Instantiates Boxes for all annotation for a particular sample_data record. If the sample_data is a
keyframe, this returns the annotations for that sample. But if the sample_data is an intermediate
sample_data, a linear interpolation is applied to estimate the location of the boxes at the time the
sample_data was captured.
:param sample_data_token: Unique sample_data identifier.
"""
global results
# Retrieve sensor & pose records
sd_record = nusc.get('sample_data', sample_data_token)
curr_sample_record = nusc.get('sample', sd_record['sample_token'])
# if curr_sample_record['prev'] == "" or sd_record['is_key_frame']:
# if sd_record['is_key_frame']:
# If no previous annotations available, or if sample_data is keyframe just return the current ones.
# boxes = list(map(get_box, nusc, curr_sample_record['anns']))
boxes = []
predictions = results[curr_sample_record['token']]
for pred in predictions:
box = Box(pred['translation'],
pred['size'],
Quaternion(pred['rotation']),
name=pred['detection_name'],
token=None)
# if you want all outputs remove if line,
# currently predictions whose scores greater than 0.7 are chosen to render
if pred['detection_score'] >= 0.7:
boxes.append(box)
# else:
# boxes = []
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 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 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 gen_2d_grid_gt_for_visualization(data_dict: dict,
grid_size: np.array,
extents: np.array = None,
frame_skip: int = 0,
reordered: bool = False,
proportion_thresh: float = 0.5,
category_num: int = 5,
one_hot_thresh: float = 0.8):
"""
Generate the 2d grid ground-truth for the input point cloud.
The ground-truth is: the displacement vectors of the occupied pixels in BEV image.
The displacement is computed w.r.t the current time and the future time
The difference between this function and gen_2d_grid_gt: the input is "data_dict" instead of sample file path.
:param data_dict: The dictionary storing point cloud data and annotations
:param grid_size: The size of each pixel
:param extents: The extents of the point cloud on the 2D xy plane. Shape (3, 2)
:param frame_skip: The number of sample frames that need to be skipped
:param reordered: Whether need to reorder the results, so that the first element corresponds to the oldest record.
:param proportion_thresh: Within a given pixel, only when the proportion of foreground points exceeds this threshold
will we compute the displacement vector for this pixel.
:param category_num: The number of categories for points.
:param one_hot_thresh: When the proportion of the majority points within a cell exceeds this threshold, we
compute the (hard) one-hot category vector for this cell, otherwise compute the soft category vector.
:return: The ground-truth displacement field. Shape (num_sweeps, image height, image width, 2).
"""
num_sweeps = data_dict['num_sweeps']
times = data_dict['times']
num_past_sweeps = len(np.where(times >= 0)[0])
num_future_sweeps = len(np.where(times < 0)[0])
assert num_past_sweeps + num_future_sweeps == num_sweeps, "The number of sweeps is incorrect!"
pc_list = []
for i in range(num_sweeps):
pc = data_dict['pc_' + str(i)]
pc_list.append(pc.T)
# Retrieve the instance boxes
num_instances = data_dict['num_instances']
instance_box_list = list()
instance_cat_list = list() # for instance categories
for i in range(num_instances):
instance = data_dict['instance_boxes_' + str(i)]
category = data_dict['category_' + str(i)]
instance_box_list.append(instance)
instance_cat_list.append(category)
# ----------------------------------------------------
# Filter and sort the reference point cloud
refer_pc = pc_list[0]
refer_pc = refer_pc[:, 0:3]
if extents is not None:
if extents.shape != (3, 2):
raise ValueError("Extents are the wrong shape {}".format(
extents.shape))
filter_idx = np.where((extents[0, 0] < refer_pc[:, 0])
& (refer_pc[:, 0] < extents[0, 1])
& (extents[1, 0] < refer_pc[:, 1])
& (refer_pc[:, 1] < extents[1, 1])
& (extents[2, 0] < refer_pc[:, 2])
& (refer_pc[:, 2] < extents[2, 1]))[0]
refer_pc = refer_pc[filter_idx]
# -- Discretize pixel coordinates to given quantization size
discrete_pts = np.floor(refer_pc[:, 0:2] / grid_size).astype(np.int32)
# -- Use Lex Sort, sort by x, then y
x_col = discrete_pts[:, 0]
y_col = discrete_pts[:, 1]
sorted_order = np.lexsort((y_col, x_col))
refer_pc = refer_pc[sorted_order]
discrete_pts = discrete_pts[sorted_order]
contiguous_array = np.ascontiguousarray(discrete_pts).view(
np.dtype(
(np.void, discrete_pts.dtype.itemsize * discrete_pts.shape[1])))
# -- The new coordinates are the discretized array with its unique indexes
_, unique_indices = np.unique(contiguous_array, return_index=True)
# -- Sort unique indices to preserve order
unique_indices.sort()
pixel_coords = discrete_pts[unique_indices]
# -- Number of points per voxel, last voxel calculated separately
num_points_in_pixel = np.diff(unique_indices)
num_points_in_pixel = np.append(num_points_in_pixel,
discrete_pts.shape[0] - unique_indices[-1])
# -- Compute the minimum and maximum voxel coordinates
if extents is not None:
min_pixel_coord = np.floor(extents.T[0, 0:2] / grid_size)
max_pixel_coord = np.ceil(extents.T[1, 0:2] / grid_size) - 1
else:
min_pixel_coord = np.amin(pixel_coords, axis=0)
max_pixel_coord = np.amax(pixel_coords, axis=0)
# -- Get the voxel grid dimensions
num_divisions = ((max_pixel_coord - min_pixel_coord) + 1).astype(np.int32)
# -- Bring the min voxel to the origin
pixel_indices = (pixel_coords - min_pixel_coord).astype(int)
# ----------------------------------------------------
# ----------------------------------------------------
# Get the point cloud subsets, which are inside different instance bounding boxes
refer_box_list = list()
refer_pc_idx_per_bbox = list()
points_category = np.zeros(refer_pc.shape[0],
dtype=np.int) # store the point categories
for i in range(num_instances):
instance_cat = instance_cat_list[i]
instance_box = instance_box_list[i]
instance_box_data = instance_box[0]
assert not np.isnan(instance_box_data).any(
), "In the keyframe, there should not be NaN box annotation!"
tmp_box = Box(center=instance_box_data[:3],
size=instance_box_data[3:6],
orientation=Quaternion(instance_box_data[6:]))
idx = point_in_hull_fast(refer_pc[:, 0:3], tmp_box)
refer_pc_idx_per_bbox.append(idx)
refer_box_list.append(tmp_box)
points_category[idx] = instance_cat
if len(refer_pc_idx_per_bbox) > 0:
refer_pc_idx_inside_box = np.concatenate(
refer_pc_idx_per_bbox).tolist()
else:
refer_pc_idx_inside_box = []
refer_pc_idx_outside_box = set(range(
refer_pc.shape[0])) - set(refer_pc_idx_inside_box)
refer_pc_idx_outside_box = list(refer_pc_idx_outside_box)
# Compute pixel (cell) categories
pixel_cat = np.zeros([unique_indices.shape[0], category_num],
dtype=np.float32)
most_freq_info = []
for h, v in enumerate(zip(unique_indices, num_points_in_pixel)):
pixel_elements_categories = points_category[v[0]:v[0] + v[1]]
elements_freq = np.bincount(pixel_elements_categories,
minlength=category_num)
assert np.sum(
elements_freq) == v[1], "The frequency count is incorrect."
elements_freq = elements_freq / float(v[1])
most_freq_cat, most_freq = np.argmax(elements_freq), np.max(
elements_freq)
most_freq_info.append([most_freq_cat, most_freq])
if most_freq >= one_hot_thresh:
one_hot_cat = np.zeros(category_num, dtype=np.float32)
one_hot_cat[most_freq_cat] = 1.0
pixel_cat[h] = one_hot_cat
else:
pixel_cat[
h] = elements_freq # we use soft category probability vector.
pixel_cat_map = np.zeros(
(num_divisions[0], num_divisions[1], category_num), dtype=np.float32)
pixel_cat_map[pixel_indices[:, 0], pixel_indices[:, 1]] = pixel_cat[:]
# Set the non-zero pixels to 1.0
# Note that the non-zero pixels correspond to the foreground and background objects
non_empty_map = np.zeros((num_divisions[0], num_divisions[1]),
dtype=np.float32)
non_empty_map[pixel_indices[:, 0], pixel_indices[:, 1]] = 1.0
# Compute the displacement vectors w.r.t. the other sweeps
all_disp_field_gt_list = list()
zero_disp_field = np.zeros((num_divisions[0], num_divisions[1], 2),
dtype=np.float32)
all_disp_field_gt_list.append(zero_disp_field)
all_valid_pixel_maps_list = list(
) # valid pixel map will be used for masking the computation of loss
all_valid_pixel_maps_list.append(non_empty_map)
# -- Skip some frames if necessary
past_part = list(range(0, num_past_sweeps, frame_skip + 1))
future_part = list(
range(num_past_sweeps + frame_skip, num_sweeps, frame_skip + 1))
frame_considered = np.asarray(past_part + future_part)
for i in frame_considered[1:]:
curr_disp_vectors = np.zeros_like(refer_pc, dtype=np.float32)
curr_disp_vectors.fill(np.nan)
curr_disp_vectors[refer_pc_idx_outside_box, ] = 0.0
# First, for each instance, compute the corresponding points displacement.
for j in range(num_instances):
instance_box = instance_box_list[j]
instance_box_data = instance_box[i] # This is for the i-th sweep
if np.isnan(instance_box_data).any(
): # It is possible that in this sweep there is no annotation
continue
tmp_box = Box(center=instance_box_data[:3],
size=instance_box_data[3:6],
orientation=Quaternion(instance_box_data[6:]))
pc_in_bbox_idx = refer_pc_idx_per_bbox[j]
disp_vectors = calc_displace_vector(refer_pc[pc_in_bbox_idx],
refer_box_list[j], tmp_box)
curr_disp_vectors[pc_in_bbox_idx] = disp_vectors[:]
# Second, compute the mean displacement vector and category for each non-empty pixel
disp_field = np.zeros(
[unique_indices.shape[0], 2],
dtype=np.float32) # we only consider the 2D field
# We only compute loss for valid pixels where there are corresponding box annotations between two frames
valid_pixels = np.zeros(unique_indices.shape[0], dtype=np.bool)
for h, v in enumerate(zip(unique_indices, num_points_in_pixel)):
# Only when the number of majority points exceeds predefined proportion, we compute
# the displacement vector for this pixel. Otherwise, We consider it is background (possibly ground plane)
# and has zero displacement.
pixel_elements_categories = points_category[v[0]:v[0] + v[1]]
most_freq_cat, most_freq = most_freq_info[h]
if most_freq >= proportion_thresh:
most_freq_cat_idx = np.where(
pixel_elements_categories == most_freq_cat)[0]
most_freq_cat_disp_vectors = curr_disp_vectors[v[0]:v[0] +
v[1], :3]
most_freq_cat_disp_vectors = most_freq_cat_disp_vectors[
most_freq_cat_idx]
if np.isnan(most_freq_cat_disp_vectors).any(
): # contains invalid disp vectors
valid_pixels[h] = 0.0
else:
mean_disp_vector = np.mean(most_freq_cat_disp_vectors,
axis=0)
disp_field[h] = mean_disp_vector[
0:2] # ignore the z direction
valid_pixels[h] = 1.0
# Finally, assemble to a 2D image
disp_field_sparse = np.zeros((num_divisions[0], num_divisions[1], 2),
dtype=np.float32)
disp_field_sparse[pixel_indices[:, 0],
pixel_indices[:, 1]] = disp_field[:]
valid_pixel_map = np.zeros((num_divisions[0], num_divisions[1]),
dtype=np.float32)
valid_pixel_map[pixel_indices[:, 0],
pixel_indices[:, 1]] = valid_pixels[:]
all_disp_field_gt_list.append(disp_field_sparse)
all_valid_pixel_maps_list.append(valid_pixel_map)
all_disp_field_gt_list = np.stack(all_disp_field_gt_list, axis=0)
all_valid_pixel_maps_list = np.stack(all_valid_pixel_maps_list, axis=0)
if reordered:
num_past = len(past_part)
all_disp_field_gt_list[0:num_past] = all_disp_field_gt_list[(num_past -
1)::-1]
all_valid_pixel_maps_list[0:num_past] = all_valid_pixel_maps_list[(
num_past - 1)::-1]
return all_disp_field_gt_list, all_valid_pixel_maps_list, non_empty_map, pixel_cat_map
def vis_model_per_sample_data(bev_input_data,
data_dict,
frame_skip=3,
voxel_size=(0.25, 0.25, 0.4),
loaded_models=None,
which_model="MotionNet",
model_path=None,
img_save_dir=None,
use_adj_frame_pred=False,
use_motion_state_pred_masking=False,
frame_idx=0,
disp=True):
"""
Visualize the prediction (ie, displacement field) results.
bev_ipput_data: the preprocessed sparse bev data
data_dict: a dictionary storing the point cloud data and annotations
frame_skip: how many frames we want to skip for future frames
voxel_size: the size of each voxel
loaded_models: the model which has loaded the pretrained weights
which_model: which model to apply ['MotionNet'/'MotionNetMGDA']
model_path: the path to the pretrained model
img_save_dir: the directory for saving the predicted image
use_adj_frame_pred: whether to predict the relative offsets between two adjacent frames
use_motion_state_pred_masking: whether to threshold the prediction with motion state estimation results
frame_idx: used for specifying the name of saved image frames
disp: whether to immediately show the predicted results
"""
if model_path is None:
raise ValueError("Need to specify saved model path.")
if img_save_dir is None:
raise ValueError("Need to specify image save path.")
device = torch.device("cuda" if torch.cuda.is_available() else "cpu")
fig, ax = plt.subplots(1, 3, figsize=(20, 8))
# Load pre-trained network weights
if which_model == "MotionNet":
model = loaded_models[0]
else:
model_encoder = loaded_models[0]
model_head = loaded_models[1]
# Prepare data for the network
padded_voxel_points, all_disp_field_gt, all_valid_pixel_maps,\
non_empty_map, pixel_cat_map_gt, num_past_pcs, num_future_pcs = bev_input_data
padded_voxel_points = torch.unsqueeze(padded_voxel_points, 0).to(device)
# Make prediction
if which_model == "MotionNet":
model.eval()
else:
model_encoder.eval()
model_head.eval()
with torch.no_grad():
if which_model == "MotionNet":
disp_pred, cat_pred, motion_pred = model(padded_voxel_points)
else:
shared_feats = model_encoder(padded_voxel_points)
disp_pred, cat_pred, motion_pred = model_head(shared_feats)
disp_pred = disp_pred.cpu().numpy()
disp_pred = np.transpose(disp_pred, (0, 2, 3, 1))
cat_pred = np.squeeze(cat_pred.cpu().numpy(), 0)
if use_adj_frame_pred: # The prediction are the displacement between adjacent frames
for c in range(1, disp_pred.shape[0]):
disp_pred[c, ...] = disp_pred[c, ...] + disp_pred[c - 1, ...]
if use_motion_state_pred_masking:
motion_pred_numpy = motion_pred.cpu().numpy()
motion_pred_numpy = np.argmax(motion_pred_numpy, axis=1)
motion_mask = motion_pred_numpy == 0
cat_pred_numpy = np.argmax(cat_pred, axis=0)
cat_mask = np.logical_and(cat_pred_numpy == 0, non_empty_map == 1)
cat_mask = np.expand_dims(cat_mask, 0)
cat_weight_map = np.ones_like(motion_pred_numpy, dtype=np.float32)
cat_weight_map[motion_mask] = 0.0
cat_weight_map[cat_mask] = 0.0
cat_weight_map = cat_weight_map[:, :, :,
np.newaxis] # (1, h, w. 1)
disp_pred = disp_pred * cat_weight_map
# ------------------------- Visualization -------------------------
# --- Load the point cloud data and annotations ---
num_sweeps = data_dict['num_sweeps']
times = data_dict['times']
num_past_sweeps = len(np.where(times >= 0)[0])
num_future_sweeps = len(np.where(times < 0)[0])
assert num_past_sweeps + num_future_sweeps == num_sweeps, "The number of sweeps is incorrect!"
# Load point cloud
pc_list = []
for i in range(num_sweeps):
pc = data_dict['pc_' + str(i)]
pc_list.append(pc)
# Reorder the pc, and skip sample frames if wanted
tmp_pc_list_1 = pc_list[0:num_past_sweeps:(frame_skip + 1)]
tmp_pc_list_1 = tmp_pc_list_1[::-1]
tmp_pc_list_2 = pc_list[(num_past_sweeps + frame_skip)::(frame_skip + 1)]
pc_list = tmp_pc_list_1 + tmp_pc_list_2
num_past_pcs = len(tmp_pc_list_1)
# Load box annotations, and reorder and skip some annotations if wanted
num_instances = data_dict['num_instances']
instance_box_list = list()
for i in range(num_instances):
instance = data_dict['instance_boxes_' + str(i)]
# Reorder the boxes
tmp_instance = np.zeros((len(pc_list), instance.shape[1]),
dtype=np.float32)
tmp_instance[(num_past_pcs -
1)::-1] = instance[0:num_past_sweeps:(frame_skip + 1)]
tmp_instance[num_past_pcs:] = instance[(num_past_sweeps +
frame_skip)::(frame_skip + 1)]
instance = tmp_instance[:]
instance_box_list.append(instance)
# Draw the LIDAR and quiver plots
# The distant points are very sparse and not reliable. We do not show them.
border_meter = 4
border_pixel = border_meter * 4
x_lim = [-(32 - border_meter), (32 - border_meter)]
y_lim = [-(32 - border_meter), (32 - border_meter)]
# We only show the cells having one-hot category vectors
max_prob = np.amax(pixel_cat_map_gt, axis=-1)
filter_mask = max_prob == 1.0
pixel_cat_map = np.argmax(
pixel_cat_map_gt,
axis=-1) + 1 # category starts from 1 (background), etc
pixel_cat_map = (pixel_cat_map * non_empty_map * filter_mask).astype(
np.int)
cat_pred = np.argmax(cat_pred, axis=0) + 1
cat_pred = (cat_pred * non_empty_map * filter_mask).astype(np.int)
# --- Visualization ---
idx = num_past_pcs - 1
points = pc_list[idx]
ax[0].scatter(points[0, :], points[1, :], c=points[2, :], s=1)
ax[0].set_xlim(x_lim[0], x_lim[1])
ax[0].set_ylim(y_lim[0], y_lim[1])
ax[0].axis('off')
ax[0].set_aspect('equal')
ax[0].title.set_text('LIDAR data')
for j in range(num_instances):
inst = instance_box_list[j]
box_data = inst[idx]
if np.isnan(box_data).any():
continue
box = Box(center=box_data[0:3],
size=box_data[3:6],
orientation=Quaternion(box_data[6:]))
box.render(ax[0])
# Plot quiver. We only show non-empty vectors. Plot each category.
field_gt = all_disp_field_gt[-1]
idx_x = np.arange(field_gt.shape[0])
idx_y = np.arange(field_gt.shape[1])
idx_x, idx_y = np.meshgrid(idx_x, idx_y, indexing='ij')
qk = [None] * len(color_map) # for quiver key
for k in range(len(color_map)):
# ------------------------ Ground-truth ------------------------
mask = pixel_cat_map == (k + 1)
# For cells with very small movements, we threshold them to be static
field_gt_norm = np.linalg.norm(field_gt, ord=2, axis=-1) # out: (h, w)
thd_mask = field_gt_norm <= 0.4
field_gt[thd_mask, :] = 0
# Get the displacement field
X = idx_x[mask]
Y = idx_y[mask]
U = field_gt[:, :, 0][mask] / voxel_size[
0] # the distance between pixels is w.r.t. grid size (e.g., 0.2m)
V = field_gt[:, :, 1][mask] / voxel_size[1]
qk[k] = ax[1].quiver(X,
Y,
U,
V,
angles='xy',
scale_units='xy',
scale=1,
color=color_map[k])
ax[1].quiverkey(qk[k],
X=0.0 + k / 5.0,
Y=1.1,
U=20,
label=cat_names[k],
labelpos='E')
ax[1].set_xlim(border_pixel, field_gt.shape[0] - border_pixel)
ax[1].set_ylim(border_pixel, field_gt.shape[1] - border_pixel)
ax[1].set_aspect('equal')
ax[1].title.set_text('Ground-truth')
ax[1].axis('off')
# ------------------------ Prediction ------------------------
# Show the prediction results. We show the cells corresponding to the non-empty one-hot gt cells.
mask_pred = cat_pred == (k + 1)
field_pred = disp_pred[
-1] # Show last prediction, ie., the 20-th frame
# For cells with very small movements, we threshold them to be static
field_pred_norm = np.linalg.norm(field_pred, ord=2,
axis=-1) # out: (h, w)
thd_mask = field_pred_norm <= 0.4
field_pred[thd_mask, :] = 0
# We use the same indices as the ground-truth, since we are currently focused on the foreground
X_pred = idx_x[mask_pred]
Y_pred = idx_y[mask_pred]
U_pred = field_pred[:, :, 0][mask_pred] / voxel_size[0]
V_pred = field_pred[:, :, 1][mask_pred] / voxel_size[1]
ax[2].quiver(X_pred,
Y_pred,
U_pred,
V_pred,
angles='xy',
scale_units='xy',
scale=1,
color=color_map[k])
ax[2].set_xlim(border_pixel, field_pred.shape[0] - border_pixel)
ax[2].set_ylim(border_pixel, field_pred.shape[1] - border_pixel)
ax[2].set_aspect('equal')
ax[2].title.set_text('Prediction')
ax[2].axis('off')
print("finish sample {}".format(frame_idx))
plt.savefig(os.path.join(img_save_dir, str(frame_idx) + '.png'))
if disp:
plt.pause(0.02)
ax[0].clear()
ax[1].clear()
ax[2].clear()
if disp:
plt.show()
# 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)