def test_orientation_error8() -> None:
""" """
yaw1 = np.deg2rad([-179])
yaw2 = np.deg2rad([-177])
error_deg = np.rad2deg(wrap_angle(yaw1 - yaw2))
assert np.allclose(error_deg, 2.0, atol=1e-2)
def get_distance(x1: Dict[str, np.ndarray], x2: Dict[str, np.ndarray],
name: str) -> float:
"""Get the distance between two poses, returns nan if distance is larger than detection threshold.
Args:
x1: first pose
x2: second pose
name: name of the field to test
Returns:
A distance value or NaN
"""
if name == "centroid":
dist = float(np.linalg.norm(x1[name][0:3] - x2[name][0:3]))
return dist if dist < 2 else float(np.nan)
elif name == "iou":
return get_distance_iou_3d(x1, x2, name)
elif name == "orientation":
theta = np.array([x1["orientation"]] - x2["orientation"])
dist = wrap_angle(theta).item()
# Convert to degrees.
return float(np.rad2deg(dist))
else:
raise NotImplementedError("Not implemented..")
def test_wrap_angle() -> None:
theta: np.ndarray = np.array([-3 * np.pi / 2])
expected_result: float = np.array([np.pi / 2])
assert wrap_angle(theta) == expected_result
