From 7e4f8d4154d059d4328b27f1740c42ae0ff533f5 Mon Sep 17 00:00:00 2001 From: firestar5683 <168790843+firestar5683@users.noreply.github.com> Date: Mon, 13 Jul 2026 12:41:08 -0500 Subject: [PATCH] patch1 --- .../build_track_proposal_detector_dataset.py | 76 +++++- .../evaluate_reviewed_route_events.py | 13 + .../mine_reviewed_sign_tracks.py | 232 ++++++++++++++---- .../test_track_proposal_detector_dataset.py | 69 +++++- selfdrive/controls/lib/latcontrol_torque.py | 9 +- .../controls/lib/latcontrol_vehicle_tunes.py | 4 + selfdrive/controls/tests/test_latcontrol.py | 38 +++ starpilot/system/speed_limit_vision.py | 12 +- .../system/tests/test_speed_limit_vision.py | 20 +- 9 files changed, 408 insertions(+), 65 deletions(-) diff --git a/scripts/speed_limit_vision/build_track_proposal_detector_dataset.py b/scripts/speed_limit_vision/build_track_proposal_detector_dataset.py index 3edd9e99b..76e435ce4 100644 --- a/scripts/speed_limit_vision/build_track_proposal_detector_dataset.py +++ b/scripts/speed_limit_vision/build_track_proposal_detector_dataset.py @@ -37,9 +37,26 @@ def parse_args() -> argparse.Namespace: parser.add_argument("--min-growth", type=float, default=1.0, help="Minimum tracked box area relative to its reviewed anchor.") parser.add_argument("--min-exact-confidence", type=float, default=0.80) parser.add_argument("--min-detector-confidence", type=float, default=0.30) + parser.add_argument("--min-tracking-confidence", type=float, default=1.01, help="Optical-flow confidence for detector-free tracks.") parser.add_argument("--max-track-rank", type=int, default=4) parser.add_argument("--important-repeat", type=int, default=2, help="Train repeats for accepted 30-65 mph samples.") parser.add_argument("--other-repeat", type=int, default=1, help="Train repeats for other accepted speed samples.") + parser.add_argument( + "--focus-eval-csv", + type=Path, + help="Optional runtime event CSV; only reviewed rows where --focus-outcome is false are added.", + ) + parser.add_argument( + "--focus-outcome", + choices=("candidate_hit", "publish_hit"), + default="publish_hit", + help="Runtime outcome used to select hard positives from --focus-eval-csv.", + ) + parser.add_argument( + "--focus-train-only", + action="store_true", + help="Exclude selected records assigned to the route-level validation split.", + ) return parser.parse_args() @@ -90,18 +107,48 @@ def yolo_label(bbox: tuple[int, int, int, int], image_path: Path) -> str | None: return f"0 {values[0]:.8f} {values[1]:.8f} {values[2]:.8f} {values[3]:.8f}\n" -def reviewed_positive_rows(queue_path: Path, labels_path: Path) -> dict[str, dict[str, str]]: +def focus_record_keys(eval_path: Path, outcome: str) -> set[str]: + selected: set[str] = set() + with eval_path.expanduser().resolve().open(encoding="utf-8", newline="") as input_file: + reader = csv.DictReader(input_file) + if outcome not in (reader.fieldnames or ()): + raise ValueError(f"{eval_path} does not contain {outcome}") + for row in reader: + record_key = row.get("record_key", "") + outcome_value = row.get(outcome, "").strip().lower() + if record_key and outcome_value not in ("1", "true", "yes"): + selected.add(record_key) + return selected + + +def reviewed_positive_rows( + queue_path: Path, + labels_path: Path, + selected_record_keys: set[str] | None = None, +) -> dict[str, dict[str, str]]: return { row.get("record_key", ""): row for row in merged_review_rows(queue_path, labels_path) - if row.get("record_key") and row.get("review_status") in POSITIVE_STATUSES and parse_speed(row.get("review_speed_limit_mph", "")) + if ( + row.get("record_key") and + (selected_record_keys is None or row.get("record_key") in selected_record_keys) and + row.get("review_status") in POSITIVE_STATUSES and + parse_speed(row.get("review_speed_limit_mph", "")) + ) } +def reviewed_row_for_track( + track_row: dict[str, str], + reviewed_rows: dict[str, dict[str, str]], +) -> dict[str, str] | None: + return reviewed_rows.get(track_row.get("track_key", "")) or reviewed_rows.get(track_row.get("source_record_key", "")) + + def trusted_track_row(row: dict[str, str], reviewed_row: dict[str, str], args: argparse.Namespace) -> bool: original_bbox = parse_bbox(reviewed_row.get("bbox", "")) corrected_bbox = parse_bbox(reviewed_row.get("review_bbox", "")) - if corrected_bbox is not None and corrected_bbox != original_bbox: + if corrected_bbox is not None and corrected_bbox != original_bbox and row.get("anchor_bbox_source") != "review_bbox": # Existing tracks predate manual box correction, so their propagated boxes are stale. return False try: @@ -110,13 +157,20 @@ def trusted_track_row(row: dict[str, str], reviewed_row: dict[str, str], args: a predicted = int(row.get("predicted_speed_limit_mph", "") or 0) read_confidence = float(row.get("read_confidence", "") or 0.0) detector_confidence = float(row.get("detector_confidence", "") or 0.0) + tracking_confidence = float(row.get("tracking_confidence", "") or 0.0) growth = float(row.get("area_ratio_to_anchor", "") or 0.0) rank = int(row.get("rank", "") or 999) except ValueError: return False exact_read = predicted == expected and read_confidence >= args.min_exact_confidence detector_snap = detector_confidence >= args.min_detector_confidence - return expected == reviewed_speed and growth >= args.min_growth and rank <= args.max_track_rank and (exact_read or detector_snap) + optical_flow_track = tracking_confidence >= args.min_tracking_confidence + return ( + expected == reviewed_speed and + growth >= args.min_growth and + rank <= args.max_track_rank and + (exact_read or detector_snap or optical_flow_track) + ) def add_sample( @@ -172,7 +226,7 @@ def add_track_samples( counts: Counter[str] = Counter() with args.track_samples.expanduser().resolve().open(encoding="utf-8", newline="") as input_file: for row in csv.DictReader(input_file): - reviewed_row = reviewed_rows.get(row.get("track_key", "")) + reviewed_row = reviewed_row_for_track(row, reviewed_rows) if reviewed_row is None or not trusted_track_row(row, reviewed_row, args): counts["track_rejected"] += 1 continue @@ -222,7 +276,14 @@ def main() -> int: (output / "images" / split).mkdir(parents=True, exist_ok=True) (output / "labels" / split).mkdir(parents=True, exist_ok=True) - reviewed_rows = reviewed_positive_rows(queue_path, labels_path) + selected_record_keys = focus_record_keys(args.focus_eval_csv, args.focus_outcome) if args.focus_eval_csv else None + reviewed_rows = reviewed_positive_rows(queue_path, labels_path, selected_record_keys) + if args.focus_train_only: + reviewed_rows = { + key: row + for key, row in reviewed_rows.items() + if split_for_key(row.get("route") or key, args.train_ratio) == "train" + } counts = add_reviewed_anchors(reviewed_rows, output, args) counts.update(add_track_samples(reviewed_rows, output, args)) dataset_yaml = write_dataset_yaml(args.base_yaml.expanduser().resolve(), output) @@ -230,6 +291,9 @@ def main() -> int: "accepted_source_records": len(reviewed_rows), "base_yaml": str(args.base_yaml.expanduser().resolve()), "dataset_yaml": str(dataset_yaml), + "focus_eval_csv": str(args.focus_eval_csv.expanduser().resolve()) if args.focus_eval_csv else None, + "focus_outcome": args.focus_outcome if args.focus_eval_csv else None, + "focus_train_only": args.focus_train_only, "output": str(output), "counts": dict(sorted(counts.items())), } diff --git a/scripts/speed_limit_vision/evaluate_reviewed_route_events.py b/scripts/speed_limit_vision/evaluate_reviewed_route_events.py index 54ae2091e..b3a06404b 100644 --- a/scripts/speed_limit_vision/evaluate_reviewed_route_events.py +++ b/scripts/speed_limit_vision/evaluate_reviewed_route_events.py @@ -55,7 +55,9 @@ def parse_args() -> argparse.Namespace: parser.add_argument("--classifier-expansion-indices", help="Comma-separated detector crop expansion indices to evaluate.") parser.add_argument("--strong-rescue-min-proposal-confidence", type=float, help="Override single-frame tiny-sign proposal confidence.") parser.add_argument("--strong-rescue-min-read-confidence", type=float, help="Override single-frame tiny-sign classifier confidence.") + parser.add_argument("--strong-rescue-min-support", type=int, help="Override agreeing crops required for tiny-sign strong rescue.") parser.add_argument("--low-speed-change-consistent-detections", type=int, help="Override reads required to change from 30+ mph to below 30 mph.") + parser.add_argument("--low-speed-change-min-confidence", type=float, help="Override confidence required to change from 30+ mph to below 30 mph.") parser.add_argument( "--allow-low-speed-strong-consensus", action="store_true", @@ -88,6 +90,7 @@ def parse_args() -> argparse.Namespace: 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.") + parser.add_argument("--change-single-read-min-confidence", type=float, help="Override confidence for a one-read speed change.") parser.add_argument("--max-cases", type=int, default=0, help="Optional evaluation cap after deduplication.") return parser.parse_args() @@ -206,8 +209,14 @@ def main() -> int: slv.DETECTOR_CLASSIFIER_STRONG_RESCUE_MIN_PROPOSAL_CONFIDENCE = args.strong_rescue_min_proposal_confidence if args.strong_rescue_min_read_confidence is not None: slv.DETECTOR_CLASSIFIER_STRONG_RESCUE_MIN_READ_CONFIDENCE = args.strong_rescue_min_read_confidence + if args.strong_rescue_min_support is not None: + if args.strong_rescue_min_support < 1: + raise ValueError("--strong-rescue-min-support must be at least 1") + slv.DETECTOR_CLASSIFIER_STRONG_RESCUE_MIN_SUPPORT = args.strong_rescue_min_support if args.low_speed_change_consistent_detections is not None: slv.LOW_SPEED_CHANGE_CONSISTENT_DETECTIONS = args.low_speed_change_consistent_detections + if args.low_speed_change_min_confidence is not None: + slv.LOW_SPEED_CHANGE_MIN_CONFIDENCE = args.low_speed_change_min_confidence if args.allow_low_speed_strong_consensus: slv.LOW_SPEED_CHANGE_ALLOW_STRONG_CONSENSUS = True if args.enable_strong_model_consensus: @@ -226,6 +235,8 @@ def main() -> int: slv.CONSISTENT_DETECTIONS = args.consistent_detections if args.change_consistent_detections is not None: slv.CHANGE_CONSISTENT_DETECTIONS = args.change_consistent_detections + 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.route_file: selected_routes = { @@ -322,6 +333,8 @@ def main() -> int: "measured_classifier_forward_seconds": args.measured_classifier_forward_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, + "change_single_read_min_confidence": slv.CHANGE_SINGLE_READ_MIN_CONFIDENCE, "low_speed_change_allow_strong_consensus": slv.LOW_SPEED_CHANGE_ALLOW_STRONG_CONSENSUS, "strong_model_consensus_enabled": slv.DETECTOR_CLASSIFIER_STRONG_MODEL_CONSENSUS_ENABLED, "strong_model_min_proposal_confidence": slv.DETECTOR_CLASSIFIER_STRONG_MODEL_MIN_PROPOSAL_CONFIDENCE, diff --git a/scripts/speed_limit_vision/mine_reviewed_sign_tracks.py b/scripts/speed_limit_vision/mine_reviewed_sign_tracks.py index d07ab39bd..ae04497e5 100644 --- a/scripts/speed_limit_vision/mine_reviewed_sign_tracks.py +++ b/scripts/speed_limit_vision/mine_reviewed_sign_tracks.py @@ -6,6 +6,7 @@ import csv import hashlib import math +from collections import deque from dataclasses import dataclass from pathlib import Path @@ -35,6 +36,7 @@ class TrackCase: frame_time_s: float expected_speed_mph: int anchor_bbox: tuple[int, int, int, int] + anchor_bbox_source: str @dataclass(frozen=True) @@ -43,6 +45,7 @@ class TrackSample: bbox: tuple[int, int, int, int] crop_bbox: tuple[int, int, int, int] detector_confidence: float + tracking_confidence: float predicted_speed_mph: int read_confidence: float sharpness: float @@ -54,17 +57,20 @@ class TrackSample: def parse_args() -> argparse.Namespace: - parser = argparse.ArgumentParser(description="Track human-reviewed signs into later, larger route frames.") + parser = argparse.ArgumentParser(description="Track human-reviewed signs into nearby route frames.") parser.add_argument("--queue", type=Path, required=True, help="Reviewed manual_review_queue.csv.") parser.add_argument("--labels", type=Path, help="Defaults to manual_review_labels.csv beside the queue.") parser.add_argument("--models-dir", type=Path, default=Path("starpilot/assets/vision_models")) parser.add_argument("--output-dir", type=Path, required=True) + parser.add_argument("--window-before", type=float, default=0.0, help="Seconds to track backward before the reviewed frame.") parser.add_argument("--window-after", type=float, default=2.5, help="Seconds to track after the reviewed anchor frame.") parser.add_argument("--sample-interval", type=float, default=0.10, help="Minimum spacing between ranked samples.") parser.add_argument("--detector-interval", type=float, default=0.20, help="How often to snap optical flow to detector proposals.") parser.add_argument("--max-samples-per-track", type=int, default=4) + parser.add_argument("--max-samples-before-track", type=int, default=4) parser.add_argument("--dedupe-seconds", type=float, default=3.0) parser.add_argument("--min-area-growth", type=float, default=0.85) + parser.add_argument("--min-backward-area-ratio", type=float, default=0.20) parser.add_argument("--limit", type=int, default=0) return parser.parse_args() @@ -200,6 +206,70 @@ def encode_jpeg(image: np.ndarray, quality: int = 90) -> bytes: return encoded.tobytes() +def ranked_samples(candidates: list[TrackSample], limit: int, sample_interval: float) -> list[TrackSample]: + selected: list[TrackSample] = [] + for candidate in sorted(candidates, key=lambda item: item.score, reverse=True): + if any(abs(candidate.time_s - kept.time_s) < max(sample_interval * 1.5, 0.12) for kept in selected): + continue + selected.append(candidate) + if len(selected) >= limit: + break + return selected + + +def make_sample( + case: TrackCase, + daemon: slv.SpeedLimitVisionDaemon, + frame: np.ndarray, + bbox: tuple[int, int, int, int], + detector_confidence: float, + tracking_confidence: float, + anchor_area: int, + time_s: float, + time_distance_s: float, + backward: bool, +) -> TrackSample | None: + height, width = frame.shape[:2] + crop_box = expanded_bbox(bbox, width, height) + x1, y1, x2, y2 = crop_box + crop = frame[y1:y2, x1:x2] + if crop.size == 0: + return None + read = daemon._classify_speed_limit_from_model(crop) + predicted_speed = int(read[0]) if read is not None else 0 + read_confidence = float(read[1]) if read is not None else 0.0 + gray_crop = cv2.cvtColor(crop, cv2.COLOR_BGR2GRAY) + sharpness = float(cv2.Laplacian(gray_crop, cv2.CV_64F).var()) + brightness = float(gray_crop.mean()) + growth = bbox_area(bbox) / anchor_area + growth_score = math.log2(max(growth, 0.25)) * (0.15 if backward else 0.55) + exact_bonus = read_confidence * 2.0 if predicted_speed == case.expected_speed_mph else 0.0 + wrong_penalty = read_confidence * 1.5 if predicted_speed and predicted_speed != case.expected_speed_mph else 0.0 + score = ( + growth_score + + min(sharpness / 180.0, 1.0) * 0.35 + + detector_confidence * 0.45 + + tracking_confidence * 0.20 + + exact_bonus - wrong_penalty + + min(time_distance_s, 1.5) * (0.04 if backward else 0.08) + ) + return TrackSample( + time_s=time_s, + bbox=bbox, + crop_bbox=crop_box, + detector_confidence=detector_confidence, + tracking_confidence=tracking_confidence, + predicted_speed_mph=predicted_speed, + read_confidence=read_confidence, + sharpness=sharpness, + brightness=brightness, + area_ratio_to_anchor=growth, + score=score, + frame_jpeg=encode_jpeg(frame), + crop_jpeg=encode_jpeg(crop, 95), + ) + + def load_cases(queue_path: Path, labels_path: Path, dedupe_seconds: float) -> tuple[list[TrackCase], list[str]]: with queue_path.open(encoding="utf-8", newline="") as queue_file: fieldnames = list(csv.DictReader(queue_file).fieldnames or ()) @@ -210,7 +280,8 @@ def load_cases(queue_path: Path, labels_path: Path, dedupe_seconds: float) -> tu if row.get("review_status") not in POSITIVE_STATUSES: continue speed = parse_speed(row.get("review_speed_limit_mph", "")) - bbox = parse_bbox(row.get("review_bbox") or row.get("bbox", "")) + reviewed_bbox = parse_bbox(row.get("review_bbox", "")) + bbox = reviewed_bbox or parse_bbox(row.get("bbox", "")) try: frame_time_s = float(row.get("frame_time_s", "")) segment = int(row.get("segment", "")) @@ -225,18 +296,96 @@ def load_cases(queue_path: Path, labels_path: Path, dedupe_seconds: float) -> tu continue seen.add(dedupe_key) digest = hashlib.sha1(f"{row.get('record_key')}:{speed}".encode()).hexdigest()[:16] - cases.append(TrackCase(row, f"sign_track_{digest}", video_path.resolve(), frame_time_s, speed, bbox)) + cases.append(TrackCase( + row, + f"sign_track_{digest}", + video_path.resolve(), + frame_time_s, + speed, + bbox, + "review_bbox" if reviewed_bbox is not None else "bbox", + )) return cases, fieldnames +def mine_backward_samples( + case: TrackCase, + daemon: slv.SpeedLimitVisionDaemon, + args: argparse.Namespace, + frames: list[tuple[int, np.ndarray]], + anchor_frame: np.ndarray, + anchor_frame_index: int, + fps: float, + anchor_bbox: tuple[int, int, int, int], + anchor_area: int, +) -> list[TrackSample]: + if not frames or args.max_samples_before_track <= 0: + return [] + previous_gray = cv2.cvtColor(anchor_frame, cv2.COLOR_BGR2GRAY) + bbox = anchor_bbox + points = feature_points(previous_gray, bbox) + next_sample_elapsed = args.sample_interval + next_detector_elapsed = 0.0 + candidates: list[TrackSample] = [] + height, width = anchor_frame.shape[:2] + + for frame_index, frame in reversed(frames): + elapsed = (anchor_frame_index - frame_index) / fps + current_gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY) + bbox, points = flow_bbox(previous_gray, current_gray, bbox, points) + previous_gray = current_gray + if bbox is None: + break + x1, y1, x2, y2 = bbox + growth = bbox_area(bbox) / anchor_area + if growth < args.min_backward_area_ratio or growth > 1.35 or x1 <= 0 or y1 <= 0 or x2 >= width or y2 >= height: + break + detector_confidence = 0.0 + if elapsed + 1e-6 >= next_detector_elapsed: + proposal = matching_proposal(daemon, frame, bbox) + next_detector_elapsed = elapsed + args.detector_interval + if proposal is not None: + detector_confidence, proposal_bbox = proposal + bbox = proposal_bbox + points = feature_points(previous_gray, bbox) + growth = bbox_area(bbox) / anchor_area + if elapsed + 1e-6 < next_sample_elapsed: + continue + next_sample_elapsed = elapsed + args.sample_interval + tracking_confidence = min(len(points) / 12.0, 1.0) if points is not None else 0.0 + sample = make_sample( + case, + daemon, + frame, + bbox, + detector_confidence, + tracking_confidence, + anchor_area, + frame_index / fps, + elapsed, + backward=True, + ) + if sample is not None: + candidates.append(sample) + return ranked_samples(candidates, args.max_samples_before_track, args.sample_interval) + + def mine_case(case: TrackCase, daemon: slv.SpeedLimitVisionDaemon, args: argparse.Namespace) -> list[TrackSample]: capture = cv2.VideoCapture(str(case.video_path)) fps = capture.get(cv2.CAP_PROP_FPS) or 20.0 anchor_frame_index = max(int(round(case.frame_time_s * fps)), 0) + before_frame_count = max(int(round(args.window_before * fps)), 0) + earlier_frames: deque[tuple[int, np.ndarray]] = deque(maxlen=before_frame_count) # Raw comma HEVC streams have no seek index. CAP_PROP_POS_FRAMES silently # returns frame zero, so advance sequentially to preserve timestamp alignment. - for _frame_index in range(anchor_frame_index): - if not capture.grab(): + for frame_index in range(anchor_frame_index): + if before_frame_count and frame_index >= anchor_frame_index - before_frame_count: + ok, frame = capture.read() + if not ok or frame is None: + capture.release() + return [] + earlier_frames.append((frame_index, frame)) + elif not capture.grab(): capture.release() return [] ok, anchor_frame = capture.read() @@ -250,6 +399,17 @@ def mine_case(case: TrackCase, daemon: slv.SpeedLimitVisionDaemon, args: argpars capture.release() return [] anchor_area = max(bbox_area(bbox), 1) + backward_samples = mine_backward_samples( + case, + daemon, + args, + list(earlier_frames), + anchor_frame, + anchor_frame_index, + fps, + bbox, + anchor_area, + ) previous_gray = cv2.cvtColor(anchor_frame, cv2.COLOR_BGR2GRAY) points = feature_points(previous_gray, bbox) next_sample_at = case.frame_time_s @@ -280,41 +440,23 @@ def mine_case(case: TrackCase, daemon: slv.SpeedLimitVisionDaemon, args: argpars points = feature_points(previous_gray, bbox) if time_s + 1e-6 >= next_sample_at: next_sample_at = time_s + args.sample_interval - crop_box = expanded_bbox(bbox, width, height) - x1, y1, x2, y2 = crop_box - crop = current_frame[y1:y2, x1:x2] - if crop.size: - read = daemon._classify_speed_limit_from_model(crop) - predicted_speed = int(read[0]) if read is not None else 0 - read_confidence = float(read[1]) if read is not None else 0.0 - gray_crop = cv2.cvtColor(crop, cv2.COLOR_BGR2GRAY) - sharpness = float(cv2.Laplacian(gray_crop, cv2.CV_64F).var()) - brightness = float(gray_crop.mean()) - growth = bbox_area(bbox) / anchor_area - exact_bonus = read_confidence * 2.0 if predicted_speed == case.expected_speed_mph else 0.0 - wrong_penalty = read_confidence * 1.5 if predicted_speed and predicted_speed != case.expected_speed_mph else 0.0 - score = ( - math.log2(max(growth, 0.25)) * 0.55 + - min(sharpness / 180.0, 1.0) * 0.35 + - detector_confidence * 0.45 + - exact_bonus - wrong_penalty + - min(max(time_s - case.frame_time_s, 0.0), 1.5) * 0.08 + growth = bbox_area(bbox) / anchor_area + if growth >= args.min_area_growth: + tracking_confidence = min(len(points) / 12.0, 1.0) if points is not None else 0.0 + sample = make_sample( + case, + daemon, + current_frame, + bbox, + detector_confidence, + tracking_confidence, + anchor_area, + time_s, + max(time_s - case.frame_time_s, 0.0), + backward=False, ) - if growth >= args.min_area_growth: - candidates.append(TrackSample( - time_s=time_s, - bbox=bbox, - crop_bbox=crop_box, - detector_confidence=detector_confidence, - predicted_speed_mph=predicted_speed, - read_confidence=read_confidence, - sharpness=sharpness, - brightness=brightness, - area_ratio_to_anchor=growth, - score=score, - frame_jpeg=encode_jpeg(current_frame), - crop_jpeg=encode_jpeg(crop, 95), - )) + if sample is not None: + candidates.append(sample) if current_index >= end_frame_index: break ok, current_frame = capture.read() @@ -323,14 +465,8 @@ def mine_case(case: TrackCase, daemon: slv.SpeedLimitVisionDaemon, args: argpars current_index += 1 capture.release() - selected: list[TrackSample] = [] - for candidate in sorted(candidates, key=lambda item: item.score, reverse=True): - if any(abs(candidate.time_s - kept.time_s) < max(args.sample_interval * 1.5, 0.12) for kept in selected): - continue - selected.append(candidate) - if len(selected) >= args.max_samples_per_track: - break - return selected + selected = ranked_samples(candidates, args.max_samples_per_track, args.sample_interval) + return [*backward_samples, *selected] def main() -> int: @@ -371,9 +507,11 @@ def main() -> int: "frame_path": str(frame_path), "crop_path": str(crop_path), "source_video_path": str(case.video_path), + "anchor_bbox_source": case.anchor_bbox_source, "bbox": ",".join(str(value) for value in sample.bbox), "crop_bbox": ",".join(str(value) for value in sample.crop_bbox), "detector_confidence": f"{sample.detector_confidence:.6f}", + "tracking_confidence": f"{sample.tracking_confidence:.6f}", "predicted_speed_limit_mph": str(sample.predicted_speed_mph or ""), "read_confidence": f"{sample.read_confidence:.6f}", "sharpness": f"{sample.sharpness:.3f}", diff --git a/scripts/speed_limit_vision/test_track_proposal_detector_dataset.py b/scripts/speed_limit_vision/test_track_proposal_detector_dataset.py index c33d0a587..2e5907249 100644 --- a/scripts/speed_limit_vision/test_track_proposal_detector_dataset.py +++ b/scripts/speed_limit_vision/test_track_proposal_detector_dataset.py @@ -18,16 +18,60 @@ def load_local_module(name: str): track_dataset = load_local_module("build_track_proposal_detector_dataset") split_for_key = track_dataset.split_for_key trusted_track_row = track_dataset.trusted_track_row +focus_record_keys = track_dataset.focus_record_keys +reviewed_row_for_track = track_dataset.reviewed_row_for_track def args() -> Namespace: - return Namespace(min_exact_confidence=0.8, min_detector_confidence=0.3, min_growth=1.0, max_track_rank=4) + return Namespace( + min_exact_confidence=0.8, + min_detector_confidence=0.3, + min_tracking_confidence=1.01, + min_growth=1.0, + max_track_rank=4, + ) def test_split_for_key_keeps_route_samples_together(): assert split_for_key("route-a", 0.85) == split_for_key("route-a", 0.85) +def test_focus_record_keys_selects_failed_outcomes(tmp_path): + eval_path = tmp_path / "runtime.csv" + eval_path.write_text( + "\n".join(( + "record_key,candidate_hit,publish_hit", + "published,true,true", + "single-read,true,false", + "missed,false,false", + "", + )), + encoding="ascii", + ) + + assert focus_record_keys(eval_path, "publish_hit") == {"single-read", "missed"} + assert focus_record_keys(eval_path, "candidate_hit") == {"missed"} + + +def test_focus_record_keys_rejects_missing_outcome(tmp_path): + eval_path = tmp_path / "runtime.csv" + eval_path.write_text("record_key,publish_hit\nmissed,false\n", encoding="ascii") + + try: + focus_record_keys(eval_path, "candidate_hit") + except ValueError as exc: + assert "candidate_hit" in str(exc) + else: + raise AssertionError("missing focus outcome was accepted") + + +def test_fresh_track_source_key_can_resolve_reviewed_row(): + reviewed = {"source-review": {"review_speed_limit_mph": "35"}} + track = {"track_key": "derived-track", "source_record_key": "source-review"} + + assert reviewed_row_for_track(track, reviewed) == reviewed["source-review"] + + def test_trusted_track_requires_current_review_speed_to_match(): track = { "expected_speed_limit_mph": "45", @@ -53,6 +97,29 @@ def test_trusted_track_accepts_detector_snap_without_classifier_read(): assert trusted_track_row(track, {"review_speed_limit_mph": "35"}, args()) +def test_trusted_track_accepts_confident_optical_flow_from_corrected_anchor(): + track = { + "expected_speed_limit_mph": "35", + "predicted_speed_limit_mph": "", + "read_confidence": "", + "detector_confidence": "0.0", + "tracking_confidence": "0.9", + "area_ratio_to_anchor": "0.5", + "rank": "2", + "anchor_bbox_source": "review_bbox", + } + review = { + "review_speed_limit_mph": "35", + "bbox": "10,10,40,50", + "review_bbox": "12,12,35,45", + } + options = args() + options.min_tracking_confidence = 0.8 + options.min_growth = 0.3 + + assert trusted_track_row(track, review, options) + + def test_trusted_track_rejects_low_growth_and_rank(): track = { "expected_speed_limit_mph": "35", diff --git a/selfdrive/controls/lib/latcontrol_torque.py b/selfdrive/controls/lib/latcontrol_torque.py index 9d9bc030f..f103e5e15 100644 --- a/selfdrive/controls/lib/latcontrol_torque.py +++ b/selfdrive/controls/lib/latcontrol_torque.py @@ -158,7 +158,10 @@ class LatControlTorque(LatControl): def update(self, active, CS, VM, params, steer_limited_by_safety, desired_curvature, curvature_limited, lat_delay, calibrated_pose, model_data, starpilot_toggles): pid_log = log.ControlsState.LateralTorqueState.new_message() pid_log.version = VERSION - set_ftm_runtime_overrides(getattr(starpilot_toggles, "ftm_active_overrides", None)) + ftm_profile_active = bool(getattr(starpilot_toggles, "ftm_trial_applied", False) and + getattr(starpilot_toggles, "ftm_active_profile_id", "")) + set_ftm_runtime_overrides(getattr(starpilot_toggles, "ftm_active_overrides", None) if ftm_profile_active else None) + ftm_surface_active = ftm_profile_active and ftm_runtime_overrides_active() if not active: output_torque = 0.0 pid_log.active = False @@ -326,7 +329,7 @@ class LatControlTorque(LatControl): friction_threshold = CIVIC_BOSCH_MODIFIED_B_FIXED_FRICTION_THRESHOLD friction_scale = get_civic_bosch_modified_b_friction_scale(CS.vEgo, setpoint, desired_lateral_jerk) friction_scale = 1.0 + ((friction_scale - 1.0) * civic_bosch_modified_a_center_taper) - if self.ftm_surface_profile_key and not ioniq_6_active: + if ftm_surface_active and self.ftm_surface_profile_key and not ioniq_6_active: universal_ftm_profile = self.ftm_surface_profile_key == FTM_UNIVERSAL_PROFILE_KEY ftm_full_surface_center_taper = get_ftm_full_surface_center_taper_scale(self.ftm_surface_profile_key, setpoint, CS.vEgo, include_base_center=universal_ftm_profile) @@ -364,7 +367,7 @@ class LatControlTorque(LatControl): actual_angle_no_offset = CS.steeringAngleDeg - params.angleOffsetDeg output_torque = get_ioniq_6_low_speed_angle_assist_torque(desired_angle_no_offset, actual_angle_no_offset, output_torque, CS.vEgo) - elif self.ftm_surface_profile_key and not CS.steeringPressed: + elif ftm_surface_active and self.ftm_surface_profile_key and not CS.steeringPressed: desired_angle_no_offset = math.degrees(VM.get_steer_from_curvature(-desired_curvature, CS.vEgo, params.roll)) actual_angle_no_offset = CS.steeringAngleDeg - params.angleOffsetDeg output_torque = get_ftm_full_surface_low_speed_angle_assist_torque(self.ftm_surface_profile_key, desired_angle_no_offset, diff --git a/selfdrive/controls/lib/latcontrol_vehicle_tunes.py b/selfdrive/controls/lib/latcontrol_vehicle_tunes.py index ae709839d..ee50a53e2 100644 --- a/selfdrive/controls/lib/latcontrol_vehicle_tunes.py +++ b/selfdrive/controls/lib/latcontrol_vehicle_tunes.py @@ -796,6 +796,10 @@ def get_ftm_runtime_overrides() -> dict: return _ftm_copy_json(_FTM_ACTIVE_OVERRIDES) if _FTM_ACTIVE_OVERRIDES else {} +def ftm_runtime_overrides_active() -> bool: + return bool(_FTM_ACTIVE_OVERRIDES) + + def _ftm_base_friction_threshold(family: str, v_ego: float, default_fn) -> float: payload = _FTM_ACTIVE_OVERRIDES.get("baseFrictionThresholds", {}).get(family, {}) values = payload.get("values", []) diff --git a/selfdrive/controls/tests/test_latcontrol.py b/selfdrive/controls/tests/test_latcontrol.py index 274627f0d..867c2afd2 100644 --- a/selfdrive/controls/tests/test_latcontrol.py +++ b/selfdrive/controls/tests/test_latcontrol.py @@ -300,6 +300,44 @@ class TestLatControl: finally: clear_ftm_runtime_overrides() + @pytest.mark.parametrize(("trial_applied", "profile_id"), [(False, "profile"), (True, "")]) + def test_ftm_surface_helpers_require_active_trial(self, monkeypatch, trial_applied, profile_id): + controller, VM, CS, params, starpilot_toggles = self._build_torque_controller(GM.CHEVROLET_BOLT_ACC_2022_2023) + starpilot_toggles.ftm_trial_applied = trial_applied + starpilot_toggles.ftm_active_profile_id = profile_id + starpilot_toggles.ftm_active_overrides = { + "vehicleKnobs": {"gm_bolt_2022_2023.highway_center_taper_max": 0.10}, + } + + def fail_if_called(*_args, **_kwargs): + raise AssertionError("inactive FTM trial reached the runtime shaping path") + + monkeypatch.setattr(latcontrol_torque, "get_ftm_full_surface_ff_scale", fail_if_called) + controller.update(True, CS, VM, params, False, 0.0025, False, 0.2, None, None, starpilot_toggles) + + assert get_ftm_runtime_overrides() == {} + + def test_ftm_surface_helpers_run_for_active_trial(self, monkeypatch): + controller, VM, CS, params, starpilot_toggles = self._build_torque_controller(GM.CHEVROLET_BOLT_ACC_2022_2023) + starpilot_toggles.ftm_trial_applied = True + starpilot_toggles.ftm_active_profile_id = "report:cleanup:recommended" + starpilot_toggles.ftm_active_overrides = { + "vehicleKnobs": {"gm_bolt_2022_2023.highway_center_taper_max": 0.10}, + } + calls = [] + + def record_call(*_args, **_kwargs): + calls.append(True) + return 1.0 + + monkeypatch.setattr(latcontrol_torque, "get_ftm_full_surface_ff_scale", record_call) + try: + controller.update(True, CS, VM, params, False, 0.0025, False, 0.2, None, None, starpilot_toggles) + assert calls + assert get_ftm_runtime_overrides()["vehicleKnobs"]["gm_bolt_2022_2023.highway_center_taper_max"] == pytest.approx(0.10) + finally: + clear_ftm_runtime_overrides() + def test_sonata_hybrid_center_taper_curve(self): assert get_sonata_hybrid_center_taper_scale(0.0, 30.0) < get_sonata_hybrid_center_taper_scale(0.0, 15.0) assert get_sonata_hybrid_center_taper_scale(0.0, 3.0) < get_sonata_hybrid_center_taper_scale(0.0, 10.0) diff --git a/starpilot/system/speed_limit_vision.py b/starpilot/system/speed_limit_vision.py index 4cf5ebbd9..f21b88e67 100644 --- a/starpilot/system/speed_limit_vision.py +++ b/starpilot/system/speed_limit_vision.py @@ -43,6 +43,7 @@ HISTORY_SECONDS = 2.0 CONSISTENT_DETECTIONS = 2 # These counts must remain achievable at the measured 1.5 Hz onroad cadence. CHANGE_CONSISTENT_DETECTIONS = 2 +CHANGE_SINGLE_READ_MIN_CONFIDENCE = 0.83 LOW_SPEED_CHANGE_CONSISTENT_DETECTIONS = 2 LOW_SPEED_CHANGE_MIN_CONFIDENCE = 0.90 LOW_SPEED_CHANGE_ALLOW_STRONG_CONSENSUS = True @@ -1903,13 +1904,18 @@ class SpeedLimitVisionDaemon: if current_speed_limit > 0 and candidate_speed_limit != current_speed_limit: required_count = CHANGE_CONSISTENT_DETECTIONS - allow_single_frame_consensus = has_strong_consensus + allow_single_frame_confirmation = ( + has_strong_consensus or best_confidence >= CHANGE_SINGLE_READ_MIN_CONFIDENCE + ) if current_speed_limit >= 30 and candidate_speed_limit < 30: required_count = LOW_SPEED_CHANGE_CONSISTENT_DETECTIONS - allow_single_frame_consensus = has_strong_consensus and LOW_SPEED_CHANGE_ALLOW_STRONG_CONSENSUS + allow_single_frame_confirmation = ( + best_confidence >= CHANGE_SINGLE_READ_MIN_CONFIDENCE or + (has_strong_consensus and LOW_SPEED_CHANGE_ALLOW_STRONG_CONSENSUS) + ) if best_confidence < LOW_SPEED_CHANGE_MIN_CONFIDENCE: return None - if candidate_count < required_count and not allow_single_frame_consensus: + if candidate_count < required_count and not allow_single_frame_confirmation: return None if candidate_count <= current_count: return None diff --git a/starpilot/system/tests/test_speed_limit_vision.py b/starpilot/system/tests/test_speed_limit_vision.py index be62f5ef9..b6f2f8d81 100644 --- a/starpilot/system/tests/test_speed_limit_vision.py +++ b/starpilot/system/tests/test_speed_limit_vision.py @@ -14,12 +14,17 @@ def daemon_with_history(current_speed, entries): return daemon -def test_speed_change_requires_two_matching_reads(): - daemon = daemon_with_history(40, [(55, 0.95)]) +def test_speed_change_requires_two_matching_reads_below_single_read_threshold(): + daemon = daemon_with_history(40, [(55, 0.82)]) assert daemon._confirm_detection() is None daemon.history.append(HistoryEntry(55, 0.76, 1.0)) - assert daemon._confirm_detection() == pytest.approx((55, 0.95)) + assert daemon._confirm_detection() == pytest.approx((55, 0.82)) + + +def test_speed_change_accepts_single_high_confidence_read(): + daemon = daemon_with_history(40, [(55, 0.84)]) + assert daemon._confirm_detection() == pytest.approx((55, 0.84)) def test_speed_change_accepts_single_strong_consensus_read(): @@ -28,14 +33,19 @@ def test_speed_change_accepts_single_strong_consensus_read(): assert daemon._confirm_detection() == pytest.approx((60, 0.74)) -def test_low_speed_change_requires_two_high_confidence_reads(): - daemon = daemon_with_history(40, [(25, 0.95)]) +def test_low_speed_change_requires_two_reads_below_low_speed_threshold(): + daemon = daemon_with_history(40, [(25, 0.89)]) assert daemon._confirm_detection() is None daemon.history.append(HistoryEntry(25, 0.96, 1.0)) assert daemon._confirm_detection() == pytest.approx((25, 0.96)) +def test_low_speed_change_accepts_single_high_confidence_read(): + daemon = daemon_with_history(40, [(25, 0.91)]) + assert daemon._confirm_detection() == pytest.approx((25, 0.91)) + + def test_low_speed_change_accepts_single_strong_consensus_read(): daemon = daemon_with_history(40, []) daemon.history.append(HistoryEntry(25, 0.95, 1.0, strong_consensus=True))