def test_affinity_center() -> None:
"""Initialize a detection and a ground truth label. Verify that calculated distance matches expected affinity
under the specified `AffFnType`.
"""
dts: List[ObjectLabelRecord] = [
ObjectLabelRecord(
quaternion=np.array([1, 0, 0, 0]),
translation=np.array([0, 0, 0]),
length=5.0,
width=5.0,
height=5.0,
occlusion=0,
)
]
gts: List[ObjectLabelRecord] = [
ObjectLabelRecord(
quaternion=np.array([1, 0, 0, 0]),
translation=np.array([3, 4, 0]),
length=5.0,
width=5.0,
height=5.0,
occlusion=0,
)
]
expected_result: float = -5
assert compute_affinity_matrix(dts, gts,
AffFnType.CENTER) == expected_result
def test_rank() -> None:
"""Test ranking of detections and scores during detection evaluation."""
dts: np.ndarray = np.array([
ObjectLabelRecord(
quaternion=np.array([1, 0, 0, 0]),
translation=np.array([0, 0, 0]),
length=5.0,
width=5.0,
height=5.0,
occlusion=0,
score=0.7,
track_id="0",
),
ObjectLabelRecord(
quaternion=np.array([1, 0, 0, 0]),
translation=np.array([10, 10, 10]),
length=5.0,
width=5.0,
height=5.0,
occlusion=0,
score=0.9,
track_id="1",
),
ObjectLabelRecord(
quaternion=np.array([1, 0, 0, 0]),
translation=np.array([20, 20, 20]),
length=5.0,
width=5.0,
height=5.0,
occlusion=0,
score=0.8,
track_id="2",
),
])
ranked_dts, ranked_scores = rank(dts)
track_ids = np.array([dt.track_id for dt in ranked_dts.tolist()])
expected_track_ids = np.array(["1", "2", "0"])
expected_scores = np.array([0.9, 0.8, 0.7])
assert np.all(track_ids == expected_track_ids) and np.all(
ranked_scores == expected_scores)
def poly_to_label(poly: Polygon,
category: str = "VEHICLE",
track_id: str = "") -> ObjectLabelRecord:
# poly in polygon format
bbox = poly.minimum_rotated_rectangle
x = bbox.exterior.xy[0]
y = bbox.exterior.xy[1]
z = np.array([z for _, _, z in poly.exterior.coords])
# z = poly.exterior.xy[2]
d1 = dist((x[0], y[0]), (x[1], y[1]))
d2 = dist((x[1], y[1]), (x[2], y[2]))
width = min(d1, d2)
length = max(d1, d2)
if max(d1, d2) == d2:
unit_v = unit_vector((x[1], y[1]), (x[2], y[2]))
else:
unit_v = unit_vector((x[0], y[0]), (x[1], y[1]))
angle = math.atan2(unit_v[1], unit_v[0])
height = max(z) - min(z)
# translation = center
center = np.array(
[bbox.centroid.xy[0][0], bbox.centroid.xy[1][0],
min(z) + height / 2])
R = np.array([
[np.cos(angle), -np.sin(angle), 0],
[np.sin(angle), np.cos(angle), 0],
[0, 0, 1],
])
q = quaternion.from_rotation_matrix(R)
return ObjectLabelRecord(
quaternion=quaternion.as_float_array(q),
translation=center,
length=length,
width=width,
height=height,
occlusion=0,
label_class=category,
track_id=track_id,
)
def build_bbox(pose, width, length, height):
"""
Convert bounding box to label format
"""
R = np.array([
[np.cos(pose[3]), -np.sin(pose[3]), 0],
[np.sin(pose[3]), np.cos(pose[3]), 0],
[0, 0, 1],
])
q = Quaternion(matrix=R)
return ObjectLabelRecord(
quaternion=[q.scalar, q.vector[0], q.vector[1], q.vector[2]],
translation=pose[0:3].copy(),
length=length,
width=width,
height=height,
occlusion=0,
)
def test_filter_instances() -> None:
"""Generate 100 different detections and filter them based on Euclidean distance."""
dts: List[ObjectLabelRecord] = [
ObjectLabelRecord(
translation=[i, i, 0],
quaternion=np.array([0, 0, 0, 0]),
length=5.0,
width=2.0,
height=3.0,
occlusion=0,
label_class="VEHICLE",
)
for i in range(100)
]
target_class_name: str = "VEHICLE"
filter_metric: FilterMetric = FilterMetric.EUCLIDEAN
max_detection_range: float = 100.0
expected_result: int = 71
assert len(filter_instances(dts, target_class_name, filter_metric, max_detection_range)) == expected_result
def place_trajectory_in_city_frame(self, traj_label: TrajectoryLabel,
log_id: str) -> np.ndarray:
"""Place trajectory in the city frame
Args:
traj_label (TrajectoryLabel): instance of the TrajectoryLabel class.
log_id (str): Log id.
Returns:
- traj_city_fr: trajectory length of NUM_CUBOID_VERTS (x,y,z) coords per cuboid.
"""
seq_len = traj_label.timestamps.shape[0]
if self.bboxes_3d:
NUM_CUBOID_VERTS = 8
else:
NUM_CUBOID_VERTS = 4
# store NUM_CUBOID_VERTS (x,y,z) coords per cuboid
traj_city_fr = np.zeros((seq_len, NUM_CUBOID_VERTS, 3))
rand_color = (
float(np.random.rand()),
float(np.random.rand()),
float(np.random.rand()),
)
logger.info(f"On log {log_id} with {traj_label.track_uuid}")
for t in range(seq_len):
obj_label_rec = ObjectLabelRecord(
quaternion=traj_label.quaternions[t],
translation=traj_label.translations[t],
length=traj_label.max_length,
width=traj_label.max_width,
height=traj_label.max_height,
occlusion=traj_label.occlusion[t],
)
timestamp = int(traj_label.timestamps[t])
if self.bboxes_3d:
bbox_ego_frame = obj_label_rec.as_3d_bbox()
else:
bbox_ego_frame = obj_label_rec.as_2d_bbox()
bbox_city_fr, pose_city_to_ego = self.convert_bbox_to_city_frame(
timestamp, self.dataset_dir, log_id, bbox_ego_frame)
if bbox_city_fr is None:
logger.warning(
f"\t {log_id}: Couldnt find the pose for {traj_label.track_uuid}!"
)
continue
self.log_egopose_dict[log_id][timestamp] = pose_city_to_ego
frame_rec = FrameRecord(
bbox_city_fr=bbox_city_fr,
bbox_ego_frame=bbox_ego_frame,
occlusion_val=obj_label_rec.occlusion,
color=rand_color,
track_uuid=traj_label.track_uuid,
obj_class_str=traj_label.obj_class_str,
)
self.log_timestamp_dict[log_id].setdefault(timestamp,
[]).append(frame_rec)
traj_city_fr[t] = bbox_city_fr
return traj_city_fr
def test_assign() -> None:
"""Verify that the assign functions as expected by checking ATE of assigned detections against known distance."""
cfg = DetectionCfg()
dts: np.ndarray = np.array([
ObjectLabelRecord(
quaternion=np.array([1, 0, 0, 0]),
translation=np.array([0, 0, 0]),
length=5.0,
width=5.0,
height=5.0,
occlusion=0,
),
ObjectLabelRecord(
quaternion=np.array([1, 0, 0, 0]),
translation=np.array([10, 10, 10]),
length=5.0,
width=5.0,
height=5.0,
occlusion=0,
),
ObjectLabelRecord(
quaternion=np.array([1, 0, 0, 0]),
translation=np.array([20, 20, 20]),
length=5.0,
width=5.0,
height=5.0,
occlusion=0,
),
])
gts: np.ndarray = np.array([
ObjectLabelRecord(
quaternion=np.array([1, 0, 0, 0]),
translation=np.array([-10, -10, -10]),
length=5.0,
width=5.0,
height=5.0,
occlusion=0,
),
ObjectLabelRecord(
quaternion=np.array([1, 0, 0, 0]),
translation=np.array([0.1, 0, 0]), # off by 0.1
length=5.0,
width=5.0,
height=5.0,
occlusion=0,
),
ObjectLabelRecord(
quaternion=np.array([1, 0, 0, 0]),
translation=np.array([10.1, 10, 10]), # off by 0.1
length=5.0,
width=5.0,
height=5.0,
occlusion=0,
),
])
metrics = assign(dts, gts, cfg)
# if these assign correctly, we should get an ATE of 0.1 for the first two
expected_result: float = 0.1
ATE_COL_IDX = 4
assert np.isclose(metrics[0, ATE_COL_IDX], expected_result) # instance 0
assert np.isclose(metrics[1, ATE_COL_IDX], expected_result) # instance 1
assert np.isnan(metrics[2, ATE_COL_IDX]) # instance 32
def poly_to_label(poly: Polygon,
category: str = "VEHICLE",
track_id: str = "") -> ObjectLabelRecord:
"""Convert a Shapely Polygon to a 3d cuboid by estimating the minimum-bounding rectangle.
Args:
poly: Shapely polygon object representing a convex hull of an object
category: object category to which object belongs, e.g. VEHICLE, PEDESTRIAN, etc
track_id: unique identifier
Returns:
object representing a 3d cuboid
"""
bbox = poly.minimum_rotated_rectangle
x = bbox.exterior.xy[0]
y = bbox.exterior.xy[1]
z = np.array([z for _, _, z in poly.exterior.coords])
# z = poly.exterior.xy[2]
d1 = dist((x[0], y[0]), (x[1], y[1]))
d2 = dist((x[1], y[1]), (x[2], y[2]))
# assign orientation so that the rectangle's longest side represents the object's length
width = min(d1, d2)
length = max(d1, d2)
if max(d1, d2) == d2:
unit_v = unit_vector((x[1], y[1]), (x[2], y[2]))
else:
unit_v = unit_vector((x[0], y[0]), (x[1], y[1]))
angle_rad = np.arctan2(unit_v[1], unit_v[0])
q = yaw_to_quaternion3d(angle_rad)
height = max(z) - min(z)
# location of object in egovehicle coordinates
center = np.array(
[bbox.centroid.xy[0][0], bbox.centroid.xy[1][0],
min(z) + height / 2])
c = np.cos(angle_rad)
s = np.sin(angle_rad)
R = np.array([
[c, -s, 0],
[s, c, 0],
[0, 0, 1],
])
return ObjectLabelRecord(
quaternion=q,
translation=center,
length=length,
width=width,
height=height,
occlusion=0,
label_class=category,
track_id=track_id,
)