From 653009185e882357fa60d594532d47c787c6d15f Mon Sep 17 00:00:00 2001 From: firestar5683 <168790843+firestar5683@users.noreply.github.com> Date: Mon, 13 Jul 2026 14:00:50 -0500 Subject: [PATCH] elantra and sped --- .../opendbc/car/hyundai/fingerprints.py | 22 +- .../opendbc/car/hyundai/hyundaican.py | 3 +- .../opendbc/car/hyundai/tests/test_hyundai.py | 15 + opendbc_repo/opendbc/car/hyundai/values.py | 17 +- opendbc_repo/opendbc/car/tests/routes.py | 3 + .../opendbc/car/torque_data/substitute.toml | 3 +- .../opendbc/dbc/hyundai_kia_generic.dbc | 2 +- opendbc_repo/opendbc/safety/modes/hyundai.h | 2 +- .../evaluate_reviewed_route_events.py | 70 ++++- .../replay_route_runtime.py | 61 +++- starpilot/system/speed_limit_vision.py | 274 +++++++++++++++++- .../system/tests/test_speed_limit_vision.py | 50 +++- 12 files changed, 491 insertions(+), 31 deletions(-) diff --git a/opendbc_repo/opendbc/car/hyundai/fingerprints.py b/opendbc_repo/opendbc/car/hyundai/fingerprints.py index 6493dc2edd..bd464b07a2 100644 --- a/opendbc_repo/opendbc/car/hyundai/fingerprints.py +++ b/opendbc_repo/opendbc/car/hyundai/fingerprints.py @@ -999,6 +999,21 @@ FW_VERSIONS = { b'\xf1\x00CN ESC \t 105 \x10\x03 58910-AA800', ], }, + CAR.HYUNDAI_ELANTRA_2024: { + (Ecu.fwdRadar, 0x7d0, None): [ + b'\xf1\x00CN7_ RDR ----- 1.00 1.01 99110-AA500 ', + ], + (Ecu.eps, 0x7d4, None): [ + b'\xf1\x00CN7 MDPS C 1.00 1.02 56300AA670\x00 4CSDC102', + ], + (Ecu.fwdCamera, 0x7c4, None): [ + b'\xf1\x00CN7 MFC AT USA LHD 1.00 1.02 99210-AA500 230420', + b'\xf1\x00CN7 MFC AT USA LHD 1.00 1.03 99210-AA500 230918', + ], + (Ecu.abs, 0x7d1, None): [ + b'\xf1\x00CN ESC \t 104#\x07\x03 58910-AA850', + ], + }, CAR.HYUNDAI_ELANTRA_HEV_2021: { (Ecu.fwdCamera, 0x7c4, None): [ b'\xf1\x00CN7HMFC AT USA LHD 1.00 1.03 99210-AA000 200819', @@ -1018,14 +1033,19 @@ FW_VERSIONS = { b'\xf1\x00CN7 MDPS C 1.00 1.04 56310BY050\x00 4CNHC104', ], }, - CAR.HYUNDAI_ELANTRA_HEV_2026: { + CAR.HYUNDAI_ELANTRA_HEV_2024: { (Ecu.fwdCamera, 0x7c4, None): [ + b'\xf1\x00CN7HMFC AT AUS RHD 1.00 1.02 99210-AA500 230420', + b'\xf1\x00CN7HMFC AT CAN LHD 1.00 1.05 99210-AA510 240509', + b'\xf1\x00CN7HMFC AT USA LHD 1.00 1.03 99210-AA500 230918', b'\xf1\x00CN7HMFC AT USA LHD 1.00 1.05 99210-AA510 240509', ], (Ecu.fwdRadar, 0x7d0, None): [ b'\xf1\x00CN7_ RDR ----- 1.00 1.01 99110-AA500 ', ], (Ecu.eps, 0x7d4, None): [ + b'\xf1\x00CN7 MDPS C 1.00 1.00 56300BY670\x00 4CSHC100', + b'\xf1\x00CN7 MDPS C 1.00 1.00 56300BY680\x00 4CSHC100', b'\xf1\x00CN7 MDPS C 1.00 1.03 56300BY670\x00 4CSHC103', ], }, diff --git a/opendbc_repo/opendbc/car/hyundai/hyundaican.py b/opendbc_repo/opendbc/car/hyundai/hyundaican.py index 2c24a6cf91..76e0fae258 100644 --- a/opendbc_repo/opendbc/car/hyundai/hyundaican.py +++ b/opendbc_repo/opendbc/car/hyundai/hyundaican.py @@ -40,7 +40,8 @@ def create_lkas11(packer, frame, CP, apply_torque, steer_req, CAR.HYUNDAI_ELANTRA_HEV_2021, CAR.HYUNDAI_SONATA_HYBRID, CAR.HYUNDAI_KONA_EV, CAR.HYUNDAI_KONA_HEV, CAR.HYUNDAI_KONA_EV_2022, CAR.HYUNDAI_SANTA_FE_2022, CAR.KIA_K5_2021, CAR.HYUNDAI_IONIQ_HEV_2022, CAR.HYUNDAI_SANTA_FE_HEV_2022, CAR.HYUNDAI_SANTA_FE_PHEV_2022, CAR.KIA_STINGER_2022, CAR.KIA_K5_HEV_2020, CAR.KIA_CEED, CAR.KIA_XCEED_PHEV, - CAR.HYUNDAI_AZERA_6TH_GEN, CAR.HYUNDAI_AZERA_HEV_6TH_GEN, CAR.HYUNDAI_CUSTIN_1ST_GEN, CAR.HYUNDAI_KONA_2022): + CAR.HYUNDAI_AZERA_6TH_GEN, CAR.HYUNDAI_AZERA_HEV_6TH_GEN, CAR.HYUNDAI_CUSTIN_1ST_GEN, CAR.HYUNDAI_KONA_2022, + CAR.HYUNDAI_ELANTRA_2024, CAR.HYUNDAI_ELANTRA_HEV_2024): values["CF_Lkas_LdwsActivemode"] = int(left_lane) + (int(right_lane) << 1) values["CF_Lkas_LdwsOpt_USM"] = 2 diff --git a/opendbc_repo/opendbc/car/hyundai/tests/test_hyundai.py b/opendbc_repo/opendbc/car/hyundai/tests/test_hyundai.py index 3a3e9ab23f..ca3f6ac895 100644 --- a/opendbc_repo/opendbc/car/hyundai/tests/test_hyundai.py +++ b/opendbc_repo/opendbc/car/hyundai/tests/test_hyundai.py @@ -686,6 +686,21 @@ class TestHyundaiFingerprint: def test_kona_ev_non_scc_has_no_dedicated_fw_coverage(self): assert CAR.HYUNDAI_KONA_EV_NON_SCC not in FW_VERSIONS + def test_elantra_hev_2026_route_fw_exact_matches_2024_platform(self): + route_fw = { + (Ecu.fwdCamera, 0x7c4): b'\xf1\x00CN7HMFC AT USA LHD 1.00 1.05 99210-AA510 240509', + (Ecu.fwdRadar, 0x7d0): b'\xf1\x00CN7_ RDR ----- 1.00 1.01 99110-AA500 ', + (Ecu.eps, 0x7d4): b'\xf1\x00CN7 MDPS C 1.00 1.03 56300BY670\x00 4CSHC103', + } + car_fw = [ + CarParams.CarFw(ecu=ecu, fwVersion=version, address=address, subAddress=0, brand="hyundai") + for (ecu, address), version in route_fw.items() + ] + + exact, matches = match_fw_to_car(car_fw, "", allow_exact=True, allow_fuzzy=False, log=False) + assert exact + assert matches == {CAR.HYUNDAI_ELANTRA_HEV_2024} + def test_kona_non_scc_fca_radar_fw_is_optional(self): fw_versions = FW_VERSIONS[CAR.HYUNDAI_KONA_NON_SCC] car_fw = [ diff --git a/opendbc_repo/opendbc/car/hyundai/values.py b/opendbc_repo/opendbc/car/hyundai/values.py index 81e8d84752..2cf0c8c7b3 100644 --- a/opendbc_repo/opendbc/car/hyundai/values.py +++ b/opendbc_repo/opendbc/car/hyundai/values.py @@ -289,19 +289,24 @@ class CAR(Platforms): CarSpecs(mass=2800 * CV.LB_TO_KG, wheelbase=2.72, steerRatio=12.9, tireStiffnessFactor=0.65), flags=HyundaiFlags.CHECKSUM_CRC8, ) + HYUNDAI_ELANTRA_2024 = HyundaiPlatformConfig( + [HyundaiCarDocs("Hyundai Elantra 2024-25", car_parts=CarParts.common([CarHarness.hyundai_k]))], + CarSpecs(mass=2797 * CV.LB_TO_KG, wheelbase=2.72, steerRatio=12.9, tireStiffnessFactor=0.65), + flags=HyundaiFlags.CHECKSUM_CRC8 | HyundaiFlags.CAMERA_SCC, + ) HYUNDAI_ELANTRA_HEV_2021 = HyundaiPlatformConfig( [HyundaiCarDocs("Hyundai Elantra Hybrid 2021-23", video="https://youtu.be/_EdYQtV52-c", car_parts=CarParts.common([CarHarness.hyundai_k]))], CarSpecs(mass=3017 * CV.LB_TO_KG, wheelbase=2.72, steerRatio=12.9, tireStiffnessFactor=0.65), flags=HyundaiFlags.CHECKSUM_CRC8 | HyundaiFlags.HYBRID, ) - # 2026 CN7 Hybrid Limited. Initial port based on route - # 24d8ddb7d33b028f/00000008--d1f2ac19cc; keep this separate from the - # 2021-23 platform until its changed CAN receive checks are validated. - HYUNDAI_ELANTRA_HEV_2026 = HyundaiPlatformConfig( - [HyundaiCarDocs("Hyundai Elantra Hybrid 2026", "Limited", car_parts=CarParts.common([CarHarness.hyundai_k]))], + HYUNDAI_ELANTRA_HEV_2024 = HyundaiPlatformConfig( + [ + HyundaiCarDocs("Hyundai Elantra Hybrid 2024-26", car_parts=CarParts.common([CarHarness.hyundai_k])), + HyundaiCarDocs("Hyundai i30 Hybrid 2024", car_parts=CarParts.common([CarHarness.hyundai_k])), + ], HYUNDAI_ELANTRA_HEV_2021.specs, - flags=HyundaiFlags.CHECKSUM_CRC8 | HyundaiFlags.HYBRID, + flags=HyundaiFlags.CHECKSUM_CRC8 | HyundaiFlags.CAMERA_SCC | HyundaiFlags.HYBRID, ) HYUNDAI_GENESIS = HyundaiPlatformConfig( [ diff --git a/opendbc_repo/opendbc/car/tests/routes.py b/opendbc_repo/opendbc/car/tests/routes.py index 7111309b56..32b3356b4f 100644 --- a/opendbc_repo/opendbc/car/tests/routes.py +++ b/opendbc_repo/opendbc/car/tests/routes.py @@ -232,7 +232,10 @@ routes = [ CarTestRoute("c5ac319aa9583f83/2021-06-01--18-18-31", HYUNDAI.HYUNDAI_ELANTRA), CarTestRoute("734ef96182ddf940/2022-10-02--16-41-44", HYUNDAI.HYUNDAI_ELANTRA_GT_I30), CarTestRoute("82e9cdd3f43bf83e/2021-05-15--02-42-51", HYUNDAI.HYUNDAI_ELANTRA_2021), + CarTestRoute("c2fd040a5e34f3ad/00000013--9211a52a3d", HYUNDAI.HYUNDAI_ELANTRA_2024), CarTestRoute("715ac05b594e9c59/2021-06-20--16-21-07", HYUNDAI.HYUNDAI_ELANTRA_HEV_2021), + CarTestRoute("65ef8b49f9b0dd24/00000141--5c8720a01c", HYUNDAI.HYUNDAI_ELANTRA_HEV_2024), + CarTestRoute("07a48901db7b2503/0000000f--697d5906e8", HYUNDAI.HYUNDAI_ELANTRA_HEV_2024), # Hyundai i30 Hybrid 2024 CarTestRoute("7120aa90bbc3add7/2021-08-02--07-12-31", HYUNDAI.HYUNDAI_SONATA_HYBRID), CarTestRoute("bc40c72b728178f2/00000006--ee76ae8c42", HYUNDAI.HYUNDAI_SONATA_HEV_2024), CarTestRoute("715ac05b594e9c59/2021-10-27--23-24-56", HYUNDAI.GENESIS_G70_2020), diff --git a/opendbc_repo/opendbc/car/torque_data/substitute.toml b/opendbc_repo/opendbc/car/torque_data/substitute.toml index 01d4b02d57..4f644c0c3c 100644 --- a/opendbc_repo/opendbc/car/torque_data/substitute.toml +++ b/opendbc_repo/opendbc/car/torque_data/substitute.toml @@ -38,9 +38,10 @@ legend = ["LAT_ACCEL_FACTOR", "MAX_LAT_ACCEL_MEASURED", "FRICTION"] "HYUNDAI_IONIQ_HEV_2022" = "HYUNDAI_IONIQ_PHEV_2019" "HYUNDAI_IONIQ_EV_2020" = "HYUNDAI_IONIQ_PHEV_2019" "HYUNDAI_ELANTRA" = "HYUNDAI_SONATA_LF" +"HYUNDAI_ELANTRA_2024" = "HYUNDAI_ELANTRA_2021" "HYUNDAI_ELANTRA_GT_I30" = "HYUNDAI_SONATA_LF" "HYUNDAI_ELANTRA_HEV_2021" = "HYUNDAI_SONATA" -"HYUNDAI_ELANTRA_HEV_2026" = "HYUNDAI_SONATA" +"HYUNDAI_ELANTRA_HEV_2024" = "HYUNDAI_SONATA" "HYUNDAI_TUCSON" = "HYUNDAI_SANTA_FE" "HYUNDAI_SANTA_FE_2022" = "HYUNDAI_SANTA_FE_HEV_2022" "KIA_K5_HEV_2020" = "KIA_K5_2021" diff --git a/opendbc_repo/opendbc/dbc/hyundai_kia_generic.dbc b/opendbc_repo/opendbc/dbc/hyundai_kia_generic.dbc index befafd0f1f..5c3b597614 100644 --- a/opendbc_repo/opendbc/dbc/hyundai_kia_generic.dbc +++ b/opendbc_repo/opendbc/dbc/hyundai_kia_generic.dbc @@ -1481,7 +1481,7 @@ BO_ 905 SCC14: 8 SCC SG_ ACCMode : 32|3@1+ (1,0) [0|7] "" CLU,HUD,LDWS_LKAS,ESC SG_ ObjGap : 56|8@1+ (1,0) [0|255] "" CLU,HUD,ESC -BO_ 1157 LFAHDA_MFC: 4 XXX +BO_ 1157 LFAHDA_MFC: 8 XXX SG_ HDA_USM : 0|2@1+ (1,0) [0|3] "" XXX SG_ HDA_Active : 2|1@1+ (1,0) [0|1] "" XXX SG_ HDA_Icon_State : 3|2@1+ (1,0) [0|3] "" XXX diff --git a/opendbc_repo/opendbc/safety/modes/hyundai.h b/opendbc_repo/opendbc/safety/modes/hyundai.h index 3565f96450..0851a3ed3d 100644 --- a/opendbc_repo/opendbc/safety/modes/hyundai.h +++ b/opendbc_repo/opendbc/safety/modes/hyundai.h @@ -28,7 +28,7 @@ const LongitudinalLimits HYUNDAI_LONG_LIMITS = { #define HYUNDAI_COMMON_TX_MSGS(scc_bus) \ {0x340, 0, 8, .check_relay = true}, /* LKAS11 Bus 0 */ \ {0x4F1, scc_bus, 4, .check_relay = false}, /* CLU11 Bus 0 (radar-SCC) or 2 (camera-SCC) */ \ - {0x485, 0, 4, .check_relay = true}, /* LFAHDA_MFC Bus 0 */ \ + {0x485, 0, 8, .check_relay = true}, /* LFAHDA_MFC Bus 0 */ \ #define HYUNDAI_LONG_COMMON_TX_MSGS(scc_bus) \ HYUNDAI_COMMON_TX_MSGS(scc_bus) \ diff --git a/scripts/speed_limit_vision/evaluate_reviewed_route_events.py b/scripts/speed_limit_vision/evaluate_reviewed_route_events.py index b3a06404b3..f7ce72671b 100644 --- a/scripts/speed_limit_vision/evaluate_reviewed_route_events.py +++ b/scripts/speed_limit_vision/evaluate_reviewed_route_events.py @@ -48,6 +48,13 @@ def parse_args() -> argparse.Namespace: parser.add_argument("--dedupe-seconds", type=float, default=3.0, help="Collapse nearby reviewed rows with the same expected value.") parser.add_argument("--measured-base-inference-seconds", type=float, default=0.44, help="Measured no-proposal comma inference cost.") parser.add_argument("--measured-classifier-forward-seconds", type=float, default=0.066, help="Measured comma cost per classifier forward.") + parser.add_argument("--measured-tracking-base-seconds", type=float, default=0.012, help="Measured optical-flow and crop preparation cost.") + parser.add_argument("--disable-temporal-tracking", action="store_true", help="Disable proposal tracking for an A/B evaluation.") + parser.add_argument("--track-classification-interval", type=float, help="Override seconds between tracked crop classifications.") + parser.add_argument("--track-detector-interval", type=float, help="Override detector cadence while a proposal track is active.") + parser.add_argument("--track-min-proposal-confidence", type=float, help="Override detector confidence required to begin tracking.") + parser.add_argument("--track-unreadable-min-proposal-confidence", type=float, help="Override confidence required to track a proposal with no readable value.") + parser.add_argument("--track-max-age", type=float, help="Override the maximum proposal track lifetime.") parser.add_argument("--crop-ocr", action="store_true", help="Evaluate with crop OCR confirmation enabled.") parser.add_argument("--classifier-min-confidence", type=float, help="Override the value classifier confidence threshold.") parser.add_argument("--trusted-model-min-confidence", type=float, help="Override tiny-box trusted model confidence.") @@ -87,6 +94,14 @@ def parse_args() -> argparse.Namespace: parser.add_argument("--positive-only", action="store_true", help="Replay only reviewed speed signs, omitting ignored-crop windows.") parser.add_argument("--negative-only", action="store_true", help="Replay only ignored not-speed-limit windows.") parser.add_argument("--route-file", type=Path, help="Only replay routes listed one per line in this file.") + parser.add_argument("--record-key-file", type=Path, help="Only replay record keys listed one per line in this file.") + parser.add_argument("--focus-eval-csv", type=Path, help="Only replay records selected from an earlier runtime evaluation.") + parser.add_argument( + "--focus-outcome", + choices=("candidate_hit", "publish_hit"), + default="candidate_hit", + help="Outcome that must be false in --focus-eval-csv.", + ) parser.add_argument("--strong-detection-confidence", type=float, help="Override one-frame publication confidence.") parser.add_argument("--consistent-detections", type=int, help="Override matching reads required for an initial publication.") parser.add_argument("--change-consistent-detections", type=int, help="Override matching reads required to change a publication.") @@ -133,13 +148,14 @@ def load_cases(queue_path: Path, labels_path: Path, dedupe_seconds: float) -> li return cases -def replay_video_cases(cases: list[ReviewedCase], args: argparse.Namespace) -> dict[str, tuple[list[dict[str, str]], int]]: +def replay_video_cases(cases: list[ReviewedCase], args: argparse.Namespace) -> dict[str, tuple[list[dict[str, str]], int, int, int, int, float]]: daemons = { case.record_key: RouteReplayDaemon( runtime_context=None, measured_inference_seconds=0.0, measured_base_inference_seconds=args.measured_base_inference_seconds, measured_classifier_forward_seconds=args.measured_classifier_forward_seconds, + measured_tracking_base_seconds=args.measured_tracking_base_seconds, ) for case in cases } @@ -182,7 +198,14 @@ def replay_video_cases(cases: list[ReviewedCase], args: argparse.Namespace) -> d results = {} for case in cases: daemon = daemons[case.record_key] - results[case.record_key] = daemon.events, daemon.inference_frames + results[case.record_key] = ( + daemon.events, + daemon.inference_frames, + daemon.detector_inference_count, + daemon.track_inference_count, + daemon.track_start_count, + daemon.max_track_proposal_confidence, + ) return results @@ -191,6 +214,18 @@ def main() -> int: queue_path = args.queue.expanduser().resolve() labels_path = args.labels.expanduser().resolve() if args.labels else queue_path.with_name("manual_review_labels.csv") configure_models(args.models_dir) + if args.disable_temporal_tracking: + slv.TEMPORAL_TRACKING_ENABLED = False + if args.track_classification_interval is not None: + slv.TRACK_CLASSIFICATION_INTERVAL = args.track_classification_interval + if args.track_detector_interval is not None: + slv.TRACK_DETECTOR_INTERVAL = args.track_detector_interval + if args.track_min_proposal_confidence is not None: + slv.TRACK_MIN_PROPOSAL_CONFIDENCE = args.track_min_proposal_confidence + if args.track_unreadable_min_proposal_confidence is not None: + slv.TRACK_UNREADABLE_MIN_PROPOSAL_CONFIDENCE = args.track_unreadable_min_proposal_confidence + if args.track_max_age is not None: + slv.TRACK_MAX_AGE_SECONDS = args.track_max_age slv.DETECTOR_CLASSIFIER_CROP_OCR_ENABLED = args.crop_ocr if args.classifier_min_confidence is not None: slv.US_CLASSIFIER_MIN_CONFIDENCE = args.classifier_min_confidence @@ -238,6 +273,19 @@ def main() -> int: if args.change_single_read_min_confidence is not None: slv.CHANGE_SINGLE_READ_MIN_CONFIDENCE = args.change_single_read_min_confidence cases = load_cases(queue_path, labels_path, args.dedupe_seconds) + if args.focus_eval_csv: + with args.focus_eval_csv.expanduser().resolve().open(encoding="utf-8", newline="") as input_file: + selected_record_keys = { + row.get("record_key", "") + for row in csv.DictReader(input_file) + if row.get("record_key") and row.get(args.focus_outcome, "").strip().lower() not in ("1", "true", "yes") + } + cases = [case for case in cases if case.record_key in selected_record_keys] + if args.record_key_file: + selected_record_keys = { + line.strip() for line in args.record_key_file.expanduser().resolve().read_text(encoding="utf-8").splitlines() if line.strip() + } + cases = [case for case in cases if case.record_key in selected_record_keys] if args.route_file: selected_routes = { line.strip() for line in args.route_file.expanduser().resolve().read_text(encoding="utf-8").splitlines() if line.strip() @@ -255,7 +303,7 @@ def main() -> int: output_rows: list[dict[str, object]] = [] positive_by_speed: dict[int, Counter[str]] = defaultdict(Counter) negative_counts: Counter[str] = Counter() - results: dict[str, tuple[list[dict[str, str]], int]] = {} + results: dict[str, tuple[list[dict[str, str]], int, int, int, int, float]] = {} cases_by_video: dict[Path, list[ReviewedCase]] = defaultdict(list) for case in cases: cases_by_video[case.source_video_path].append(case) @@ -265,7 +313,9 @@ def main() -> int: print(f"Replayed {index}/{len(cases_by_video)} video segments", flush=True) for case in cases: - events, inference_frames = results.get(case.record_key, ([], 0)) + events, inference_frames, detector_inference_frames, track_inference_frames, track_starts, max_track_confidence = results.get( + case.record_key, ([], 0, 0, 0, 0, 0.0), + ) candidate_events = [event for event in events if event["event"] == "candidate"] publish_events = [event for event in events if event["event"] == "publish"] candidates = [int(event["candidateSpeedLimitMph"]) for event in candidate_events] @@ -307,6 +357,10 @@ def main() -> int: "false_candidate": false_candidate, "false_publish": false_publish, "inference_frames": inference_frames, + "detector_inference_frames": detector_inference_frames, + "track_inference_frames": track_inference_frames, + "track_starts": track_starts, + "max_track_proposal_confidence": f"{max_track_confidence:.4f}", "source_video_path": str(case.source_video_path), }) output_path = args.output_csv.expanduser().resolve() @@ -331,6 +385,14 @@ def main() -> int: "classifier_min_confidence": slv.US_CLASSIFIER_MIN_CONFIDENCE, "measured_base_inference_seconds": args.measured_base_inference_seconds, "measured_classifier_forward_seconds": args.measured_classifier_forward_seconds, + "measured_tracking_base_seconds": args.measured_tracking_base_seconds, + "temporal_tracking_enabled": slv.TEMPORAL_TRACKING_ENABLED, + "track_confirmed_proposals_enabled": slv.TRACK_CONFIRMED_PROPOSALS_ENABLED, + "track_classification_interval": slv.TRACK_CLASSIFICATION_INTERVAL, + "track_detector_interval": slv.TRACK_DETECTOR_INTERVAL, + "track_min_proposal_confidence": slv.TRACK_MIN_PROPOSAL_CONFIDENCE, + "track_unreadable_min_proposal_confidence": slv.TRACK_UNREADABLE_MIN_PROPOSAL_CONFIDENCE, + "track_max_age_seconds": slv.TRACK_MAX_AGE_SECONDS, "initial_speed_limit_mph": args.initial_speed_limit, "low_speed_change_consistent_detections": slv.LOW_SPEED_CHANGE_CONSISTENT_DETECTIONS, "low_speed_change_min_confidence": slv.LOW_SPEED_CHANGE_MIN_CONFIDENCE, diff --git a/scripts/speed_limit_vision/replay_route_runtime.py b/scripts/speed_limit_vision/replay_route_runtime.py index af1280a956..2e37ba840d 100644 --- a/scripts/speed_limit_vision/replay_route_runtime.py +++ b/scripts/speed_limit_vision/replay_route_runtime.py @@ -67,6 +67,7 @@ class RouteReplayDaemon(slv.SpeedLimitVisionDaemon): measured_inference_seconds: float, measured_base_inference_seconds: float | None = None, measured_classifier_forward_seconds: float = 0.0, + measured_tracking_base_seconds: float = 0.012, ): super().__init__(use_runtime=False) self.runtime_context = runtime_context @@ -75,6 +76,7 @@ class RouteReplayDaemon(slv.SpeedLimitVisionDaemon): max(float(measured_base_inference_seconds), 0.0) if measured_base_inference_seconds is not None else None ) self.measured_classifier_forward_seconds = max(float(measured_classifier_forward_seconds), 0.0) + self.measured_tracking_base_seconds = max(float(measured_tracking_base_seconds), 0.0) self.next_available_at = -float("inf") self.now = 0.0 self.sampled_frames = 0 @@ -132,27 +134,35 @@ class RouteReplayDaemon(slv.SpeedLimitVisionDaemon): return self.current_frame_bgr = frame_bgr + track_due = self._track_classification_due(now) inference_interval = self._inference_interval(now) - next_due = max(self.next_available_at, self.last_inference_at + inference_interval) - if now < next_due: + detector_interval = max(inference_interval, slv.TRACK_DETECTOR_INTERVAL) if self.proposal_track is not None else inference_interval + detector_due = now >= self.last_inference_at + detector_interval + if not track_due and not detector_due: if self.published_speed_limit_mph > 0 and self._published_detection_stale(now): self._write_debug_event("stale_clear", reason="inference_interval") self._clear_detection() return - self.last_inference_at = now self.inference_frames += 1 self.last_detector_forward_count = 0 self.last_detector_forward_duration_s = 0.0 self.last_classifier_forward_count = 0 self.last_classifier_forward_duration_s = 0.0 - detection = self._detect_sign(frame_bgr) - inference_seconds = self.measured_inference_seconds - if self.measured_base_inference_seconds is not None: - inference_seconds = ( - self.measured_base_inference_seconds + - self.last_classifier_forward_count * self.measured_classifier_forward_seconds - ) + if detector_due: + self.detector_inference_count += 1 + self.last_inference_at = now + detection = self._detect_sign(frame_bgr) + self._start_latest_detector_track(frame_bgr, now) + inference_seconds = self.measured_inference_seconds + if self.measured_base_inference_seconds is not None: + inference_seconds = ( + self.measured_base_inference_seconds + + self.last_classifier_forward_count * self.measured_classifier_forward_seconds + ) + else: + detection = self._classify_proposal_track(frame_bgr, now) + inference_seconds = self.measured_tracking_base_seconds + self.last_classifier_forward_count * self.measured_classifier_forward_seconds self.next_available_at = now + inference_seconds if detection is not None: self._update_detection(detection) @@ -160,6 +170,16 @@ class RouteReplayDaemon(slv.SpeedLimitVisionDaemon): self._write_debug_event("stale_clear", reason="no_detection") self._clear_detection() + def next_processing_due(self, now: float) -> float: + if self.proposal_track is not None: + if now - self.proposal_track.started_at > slv.TRACK_MAX_AGE_SECONDS: + self._clear_proposal_track() + else: + track_due = self.proposal_track.last_classified_at + self._track_classification_interval(now) + detector_due = self.last_inference_at + max(self._inference_interval(now), slv.TRACK_DETECTOR_INTERVAL) + return max(self.next_available_at, min(track_due, detector_due)) + return max(self.next_available_at, self.last_inference_at + self._inference_interval(now)) + def parse_args() -> argparse.Namespace: parser = argparse.ArgumentParser(description="Replay downloaded route camera segments through the runtime speed-limit vision cadence.") @@ -184,6 +204,15 @@ def parse_args() -> argparse.Namespace: default=0.0, help="Additional measured comma cost per classifier forward when the dynamic cost model is enabled.", ) + parser.add_argument( + "--measured-tracking-base-seconds", + type=float, + default=0.012, + help="Measured optical-flow and crop-preparation cost for one tracked frame.", + ) + parser.add_argument("--disable-temporal-tracking", action="store_true", help="Disable proposal tracking for an A/B replay.") + parser.add_argument("--track-unreadable-min-proposal-confidence", type=float, help="Override confidence required to track an unreadable proposal.") + parser.add_argument("--track-detector-interval", type=float, help="Override detector cadence while tracking a proposal.") parser.add_argument( "--detector-region-mode", choices=("full", "right_roi", "full_and_right_roi"), @@ -322,6 +351,12 @@ def configure_runtime_options(args: argparse.Namespace) -> None: slv.LOW_SPEED_CHANGE_ALLOW_STRONG_CONSENSUS = True if args.enable_strong_model_consensus: slv.DETECTOR_CLASSIFIER_STRONG_MODEL_CONSENSUS_ENABLED = True + if args.disable_temporal_tracking: + slv.TEMPORAL_TRACKING_ENABLED = False + if args.track_unreadable_min_proposal_confidence is not None: + slv.TRACK_UNREADABLE_MIN_PROPOSAL_CONFIDENCE = args.track_unreadable_min_proposal_confidence + if args.track_detector_interval is not None: + slv.TRACK_DETECTOR_INTERVAL = args.track_detector_interval if args.right_roi_bounds: parts = [float(part.strip()) for part in args.right_roi_bounds.split(",")] @@ -370,6 +405,7 @@ def replay_route( measured_inference_seconds: float, measured_base_inference_seconds: float | None = None, measured_classifier_forward_seconds: float = 0.0, + measured_tracking_base_seconds: float = 0.012, initial_speed_limit_mph: int = 0, ) -> tuple[RouteSummary, list[dict[str, str]]]: daemon = RouteReplayDaemon( @@ -377,6 +413,7 @@ def replay_route( measured_inference_seconds, measured_base_inference_seconds, measured_classifier_forward_seconds, + measured_tracking_base_seconds, ) daemon.published_speed_limit_mph = initial_speed_limit_mph for segment_path in segments: @@ -403,8 +440,7 @@ def replay_route( frame_index = skip_to_frame(capture, frame_index, frame_index + 1, fast_seek) continue - inference_interval = daemon._inference_interval(now) - next_due = max(daemon.next_available_at, daemon.last_inference_at + inference_interval) + next_due = daemon.next_processing_due(now) if now < next_due: target_index = max(frame_index + 1, int(round((next_due - segment_start_s) * fps))) if total_frames > 0: @@ -512,6 +548,7 @@ def main() -> int: args.measured_inference_seconds, args.measured_base_inference_seconds, args.measured_classifier_forward_seconds, + args.measured_tracking_base_seconds, args.initial_speed_limit, ) all_events.extend((log_id, event) for event in events) diff --git a/starpilot/system/speed_limit_vision.py b/starpilot/system/speed_limit_vision.py index f21b88e67e..e45dc116c7 100644 --- a/starpilot/system/speed_limit_vision.py +++ b/starpilot/system/speed_limit_vision.py @@ -2,6 +2,7 @@ from __future__ import annotations import json +import math import time from collections import Counter, deque @@ -20,6 +21,19 @@ RUNTIME_LOOP_HZ = 20 INFERENCE_INTERVAL = 0.15 FOLLOWUP_INFERENCE_INTERVAL = 0.10 FOLLOWUP_WINDOW_SECONDS = 2.0 +TEMPORAL_TRACKING_ENABLED = True +TRACK_CONFIRMED_PROPOSALS_ENABLED = False +TRACK_CLASSIFICATION_INTERVAL = 0.12 +TRACK_BUSY_CLASSIFICATION_INTERVAL = 0.35 +TRACK_DETECTOR_INTERVAL = 0.55 +TRACK_MAX_AGE_SECONDS = 2.0 +TRACK_MIN_PROPOSAL_CONFIDENCE = 0.10 +TRACK_UNREADABLE_MIN_PROPOSAL_CONFIDENCE = 0.22 +TRACK_MAX_CONSECUTIVE_FAILED_READS = 2 +TRACK_MIN_FEATURE_COUNT = 4 +TRACK_MAX_AREA_RATIO = 0.18 +TRACK_CROP_PADDING_RATIO = 0.06 +TRACK_REPEAT_CONFIDENCE_BONUS = 0.12 BUSY_INFERENCE_INTERVAL = 1.0 LIVE_POSE_RECOVERY_THROTTLE_SECONDS = 2.0 LIVE_POSE_RECOVERY_INFERENCE_INTERVAL = 1.0 @@ -238,6 +252,27 @@ class Detection: strong_consensus: bool = False +@dataclass(frozen=True) +class DetectorProposal: + confidence: float + class_id: int + bbox: tuple[int, int, int, int] + speed_limit_mph: int = 0 + + +@dataclass +class ProposalTrack: + proposal: DetectorProposal + bbox: tuple[int, int, int, int] + previous_gray: np.ndarray + points: np.ndarray + started_at: float + last_classified_at: float + last_speed_limit_mph: int = 0 + consistent_reads: int = 0 + consecutive_failed_reads: int = 0 + + @dataclass class HistoryEntry: speed_limit_mph: int @@ -288,6 +323,12 @@ class SpeedLimitVisionDaemon: self.last_live_pose_inputs_not_ok_at = -float("inf") self.last_road_name = "" self.followup_until = 0.0 + self.latest_detector_proposal = None + self.proposal_track = None + self.track_inference_count = 0 + self.track_failure_count = 0 + self.track_start_count = 0 + self.max_track_proposal_confidence = 0.0 self.started_prev = False self.history: deque[HistoryEntry] = deque() @@ -324,6 +365,7 @@ class SpeedLimitVisionDaemon: self.last_debug_heartbeat_at = 0.0 self.loop_count = 0 self.inference_count = 0 + self.detector_inference_count = 0 self.interval_skip_count = 0 self.busy_skip_count = 0 self.camera_unavailable_count = 0 @@ -985,7 +1027,211 @@ class SpeedLimitVisionDaemon: image = cv2.copyMakeBorder(image, top, bottom, left, right, cv2.BORDER_CONSTANT, value=color) return image, ratio, pad_width, pad_height + @staticmethod + def _clamp_track_bbox(bbox, width, height): + x1, y1, x2, y2 = bbox + result = ( + max(int(round(x1)), 0), + max(int(round(y1)), 0), + min(int(round(x2)), width), + min(int(round(y2)), height), + ) + return result if result[2] > result[0] and result[3] > result[1] else None + + @staticmethod + def _track_feature_points(gray, bbox): + height, width = gray.shape[:2] + x1, y1, x2, y2 = bbox + box_width = x2 - x1 + box_height = y2 - y1 + pad_x = max(int(box_width * 0.20), 2) + pad_y = max(int(box_height * 0.20), 2) + mask = np.zeros_like(gray) + mask[max(y1 - pad_y, 0):min(y2 + pad_y, height), max(x1 - pad_x, 0):min(x2 + pad_x, width)] = 255 + return cv2.goodFeaturesToTrack(gray, mask=mask, maxCorners=40, qualityLevel=0.005, minDistance=3, blockSize=5) + + @classmethod + def _flow_track_bbox(cls, previous_gray, current_gray, bbox, points): + if points is None or len(points) < TRACK_MIN_FEATURE_COUNT: + points = cls._track_feature_points(previous_gray, bbox) + if points is None or len(points) < TRACK_MIN_FEATURE_COUNT: + return None, None + + next_points, status, errors = cv2.calcOpticalFlowPyrLK( + previous_gray, + current_gray, + points, + None, + winSize=(25, 25), + maxLevel=3, + criteria=(cv2.TERM_CRITERIA_EPS | cv2.TERM_CRITERIA_COUNT, 20, 0.03), + ) + if next_points is None or status is None: + return None, None + good = status.reshape(-1).astype(bool) + if errors is not None: + good &= errors.reshape(-1) < 35.0 + old = points.reshape(-1, 2)[good] + new = next_points.reshape(-1, 2)[good] + if len(old) < TRACK_MIN_FEATURE_COUNT: + return None, None + + transform, inliers = cv2.estimateAffinePartial2D(old, new, method=cv2.RANSAC, ransacReprojThreshold=3.0) + if transform is None or inliers is None or int(inliers.sum()) < TRACK_MIN_FEATURE_COUNT: + return None, None + scale = math.hypot(float(transform[0, 0]), float(transform[0, 1])) + if not 0.84 <= scale <= 1.24: + return None, None + + x1, y1, x2, y2 = bbox + corners = np.float32(((x1, y1), (x2, y1), (x2, y2), (x1, y2))).reshape(-1, 1, 2) + moved = cv2.transform(corners, transform).reshape(-1, 2) + tracked = cls._clamp_track_bbox( + (moved[:, 0].min(), moved[:, 1].min(), moved[:, 0].max(), moved[:, 1].max()), + current_gray.shape[1], + current_gray.shape[0], + ) + if tracked is None: + return None, None + inlier_points = new[inliers.reshape(-1).astype(bool)].reshape(-1, 1, 2) + return tracked, inlier_points + + def _remember_detector_proposal(self, confidence, class_id, bbox, speed_limit_mph=0, preferred=False): + min_confidence = TRACK_MIN_PROPOSAL_CONFIDENCE if speed_limit_mph else TRACK_UNREADABLE_MIN_PROPOSAL_CONFIDENCE + if not TEMPORAL_TRACKING_ENABLED or class_id == 1 or confidence < min_confidence: + return + proposal = DetectorProposal(float(confidence), int(class_id), bbox, int(speed_limit_mph)) + latest_proposal = getattr(self, "latest_detector_proposal", None) + if preferred or latest_proposal is None or proposal.confidence > latest_proposal.confidence: + self.latest_detector_proposal = proposal + + def _start_latest_detector_track(self, frame_bgr, now): + proposal = self.latest_detector_proposal + self.latest_detector_proposal = None + if ( + not TEMPORAL_TRACKING_ENABLED or + proposal is None or + (proposal.speed_limit_mph and not TRACK_CONFIRMED_PROPOSALS_ENABLED) + ): + return False + gray = cv2.cvtColor(frame_bgr, cv2.COLOR_BGR2GRAY) + points = self._track_feature_points(gray, proposal.bbox) + if points is None or len(points) < TRACK_MIN_FEATURE_COUNT: + self.track_failure_count += 1 + return False + self.proposal_track = ProposalTrack( + proposal=proposal, + bbox=proposal.bbox, + previous_gray=gray, + points=points, + started_at=now, + last_classified_at=now, + ) + self.track_start_count += 1 + self.max_track_proposal_confidence = max(self.max_track_proposal_confidence, proposal.confidence) + return True + + def _clear_proposal_track(self, failed=False): + if failed and self.proposal_track is not None: + self.track_failure_count += 1 + self.proposal_track = None + + def _track_classification_interval(self, now): + interval = TRACK_CLASSIFICATION_INTERVAL + if now - self.last_live_pose_inputs_not_ok_at < LIVE_POSE_RECOVERY_THROTTLE_SECONDS: + return max(interval, LIVE_POSE_RECOVERY_INFERENCE_INTERVAL) + if self._device_cpu_busy(): + return max(interval, TRACK_BUSY_CLASSIFICATION_INTERVAL) + return interval + + def _track_classification_due(self, now): + track = self.proposal_track + if track is None: + return False + if now - track.started_at > TRACK_MAX_AGE_SECONDS: + self._clear_proposal_track() + return False + return now - track.last_classified_at >= self._track_classification_interval(now) + + def _classify_proposal_track(self, frame_bgr, now): + track = self.proposal_track + if track is None: + return None + current_gray = cv2.cvtColor(frame_bgr, cv2.COLOR_BGR2GRAY) + bbox, points = self._flow_track_bbox(track.previous_gray, current_gray, track.bbox, track.points) + if bbox is None or points is None: + self._clear_proposal_track(failed=True) + return None + + frame_height, frame_width = frame_bgr.shape[:2] + x1, y1, x2, y2 = bbox + box_width = x2 - x1 + box_height = y2 - y1 + area_ratio = box_width * box_height / max(frame_width * frame_height, 1) + if ( + box_width < MODEL_PROPOSAL_MIN_WIDTH or + box_height < MODEL_PROPOSAL_MIN_HEIGHT or + area_ratio > TRACK_MAX_AREA_RATIO or + (x1 + x2) / 2 < frame_width * MODEL_PROPOSAL_MIN_X_RATIO + ): + self._clear_proposal_track(failed=True) + return None + + track.bbox = bbox + track.previous_gray = current_gray + track.points = points + track.last_classified_at = now + self.track_inference_count += 1 + + pad_x = int(box_width * TRACK_CROP_PADDING_RATIO) + pad_y = int(box_height * TRACK_CROP_PADDING_RATIO) + crop_x1 = max(x1 - pad_x, 0) + crop_y1 = max(y1 - pad_y, 0) + crop_x2 = min(x2 + pad_x, frame_width) + crop_y2 = min(y2 + pad_y, frame_height) + sign_crop = frame_bgr[crop_y1:crop_y2, crop_x1:crop_x2] + if sign_crop.size == 0: + self._clear_proposal_track(failed=True) + return None + + read_result = self._classify_speed_limit_from_model(sign_crop) + if read_result is None: + track.consecutive_failed_reads += 1 + track.last_speed_limit_mph = 0 + track.consistent_reads = 0 + if track.consecutive_failed_reads >= TRACK_MAX_CONSECUTIVE_FAILED_READS: + self._clear_proposal_track() + return None + speed_limit_mph, read_confidence = read_result + if track.proposal.speed_limit_mph and speed_limit_mph != track.proposal.speed_limit_mph: + self._clear_proposal_track() + return None + if track.proposal.class_id == 2 and speed_limit_mph not in SCHOOL_ZONE_SPEED_VALUES: + track.last_speed_limit_mph = 0 + track.consistent_reads = 0 + return None + + track.consecutive_failed_reads = 0 + + if speed_limit_mph == track.last_speed_limit_mph: + track.consistent_reads += 1 + else: + track.last_speed_limit_mph = speed_limit_mph + track.consistent_reads = 1 + + regulatory_bonus = 0.04 if self._is_regulatory_speed_sign(sign_crop) or track.proposal.class_id == 2 else 0.0 + repeat_bonus = TRACK_REPEAT_CONFIDENCE_BONUS if track.consistent_reads >= 2 else 0.0 + score = min( + read_confidence * 0.78 + + track.proposal.confidence * 0.12 + + regulatory_bonus + + repeat_bonus, + 0.95, + ) + return self._publishable_detection(Detection(speed_limit_mph, score)) + def _detect_sign(self, frame_bgr): + self.latest_detector_proposal = None if self.net is None: if FULL_FRAME_OCR_FALLBACK_ENABLED: return self._publishable_detection(self._detect_sign_from_ocr_candidates(frame_bgr)) @@ -1380,6 +1626,7 @@ class SpeedLimitVisionDaemon: proposal_area_ratio < DETECTOR_CLASSIFIER_TINY_LOW_CONF_AREA_RATIO and proposal_confidence < DETECTOR_CLASSIFIER_TINY_LOW_CONF_MIN_CONFIDENCE ) + self._remember_detector_proposal(proposal_confidence, class_id, (x1, y1, x2, y2)) if class_id == 2: school_scores: dict[int, float] = {} @@ -1434,6 +1681,9 @@ class SpeedLimitVisionDaemon: 0.95, ) if score >= SCHOOL_ZONE_SHORT_CIRCUIT_CONFIDENCE: + self._remember_detector_proposal( + proposal_confidence, class_id, (x1, y1, x2, y2), speed_limit_mph, preferred=True, + ) return Detection(speed_limit_mph, score) speed_scores: dict[int, float] = {} @@ -1625,6 +1875,9 @@ class SpeedLimitVisionDaemon: if selection_score > best_score: best_score = selection_score best_detection = Detection(speed_limit_mph, published_score, strong_rescue) + self._remember_detector_proposal( + proposal_confidence, class_id, (x1, y1, x2, y2), speed_limit_mph, preferred=True, + ) if best_detection is not None and best_detection.confidence >= MODEL_DETECTION_SHORT_CIRCUIT_CONFIDENCE: return best_detection @@ -1928,6 +2181,7 @@ class SpeedLimitVisionDaemon: def _clear_detection(self): self.history.clear() self.followup_until = 0.0 + self._clear_proposal_track() self.pending_auto_bookmark = None self.pending_training_capture = None self.previous_published_speed_limit_mph = self.published_speed_limit_mph @@ -2012,11 +2266,17 @@ class SpeedLimitVisionDaemon: "debugSession": self.debug_session_id, "loopCount": self.loop_count, "inferenceCount": self.inference_count, + "detectorInferenceCount": self.detector_inference_count, "intervalSkipCount": self.interval_skip_count, "busySkipCount": self.busy_skip_count, "cameraUnavailableCount": self.camera_unavailable_count, "emptyFrameCount": self.empty_frame_count, "detectionCount": self.detection_count, + "trackInferenceCount": self.track_inference_count, + "trackFailureCount": self.track_failure_count, + "trackStartCount": self.track_start_count, + "maxTrackProposalConfidence": round(self.max_track_proposal_confidence, 4), + "proposalTrackActive": self.proposal_track is not None, "lastInferenceAgeS": round(max(now - self.last_inference_at, 0.0), 3), "lastInferenceIntervalS": round(float(self.last_inference_interval), 3), "lastInferenceIntervalReason": self.last_inference_interval_reason, @@ -2208,7 +2468,10 @@ class SpeedLimitVisionDaemon: continue inference_interval = self._inference_interval(now) - if now - self.last_inference_at < inference_interval: + track_due = self._track_classification_due(now) + detector_interval = max(inference_interval, TRACK_DETECTOR_INTERVAL) if self.proposal_track is not None else inference_interval + detector_due = now - self.last_inference_at >= detector_interval + if not track_due and not detector_due: self.interval_skip_count += 1 if self.last_inference_interval_reason == "cpu_busy": self.busy_skip_count += 1 @@ -2223,7 +2486,6 @@ class SpeedLimitVisionDaemon: buffer = self.client.recv() if self.client is not None else None self.inference_count += 1 - self.last_inference_at = now inference_started_at = time.monotonic() self.last_frame_process_duration_s = 0.0 self.last_detector_forward_count = 0 @@ -2245,7 +2507,13 @@ class SpeedLimitVisionDaemon: frame_bgr = cv2.cvtColor(image[:self.client.height * 3 // 2, :self.client.width], cv2.COLOR_YUV2BGR_NV12) self.current_frame_bgr = frame_bgr - detection = self._detect_sign(frame_bgr) + if detector_due: + self.detector_inference_count += 1 + self.last_inference_at = now + detection = self._detect_sign(frame_bgr) + self._start_latest_detector_track(frame_bgr, now) + else: + detection = self._classify_proposal_track(frame_bgr, now) self.last_frame_process_duration_s = time.monotonic() - inference_started_at if detection is not None: self.detection_count += 1 diff --git a/starpilot/system/tests/test_speed_limit_vision.py b/starpilot/system/tests/test_speed_limit_vision.py index b6f2f8d819..ac6bc50516 100644 --- a/starpilot/system/tests/test_speed_limit_vision.py +++ b/starpilot/system/tests/test_speed_limit_vision.py @@ -4,7 +4,7 @@ import numpy as np import pytest import starpilot.system.speed_limit_vision as slv -from starpilot.system.speed_limit_vision import HistoryEntry, SpeedLimitVisionDaemon +from starpilot.system.speed_limit_vision import DetectorProposal, HistoryEntry, ProposalTrack, SpeedLimitVisionDaemon def daemon_with_history(current_speed, entries): @@ -57,6 +57,54 @@ def test_low_speed_change_rejects_low_confidence_sequence(): assert daemon._confirm_detection() is None +def textured_track_frame(offset_x=0, offset_y=0): + frame = np.zeros((120, 180, 3), dtype=np.uint8) + x1, y1, x2, y2 = 90 + offset_x, 30 + offset_y, 130 + offset_x, 90 + offset_y + frame[y1:y2, x1:x2] = 220 + cv2 = pytest.importorskip("cv2") + cv2.rectangle(frame, (x1 + 3, y1 + 3), (x2 - 3, y2 - 3), (20, 20, 20), 2) + cv2.putText(frame, "55", (x1 + 5, y1 + 42), cv2.FONT_HERSHEY_SIMPLEX, 0.55, (10, 10, 10), 2) + return frame, (x1, y1, x2, y2) + + +def test_flow_track_bbox_follows_translation(): + cv2 = pytest.importorskip("cv2") + first, bbox = textured_track_frame() + second, expected_bbox = textured_track_frame(4, 3) + first_gray = cv2.cvtColor(first, cv2.COLOR_BGR2GRAY) + second_gray = cv2.cvtColor(second, cv2.COLOR_BGR2GRAY) + points = SpeedLimitVisionDaemon._track_feature_points(first_gray, bbox) + + tracked_bbox, tracked_points = SpeedLimitVisionDaemon._flow_track_bbox(first_gray, second_gray, bbox, points) + + assert tracked_bbox == pytest.approx(expected_bbox, abs=1) + assert tracked_points is not None and len(tracked_points) >= 4 + + +def test_temporal_track_boosts_two_consistent_model_reads(): + cv2 = pytest.importorskip("cv2") + frame, bbox = textured_track_frame() + gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY) + points = SpeedLimitVisionDaemon._track_feature_points(gray, bbox) + daemon = SpeedLimitVisionDaemon.__new__(SpeedLimitVisionDaemon) + daemon.proposal_track = ProposalTrack(DetectorProposal(0.20, 0, bbox), bbox, gray, points, 0.0, 0.0) + daemon.track_inference_count = 0 + daemon.track_failure_count = 0 + daemon.last_classifier_forward_count = 0 + daemon.last_classifier_forward_duration_s = 0.0 + daemon._classify_speed_limit_from_model = lambda _crop: (55, 0.90) + daemon._is_regulatory_speed_sign = lambda _crop: True + + first = daemon._classify_proposal_track(frame, 0.2) + second = daemon._classify_proposal_track(frame, 0.4) + + assert first.speed_limit_mph == 55 + assert second.speed_limit_mph == 55 + assert first.confidence < slv.CHANGE_SINGLE_READ_MIN_CONFIDENCE + assert second.confidence >= slv.CHANGE_SINGLE_READ_MIN_CONFIDENCE + assert daemon.track_inference_count == 2 + + def detector_classifier_daemon(*, regulatory: bool, model_read, bbox=(700, 100, 780, 220), proposal_confidence=0.80): daemon = SpeedLimitVisionDaemon.__new__(SpeedLimitVisionDaemon) daemon._collect_detector_classifier_proposals = lambda _frame: [(proposal_confidence, 0, bbox)]