def annotation_3dobject(self, idx, raw=False):
seq_id, frame_idx = idx
fname = "label_lidars/%04d.json" % frame_idx
if self._return_file_path:
return self.base_path / seq_id / fname
if self.inzip:
with PatchedZipFile(self.base_path / (seq_id + ".zip"),
to_extract=fname) as ar:
data = json.loads(ar.read(fname))
else:
with (self.base_path / seq_id / fname).open() as fin:
data = json.load(fin)
labels = list(map(edict, data))
if raw:
return labels
arr = Target3DArray(frame="vehicle") # or frame=None
for label in labels:
tid = base64.urlsafe_b64decode(label.id[:12])
tid, = struct.unpack('Q', tid[:8])
target = ObjectTarget3D(label.center,
Rotation.from_euler("z", label.heading),
label.size,
ObjectTag(label.label, WaymoObjectClass),
tid=tid)
arr.append(target)
return arr
def report(self) -> Target3DArray:
'''
Return the collection of valid tracked targets
'''
array = Target3DArray(frame=self._last_frameid,
timestamp=self._last_timestamp)
for tid in self.tracked_ids:
target = TrackingTarget3D(
position=self._tracked_poses[tid].position,
orientation=self._tracked_poses[tid].orientation,
dimension=self._tracked_features[tid].dimension,
velocity=self._tracked_poses[tid].velocity,
angular_velocity=self._tracked_poses[tid].angular_velocity,
tag=self._tracked_features[tid].classification,
tid=tid,
position_var=self._tracked_poses[tid].position_var,
orientation_var=self._tracked_poses[tid].orientation_var,
dimension_var=self._tracked_features[tid].dimension_var,
velocity_var=self._tracked_poses[tid].velocity_var,
angular_velocity_var=self._tracked_poses[tid].
angular_velocity_var,
history=self._timer_track[
tid] # TODO: discount the object score by lost time
)
array.append(target)
return array
def _preload_tracklets(self, seq_id):
if seq_id in self._tracklet_cache:
return
date = self._get_date(seq_id)
fname = Path(date, seq_id, "tracklet_labels.xml")
if self.inzip:
zname = seq_id[:-len(self.datatype)] + "tracklets"
with ZipFile(self.base_path / f"{zname}.zip") as data:
tracklets = utils.load_tracklets(data, fname)
else:
tracklets = utils.load_tracklets(self.base_path, fname)
# inverse mapping
objs = defaultdict(list) # (frame -> list of objects)
for tid, tr in enumerate(tracklets):
dim = [tr.l, tr.w, tr.h]
tag = ObjectTag(tr.objectType, KittiObjectClass)
for pose_idx, pose in enumerate(tr.poses):
pos = [pose.tx, pose.ty, pose.tz]
pos[2] += dim[2] / 2
ori = Rotation.from_euler("ZYX", (pose.rz, pose.ry, pose.rx))
objs[pose_idx + tr.first_frame].append(
ObjectTarget3D(pos, ori, dim, tag, tid=tid))
self._tracklet_cache[seq_id] = {
k: Target3DArray(l, frame="velo")
for k, l in objs.items()
}
def visualize_detections_bev(ax: axes.Axes,
visualizer_frame: str,
targets: Target3DArray,
calib: TransformSet,
box_color=(0, 1, 0),
thickness=2,
tags=None):
# change frame to the same
if targets.frame != visualizer_frame:
targets = calib.transform_objects(targets, frame_to=visualizer_frame)
for target in targets.filter_tag(tags):
# draw vehicle frames
points = target.corners
pairs = [(0, 1), (2, 3), (0, 2), (1, 3)]
for i, j in pairs:
ax.add_line(
lines.Line2D((points[i, 0], points[j, 0]),
(points[i, 1], points[j, 1]),
c=box_color,
lw=thickness))
# draw velocity
if isinstance(target, TrackingTarget3D):
pstart = target.position[:2]
pend = target.position[:2] + target.velocity[:2]
ax.add_line(
lines.Line2D((pstart[0], pend[0]), (pstart[1], pend[1]),
c=box_color,
lw=thickness))
def parse_label(label: list, raw_calib: dict):
'''
Generate object array from loaded label or result text
'''
Tr = raw_calib['Tr_velo_to_cam'].reshape(3, 4)
RRect = Rotation.from_matrix(raw_calib['R0_rect'].reshape(3, 3))
HR, HT = Rotation.from_matrix(Tr[:, :3]), Tr[:, 3]
objects = Target3DArray()
objects.frame = "velo"
for item in label:
if item[0] == KittiObjectClass.DontCare:
continue
h, w, l = item[8:11] # height, width, length
position = item[11:14] # x, y, z in camera coordinate
ry = item[14] # rotation of y axis in camera coordinate
position[1] -= h / 2
# parse object position and orientation
position = np.dot(position, RRect.inv().as_matrix().T)
position = HR.inv().as_matrix().dot(position - HT)
orientation = HR.inv() * RRect.inv() * Rotation.from_euler('y', ry)
orientation *= Rotation.from_euler(
"x", np.pi / 2) # change dimension from l,h,w to l,w,h
score = item[15] if len(item) == 16 else None
tag = ObjectTag(item[0], KittiObjectClass, scores=score)
target = ObjectTarget3D(position, orientation, [l, w, h], tag)
objects.append(target)
return objects
def set_up_matcher(self):
# src boxes
r = Rotation.from_euler("Z", 0)
d = [2, 2, 2]
dt1 = ObjectTarget3D([0, 0, 0], r, d,
ObjectTag(KittiObjectClass.Car, scores=0.8))
dt2 = ObjectTarget3D([1, 1, 0], r, d,
ObjectTag(KittiObjectClass.Van, scores=0.7))
dt3 = ObjectTarget3D([-1, -1, 0], r, d,
ObjectTag(KittiObjectClass.Car, scores=0.8))
dt_boxes = Target3DArray([dt1, dt2, dt3], frame="test")
# dst boxes
r = Rotation.from_euler("Z", 0)
d = [2, 2, 2]
gt1 = ObjectTarget3D([0, 0, 0], r, d, ObjectTag(KittiObjectClass.Van))
gt2 = ObjectTarget3D([-1, 1, 0], r, d, ObjectTag(KittiObjectClass.Car))
gt3 = ObjectTarget3D([1, -1, 0], r, d, ObjectTag(KittiObjectClass.Van))
gt_boxes = Target3DArray([gt1, gt2, gt3], frame="test")
self.matcher_case1 = (dt_boxes, gt_boxes)
def visualize_detections(ax: axes.Axes,
image_frame: str,
targets: Target3DArray,
calib: TransformSet,
box_color=(0, 1, 0),
thickness=2,
tags=None):
'''
Draw detected object on matplotlib canvas
'''
for target in targets.filter_tag(tags):
# add points for direction indicator
points = target.corners
indicator = np.array(
[[0, 0, -target.dimension[2] / 2],
[target.dimension[0] / 2, 0,
-target.dimension[2] / 2]]).dot(target.orientation.as_matrix().T)
points = np.vstack([points, target.position + indicator])
# project points
uv, mask, dmask = calib.project_points_to_camera(
points,
frame_to=image_frame,
frame_from=targets.frame,
remove_outlier=False,
return_dmask=True)
if len(uv[mask]) < 1:
continue # ignore boxes that is outside the image
uv = uv.astype(int)
# draw box
pairs = [(0, 1), (2, 3), (4, 5), (6, 7), (0, 4), (1, 5), (2, 6),
(3, 7), (0, 2), (1, 3), (4, 6), (5, 7)]
inlier = [i in mask for i in range(len(uv))]
for i, j in pairs:
if not inlier[i] and not inlier[j]:
continue
if i not in dmask or j not in dmask:
continue # only calculate for points ahead
ax.add_line(
lines.Line2D((uv[i, 0], uv[j, 0]), (uv[i, 1], uv[j, 1]),
c=box_color,
lw=thickness))
# draw direction
ax.add_line(
lines.Line2D((uv[-2, 0], uv[-1, 0]), (uv[-2, 1], uv[-1, 1]),
c=box_color,
lw=thickness))
def _current_objects_array(self) -> Target3DArray:
'''
Create Target3DArray from current tracked objects
'''
array = Target3DArray(frame=self._last_frameid,
timestamp=self._last_timestamp)
for tid in self.tracked_ids:
target = ObjectTarget3D(
position=self._tracked_poses[tid].position,
orientation=self._tracked_poses[tid].orientation,
dimension=self._tracked_features[tid].dimension,
tag=self._tracked_features[tid].classification,
tid=tid,
position_var=self._tracked_poses[tid].position_var,
orientation_var=self._tracked_poses[tid].orientation_var,
dimension_var=self._tracked_features[tid].dimension_var)
array.append(target)
return array
def annotation_3dobject(
self,
idx,
raw=False,
visible_range=80
): # TODO: it seems that dynamic objects need interpolation
'''
:param visible_range: range for visible objects. Objects beyond that distance will be removed when reporting
'''
assert not self._return_file_path, "The annotation is not in a single file!"
seq_id, frame_idx = idx
self._preload_3dobjects(seq_id)
objects = [
self._3dobjects_cache[seq_id][i.data]
for i in self._3dobjects_mapping[seq_id][frame_idx]
]
if raw:
return objects
self._preload_poses(seq_id)
pr, pt = self._poses_r[seq_id][frame_idx], self._poses_t[seq_id][
frame_idx]
boxes = Target3DArray(frame="pose")
for box in objects:
RS, T = box.transform[:3, :3], box.transform[:3, 3]
S = np.linalg.norm(RS, axis=0) # scale
R = Rotation.from_matrix(RS / S) # rotation
R = pr.inv() * R # relative rotation
T = pr.inv().as_matrix().dot(T - pt) # relative translation
# skip static objects beyond vision
if np.linalg.norm(T) > visible_range:
continue
global_id = box.semanticID * 1000 + box.instanceID
tag = ObjectTag(kittiId2label[box.semanticId].name, Kitti360Class)
boxes.append(ObjectTarget3D(T, R, S, tag, tid=global_id))
return boxes
def test_scenarios(self):
eval_classes = [KittiObjectClass.Car, KittiObjectClass.Van]
evaluator = TrackingEvaluator(eval_classes, [0.5, 1])
# scenario: X-crossing switch
r = Rotation.from_euler("Z", 0)
d = [1, 1, 1]
t = ObjectTag(KittiObjectClass.Car, scores=0.8)
v = [0, 0, 0]
tid = 1
traj1 = [
TrackingTarget3D([-2, 2, 0], r, d, v, v, t, tid=tid),
TrackingTarget3D([-1, 1, 0], r, d, v, v, t, tid=tid),
TrackingTarget3D([0, 0, 0], r, d, v, v, t, tid=tid),
TrackingTarget3D([1, 1, 0], r, d, v, v, t, tid=tid),
TrackingTarget3D([2, 2, 0], r, d, v, v, t, tid=tid),
]
t = ObjectTag(KittiObjectClass.Car, scores=0.9)
tid = 2
traj2 = [
TrackingTarget3D([-2, -2, 0], r, d, v, v, t, tid=tid),
TrackingTarget3D([-1, -1, 0], r, d, v, v, t, tid=tid),
TrackingTarget3D([0, 0, 0], r, d, v, v, t, tid=tid),
TrackingTarget3D([1, -1, 0], r, d, v, v, t, tid=tid),
TrackingTarget3D([2, -2, 0], r, d, v, v, t, tid=tid),
]
dt_trajs = [
Target3DArray([t1, t2], frame="test")
for t1, t2 in zip(traj1, traj2)
]
r = Rotation.from_euler("Z", 0.01)
d = [1.1, 1.1, 1.1]
t = ObjectTag(KittiObjectClass.Car)
tid = 1001
gt1 = [
ObjectTarget3D([-2.1, 2.1, 0], r, d, t, tid=tid),
ObjectTarget3D([-1.1, 0.9, 0], r, d, t, tid=tid),
ObjectTarget3D([-0.1, 0.1, 0], r, d, t, tid=tid),
ObjectTarget3D([0.9, -1.1, 0], r, d, t, tid=tid),
ObjectTarget3D([1.9, -1.9, 0], r, d, t, tid=tid),
]
tid = 1002
gt2 = [
ObjectTarget3D([-2.1, -2.1, 0], r, d, t, tid=tid),
ObjectTarget3D([-1.1, -0.9, 0], r, d, t, tid=tid),
ObjectTarget3D([-0.1, 0.1, 0], r, d, t, tid=tid),
ObjectTarget3D([0.9, 1.1, 0], r, d, t, tid=tid),
ObjectTarget3D([1.9, 1.9, 0], r, d, t, tid=tid),
]
gt_trajs = [
Target3DArray([t1, t2], frame="test") for t1, t2 in zip(gt1, gt2)
]
for dt_array, gt_array in zip(dt_trajs, gt_trajs):
stats = evaluator.calc_stats(gt_array, dt_array)
evaluator.add_stats(stats)
assert evaluator.tp()[KittiObjectClass.Car] == 10
assert evaluator.fp()[KittiObjectClass.Car] == 0
assert evaluator.fn()[KittiObjectClass.Car] == 0
assert evaluator.fn()[KittiObjectClass.Car] == 0
assert evaluator.id_switches()[KittiObjectClass.Car] == 2
assert evaluator.fragments()[KittiObjectClass.Car] == 2
# scenario: X-crossing with 3 tracklets
evaluator.reset()
r = Rotation.from_euler("Z", 0)
d = [1, 1, 1]
t = ObjectTag(KittiObjectClass.Car, scores=0.8)
v = [0, 0, 0]
tid = 1
traj1 = [
TrackingTarget3D([-2, 2, 0], r, d, v, v, t, tid=tid),
TrackingTarget3D([-1, 1, 0], r, d, v, v, t, tid=tid),
TrackingTarget3D([0, 0, 0], r, d, v, v, t, tid=tid),
TrackingTarget3D([1, 1, 0], r, d, v, v, t, tid=tid),
TrackingTarget3D([2, 2, 0], r, d, v, v, t, tid=tid),
]
t = ObjectTag(KittiObjectClass.Car, scores=0.9)
tid = 2
traj2 = [
TrackingTarget3D([-2, -2, 0], r, d, v, v, t, tid=tid),
TrackingTarget3D([-1, -1, 0], r, d, v, v, t, tid=tid),
TrackingTarget3D([0, 0, 0], r, d, v, v, t, tid=tid)
]
tid = 3
traj3 = [
TrackingTarget3D([1, -1, 0], r, d, v, v, t, tid=tid),
TrackingTarget3D([2, -2, 0], r, d, v, v, t, tid=tid)
]
dt_trajs = [
Target3DArray([t2, t1], frame="test")
for t1, t2 in zip(traj1[:3], traj2)
]
dt_trajs += [
Target3DArray([t3, t1], frame="test")
for t1, t3 in zip(traj1[3:], traj3)
]
for dt_array, gt_array in zip(dt_trajs, gt_trajs):
stats = evaluator.calc_stats(gt_array, dt_array)
evaluator.add_stats(stats)
assert evaluator.tp()[KittiObjectClass.Car] == 10
assert evaluator.fp()[KittiObjectClass.Car] == 0
assert evaluator.fn()[KittiObjectClass.Car] == 0
assert evaluator.fn()[KittiObjectClass.Car] == 0
assert evaluator.id_switches()[KittiObjectClass.Car] == 2
assert evaluator.fragments()[KittiObjectClass.Car] == 1
assert evaluator.tracked_ratio()[KittiObjectClass.Car] == 1.
assert evaluator.lost_ratio()[KittiObjectClass.Car] == 0.
def test_calc_stats(self):
eval_classes = [KittiObjectClass.Car, KittiObjectClass.Van]
evaluator = DetectionEvaluator(eval_classes, [0.1, 0.2])
r = Rotation.from_euler("Z", 0)
d = [2, 2, 2]
dt1 = ObjectTarget3D([0, 0, 0], r, d,
ObjectTag(KittiObjectClass.Car, scores=0.8))
dt2 = ObjectTarget3D([1, 1, 1], r, d,
ObjectTag(KittiObjectClass.Van, scores=0.7))
dt3 = ObjectTarget3D([-1, -1, -1], r, d,
ObjectTag(KittiObjectClass.Pedestrian,
scores=0.8))
dt_boxes = Target3DArray([dt1, dt2, dt3], frame="test")
# test match with self
result = evaluator.calc_stats(dt_boxes, dt_boxes)
for clsobj in eval_classes:
clsid = clsobj.value
assert result.ngt[clsid] == 1
assert result.ndt[clsid][0] == 1 and result.ndt[clsid][-1] == 0
assert result.tp[clsid][0] == 1 and result.tp[clsid][-1] == 0
assert result.fp[clsid][0] == 0 and result.fp[clsid][-1] == 0
assert result.fn[clsid][0] == 0 and result.fn[clsid][-1] == 1
assert np.isclose(result.acc_iou[clsid][0], 1) and np.isnan(
result.acc_iou[clsid][-1])
assert np.isclose(result.acc_angular[clsid][0], 0) and np.isnan(
result.acc_angular[clsid][-1])
assert np.isclose(result.acc_dist[clsid][0], 0) and np.isnan(
result.acc_dist[clsid][-1])
assert np.isclose(result.acc_box[clsid][0], 0) and np.isnan(
result.acc_box[clsid][-1])
assert np.isinf(result.acc_var[clsid][0]) and np.isnan(
result.acc_var[clsid][-1])
# test match with other boxes
r = Rotation.from_euler("Z", 0.01)
d = [2.1, 2.1, 2.1]
gt1 = ObjectTarget3D([0, 0, 0], r, d, ObjectTag(KittiObjectClass.Van))
gt2 = ObjectTarget3D([-1, 1, 0], r, d, ObjectTag(KittiObjectClass.Car))
gt3 = ObjectTarget3D([1, -1, 0], r, d,
ObjectTag(KittiObjectClass.Pedestrian))
gt_boxes = Target3DArray([gt1, gt2, gt3], frame="test")
result = evaluator.calc_stats(gt_boxes, dt_boxes)
for clsobj in eval_classes:
clsid = clsobj.value
assert result.ngt[clsid] == 1
assert result.ndt[clsid][0] == 1 and result.ndt[clsid][-1] == 0
if clsobj == KittiObjectClass.Car:
assert result.tp[clsid][0] == 1 and result.tp[clsid][-1] == 0
assert result.fp[clsid][0] == 0 and result.fp[clsid][-1] == 0
assert result.fn[clsid][0] == 0 and result.fn[clsid][-1] == 1
assert result.acc_iou[clsid][0] > 0.1 and np.isnan(
result.acc_iou[clsid][-1])
assert result.acc_angular[clsid][0] > 0 and np.isnan(
result.acc_angular[clsid][-1])
assert result.acc_dist[clsid][0] > 1 and np.isnan(
result.acc_dist[clsid][-1])
assert result.acc_box[clsid][0] > 0 and np.isnan(
result.acc_box[clsid][-1])
assert np.isinf(result.acc_var[clsid][0]) and np.isnan(
result.acc_var[clsid][-1])
else:
assert result.tp[clsid][0] == 0 and result.tp[clsid][-1] == 0
assert result.fp[clsid][0] == 1 and result.fp[clsid][-1] == 0
assert result.fn[clsid][0] == 1 and result.fn[clsid][-1] == 1
assert np.isnan(result.acc_iou[clsid][0]) and np.isnan(
result.acc_iou[clsid][-1])
assert np.isnan(result.acc_angular[clsid][0]) and np.isnan(
result.acc_angular[clsid][-1])
assert np.isnan(result.acc_dist[clsid][0]) and np.isnan(
result.acc_dist[clsid][-1])
assert np.isnan(result.acc_box[clsid][0]) and np.isnan(
result.acc_box[clsid][-1])
assert np.isnan(result.acc_var[clsid][0]) and np.isnan(
result.acc_var[clsid][-1])
def test_dump_and_load(self):
obj_arr = Target3DArray(frame="someframe", timestamp=1.2345)
track_arr = Target3DArray(frame="fixed", timestamp=0.1234)
# add targets
for i in range(10):
position = np.array([i] * 3)
position_var = np.diag(position)
dimension = np.array([i] * 3)
dimension_var = np.diag(position)
orientation = Rotation.from_euler("Z", i)
tid = "test%d" % i
tag = ObjectTag(KittiObjectClass.Car, KittiObjectClass, 0.9)
obj = ObjectTarget3D(position,
orientation,
dimension,
tag,
tid,
position_var=position_var,
dimension_var=dimension_var)
obj_arr.append(obj)
velocity = np.random.rand(3)
velocity_var = np.random.rand(3, 3)
avel = np.random.rand(3)
avel_var = np.random.rand(3, 3)
history = i * 0.1
track = TrackingTarget3D(position,
orientation,
dimension,
velocity,
avel,
tag,
tid=tid,
position_var=position_var,
dimension_var=dimension_var,
velocity_var=velocity_var,
angular_velocity_var=avel_var,
history=history)
track_arr.append(track)
data = msgpack.packb(obj_arr.serialize(), use_single_float=True)
obj_arr_copy = Target3DArray.deserialize(msgpack.unpackb(data))
assert len(obj_arr_copy) == len(obj_arr)
assert obj_arr_copy.frame == obj_arr.frame
assert obj_arr_copy.timestamp == obj_arr.timestamp
for i in range(10):
assert np.allclose(obj_arr_copy[i].position, obj_arr[i].position)
assert np.allclose(obj_arr_copy[i].position_var,
obj_arr[i].position_var)
assert np.allclose(obj_arr_copy[i].dimension, obj_arr[i].dimension)
assert np.allclose(obj_arr_copy[i].dimension_var,
obj_arr[i].dimension_var)
assert np.allclose(obj_arr_copy[i].orientation.as_quat(),
obj_arr[i].orientation.as_quat())
assert obj_arr_copy[i].tid == obj_arr[i].tid
assert obj_arr_copy[i].tag.mapping == obj_arr[i].tag.mapping
assert obj_arr_copy[i].tag.labels == obj_arr[i].tag.labels
assert np.allclose(obj_arr_copy[i].tag.scores,
obj_arr[i].tag.scores)
data = msgpack.packb(track_arr.serialize(), use_single_float=True)
track_arr_copy = Target3DArray.deserialize(msgpack.unpackb(data))
assert len(track_arr_copy) == len(track_arr)
assert track_arr_copy.frame == track_arr.frame
assert track_arr_copy.timestamp == track_arr.timestamp
for i in range(10):
assert np.allclose(track_arr_copy[i].position,
track_arr[i].position)
assert np.allclose(track_arr_copy[i].position_var,
track_arr[i].position_var)
assert np.allclose(track_arr_copy[i].dimension,
track_arr[i].dimension)
assert np.allclose(track_arr_copy[i].dimension_var,
track_arr[i].dimension_var)
assert np.allclose(track_arr_copy[i].orientation.as_quat(),
track_arr[i].orientation.as_quat())
assert np.allclose(track_arr_copy[i].velocity,
track_arr[i].velocity)
assert np.allclose(track_arr_copy[i].velocity_var,
track_arr[i].velocity_var)
assert np.allclose(track_arr_copy[i].angular_velocity,
track_arr[i].angular_velocity)
assert np.allclose(track_arr_copy[i].angular_velocity_var,
track_arr[i].angular_velocity_var)
assert track_arr_copy[i].tid == track_arr[i].tid
assert track_arr_copy[i].history == track_arr[i].history
assert track_arr_copy[i].tag.mapping == track_arr[i].tag.mapping
assert track_arr_copy[i].tag.labels == track_arr[i].tag.labels
assert np.allclose(track_arr_copy[i].tag.scores,
track_arr[i].tag.scores)
def visualize_detections(visualizer: Visualizer,
visualizer_frame: str,
targets: Target3DArray,
calib: TransformSet,
text_scale=0.8,
box_color=(1, 1, 1),
text_color=(1, 0.8, 1),
id_prefix="",
tags=None,
text_offset=None,
viewport=0):
'''
Visualize detection targets in PCL Visualizer.
:param visualizer: The pcl.Visualizer instance used for visualization
:param visualizer_frame: The frame that visualizer is in
:param targets: Object array to be visualized
:param calib: TransformSet object storing calibration information. This is mandatory if the
targets are in different frames
:param text_scale: The scale for text tags. Set to 0 or negative if you want to suppress text
visualization
:param box_color: Specifying the color of bounding boxes.
If it's a tuple, then it's assumed that it contains three RGB values in range 0-1.
If it's a str or matplotlib colormap object, then the color comes from applying colormap to the object id.
:param text_color: Specifying the color of text tags.
:param id_prefix: Prefix of actor ids in PCL Visualizer, essential when this function is called multiple times
:param text_offset: Relative position of text tags with regard to the box center
:param viewport: Viewport for objects to be added. This is a PCL related feature
'''
if not _pcl_available:
raise RuntimeError(
"pcl is not available, please check the installation of package pcl.py"
)
if id_prefix != "" and not id_prefix.endswith("/"):
id_prefix = id_prefix + "/"
# change frame to the same
if targets.frame != visualizer_frame:
targets = calib.transform_objects(targets, frame_to=visualizer_frame)
# convert colormaps
if isinstance(box_color, str):
box_color = cm.get_cmap(box_color)
if isinstance(text_color, str):
text_color = cm.get_cmap(text_color)
for i, target in enumerate(targets.filter_tag(tags)):
tid = target.tid or i
# convert coordinate
orientation = target.orientation.as_quat()
orientation = [orientation[3]
] + orientation[:3].tolist() # To PCL quaternion
lx, ly, lz = target.dimension
cube_id = (id_prefix + "target%d") % i
color = box_color(tid % 256) if isinstance(box_color,
Colormap) else box_color
alpha = color[3] if len(color) > 3 else 0.8
visualizer.addCube(target.position,
orientation,
lx,
ly,
lz,
id=cube_id,
viewport=viewport)
visualizer.setShapeRenderingProperties(pv.RenderingProperties.Opacity,
alpha,
id=cube_id)
visualizer.setShapeRenderingProperties(pv.RenderingProperties.Color,
color[:3],
id=cube_id)
# draw tag
if text_scale >= 0:
text_id = (id_prefix + "target%d/tag") % i
if target.tid:
disp_text = "%s: %s" % (target.tid64, target.tag_top.name)
else:
disp_text = "#%d: %s" % (i, target.tag_top.name)
aux_text = []
if target.tag_top_score < 1:
aux_text.append("%.2f" % target.tag_top_score)
position_var = np.power(np.linalg.det(target.position_var),
1 / 6) # display standard deviation
if position_var > 0:
aux_text.append("%.2f" % position_var)
dimension_var = np.power(np.linalg.det(target.dimension_var),
1 / 6)
if dimension_var > 0:
aux_text.append("%.2f" % dimension_var)
if target.orientation_var > 0:
aux_text.append("%.2f" % target.orientation_var)
if len(aux_text) > 0:
disp_text += " (" + ", ".join(aux_text) + ")"
disp_pos = np.copy(target.position)
disp_pos[2] += lz / 2 # lift the text out of box
if text_offset is not None: # apply offset
disp_pos += text_offset
color = text_color(tid % 256) if isinstance(
text_color, Colormap) else text_color
visualizer.addText3D(disp_text,
list(disp_pos),
text_scale=text_scale,
color=text_color[:3],
id=text_id,
viewport=viewport)
# draw orientation
arrow_id = (id_prefix + "target%d/direction") % i
dir_x, dir_y, dir_z = np.hsplit(target.orientation.as_matrix(), 3)
off_x, off_y, off_z = dir_x * lx / 2, dir_y * ly / 2, dir_z * lz / 2
off_x, off_y, off_z = off_x.flatten(), off_y.flatten(), off_z.flatten()
pos_bottom = target.position - off_z
visualizer.addLine(pos_bottom - off_y - off_x,
pos_bottom + off_x,
id=arrow_id + "_1",
viewport=viewport)
visualizer.addLine(pos_bottom + off_y - off_x,
pos_bottom + off_x,
id=arrow_id + "_2",
viewport=viewport)
# draw velocity
if isinstance(target, TrackingTarget3D):
arrow_id = (id_prefix + "target%d/velocity") % i
pstart = target.position
pend = target.position + target.velocity
visualizer.addLine(pstart,
pend,
color=(0.5, 0.5, 1),
id=arrow_id,
viewport=viewport)
def annotation_3dobject(self,
idx,
raw=False,
convert_tag=True,
with_velocity=True):
seq_id, frame_idx = idx
fname = "annotation/%03d.json" % frame_idx
if self._return_file_path:
return self.base_path / seq_id / fname
if self.inzip:
with PatchedZipFile(self.base_path / f"{seq_id}.zip",
to_extract=fname) as ar:
labels = json.loads(ar.read(fname))
else:
with Path(self.base_path, seq_id, fname).open() as fin:
labels = json.load(fin)
labels = list(map(edict, labels))
if raw:
return labels
# parse annotations
ego_pose = self.pose(idx, bypass=True)
ego_r, ego_t = ego_pose.orientation, ego_pose.position
ego_t = ego_r.as_matrix().dot(
ego_t) # convert to original representation
ego_ri = ego_r.inv()
ego_rim = ego_ri.as_matrix()
outputs = Target3DArray(frame="ego")
for label in labels:
# convert tags
tag = NuscenesObjectClass.parse(label.category)
for attr in label.attribute:
tag = tag | NuscenesObjectClass.parse(attr)
if convert_tag:
tag = ObjectTag(tag.to_detection(), NuscenesDetectionClass)
else:
tag = ObjectTag(tag, NuscenesObjectClass)
aux = dict(num_lidar_pts=label['num_lidar_pts'],
num_radar_pts=label['num_radar_pts'])
# caculate relative pose
r = Rotation.from_quat(label.rotation[1:] + [label.rotation[0]])
t = label.translation
rel_r = ego_ri * r
rel_t = np.dot(ego_rim, t - ego_t)
size = [label.size[1], label.size[0], label.size[2]] # wlh -> lwh
tid = int(label.instance[:8], 16) # truncate into uint64
# create object
if with_velocity:
v = np.dot(ego_rim, label.velocity)
w = label.angular_velocity
target = TrackingTarget3D(rel_t,
rel_r,
size,
v,
w,
tag,
tid=tid,
aux=aux)
outputs.append(target)
else:
target = ObjectTarget3D(rel_t,
rel_r,
size,
tag,
tid=tid,
aux=aux)
outputs.append(target)
return outputs