From 30786472cbc761c54678e7fe401c7d2d6ad039ed Mon Sep 17 00:00:00 2001 From: firestar5683 <168790843+firestar5683@users.noreply.github.com> Date: Tue, 30 Jun 2026 16:14:18 -0500 Subject: [PATCH] i'm 13 and this is --- .../opendbc/car/hyundai/carcontroller.py | 20 +- .../opendbc/car/hyundai/tests/test_hyundai.py | 15 +- scripts/model_compiler.py | 19 +- ...rt_localized_bookmark_detector_examples.py | 3 +- .../ingest_route_bundles.py | 358 ++++++++++ .../mine_connect_route_bookmarks.py | 6 +- .../mine_route_training_samples.py | 648 ++++++++++++++++++ .../rebalance_detector_dataset.py | 29 +- selfdrive/modeld/compile_modeld.py | 118 +++- selfdrive/modeld/modeld.py | 34 +- 10 files changed, 1206 insertions(+), 44 deletions(-) create mode 100644 scripts/speed_limit_vision/ingest_route_bundles.py create mode 100644 scripts/speed_limit_vision/mine_route_training_samples.py diff --git a/opendbc_repo/opendbc/car/hyundai/carcontroller.py b/opendbc_repo/opendbc/car/hyundai/carcontroller.py index 6fe5cdf05..de9481dad 100644 --- a/opendbc_repo/opendbc/car/hyundai/carcontroller.py +++ b/opendbc_repo/opendbc/car/hyundai/carcontroller.py @@ -69,8 +69,8 @@ DEFAULT_ANGLE_SMOOTHING_ALPHA_V = [0.2, 0.1, 0.0] EV9_HIGH_ANGLE_GAIN_BP = [70.0, 120.0, 220.0, 320.0] EV9_HIGH_ANGLE_GAIN_CAP_V = [0.85, 0.55, 0.30, 0.16] EV9_HIGH_ANGLE_GAIN_MIN = 0.004 -EV9_DRIVER_OVERRIDE_GAIN_BP = [125.0, 250.0, 375.0] -EV9_DRIVER_OVERRIDE_GAIN_CAP_V = [0.70, 0.12, 0.0] +EV9_DRIVER_OVERRIDE_GAIN_BP = [175.0, 350.0, 525.0] +EV9_DRIVER_OVERRIDE_GAIN_CAP_V = [0.70, 0.20, 0.04] def egmp_dynamic_longitudinal_tuning(CP) -> bool: @@ -264,6 +264,11 @@ def apply_ev9_high_angle_gain_cap(CP, gain: float, steering_angle_deg: float, la return gain +def ev9_driver_override_active(CP, steering_torque: float, steering_pressed: bool, lat_active: bool) -> bool: + return CP.carFingerprint == CAR.KIA_EV9 and CP.flags & HyundaiFlags.CANFD_ANGLE_STEERING and lat_active and \ + (steering_pressed or abs(steering_torque) >= EV9_DRIVER_OVERRIDE_GAIN_BP[0]) + + def process_hud_alert(enabled, fingerprint, hud_control): sys_warning = (hud_control.visualAlert in (VisualAlert.steerRequired, VisualAlert.ldw)) @@ -408,9 +413,16 @@ class CarController(CarControllerBase): desired_angle = float(np.clip(actuators.steeringAngleDeg, -self.params.ANGLE_LIMITS.STEER_ANGLE_MAX, self.params.ANGLE_LIMITS.STEER_ANGLE_MAX)) + ev9_driver_override = ev9_driver_override_active(self.CP, CS.out.steeringTorque, CS.out.steeringPressed, CC.latActive) - self.angle_filter.update_alpha(get_angle_smoothing_alpha(self.CP, CS.out.vEgo)) - desired_angle = self.angle_filter.update(desired_angle) + if ev9_driver_override: + desired_angle = float(np.clip(CS.out.steeringAngleDeg, + -self.params.ANGLE_LIMITS.STEER_ANGLE_MAX, + self.params.ANGLE_LIMITS.STEER_ANGLE_MAX)) + self.angle_filter.x = desired_angle + else: + self.angle_filter.update_alpha(get_angle_smoothing_alpha(self.CP, CS.out.vEgo)) + desired_angle = self.angle_filter.update(desired_angle) apply_angle = apply_steer_angle_limits_vm(desired_angle, self.apply_angle_last, v_ego_raw, CS.out.steeringAngleDeg, CC.latActive, self.params, self.VM) diff --git a/opendbc_repo/opendbc/car/hyundai/tests/test_hyundai.py b/opendbc_repo/opendbc/car/hyundai/tests/test_hyundai.py index ba6b6dea8..0c3477df5 100644 --- a/opendbc_repo/opendbc/car/hyundai/tests/test_hyundai.py +++ b/opendbc_repo/opendbc/car/hyundai/tests/test_hyundai.py @@ -11,7 +11,7 @@ from opendbc.car.hyundai.carcontroller import CarController, Ioniq6LongitudinalT update_ioniq_6_longitudinal_tuning, \ update_genesis_g90_longitudinal_tuning, egmp_dynamic_longitudinal_tuning, \ should_reset_ev6_gt_line_longitudinal_tuning, reset_ev6_gt_line_longitudinal_tuning, \ - get_angle_smoothing_alpha, apply_ev9_high_angle_gain_cap + get_angle_smoothing_alpha, apply_ev9_high_angle_gain_cap, ev9_driver_override_active from opendbc.car.hyundai.carstate import CarState, decode_canfd_camera_lead, decode_ioniq_6_blindspot_radar_state from opendbc.car.hyundai.interface import CarInterface from opendbc.car.hyundai import hyundaican, hyundaicanfd @@ -297,11 +297,20 @@ class TestHyundaiFingerprint: assert apply_ev9_high_angle_gain_cap(ev9_cp, 0.70, 320.0, True) == pytest.approx(0.16) assert apply_ev9_high_angle_gain_cap(ev9_cp, 0.0, 320.0, True) > 0.0 assert apply_ev9_high_angle_gain_cap(ev9_cp, 0.70, 320.0, False) == pytest.approx(0.70) - assert apply_ev9_high_angle_gain_cap(ev9_cp, 0.70, 30.0, True, 250.0, True) == pytest.approx(0.12) - assert apply_ev9_high_angle_gain_cap(ev9_cp, 0.70, 30.0, True, 400.0, True) == pytest.approx(0.0) + assert apply_ev9_high_angle_gain_cap(ev9_cp, 0.70, 30.0, True, 350.0, True) == pytest.approx(0.20) + assert apply_ev9_high_angle_gain_cap(ev9_cp, 0.70, 30.0, True, 600.0, True) == pytest.approx(0.04) assert apply_ev9_high_angle_gain_cap(sportage_cp, 0.70, 320.0, True) == pytest.approx(0.70) assert apply_ev9_high_angle_gain_cap(sportage_cp, 0.70, 30.0, True, 400.0, True) == pytest.approx(0.70) + def test_ev9_driver_override_detection_is_ev9_only(self): + ev9_cp = SimpleNamespace(carFingerprint=CAR.KIA_EV9, flags=int(HyundaiFlags.CANFD_ANGLE_STEERING)) + sportage_cp = SimpleNamespace(carFingerprint=CAR.KIA_SPORTAGE_HEV_2026, flags=int(HyundaiFlags.CANFD_ANGLE_STEERING)) + + assert ev9_driver_override_active(ev9_cp, 0.0, True, True) + assert ev9_driver_override_active(ev9_cp, 200.0, False, True) + assert not ev9_driver_override_active(ev9_cp, 200.0, False, False) + assert not ev9_driver_override_active(sportage_cp, 400.0, True, True) + def test_ev9_allows_lateral_at_standstill_without_changing_other_angle_platforms(self): ev9_cp = CarInterface.get_params(CAR.KIA_EV9, gen_empty_fingerprint(), [], False, False, False, None) sportage_cp = CarInterface.get_params(CAR.KIA_SPORTAGE_HEV_2026, gen_empty_fingerprint(), [], False, False, False, None) diff --git a/scripts/model_compiler.py b/scripts/model_compiler.py index 20f52454e..a5a54abe6 100644 --- a/scripts/model_compiler.py +++ b/scripts/model_compiler.py @@ -82,6 +82,12 @@ def parse_args() -> argparse.Namespace: parser.add_argument("--force", action="store_true", help="Accepted for compatibility; selected outputs are always replaced.") parser.add_argument("--split-artifact", type=Path, help="Split an existing oversized PKL without compiling.") parser.add_argument("--chunk-size-mib", type=int, default=95, help="Multipart size in MiB; must be below 100.") + parser.add_argument( + "--image-history-pipeline", + choices=("policy", "warp"), + default="policy", + help="Driving artifact ABI. 'policy' is the newer faster path; 'warp' reproduces legacy v22 artifacts.", + ) args, unknown = parser.parse_known_args() dynamic_flags = [value[2:] for value in unknown if value.startswith("--")] @@ -342,7 +348,14 @@ def split_oversized_artifact( return [*part_paths, checksum_path] -def compile_driving(model_key: str, files: dict[str, Path], input_format: str, version: str, output_dir: Path) -> Path: +def compile_driving( + model_key: str, + files: dict[str, Path], + input_format: str, + version: str, + output_dir: Path, + image_history_pipeline: str, +) -> Path: model_type, source_args = driving_compile_args(files, input_format) output_path = output_dir / f"{model_key}_driving_tinygrad.pkl" removed = remove_paths(sorted({ @@ -371,6 +384,8 @@ def compile_driving(model_key: str, files: dict[str, Path], input_format: str, v str(output_path), "--frame-skip", str(frame_skip), + "--image-history-pipeline", + image_history_pipeline, *source_args, ] if version: @@ -472,7 +487,7 @@ def main() -> int: version = "v15" version_label = version or "unspecified behavior" print(f"Compiling {model_key} ({input_format}, {version_label}) from {args.input_dir} -> {args.output_dir}") - output = compile_driving(model_key, files, input_format, version, args.output_dir) + output = compile_driving(model_key, files, input_format, version, args.output_dir, args.image_history_pipeline) print(f" saved {output.name}") multipart_outputs = split_oversized_artifact(output) if multipart_outputs: diff --git a/scripts/speed_limit_vision/import_localized_bookmark_detector_examples.py b/scripts/speed_limit_vision/import_localized_bookmark_detector_examples.py index c7be60c1d..ce54ccde1 100644 --- a/scripts/speed_limit_vision/import_localized_bookmark_detector_examples.py +++ b/scripts/speed_limit_vision/import_localized_bookmark_detector_examples.py @@ -187,7 +187,8 @@ def main() -> int: continue class_id = int(row["class_id"]) - stem_base = f"real_localized_{row['session_id']}_{int(row['bookmark_number']):03d}" + frame_stem = Path(row["frame_path"]).stem + stem_base = f"real_localized_{frame_stem}" source_video_path = Path(row["source_video_path"]) if row.get("source_video_path") else None relative_time_s = float(row["relative_time_s"]) if row.get("relative_time_s") else 0.0 diff --git a/scripts/speed_limit_vision/ingest_route_bundles.py b/scripts/speed_limit_vision/ingest_route_bundles.py new file mode 100644 index 000000000..75abbc31c --- /dev/null +++ b/scripts/speed_limit_vision/ingest_route_bundles.py @@ -0,0 +1,358 @@ +#!/usr/bin/env python3 +from __future__ import annotations + +import argparse +import csv +import json +import re +import shutil +import time +import zipfile + +from dataclasses import dataclass +from pathlib import Path + +if __package__ in (None, ""): + import sys + sys.path.insert(0, str(Path(__file__).resolve().parent)) + from common import ensure_dir, preferred_clip_root, preferred_files_manifest_path, preferred_qlog_mtimes_path, resolve_workspace # type: ignore +else: + from .common import ensure_dir, preferred_clip_root, preferred_files_manifest_path, preferred_qlog_mtimes_path, resolve_workspace + + +DEFAULT_ROUTES_DIR = Path("/Volumes/T5/routes") +DEFAULT_WORKSPACE = Path("/Volumes/T5/starpilot_speed_limit/workspace/speed_limit_training_clean") +DEFAULT_BUNDLE_MANIFEST_DIR = Path("/Volumes/T5/starpilot_speed_limit/analysis/route_bundles/manifests") +DEFAULT_STATE_DIR = Path("/Volumes/T5/starpilot_speed_limit/analysis/route_bundles/state") + +SEGMENT_FILE_RE = re.compile(r"/data/media/0/realdata/([^/]+)/(fcamera\.hevc|qlog\.zst|rlog\.zst|qlog\.bz2|rlog\.bz2)$") + + +@dataclass(frozen=True) +class BundleSummary: + zip_path: Path + bundle_name: str + route_id: str + dongle_id: str + log_id: str + size_bytes: int + expected_segments: int + segment_count: int + fcamera_count: int + qlog_count: int + rlog_count: int + git_commit: str + platform: str + start_time: str + is_public: str + maps_selected: str + vision_detection: str + auto_bookmark: str + + +def parse_args() -> argparse.Namespace: + parser = argparse.ArgumentParser(description="Inventory and extract StarPilot speed-limit route zip bundles into the SSD route corpus.") + parser.add_argument("--routes-dir", type=Path, default=DEFAULT_ROUTES_DIR, help="Directory containing speed_limit_route_bundle_*.zip files.") + parser.add_argument("--workspace", type=Path, default=DEFAULT_WORKSPACE, help="Training workspace root for review manifests.") + parser.add_argument("--clip-root", type=Path, default=preferred_clip_root(), help="Destination realdata root.") + parser.add_argument("--manifest-dir", type=Path, default=DEFAULT_BUNDLE_MANIFEST_DIR, help="Where bundle_manifest.json files are copied.") + parser.add_argument("--state-dir", type=Path, default=DEFAULT_STATE_DIR, help="Where extraction marker files are written.") + parser.add_argument("--inventory-out", type=Path, help="CSV inventory path. Defaults to /review/route_bundle_inventory.csv.") + parser.add_argument("--corpus-out", type=Path, help="CSV corpus path. Defaults to /review/connect_route_corpus.csv.") + parser.add_argument("--limit", type=int, default=0, help="Optional max number of bundles to process.") + parser.add_argument("--extract", action="store_true", help="Extract route files into --clip-root.") + parser.add_argument("--force", action="store_true", help="Re-extract files even when the bundle marker exists.") + parser.add_argument("--overwrite-files", action="store_true", help="Overwrite existing destination segment files.") + parser.add_argument("--dry-run", action="store_true", help="Inventory only; do not write extracted files or marker files.") + return parser.parse_args() + + +def route_parts(route_id: str, fallback_log_id: str = "") -> tuple[str, str]: + normalized = route_id.replace("|", "/") + if "/" in normalized: + dongle_id, log_id = normalized.split("/", 1) + return dongle_id, log_id + return "", fallback_log_id + + +def bundle_manifest_member(zip_file: zipfile.ZipFile) -> str | None: + for name in zip_file.namelist(): + if name.endswith("/bundle_manifest.json") or name == "bundle_manifest.json": + return name + return None + + +def read_bundle_manifest(zip_path: Path) -> tuple[str, dict]: + with zipfile.ZipFile(zip_path) as zip_file: + member = bundle_manifest_member(zip_file) + if member is None: + return "", {} + with zip_file.open(member) as manifest_file: + return member.split("/", 1)[0], json.loads(manifest_file.read().decode("utf-8")) + + +def summarize_bundle(zip_path: Path) -> BundleSummary: + bundle_name, manifest = read_bundle_manifest(zip_path) + route_id = str(manifest.get("routeId") or manifest.get("routeFullname") or "") + if not route_id: + stem = zip_path.stem.replace("speed_limit_route_bundle_", "") + parts = stem.split("_", 1) + route_id = f"{parts[0]}/{parts[1]}" if len(parts) == 2 else stem + + _, inferred_log_id = route_parts(route_id) + route_files = list(manifest.get("files") or []) + segments = {str(row.get("segment")) for row in route_files if row.get("segment") is not None} + fcamera_count = sum(1 for row in route_files if str(row.get("filename")) == "fcamera.hevc") + qlog_count = sum(1 for row in route_files if str(row.get("filename")) in ("qlog.zst", "qlog.bz2") or str(row.get("stream")) == "qlog") + rlog_count = sum(1 for row in route_files if str(row.get("filename")) in ("rlog.zst", "rlog.bz2") or str(row.get("stream")) == "rlog") + + if not route_files: + with zipfile.ZipFile(zip_path) as zip_file: + for member in zip_file.namelist(): + match = SEGMENT_FILE_RE.search(member) + if match is None: + continue + segment_name, filename = match.groups() + segments.add(segment_name.rsplit("--", 1)[-1]) + fcamera_count += filename == "fcamera.hevc" + qlog_count += filename.startswith("qlog.") + rlog_count += filename.startswith("rlog.") + + dongle_id, log_id = route_parts(route_id, inferred_log_id) + route_meta = manifest.get("routeMeta") or {} + validation = manifest.get("validation") or {} + return BundleSummary( + zip_path=zip_path, + bundle_name=bundle_name or zip_path.stem, + route_id=f"{dongle_id}/{log_id}" if dongle_id and log_id else route_id, + dongle_id=dongle_id, + log_id=log_id, + size_bytes=zip_path.stat().st_size, + expected_segments=int(manifest.get("expectedSegments") or 0), + segment_count=len(segments), + fcamera_count=int(fcamera_count), + qlog_count=int(qlog_count), + rlog_count=int(rlog_count), + git_commit=str(route_meta.get("gitCommit") or ""), + platform=str(route_meta.get("platform") or route_meta.get("make") or ""), + start_time=str(route_meta.get("startTime") or ""), + is_public=str(validation.get("isPublic") if "isPublic" in validation else ""), + maps_selected=str(validation.get("mapsSelected") or ""), + vision_detection=str(validation.get("visionSpeedLimitDetection") or ""), + auto_bookmark=str(validation.get("visionSpeedLimitAutoBookmark") or ""), + ) + + +def marker_path(state_dir: Path, summary: BundleSummary) -> Path: + safe_route = summary.route_id.replace("/", "_").replace("|", "_") + return state_dir / f"{safe_route}.json" + + +def write_json(path: Path, data: dict) -> None: + ensure_dir(path.parent) + path.write_text(json.dumps(data, indent=2, sort_keys=True) + "\n", encoding="utf-8") + + +def extract_member(zip_file: zipfile.ZipFile, member: str, destination: Path, overwrite: bool) -> bool: + if destination.exists() and not overwrite: + return False + ensure_dir(destination.parent) + tmp_path = destination.with_name(f".{destination.name}.tmp") + with zip_file.open(member) as src, tmp_path.open("wb") as dst: + shutil.copyfileobj(src, dst, length=1024 * 1024) + tmp_path.replace(destination) + return True + + +def extract_bundle(summary: BundleSummary, clip_root: Path, manifest_dir: Path, state_dir: Path, overwrite_files: bool, force: bool, dry_run: bool) -> dict: + marker = marker_path(state_dir, summary) + if marker.exists() and not force: + return {"status": "skipped", "reason": "marker_exists", "written": 0, "skipped": 0} + + written = 0 + skipped = 0 + started_at = time.time() + with zipfile.ZipFile(summary.zip_path) as zip_file: + manifest_member = bundle_manifest_member(zip_file) + if manifest_member is not None and not dry_run: + extract_member(zip_file, manifest_member, manifest_dir / f"{summary.route_id.replace('/', '_')}.json", overwrite=True) + + for member in zip_file.namelist(): + match = SEGMENT_FILE_RE.search(member) + if match is None: + continue + segment_name, filename = match.groups() + destination = clip_root / segment_name / filename + if dry_run: + skipped += int(destination.exists()) + written += int(not destination.exists()) + continue + if extract_member(zip_file, member, destination, overwrite=overwrite_files): + written += 1 + else: + skipped += 1 + + result = { + "status": "dry_run" if dry_run else "extracted", + "route_id": summary.route_id, + "zip_path": str(summary.zip_path), + "written": written, + "skipped": skipped, + "duration_s": round(time.time() - started_at, 3), + } + if not dry_run: + write_json(marker, result) + return result + + +def write_inventory(path: Path, summaries: list[BundleSummary]) -> None: + ensure_dir(path.parent) + fieldnames = [ + "route_id", "dongle_id", "log_id", "zip_path", "size_bytes", "expected_segments", "segment_count", + "fcamera_count", "qlog_count", "rlog_count", "git_commit", "platform", "start_time", "is_public", + "maps_selected", "vision_detection", "auto_bookmark", + ] + with path.open("w", encoding="utf-8", newline="") as csv_file: + writer = csv.DictWriter(csv_file, fieldnames=fieldnames) + writer.writeheader() + for summary in summaries: + writer.writerow({ + "route_id": summary.route_id, + "dongle_id": summary.dongle_id, + "log_id": summary.log_id, + "zip_path": str(summary.zip_path), + "size_bytes": summary.size_bytes, + "expected_segments": summary.expected_segments, + "segment_count": summary.segment_count, + "fcamera_count": summary.fcamera_count, + "qlog_count": summary.qlog_count, + "rlog_count": summary.rlog_count, + "git_commit": summary.git_commit, + "platform": summary.platform, + "start_time": summary.start_time, + "is_public": summary.is_public, + "maps_selected": summary.maps_selected, + "vision_detection": summary.vision_detection, + "auto_bookmark": summary.auto_bookmark, + }) + + +def load_existing_corpus(path: Path) -> dict[str, dict[str, str]]: + if not path.is_file(): + return {} + with path.open("r", encoding="utf-8", newline="") as csv_file: + reader = csv.DictReader(csv_file) + rows = {} + for row in reader: + route = row.get("route") or row.get("route_id") or "" + if route: + rows[route] = row + return rows + + +def write_corpus(path: Path, summaries: list[BundleSummary]) -> None: + ensure_dir(path.parent) + existing = load_existing_corpus(path) + for summary in summaries: + full_video = summary.expected_segments == 0 or summary.fcamera_count >= min(summary.expected_segments, summary.segment_count) + route_status = "full_fcamera" if full_video and summary.fcamera_count else "partial_or_missing_fcamera" + existing[summary.route_id] = { + "route": summary.route_id, + "dongle_id": summary.dongle_id, + "log_id": summary.log_id, + "status": route_status, + "source": "route_zip_bundle", + "expected_segments": str(summary.expected_segments), + "fcamera_segments": str(summary.fcamera_count), + "qlog_segments": str(summary.qlog_count), + "rlog_segments": str(summary.rlog_count), + "bookmark_count": existing.get(summary.route_id, {}).get("bookmark_count", ""), + "vision_source_messages": existing.get(summary.route_id, {}).get("vision_source_messages", ""), + "zip_path": str(summary.zip_path), + "git_commit": summary.git_commit, + "platform": summary.platform, + "start_time": summary.start_time, + "notes": "Imported from /Volumes/T5/routes zip bundle; not treated as labeled positives by default.", + } + + fieldnames = [ + "route", "dongle_id", "log_id", "status", "source", "expected_segments", "fcamera_segments", + "qlog_segments", "rlog_segments", "bookmark_count", "vision_source_messages", "zip_path", + "git_commit", "platform", "start_time", "notes", + ] + with path.open("w", encoding="utf-8", newline="") as csv_file: + writer = csv.DictWriter(csv_file, fieldnames=fieldnames, extrasaction="ignore") + writer.writeheader() + for route in sorted(existing): + writer.writerow(existing[route]) + + +def refresh_route_meta_files(clip_root: Path, files_manifest: Path, qlog_mtimes: Path) -> None: + ensure_dir(files_manifest.parent) + rows: list[str] = [] + mtimes: list[str] = [] + for segment_dir in sorted((path for path in clip_root.iterdir() if path.is_dir()), key=lambda path: path.name): + for filename in ("fcamera.hevc", "qlog.zst", "rlog.zst", "qlog.bz2", "rlog.bz2"): + file_path = segment_dir / filename + if not file_path.is_file(): + continue + rows.append(str(file_path)) + if filename.startswith("qlog."): + mtimes.append(f"{file_path} {int(file_path.stat().st_mtime)}") + files_manifest.write_text("\n".join(rows) + ("\n" if rows else ""), encoding="utf-8") + qlog_mtimes.write_text("\n".join(mtimes) + ("\n" if mtimes else ""), encoding="utf-8") + + +def main() -> int: + args = parse_args() + workspace = resolve_workspace(args.workspace) + routes_dir = args.routes_dir.expanduser().resolve() + clip_root = args.clip_root.expanduser().resolve() + manifest_dir = args.manifest_dir.expanduser().resolve() + state_dir = args.state_dir.expanduser().resolve() + inventory_out = args.inventory_out.expanduser().resolve() if args.inventory_out else (ensure_dir(workspace / "review") / "route_bundle_inventory.csv") + corpus_out = args.corpus_out.expanduser().resolve() if args.corpus_out else (ensure_dir(workspace / "review") / "connect_route_corpus.csv") + + zip_paths = sorted(routes_dir.glob("*.zip")) + if args.limit > 0: + zip_paths = zip_paths[:args.limit] + if not zip_paths: + raise FileNotFoundError(f"No .zip bundles found under {routes_dir}") + + ensure_dir(clip_root) + ensure_dir(manifest_dir) + ensure_dir(state_dir) + + summaries: list[BundleSummary] = [] + for zip_path in zip_paths: + try: + summaries.append(summarize_bundle(zip_path)) + except Exception as exc: + print(f"ERROR inventory {zip_path}: {exc}") + + write_inventory(inventory_out, summaries) + write_corpus(corpus_out, summaries) + print(f"Inventory: {inventory_out}") + print(f"Corpus: {corpus_out}") + print(f"Bundles: {len(summaries)}") + print(f"Segments: {sum(summary.segment_count for summary in summaries)}") + print(f"fcamera: {sum(summary.fcamera_count for summary in summaries)}") + + if args.extract: + for index, summary in enumerate(summaries, start=1): + result = extract_bundle(summary, clip_root, manifest_dir, state_dir, args.overwrite_files, args.force, args.dry_run) + print( + f"[{index}/{len(summaries)}] {summary.route_id}: {result['status']} " + f"written={result['written']} skipped={result['skipped']} {result.get('reason', '')}" + ) + if not args.dry_run: + refresh_route_meta_files(clip_root, preferred_files_manifest_path(), preferred_qlog_mtimes_path()) + print(f"Clip root: {clip_root}") + print(f"Files manifest: {preferred_files_manifest_path()}") + print(f"Qlog mtimes: {preferred_qlog_mtimes_path()}") + + return 0 + + +if __name__ == "__main__": + raise SystemExit(main()) diff --git a/scripts/speed_limit_vision/mine_connect_route_bookmarks.py b/scripts/speed_limit_vision/mine_connect_route_bookmarks.py index fcf4e0a23..aa1a35b8c 100644 --- a/scripts/speed_limit_vision/mine_connect_route_bookmarks.py +++ b/scripts/speed_limit_vision/mine_connect_route_bookmarks.py @@ -79,7 +79,11 @@ def load_route_bookmarks(clip_root: Path, log_id: str) -> list[dict]: if not rlog_path.exists(): continue - events = list(log.Event.read_multiple_bytes(read_log_bytes(rlog_path))) + try: + events = list(log.Event.read_multiple_bytes(read_log_bytes(rlog_path))) + except Exception as exc: + print(f"{segment_dir.name}: skipping unreadable rlog {rlog_path.name}: {exc}") + continue if not events: continue if route_start_monotime is None: diff --git a/scripts/speed_limit_vision/mine_route_training_samples.py b/scripts/speed_limit_vision/mine_route_training_samples.py new file mode 100644 index 000000000..e1c2bedfa --- /dev/null +++ b/scripts/speed_limit_vision/mine_route_training_samples.py @@ -0,0 +1,648 @@ +#!/usr/bin/env python3 +from __future__ import annotations + +import argparse +import bz2 +import csv +import hashlib +import json +import math + +from dataclasses import dataclass +from pathlib import Path + +import cv2 +import zstandard as zstd +from cereal import log + +import starpilot.system.speed_limit_vision as slv + +if __package__ in (None, ""): + import sys + sys.path.insert(0, str(Path(__file__).resolve().parent)) + from common import VALUE_LABEL_FIELDS, ensure_dir, preferred_clip_root, resolve_workspace # type: ignore + from localize_bookmark_signs import configure_models, score_frame # type: ignore +else: + from .common import VALUE_LABEL_FIELDS, ensure_dir, preferred_clip_root, resolve_workspace + from .localize_bookmark_signs import configure_models, score_frame + + +DEFAULT_ROUTE_BUNDLE_STATE_DIR = Path("/Volumes/T5/starpilot_speed_limit/analysis/route_bundles/state") +DEFAULT_WORKSPACE = Path("/Volumes/T5/starpilot_speed_limit/workspace/speed_limit_training_clean") +DEFAULT_REVIEW_MANIFEST_NAME = "route_training_samples.csv" +MPH_PER_MS = 2.2369362920544 +VALID_WEAK_LABEL_VALUES = set(slv.US_CLASSIFIER_SPEED_VALUES) +POSITIVE_FIELDNAMES = [ + "record_key", + "route", + "dongle_id", + "log_id", + "segment", + "frame_time_s", + "split", + "sample_type", + "dataset_image", + "dataset_label", + "speed_limit_mph", + "class_id", + "bbox", + "score", + "proposal_confidence", + "consistent_read_count", + "model_read", + "ocr_read", + "full_detection", + "map_current_speed_limit_mph", + "map_next_speed_limit_mph", + "map_next_speed_limit_distance_m", + "map_relation", + "source_video_path", +] + + +@dataclass(frozen=True) +class MapContext: + time_s: float + current_speed_limit_mph: int + next_speed_limit_mph: int + next_speed_limit_distance_m: float + + +@dataclass(frozen=True) +class SegmentInfo: + segment: int + path: Path + video_path: Path + + +def parse_args() -> argparse.Namespace: + parser = argparse.ArgumentParser(description="Mine completed comma route clips into weakly labeled speed-limit detector/classifier samples.") + parser.add_argument("routes", nargs="*", help="Optional route ids like 'dongle/logid'. Defaults to completed route-bundle markers.") + parser.add_argument("--routes-file", type=Path, help="Optional text file with one route id per line.") + parser.add_argument("--workspace", type=Path, default=DEFAULT_WORKSPACE, help="Training workspace root.") + parser.add_argument("--clip-root", type=Path, default=preferred_clip_root(), help="Route realdata root.") + parser.add_argument("--bundle-state-dir", type=Path, default=DEFAULT_ROUTE_BUNDLE_STATE_DIR, help="Completed extraction marker directory.") + parser.add_argument("--models-dir", type=Path, help="Optional model directory for mining with non-repo ONNXs.") + parser.add_argument("--manifest-out", type=Path, help=f"Review manifest path. Defaults to /review/{DEFAULT_REVIEW_MANIFEST_NAME}.") + parser.add_argument("--sample-every", type=float, default=4.0, help="Seconds between regular video samples.") + parser.add_argument("--seek-sampling", action="store_true", help="Seek directly to sampled frames instead of sequentially grabbing through each segment.") + parser.add_argument("--transition-radius", type=float, default=10.0, help="Extra seconds around map speed transitions to sample densely.") + parser.add_argument("--transition-step", type=float, default=1.0, help="Seconds between transition-window samples.") + parser.add_argument("--max-frames-per-route", type=int, default=360, help="Maximum frames to score per route.") + parser.add_argument("--max-positives-per-route", type=int, default=90, help="Maximum positive training images to add per route.") + parser.add_argument("--max-negatives-per-route", type=int, default=160, help="Maximum empty-label negatives to add per route.") + parser.add_argument("--positive-min-score", type=float, default=1.35, help="Minimum localization score for map-agreeing positives.") + parser.add_argument("--no-map-min-score", type=float, default=1.65, help="Minimum score for positives without map agreement.") + parser.add_argument("--min-proposal-confidence", type=float, default=0.18, help="Minimum detector proposal confidence for positives.") + parser.add_argument("--min-width", type=int, default=24, help="Minimum mined bbox width in pixels.") + parser.add_argument("--min-height", type=int, default=36, help="Minimum mined bbox height in pixels.") + parser.add_argument("--next-limit-distance", type=float, default=180.0, help="Treat map next-speed as agreeing only within this many meters.") + parser.add_argument("--val-route-modulo", type=int, default=5, help="Use route hash modulo N to choose validation routes. 0 disables.") + parser.add_argument("--val-route-remainder", type=int, default=0, help="Validation route hash remainder.") + parser.add_argument("--limit-routes", type=int, default=0, help="Optional maximum routes to mine.") + parser.add_argument("--force", action="store_true", help="Re-mine routes even if the route mining marker exists.") + parser.add_argument("--overwrite", action="store_true", help="Overwrite existing mined image/label files.") + parser.add_argument("--dry-run", action="store_true", help="Score frames and print counts without writing dataset files.") + return parser.parse_args() + + +def safe_key(text: str) -> str: + return text.replace("/", "_").replace("|", "_").replace(":", "_") + + +def parse_route_id(text: str) -> tuple[str, str, str]: + normalized = text.strip().replace("|", "/") + if "/" not in normalized: + raise ValueError(f"Route id must be dongle/logid: {text}") + dongle_id, log_id = normalized.split("/", 1) + return f"{dongle_id}/{log_id}", dongle_id, log_id + + +def route_split(route_id: str, val_modulo: int, val_remainder: int) -> str: + if val_modulo <= 0: + return "train" + digest = hashlib.sha1(route_id.encode("utf-8")).hexdigest() + return "val" if int(digest[:8], 16) % val_modulo == val_remainder else "train" + + +def completed_marker_routes(state_dir: Path) -> list[str]: + routes: list[str] = [] + if not state_dir.is_dir(): + return routes + for marker in sorted(path for path in state_dir.glob("*.json") if not path.name.startswith("._")): + try: + data = json.loads(marker.read_text(encoding="utf-8")) + except Exception: + continue + if data.get("status") == "extracted" and data.get("route_id"): + routes.append(str(data["route_id"])) + return routes + + +def read_routes(args: argparse.Namespace) -> list[str]: + routes = list(args.routes) + if args.routes_file: + routes.extend( + line.strip() + for line in args.routes_file.expanduser().read_text(encoding="utf-8").splitlines() + if line.strip() and not line.lstrip().startswith("#") + ) + if not routes: + routes = completed_marker_routes(args.bundle_state_dir.expanduser().resolve()) + deduped = [] + seen = set() + for route in routes: + normalized, _, _ = parse_route_id(route) + if normalized in seen: + continue + seen.add(normalized) + deduped.append(normalized) + if args.limit_routes > 0: + deduped = deduped[:args.limit_routes] + return deduped + + +def segment_number(path: Path) -> int: + try: + return int(path.name.rsplit("--", 1)[-1]) + except ValueError: + return -1 + + +def route_segments(clip_root: Path, log_id: str) -> list[SegmentInfo]: + segments = [] + for segment_dir in sorted(clip_root.glob(f"{log_id}--*"), key=segment_number): + video_path = segment_dir / "fcamera.hevc" + if video_path.is_file(): + segments.append(SegmentInfo(segment_number(segment_dir), segment_dir, video_path)) + return segments + + +def read_log_bytes(path: Path) -> bytes: + if path.suffix == ".zst": + with path.open("rb") as handle: + return zstd.ZstdDecompressor().stream_reader(handle).read() + if path.suffix == ".bz2": + return bz2.decompress(path.read_bytes()) + return path.read_bytes() + + +def round_mph_from_ms(speed_ms: float) -> int: + if speed_ms <= 0.0: + return 0 + mph = speed_ms * MPH_PER_MS + rounded = int(round(mph / 5.0) * 5) + return rounded if rounded in VALID_WEAK_LABEL_VALUES else 0 + + +def load_segment_map_context(segment_dir: Path) -> list[MapContext]: + log_path = segment_dir / "qlog.zst" + if not log_path.exists(): + log_path = segment_dir / "qlog.bz2" + if not log_path.exists(): + log_path = segment_dir / "rlog.zst" + if not log_path.exists(): + log_path = segment_dir / "rlog.bz2" + if not log_path.exists(): + return [] + + try: + events = list(log.Event.read_multiple_bytes(read_log_bytes(log_path))) + except Exception: + return [] + if not events: + return [] + + start_mono = events[0].logMonoTime + rows: list[MapContext] = [] + last_current = 0 + last_next = 0 + last_next_distance = 0.0 + + for event in events: + event_type = event.which() + current = 0 + next_speed = 0 + next_distance = 0.0 + if event_type == "mapdOut": + mapd = event.mapdOut + current = round_mph_from_ms(float(mapd.speedLimit or 0.0)) + next_speed = round_mph_from_ms(float(mapd.nextSpeedLimit or 0.0)) + next_distance = float(mapd.nextSpeedLimitDistance or 0.0) + elif event_type == "starpilotPlan": + plan = event.starpilotPlan + current = round_mph_from_ms(float(plan.slcMapSpeedLimit or 0.0)) + next_speed = round_mph_from_ms(float(plan.slcNextSpeedLimit or 0.0)) + next_distance = last_next_distance + else: + continue + + if current == 0: + current = last_current + if next_speed == 0: + next_speed = last_next + if next_distance <= 0.0: + next_distance = last_next_distance + if current == 0 and next_speed == 0: + continue + + last_current = current + last_next = next_speed + last_next_distance = next_distance + rows.append(MapContext((event.logMonoTime - start_mono) / 1e9, current, next_speed, next_distance)) + + return rows + + +def nearest_context(rows: list[MapContext], time_s: float, max_delta_s: float = 2.5) -> MapContext: + if not rows: + return MapContext(time_s, 0, 0, 0.0) + best = min(rows, key=lambda row: abs(row.time_s - time_s)) + if abs(best.time_s - time_s) > max_delta_s: + return MapContext(time_s, 0, 0, 0.0) + return best + + +def transition_times(rows: list[MapContext]) -> list[float]: + times = [] + previous = 0 + for row in rows: + current = row.current_speed_limit_mph + if current > 0 and previous > 0 and current != previous: + times.append(row.time_s) + if current > 0: + previous = current + return times + + +def sample_times(duration_s: float, regular_step_s: float, transition_centers: list[float], transition_radius_s: float, transition_step_s: float) -> list[float]: + times = set() + if regular_step_s > 0.0: + sample_count = max(int(math.floor(duration_s / regular_step_s)), 0) + for index in range(sample_count + 1): + value = min(index * regular_step_s, max(duration_s - 0.05, 0.0)) + times.add(round(value, 3)) + + if transition_radius_s > 0.0 and transition_step_s > 0.0: + for center in transition_centers: + offset = -transition_radius_s + while offset <= transition_radius_s + 1e-6: + value = center + offset + if 0.0 <= value < duration_s: + times.add(round(value, 3)) + offset += transition_step_s + + return sorted(times) + + +def iter_frames_at_times(capture: cv2.VideoCapture, fps: float, times: list[float]): + targets: list[tuple[int, float]] = [] + previous_frame_index = -1 + for time_s in times: + frame_index = max(int(round(time_s * fps)), 0) + if frame_index == previous_frame_index: + continue + targets.append((frame_index, time_s)) + previous_frame_index = frame_index + + current_frame_index = -1 + for target_frame_index, time_s in targets: + while current_frame_index + 1 < target_frame_index: + if not capture.grab(): + return + current_frame_index += 1 + + ok, frame_bgr = capture.read() + current_frame_index += 1 + if not ok or frame_bgr is None: + return + yield time_s, frame_bgr + + +def read_frame_at(capture: cv2.VideoCapture, fps: float, target_time_s: float): + frame_index = max(int(round(target_time_s * fps)), 0) + capture.set(cv2.CAP_PROP_POS_FRAMES, frame_index) + ok, frame_bgr = capture.read() + if not ok or frame_bgr is None: + return None + return frame_bgr + + +def detector_label_line(detector_class: int, x1: int, y1: int, x2: int, y2: int, image_shape: tuple[int, int, int]) -> str: + image_h, image_w = image_shape[:2] + x_center = ((x1 + x2) / 2) / image_w + y_center = ((y1 + y2) / 2) / image_h + width = (x2 - x1) / image_w + height = (y2 - y1) / image_h + return f"{detector_class} {x_center:.6f} {y_center:.6f} {width:.6f} {height:.6f}\n" + + +def fmt_read(result) -> str: + if result is None: + return "" + if hasattr(result, "speed_limit_mph"): + return f"{result.speed_limit_mph}@{result.confidence:.3f}" + return f"{result[0]}@{result[1]:.3f}" + + +def dominant_read(scored: dict) -> tuple[int, int]: + counts: dict[int, int] = {} + for key in ("model_read", "ocr_read"): + result = scored.get(key) + if result is not None: + counts[int(result[0])] = counts.get(int(result[0]), 0) + 1 + full_detection = scored.get("full_detection") + if full_detection is not None: + counts[int(full_detection.speed_limit_mph)] = counts.get(int(full_detection.speed_limit_mph), 0) + 1 + if not counts: + read_result = scored.get("read_result") + if read_result is None: + return 0, 0 + return int(read_result[0]), 1 + value, count = max(counts.items(), key=lambda item: (item[1], item[0])) + return value, count + + +def map_relation(speed_limit_mph: int, context: MapContext, next_limit_distance_m: float) -> str: + if speed_limit_mph <= 0: + return "no_read" + if context.current_speed_limit_mph == speed_limit_mph: + return "agree_current" + if ( + context.next_speed_limit_mph == speed_limit_mph and + 0.0 < context.next_speed_limit_distance_m <= next_limit_distance_m + ): + return "agree_next" + if context.current_speed_limit_mph or context.next_speed_limit_mph: + return "map_disagreement" + return "no_map" + + +def should_keep_positive(scored: dict, speed_limit_mph: int, consistent_count: int, relation: str, args: argparse.Namespace) -> bool: + if speed_limit_mph not in VALID_WEAK_LABEL_VALUES: + return False + if scored.get("class_id") == 1: + return False + if not scored.get("is_regulatory") and scored.get("class_id") != 2: + return False + x1, y1, x2, y2 = scored["box"] + if x2 - x1 < args.min_width or y2 - y1 < args.min_height: + return False + if float(scored["proposal_confidence"]) < args.min_proposal_confidence: + return False + if relation in ("agree_current", "agree_next"): + return float(scored["score"]) >= args.positive_min_score and consistent_count >= 1 + return float(scored["score"]) >= args.no_map_min_score and consistent_count >= 2 + + +def load_csv_by_key(path: Path, key_field: str) -> dict[str, dict[str, str]]: + if not path.is_file(): + return {} + with path.open("r", encoding="utf-8", newline="") as handle: + reader = csv.DictReader(handle) + rows = {} + for row in reader: + key = row.get(key_field, "") + if key: + rows[key] = row + return rows + + +def write_csv(path: Path, fieldnames: list[str], rows: list[dict[str, object]]) -> None: + ensure_dir(path.parent) + with path.open("w", encoding="utf-8", newline="") as handle: + writer = csv.DictWriter(handle, fieldnames=fieldnames, extrasaction="ignore") + writer.writeheader() + for row in rows: + writer.writerow(row) + + +def merge_review_rows(path: Path, new_rows: list[dict[str, object]]) -> None: + existing = load_csv_by_key(path, "record_key") + for row in new_rows: + existing[str(row["record_key"])] = {key: str(value) for key, value in row.items()} + write_csv(path, POSITIVE_FIELDNAMES, [existing[key] for key in sorted(existing)]) + + +def merge_value_labels(path: Path, new_rows: list[dict[str, object]]) -> None: + existing = load_csv_by_key(path, "image_path") + for row in new_rows: + existing[str(row["image_path"])] = {key: str(value) for key, value in row.items()} + write_csv(path, VALUE_LABEL_FIELDS, [existing[key] for key in sorted(existing)]) + + +def write_sample(frame_bgr, image_path: Path, label_path: Path, label_text: str, overwrite: bool, dry_run: bool) -> bool: + if dry_run: + return True + if image_path.exists() and label_path.exists() and not overwrite: + return False + ensure_dir(image_path.parent) + ensure_dir(label_path.parent) + cv2.imwrite(str(image_path), frame_bgr, [cv2.IMWRITE_JPEG_QUALITY, 92]) + label_path.write_text(label_text, encoding="utf-8") + return True + + +def mine_route(route_id: str, daemon: slv.SpeedLimitVisionDaemon, args: argparse.Namespace, workspace: Path, clip_root: Path, manifest_path: Path, route_state_dir: Path) -> dict[str, int | str | float]: + route_id, dongle_id, log_id = parse_route_id(route_id) + route_key = safe_key(route_id) + state_path = route_state_dir / f"{route_key}.json" + if state_path.exists() and not args.force: + return {"route": route_id, "status": "skipped", "positives": 0, "negatives": 0, "scored": 0} + + split = route_split(route_id, args.val_route_modulo, args.val_route_remainder) + image_dir = ensure_dir(workspace / "detector" / "images" / split) + label_dir = ensure_dir(workspace / "detector" / "labels" / split) + segments = route_segments(clip_root, log_id) + if not segments: + return {"route": route_id, "status": "missing_segments", "positives": 0, "negatives": 0, "scored": 0} + + route_rows: list[dict[str, object]] = [] + value_rows: list[dict[str, object]] = [] + positives = 0 + negatives = 0 + scored_frames = 0 + + for segment in segments: + if scored_frames >= args.max_frames_per_route: + break + contexts = load_segment_map_context(segment.path) + capture = cv2.VideoCapture(str(segment.video_path)) + fps = capture.get(cv2.CAP_PROP_FPS) or 20.0 + frame_count = capture.get(cv2.CAP_PROP_FRAME_COUNT) or 0 + duration_s = frame_count / fps if frame_count > 0 else 60.0 + times = sample_times(duration_s, args.sample_every, transition_times(contexts), args.transition_radius, args.transition_step) + + if args.seek_sampling: + frame_iter = ((time_s, read_frame_at(capture, fps, time_s)) for time_s in times) + else: + frame_iter = iter_frames_at_times(capture, fps, times) + + for time_s, frame_bgr in frame_iter: + if scored_frames >= args.max_frames_per_route: + break + if positives >= args.max_positives_per_route and negatives >= args.max_negatives_per_route: + break + if frame_bgr is None: + continue + + scored_frames += 1 + scored = score_frame(daemon, frame_bgr) + context = nearest_context(contexts, time_s) + + if scored is None: + if negatives >= args.max_negatives_per_route: + continue + sample_index = f"s{segment.segment:04d}_t{time_s:06.2f}".replace(".", "p") + record_key = f"real_route_negative_{route_key}_{sample_index}" + image_path = image_dir / f"{record_key}.jpg" + label_path = label_dir / f"{record_key}.txt" + if write_sample(frame_bgr, image_path, label_path, "", args.overwrite, args.dry_run): + negatives += 1 + route_rows.append({ + "record_key": record_key, + "route": route_id, + "dongle_id": dongle_id, + "log_id": log_id, + "segment": segment.segment, + "frame_time_s": f"{time_s:.3f}", + "split": split, + "sample_type": "negative_empty", + "dataset_image": str(image_path), + "dataset_label": str(label_path), + "speed_limit_mph": "", + "class_id": "", + "bbox": "", + "score": "", + "proposal_confidence": "", + "consistent_read_count": "", + "model_read": "", + "ocr_read": "", + "full_detection": "", + "map_current_speed_limit_mph": context.current_speed_limit_mph, + "map_next_speed_limit_mph": context.next_speed_limit_mph, + "map_next_speed_limit_distance_m": f"{context.next_speed_limit_distance_m:.1f}", + "map_relation": "no_candidate", + "source_video_path": str(segment.video_path), + }) + continue + + speed_limit_mph, consistent_count = dominant_read(scored) + relation = map_relation(speed_limit_mph, context, args.next_limit_distance) + if not should_keep_positive(scored, speed_limit_mph, consistent_count, relation, args): + continue + if positives >= args.max_positives_per_route: + continue + + sample_index = f"s{segment.segment:04d}_t{time_s:06.2f}".replace(".", "p") + record_key = f"real_route_positive_{route_key}_{sample_index}" + image_path = image_dir / f"{record_key}.jpg" + label_path = label_dir / f"{record_key}.txt" + x1, y1, x2, y2 = scored["box"] + detector_class = 2 if int(scored["class_id"]) == 2 else 0 + label_text = detector_label_line(detector_class, x1, y1, x2, y2, frame_bgr.shape) + if not write_sample(frame_bgr, image_path, label_path, label_text, args.overwrite, args.dry_run): + continue + + positives += 1 + bbox = ",".join(str(value) for value in scored["box"]) + route_rows.append({ + "record_key": record_key, + "route": route_id, + "dongle_id": dongle_id, + "log_id": log_id, + "segment": segment.segment, + "frame_time_s": f"{time_s:.3f}", + "split": split, + "sample_type": "positive_weak_map" if relation in ("agree_current", "agree_next") else "positive_weak_nomap", + "dataset_image": str(image_path), + "dataset_label": str(label_path), + "speed_limit_mph": speed_limit_mph, + "class_id": detector_class, + "bbox": bbox, + "score": f"{scored['score']:.4f}", + "proposal_confidence": f"{scored['proposal_confidence']:.4f}", + "consistent_read_count": consistent_count, + "model_read": fmt_read(scored.get("model_read")), + "ocr_read": fmt_read(scored.get("ocr_read")), + "full_detection": fmt_read(scored.get("full_detection")), + "map_current_speed_limit_mph": context.current_speed_limit_mph, + "map_next_speed_limit_mph": context.next_speed_limit_mph, + "map_next_speed_limit_distance_m": f"{context.next_speed_limit_distance_m:.1f}", + "map_relation": relation, + "source_video_path": str(segment.video_path), + }) + value_rows.append({ + "image_path": str(image_path), + "split": split, + "speed_limit_mph": speed_limit_mph, + "bbox_index": 0, + "padding": 0.12, + "label_path": str(label_path), + }) + + capture.release() + + if not args.dry_run: + merge_review_rows(manifest_path, route_rows) + merge_value_labels(workspace / "classifier" / "value_labels.csv", value_rows) + state_path.write_text(json.dumps({ + "route": route_id, + "status": "mined", + "positives": positives, + "negatives": negatives, + "scored": scored_frames, + "segments": len(segments), + }, indent=2, sort_keys=True) + "\n", encoding="utf-8") + + return { + "route": route_id, + "status": "mined", + "positives": positives, + "negatives": negatives, + "scored": scored_frames, + } + + +def main() -> int: + try: + cv2.setLogLevel(1) + except Exception: + pass + + args = parse_args() + workspace = resolve_workspace(args.workspace) + clip_root = args.clip_root.expanduser().resolve() + manifest_path = args.manifest_out.expanduser().resolve() if args.manifest_out else (ensure_dir(workspace / "review") / DEFAULT_REVIEW_MANIFEST_NAME) + route_state_dir = ensure_dir(workspace / "review" / "route_training_samples_state") + routes = read_routes(args) + if not routes: + raise SystemExit("No routes to mine. Pass route ids, --routes-file, or completed bundle markers.") + + configure_models(args.models_dir) + daemon = slv.SpeedLimitVisionDaemon(use_runtime=False) + + total_positive = 0 + total_negative = 0 + total_scored = 0 + for index, route in enumerate(routes, start=1): + result = mine_route(route, daemon, args, workspace, clip_root, manifest_path, route_state_dir) + total_positive += int(result.get("positives", 0)) + total_negative += int(result.get("negatives", 0)) + total_scored += int(result.get("scored", 0)) + print( + f"[{index}/{len(routes)}] {result['route']}: {result['status']} " + f"positives={result['positives']} negatives={result['negatives']} scored={result['scored']}", + flush=True, + ) + + print( + f"Route mining complete: routes={len(routes)} positives={total_positive} negatives={total_negative} scored={total_scored}", + flush=True, + ) + print(f"Review manifest: {manifest_path}", flush=True) + return 0 + + +if __name__ == "__main__": + raise SystemExit(main()) diff --git a/scripts/speed_limit_vision/rebalance_detector_dataset.py b/scripts/speed_limit_vision/rebalance_detector_dataset.py index 3c5e99b20..bba608a26 100644 --- a/scripts/speed_limit_vision/rebalance_detector_dataset.py +++ b/scripts/speed_limit_vision/rebalance_detector_dataset.py @@ -20,6 +20,8 @@ def parse_args() -> argparse.Namespace: parser.add_argument("--workspace", type=Path, default=DEFAULT_WORKSPACE, help="Training workspace root.") parser.add_argument("--output-root", type=Path, help="Output dataset root. Defaults to /detector_rebalanced.") parser.add_argument("--max-other-train", type=int, default=4000, help="Maximum number of non-real train images to keep.") + parser.add_argument("--max-real-negative-train", type=int, default=0, help="Maximum number of empty-label real train images to keep. 0 keeps all.") + parser.add_argument("--max-real-positive-train", type=int, default=0, help="Maximum number of labeled real train images to keep. 0 keeps all.") parser.add_argument("--real-val-count", type=int, default=0, help="Hold out this many real train images as extra validation examples.") parser.add_argument( "--source-cap", @@ -100,6 +102,12 @@ def prefix_for_path(path: Path) -> str: return name.split("_", 1)[0] +def has_detector_label(label_path: Path) -> bool: + if not label_path.is_file(): + return False + return bool(label_path.read_text(encoding="utf-8").strip()) + + def main() -> int: args = parse_args() workspace = resolve_workspace(args.workspace) @@ -117,7 +125,24 @@ def main() -> int: other_images = [path for path in train_images if not path.name.startswith("real_")] rng.shuffle(real_images) heldout_real_images = sorted(real_images[:max(args.real_val_count, 0)], key=lambda path: path.name) - target_real_train_images = sorted(real_images[max(args.real_val_count, 0):], key=lambda path: path.name) + candidate_real_train_images = real_images[max(args.real_val_count, 0):] + real_positive_images = [] + real_negative_images = [] + for image_path in candidate_real_train_images: + label_path = train_labels / f"{image_path.stem}.txt" + if has_detector_label(label_path): + real_positive_images.append(image_path) + else: + real_negative_images.append(image_path) + + rng.shuffle(real_positive_images) + rng.shuffle(real_negative_images) + if args.max_real_positive_train > 0: + real_positive_images = real_positive_images[:args.max_real_positive_train] + if args.max_real_negative_train > 0: + real_negative_images = real_negative_images[:args.max_real_negative_train] + + target_real_train_images = sorted(real_positive_images + real_negative_images, key=lambda path: path.name) grouped_other_images: dict[str, list[Path]] = defaultdict(list) for image_path in other_images: @@ -180,6 +205,8 @@ def main() -> int: print(f"Dataset YAML: {dataset_yaml}") print(f"Train images: {visible_file_count(output_train_image_dir)}") print(f" real train: {len(target_real_train_images)}") + print(f" real positive train: {len(real_positive_images)}") + print(f" real negative train: {len(real_negative_images)}") print(f" real held out to val: {len(heldout_real_images)}") print(f" sampled other: {len(sampled_other_images)}") if source_caps: diff --git a/selfdrive/modeld/compile_modeld.py b/selfdrive/modeld/compile_modeld.py index 400d1b8e2..533387922 100644 --- a/selfdrive/modeld/compile_modeld.py +++ b/selfdrive/modeld/compile_modeld.py @@ -16,7 +16,10 @@ def _patch_tinygrad_fetch_fw(): import hashlib import pathlib - import zstandard + try: + import zstandard + except ImportError: + return from tinygrad import helpers original_fetch_fw = getattr(helpers, "fetch_fw", None) @@ -45,9 +48,16 @@ from tinygrad.tensor import Tensor ARTIFACT_FORMAT_VERSION = 1 MODEL_TYPES = ("vision_policy", "vision_multi_policy", "supercombo") NV12Frame = namedtuple("NV12Frame", ["width", "height", "stride", "y_height", "uv_height", "size"]) -WARP_INPUTS = ("img_q", "big_img_q", "tfm", "big_tfm") -SPLIT_POLICY_INPUTS = ("feat_q", "desire_q", "packed_npy_inputs") -SUPERCOMBO_POLICY_INPUTS = ("feat_q", "desire_q", "packed_npy_inputs") +IMAGE_HISTORY_IN_WARP = "warp" +IMAGE_HISTORY_IN_POLICY = "policy" +IMAGE_HISTORY_PIPELINES = (IMAGE_HISTORY_IN_WARP, IMAGE_HISTORY_IN_POLICY) +LEGACY_WARP_INPUTS = ("img_q", "big_img_q", "tfm", "big_tfm") +FAST_WARP_INPUTS = ("tfm", "big_tfm") +BASE_POLICY_INPUTS = ("feat_q", "desire_q", "packed_npy_inputs") +FAST_POLICY_INPUTS = ("img_q", "big_img_q", *BASE_POLICY_INPUTS) +WARP_INPUTS = LEGACY_WARP_INPUTS +SPLIT_POLICY_INPUTS = BASE_POLICY_INPUTS +SUPERCOMBO_POLICY_INPUTS = BASE_POLICY_INPUTS WARP_DEV = os.getenv("WARP_DEV") @@ -263,8 +273,22 @@ def sample_desire(buffer, frame_skip): return buffer.reshape(-1, frame_skip, *buffer.shape[1:]).max(1).flatten(0, 1).unsqueeze(0) -def make_warp(nv12, model_w, model_h, frame_skip): +def make_warp(nv12, model_w, model_h, frame_skip, image_history_pipeline=IMAGE_HISTORY_IN_POLICY): frame_prepare = make_frame_prepare(nv12, model_w, model_h) + + if image_history_pipeline == IMAGE_HISTORY_IN_POLICY: + def warp(tfm, big_tfm, frame, big_frame): + tfm = tfm.to(WARP_DEV) + big_tfm = big_tfm.to(WARP_DEV) + Tensor.realize(tfm, big_tfm) + + return Tensor.cat( + frame_prepare(frame, tfm).unsqueeze(0), + frame_prepare(big_frame, big_tfm).unsqueeze(0), + ) + + return warp + sample_skip_fn = partial(sample_skip, frame_skip=frame_skip) def warp_enqueue(img_q, big_img_q, tfm, big_tfm, frame, big_frame): @@ -283,7 +307,8 @@ def make_warp(nv12, model_w, model_h, frame_skip): return warp_enqueue -def make_run_split_policy(vision_runner, policy_runners, metadata, policy_order, frame_skip): +def make_run_split_policy(vision_runner, policy_runners, metadata, policy_order, frame_skip, + image_history_pipeline=IMAGE_HISTORY_IN_POLICY): sample_desire_fn = partial(sample_desire, frame_skip=frame_skip) sample_skip_fn = partial(sample_skip, frame_skip=frame_skip) vision_metadata = metadata["vision"] @@ -293,8 +318,7 @@ def make_run_split_policy(vision_runner, policy_runners, metadata, policy_order, packed_shapes, packed_sizes = _packed_policy_shapes(policy_metadata["input_shapes"]) road_key, wide_key = _detect_vision_keys(vision_metadata["input_shapes"]) - def run_policy(img, big_img, feat_q, desire_q, packed_npy_inputs): - packed_npy_inputs = packed_npy_inputs.to(Device.DEFAULT).realize() + def run_model(img, big_img, feat_q, desire_q, packed_npy_inputs): unpacked = { key: tensor.reshape(shape) for (key, shape), tensor in zip( @@ -319,10 +343,25 @@ def make_run_split_policy(vision_runner, policy_runners, metadata, policy_order, ] return (vision_output, *policy_outputs) + if image_history_pipeline == IMAGE_HISTORY_IN_POLICY: + def run_policy(warped, img_q, big_img_q, feat_q, desire_q, packed_npy_inputs): + packed_npy_inputs = packed_npy_inputs.to(Device.DEFAULT) + warped = warped.to(Device.DEFAULT) + Tensor.realize(packed_npy_inputs, warped) + img = shift_and_sample(img_q, warped[0:1], sample_skip_fn) + big_img = shift_and_sample(big_img_q, warped[1:2], sample_skip_fn) + return run_model(img, big_img, feat_q, desire_q, packed_npy_inputs) + + return run_policy + + def run_policy(img, big_img, feat_q, desire_q, packed_npy_inputs): + packed_npy_inputs = packed_npy_inputs.to(Device.DEFAULT).realize() + return run_model(img, big_img, feat_q, desire_q, packed_npy_inputs) + return run_policy -def make_run_supercombo(model_runner, metadata, frame_skip): +def make_run_supercombo(model_runner, metadata, frame_skip, image_history_pipeline=IMAGE_HISTORY_IN_POLICY): input_shapes = metadata["model"]["input_shapes"] output_slices = metadata["model"]["output_slices"] sample_desire_fn = partial(sample_desire, frame_skip=frame_skip) @@ -331,8 +370,7 @@ def make_run_supercombo(model_runner, metadata, frame_skip): packed_shapes, packed_sizes = _packed_policy_shapes(input_shapes, include_prev_feature=True) road_key, wide_key = _detect_vision_keys(input_shapes) - def run_policy(img, big_img, feat_q, desire_q, packed_npy_inputs): - packed_npy_inputs = packed_npy_inputs.to(Device.DEFAULT).realize() + def run_model(img, big_img, feat_q, desire_q, packed_npy_inputs): unpacked = { key: tensor.reshape(shape) for (key, shape), tensor in zip( @@ -356,6 +394,21 @@ def make_run_supercombo(model_runner, metadata, frame_skip): model_output = next(iter(model_runner(model_inputs).values())).cast("float32") return model_output, + if image_history_pipeline == IMAGE_HISTORY_IN_POLICY: + def run_policy(warped, img_q, big_img_q, feat_q, desire_q, packed_npy_inputs): + packed_npy_inputs = packed_npy_inputs.to(Device.DEFAULT) + warped = warped.to(Device.DEFAULT) + Tensor.realize(packed_npy_inputs, warped) + img = shift_and_sample(img_q, warped[0:1], sample_skip_fn) + big_img = shift_and_sample(big_img_q, warped[1:2], sample_skip_fn) + return run_model(img, big_img, feat_q, desire_q, packed_npy_inputs) + + return run_policy + + def run_policy(img, big_img, feat_q, desire_q, packed_npy_inputs): + packed_npy_inputs = packed_npy_inputs.to(Device.DEFAULT).realize() + return run_model(img, big_img, feat_q, desire_q, packed_npy_inputs) + return run_policy @@ -450,12 +503,19 @@ def main(): parser.add_argument("--off-policy-onnx") parser.add_argument("--on-policy-onnx") parser.add_argument("--supercombo-onnx") + parser.add_argument( + "--image-history-pipeline", + choices=IMAGE_HISTORY_PIPELINES, + default=IMAGE_HISTORY_IN_POLICY, + help="Where img/big_img history queues are updated. 'policy' is the newer faster ABI; 'warp' reproduces legacy v22 artifacts.", + ) args = parser.parse_args() output = { "format_version": ARTIFACT_FORMAT_VERSION, "model_type": args.model_type, "metadata": {}, + "image_history_pipeline": args.image_history_pipeline, } if args.behavior_version: output["behavior_version"] = args.behavior_version @@ -470,9 +530,11 @@ def main(): policy_shapes = output["metadata"]["model"]["input_shapes"] frame_skip = args.frame_skip or derive_frame_skip(policy_shapes) make_policy_queues = partial(make_supercombo_input_queues, policy_shapes, frame_skip) - run_policy = make_run_supercombo(model_runner, output["metadata"], frame_skip) + run_policy = make_run_supercombo( + model_runner, output["metadata"], frame_skip, args.image_history_pipeline, + ) image_shapes = policy_shapes - policy_input_keys = SUPERCOMBO_POLICY_INPUTS + policy_input_keys = FAST_POLICY_INPUTS if args.image_history_pipeline == IMAGE_HISTORY_IN_POLICY else SUPERCOMBO_POLICY_INPUTS else: if not args.vision_onnx: parser.error("--vision-onnx is required for split models") @@ -513,19 +575,30 @@ def main(): ) run_policy = make_run_split_policy( vision_runner, policy_runners, output["metadata"], policy_order, frame_skip, + args.image_history_pipeline, ) image_shapes = output["metadata"]["vision"]["input_shapes"] - policy_input_keys = SPLIT_POLICY_INPUTS + policy_input_keys = FAST_POLICY_INPUTS if args.image_history_pipeline == IMAGE_HISTORY_IN_POLICY else SPLIT_POLICY_INPUTS output["frame_skip"] = frame_skip output["policy_input_keys"] = policy_input_keys + warp_input_keys = FAST_WARP_INPUTS if args.image_history_pipeline == IMAGE_HISTORY_IN_POLICY else LEGACY_WARP_INPUTS + output["warp_input_keys"] = warp_input_keys run_policy_jit = TinyJit(run_policy, prune=True) road_key, wide_key = _detect_vision_keys(image_shapes) - make_random_model_inputs = partial( - make_random_images, - keys=[road_key, wide_key], - shape=image_shapes[road_key], - ) + if args.image_history_pipeline == IMAGE_HISTORY_IN_POLICY: + make_random_model_inputs = partial( + make_random_images, + keys=["warped"], + shape=(2, 6, *image_shapes[road_key][2:]), + device=WARP_DEV, + ) + else: + make_random_model_inputs = partial( + make_random_images, + keys=[road_key, wide_key], + shape=image_shapes[road_key], + ) output["run_policy"] = compile_jit( run_policy_jit, make_random_model_inputs, policy_input_keys, make_policy_queues, ) @@ -533,13 +606,16 @@ def main(): model_w, model_h = args.model_size for cam_w, cam_h in args.camera_resolutions: nv12 = NV12Frame(cam_w, cam_h, *get_nv12_info(cam_w, cam_h)) - warp_enqueue = TinyJit(make_warp(nv12, model_w, model_h, frame_skip), prune=True) + warp_enqueue = TinyJit( + make_warp(nv12, model_w, model_h, frame_skip, args.image_history_pipeline), + prune=True, + ) make_random_warp_inputs = make_random_blob_images( keys=["frame", "big_frame"], size=nv12.size, device=WARP_DEV, ) make_warp_queues = partial(make_warp_input_queues, image_shapes, frame_skip) output[(cam_w, cam_h)] = compile_jit( - warp_enqueue, make_random_warp_inputs, WARP_INPUTS, make_warp_queues, + warp_enqueue, make_random_warp_inputs, warp_input_keys, make_warp_queues, ) with open(args.output, "wb") as artifact_file: diff --git a/selfdrive/modeld/modeld.py b/selfdrive/modeld/modeld.py index 67d788ce2..5363b638f 100755 --- a/selfdrive/modeld/modeld.py +++ b/selfdrive/modeld/modeld.py @@ -31,7 +31,9 @@ from openpilot.selfdrive.modeld.fill_model_msg import fill_model_msg, fill_pose_ from openpilot.selfdrive.modeld.constants import ModelConstants, Plan from openpilot.selfdrive.modeld.compile_modeld import ( ARTIFACT_FORMAT_VERSION, - WARP_INPUTS, + IMAGE_HISTORY_IN_POLICY, + IMAGE_HISTORY_IN_WARP, + LEGACY_WARP_INPUTS, _detect_vision_keys, make_split_input_queues, make_supercombo_input_queues, @@ -224,9 +226,12 @@ class ModelState: self.metadata = artifact["metadata"] self.policy_order = artifact.get("policy_order", []) self.frame_skip = int(artifact["frame_skip"]) + self.image_history_pipeline = artifact.get("image_history_pipeline", IMAGE_HISTORY_IN_WARP) + self.warp_input_keys = tuple(artifact.get("warp_input_keys", LEGACY_WARP_INPUTS)) self.policy_input_keys = tuple(artifact["policy_input_keys"]) self.run_policy = artifact["run_policy"] self.warp_enqueue = artifact[(cam_w, cam_h)] + self.can_prepare_only = self.image_history_pipeline == IMAGE_HISTORY_IN_WARP if self.model_type == "supercombo": input_shapes = self.metadata["model"]["input_shapes"] @@ -359,19 +364,26 @@ class ModelState: self.npy["tfm"][:] = transforms[self.road_key] self.npy["big_tfm"][:] = transforms[self.wide_key] - img, big_img = self.warp_enqueue( - **{key: self.input_queues[key] for key in WARP_INPUTS}, + warp_output = self.warp_enqueue( + **{key: self.input_queues[key] for key in self.warp_input_keys}, frame=frames[self.road_key], big_frame=frames[self.wide_key], ) - if prepare_only: - return None - output_tensors = self.run_policy( - **{key: self.input_queues[key] for key in self.policy_input_keys}, - img=img, - big_img=big_img, - ) + if self.image_history_pipeline == IMAGE_HISTORY_IN_POLICY: + output_tensors = self.run_policy( + **{key: self.input_queues[key] for key in self.policy_input_keys}, + warped=warp_output, + ) + else: + img, big_img = warp_output + if prepare_only: + return None + output_tensors = self.run_policy( + **{key: self.input_queues[key] for key in self.policy_input_keys}, + img=img, + big_img=big_img, + ) outputs = [output.numpy().flatten() for output in output_tensors] if self.model_type == "supercombo": @@ -543,7 +555,7 @@ def main(demo=False): run_count = run_count + 1 frame_drop_ratio = frames_dropped / (1 + frames_dropped) - prepare_only = vipc_dropped_frames > 0 + prepare_only = model.can_prepare_only and vipc_dropped_frames > 0 if prepare_only: cloudlog.error(f"skipping model eval. Dropped {vipc_dropped_frames} frames")