diff --git a/openpilot/starpilot/speed_limits/vision/speed_face.py b/openpilot/starpilot/speed_limits/vision/speed_face.py index a805cabedf..ac4cd8195e 100644 --- a/openpilot/starpilot/speed_limits/vision/speed_face.py +++ b/openpilot/starpilot/speed_limits/vision/speed_face.py @@ -57,6 +57,40 @@ def crop(np, cv2, rgb256, box): return np.ascontiguousarray(np.clip(result, 0, 1).transpose(2, 0, 1)) +def warning_context_box(rgb256, box): + from openpilot.starpilot.speed_limits.vision import model + + image = np.rint(rgb256.transpose(1, 2, 0) * np.float32(255)).astype(np.uint8) + hsv = cv2.cvtColor(image, cv2.COLOR_RGB2HSV) + hue, saturation, value = hsv[:, :, 0], hsv[:, :, 1], hsv[:, :, 2] + mask = ((hue >= model.REGULATORY_YELLOW_HUE_MIN) & (hue <= model.REGULATORY_YELLOW_HUE_MAX) & + (saturation >= model.REGULATORY_YELLOW_SAT_MIN) & (value >= model.REGULATORY_YELLOW_VALUE_MIN)).astype(np.uint8) + contours, _ = cv2.findContours(mask, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE) + center = (float((box[0] + box[2]) / 2), float((box[1] + box[3]) / 2)) + matches = [] + for contour in contours: + hull = cv2.convexHull(contour) + if (hull <= 0).any() or (hull >= 255).any(): + continue + quad = cv2.approxPolyDP(hull, .02 * cv2.arcLength(hull, True), True) + if len(quad) != 4 or not cv2.isContourConvex(quad): + continue + points = quad.reshape(4, 2) + top, right = points[np.argmin(points[:, 1])], points[np.argmax(points[:, 0])] + bottom, left = points[np.argmax(points[:, 1])], points[np.argmin(points[:, 0])] + if (len({tuple(p) for p in (top, right, bottom, left)}) != 4 or + not (top[1] < min(left[1], right[1]) and bottom[1] > max(left[1], right[1]) and + left[0] < min(top[0], bottom[0]) and right[0] > max(top[0], bottom[0]))): + continue + x, y, width, height = cv2.boundingRect(hull) + if (cv2.pointPolygonTest(quad, center, False) >= 0 and + x < box[0] and y < box[1] and x + width > box[2] and y + height > box[3]): + outline = cv2.approxPolyDP(contour, .02 * cv2.arcLength(contour, True), True) + matches.append(np.asarray([x, y, x + width, y + height], np.float32) + if len(outline) == 4 and cv2.isContourConvex(outline) else None) + return matches[0] if len(matches) == 1 else None + + class SpeedFaceVerifier: def __init__(self, path): self.net = None @@ -123,6 +157,9 @@ class SpeedFaceVerifier: patch = crop(np, cv2, blob, box) if patch is None: return False + context_box = warning_context_box(blob, box) + if context_box is not None: + patch = crop(np, cv2, blob, context_box) started = time.monotonic() self.last_forward_count += 1 try: diff --git a/openpilot/starpilot/speed_limits/vision/tests/test_speed_warning_context.py b/openpilot/starpilot/speed_limits/vision/tests/test_speed_warning_context.py new file mode 100644 index 0000000000..f499d0d3c9 --- /dev/null +++ b/openpilot/starpilot/speed_limits/vision/tests/test_speed_warning_context.py @@ -0,0 +1,112 @@ +"""Pure pixel geometry and single-call verifier behavior; no model loading.""" +import unittest +from unittest import mock + +import numpy as np + +from openpilot.starpilot.speed_limits.vision import speed_face + +cv2 = speed_face.cv2 + + +class TestSpeedWarningContext(unittest.TestCase): + def setUp(self): + if cv2 is None: + self.skipTest("OpenCV is unavailable") + + def image(self): + return np.zeros((256, 256, 3), np.uint8) + + def blob(self, image): + return np.ascontiguousarray(image.transpose(2, 0, 1), dtype=np.float32) / np.float32(255) + + def diamond(self, image, points=None): + points = points or [[128, 40], [216, 128], [128, 216], [40, 128]] + cv2.fillConvexPoly(image, np.asarray(points, np.int32), (255, 255, 0)) + return image + + def test_diamond_keeps_entire_original_box_support(self): + image = self.diamond(self.image()) + box = np.array([75, 115, 150, 190], np.float32) + context = speed_face.warning_context_box(self.blob(image), box) + np.testing.assert_array_equal(context, [40, 40, 217, 217]) + self.assertTrue(np.all(context[:2] < box[:2]) and np.all(context[2:] > box[2:])) + + def test_circle_ellipse_rectangle_and_school_header_fall_back(self): + circle, ellipse, rectangle, header = [self.image() for _ in range(4)] + cv2.circle(circle, (128, 128), 85, (255, 255, 0), -1) + cv2.ellipse(ellipse, (128, 128), (95, 60), 20, 0, 360, (255, 255, 0), -1) + cv2.rectangle(rectangle, (40, 40), (216, 216), (255, 255, 0), -1) + cv2.rectangle(header, (70, 30), (180, 65), (255, 255, 0), -1) + for image in [circle, ellipse, rectangle, header]: + with self.subTest(kind=id(image)): + self.assertIsNone(speed_face.warning_context_box(self.blob(image), [100, 90, 155, 180])) + + def test_border_outside_center_and_incomplete_containment_fall_back(self): + image = self.diamond(self.image()) + self.assertIsNone(speed_face.warning_context_box(self.blob(image), [45, 45, 65, 65])) + self.assertIsNone(speed_face.warning_context_box(self.blob(image), [20, 100, 180, 180])) + touching = self.diamond(self.image(), [[128, 0], [240, 128], [128, 240], [16, 128]]) + self.assertIsNone(speed_face.warning_context_box(self.blob(touching), [100, 100, 160, 160])) + + def test_multiple_associations_are_ambiguous(self): + image = self.diamond(self.image()) + contour = np.asarray([[128, 40], [216, 128], [128, 216], [40, 128]], np.int32)[:, None] + with mock.patch.object(speed_face.cv2, 'findContours', return_value=([contour, contour.copy()], None)): + self.assertIsNone(speed_face.warning_context_box(self.blob(image), [100, 100, 160, 160])) + + def test_verifier_keeps_dense_binding_and_one_speed_request(self): + blob = self.blob(self.diamond(self.image())) + verifier = object.__new__(speed_face.SpeedFaceVerifier) + verifier.net = mock.Mock() + verifier.net.forward.return_value = [np.array([[-8]], np.float32)] + verifier.begin_frame() + candidate = (.8, 0, (1, 2, 3, 4)) + box = np.array([100, 100, 160, 160], np.float32) + verifier._proposals[candidate] = [(blob, box, 77)] + self.assertTrue(verifier.reject_once(candidate)) + self.assertEqual(verifier.last_dense_id, 77) + self.assertEqual(verifier.last_forward_count, 1) + verifier.net.forward.assert_called_once_with(['speed_face_logit']) + self.assertFalse(verifier.reject_once(candidate)) + self.assertEqual(verifier.net.forward.call_count, 1) + + def test_original_unresolved_and_invalid_crop_gates_precede_context(self): + verifier = object.__new__(speed_face.SpeedFaceVerifier) + verifier.net = mock.Mock() + candidate = (.8, 0, (1, 2, 3, 4)) + for blob, box, invalid in [ + (self.blob(self.image()), [10, 10, 12, 12], False), + (np.full((3, 256, 256), np.nan, np.float32), [10, 10, 30, 30], True), + ]: + verifier.begin_frame() + verifier._proposals[candidate] = [(blob, np.asarray(box, np.float32), 77)] + with mock.patch.object(speed_face, 'warning_context_box') as context: + if invalid: + with self.assertRaises(ValueError): + verifier.reject_once(candidate) + else: + self.assertFalse(verifier.reject_once(candidate)) + context.assert_not_called() + verifier.net.forward.assert_not_called() + self.assertEqual(verifier.last_forward_count, 0) + + def test_concave_outline_cannot_borrow_its_convex_hull_shape(self): + contour = np.asarray([[128, 40], [216, 128], [145, 128], [128, 216], + [40, 128], [110, 128]], np.int32)[:, None] + hull = cv2.convexHull(contour) + quad = cv2.approxPolyDP(hull, .02 * cv2.arcLength(hull, True), True) + self.assertEqual(len(quad), 4) + self.assertTrue(cv2.isContourConvex(quad)) + self.assertFalse(cv2.isContourConvex(contour)) + with mock.patch.object(cv2, 'findContours', return_value=([contour], None)): + self.assertIsNone(speed_face.warning_context_box(self.blob(self.image()), [100, 100, 160, 160])) + + def test_invalid_outline_does_not_resolve_multiple_hull_associations(self): + valid = np.asarray([[128, 40], [216, 128], [128, 216], [40, 128]], np.int32)[:, None] + invalid = np.asarray([[128, 40], [216, 128], [145, 128], [128, 216], + [40, 128], [110, 128]], np.int32)[:, None] + for contours in ([invalid, valid], [valid, invalid]): + with self.subTest(invalid_first=contours[0] is invalid): + with mock.patch.object(cv2, 'findContours', return_value=(contours, None)): + self.assertIsNone(speed_face.warning_context_box(self.blob(self.image()), [100, 100, 160, 160])) diff --git a/prebuilt.json b/prebuilt.json index e6cd115899..3683b3bba7 100644 --- a/prebuilt.json +++ b/prebuilt.json @@ -1 +1 @@ -{"protected_inventory_sha256": "eaefaed6396e84e2a96158003a1f10e0b87b56f711676f4a86563ce20c1bc57e", "schema_version": 1} +{"protected_inventory_sha256": "14e194326bcf0efef228ba09eaa7695844b0917bf1af867aa5ba510889396221", "schema_version": 1}