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