Refactor

scripts/speed_limit_vision/build_localized_bookmark_review_queue.py

@@ -95,6 +95,14 @@ def queue_row(row: dict[str, str]) -> dict[str, str]:
   read_sources = "model"
   if row.get("full_detection", ""):
     read_sources += ";full_detection"
+  if row.get("event_type", "") == "visionPublish":
+    read_sources += ";logged_vision_publish"
+
+  review_reasons = ["corrected_source_timing"]
+  if row.get("event_type", "") == "visionPublish":
+    review_reasons.extend(("route_vision_publish", f"published_{row.get('published_speed', '')}"))
+  else:
+    review_reasons.append("route_bookmark")
 
   item = dict.fromkeys(FIELDNAMES, "")
   item.update({
@@ -121,8 +129,10 @@ def queue_row(row: dict[str, str]) -> dict[str, str]:
     "read_sources": read_sources,
     "read_support_count": "1",
     "is_regulatory": row.get("is_regulatory", ""),
+    "map_current_speed_limit_mph": row.get("map_speed", ""),
+    "map_next_speed_limit_mph": row.get("next_speed", ""),
     "review_priority": row.get("score", ""),
-    "review_reasons": "route_bookmark;corrected_source_timing",
+    "review_reasons": ";".join(review_reasons),
   })
   return item
 

scripts/speed_limit_vision/localize_bookmark_signs.py

@@ -56,19 +56,23 @@ def configure_models(models_dir: Path | None):
   slv.US_CLASSIFIER_MODEL_PATH = classifier_path
 
 
-def iter_video_samples(clip_path: Path, start_s: float, end_s: float, sample_every: float):
+def iter_video_samples(clip_path: Path, start_s: float, end_s: float, sample_every: float, seek: bool = False):
   capture = cv2.VideoCapture(str(clip_path))
   fps = common.source_video_fps(clip_path, capture.get(cv2.CAP_PROP_FPS))
   start_frame = max(int(start_s * fps), 0)
   end_frame = max(int(end_s * fps), start_frame)
 
   frame_index = 0
-  while frame_index < start_frame:
-    ok, _ = capture.read()
-    if not ok:
-      capture.release()
-      return
-    frame_index += 1
+  if seek:
+    capture.set(cv2.CAP_PROP_POS_FRAMES, start_frame)
+    frame_index = max(round(capture.get(cv2.CAP_PROP_POS_FRAMES)), 0)
+  else:
+    while frame_index < start_frame:
+      ok, _ = capture.read()
+      if not ok:
+        capture.release()
+        return
+      frame_index += 1
 
   next_sample_s = start_s
   while frame_index <= end_frame:
@@ -86,7 +90,14 @@ def iter_video_samples(clip_path: Path, start_s: float, end_s: float, sample_eve
   capture.release()
 
 
-def iter_context_frames(clip_root: Path, window: ebl.BookmarkWindow, search_before: float, search_after: float, sample_every: float):
+def iter_context_frames(
+  clip_root: Path,
+  window: ebl.BookmarkWindow,
+  search_before: float,
+  search_after: float,
+  sample_every: float,
+  seek: bool = False,
+):
   ranges: list[tuple[Path, float, float]] = []
   start_s = window.segment_offset_s - search_before
   end_s = window.segment_offset_s + search_after
@@ -102,7 +113,7 @@ def iter_context_frames(clip_root: Path, window: ebl.BookmarkWindow, search_befo
     ranges.append((current_clip, max(start_s, 0.0), min(end_s, 60.0)))
 
   for clip_path, range_start_s, range_end_s in ranges:
-    for source_time_s, frame_bgr in iter_video_samples(clip_path, range_start_s, range_end_s, sample_every):
+    for source_time_s, frame_bgr in iter_video_samples(clip_path, range_start_s, range_end_s, sample_every, seek=seek):
       if clip_path.parent.name.endswith(f"--{window.segment - 1}"):
         relative_time_s = source_time_s - 60.0
       else:

scripts/speed_limit_vision/mine_connect_route_bookmarks.py

@@ -30,6 +30,7 @@ else:
 DEFAULT_WORKSPACE = Path("/Volumes/T5/starpilot_speed_limit/workspace/speed_limit_training_clean")
 ROUTE_ID_RE = re.compile(r"([0-9a-f]{16})/([^/]+)")
 BOOKMARK_TYPES = ("bookmarkButton", "userBookmark")
+MS_TO_MPH = 2.2369362920544
 
 
 def parse_args() -> argparse.Namespace:
@@ -38,6 +39,12 @@ def parse_args() -> argparse.Namespace:
   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="Downloaded route clip root.")
   parser.add_argument("--output-dir", type=Path, help="Output directory. Defaults to <workspace>/review/connect_route_bookmarks.")
+  parser.add_argument(
+    "--event-types",
+    choices=("bookmark", "vision", "both"),
+    default="bookmark",
+    help="Mine user bookmarks, logged Vision publications, or both.",
+  )
   parser.add_argument("--models-dir", type=Path, help="Optional directory containing speed_limit_us_detector.onnx and speed_limit_us_value_classifier.onnx.")
   parser.add_argument("--lead-in", type=float, default=7.0, help="Seconds before each bookmark to sample into review sheets.")
   parser.add_argument("--sample-every", type=float, default=0.5, help="Seconds between sampled lead-in frames.")
@@ -47,6 +54,8 @@ def parse_args() -> argparse.Namespace:
   parser.add_argument("--localize-sample-every", type=float, default=0.25, help="Seconds between frames while searching for the best sign candidate.")
   parser.add_argument("--top-k", type=int, default=1, help="Number of localized candidates to keep per bookmark.")
   parser.add_argument("--model-only", action="store_true", help="Match the production detector/classifier path without crop OCR.")
+  parser.add_argument("--seek-sampling", action="store_true", help="Seek directly to each context window instead of decoding from clip start.")
+  parser.add_argument("--skip-contact-sheets", action="store_true", help="Skip lead-in frames and contact sheets when only a review queue is needed.")
   parser.add_argument("--overwrite", action="store_true", help="Overwrite any existing outputs.")
   return parser.parse_args()
 
@@ -67,15 +76,20 @@ def read_log_bytes(path: Path) -> bytes:
   return path.read_bytes()
 
 
-def load_route_bookmarks(clip_root: Path, log_id: str) -> list[dict]:
+def load_route_bookmarks(clip_root: Path, log_id: str, event_types: str = "bookmark") -> list[dict]:
   segment_dirs = sorted(clip_root.glob(f"{log_id}--*"), key=lambda path: int(path.name.rsplit("--", 1)[-1]))
   if not segment_dirs:
     raise FileNotFoundError(f"No downloaded segments found for {log_id} under {clip_root}")
 
-  route_start_monotime = None
-  raw_events: list[tuple[float, str]] = []
+  raw_events: list[dict] = []
+  last_vision_speed: int | None = None
   for segment_dir in segment_dirs:
-    log_path = next((segment_dir / name for name in ("rlog.zst", "rlog.bz2", "qlog.zst", "qlog.bz2") if (segment_dir / name).exists()), None)
+    log_names = (
+      ("qlog.zst", "qlog.bz2", "rlog.zst", "rlog.bz2")
+      if event_types == "vision"
+      else ("rlog.zst", "rlog.bz2", "qlog.zst", "qlog.bz2")
+    )
+    log_path = next((segment_dir / name for name in log_names if (segment_dir / name).exists()), None)
     if log_path is None:
       continue
 
@@ -86,35 +100,66 @@ def load_route_bookmarks(clip_root: Path, log_id: str) -> list[dict]:
       continue
     if not events:
       continue
-    if route_start_monotime is None:
-      route_start_monotime = events[0].logMonoTime
+    segment = int(segment_dir.name.rsplit("--", 1)[-1])
+    road_camera_times = [event.logMonoTime for event in events if event.which() == "roadCameraState"]
+    fallback_times = [event.logMonoTime for event in events if event.which() != "initData"]
+    segment_start_monotime = min(road_camera_times or fallback_times or [events[0].logMonoTime])
 
     for event in events:
       event_type = event.which()
-      if event_type not in BOOKMARK_TYPES:
+      if event_types in ("bookmark", "both") and event_type in BOOKMARK_TYPES:
+        segment_offset_s = max((event.logMonoTime - segment_start_monotime) / 1e9, 0.0)
+        route_time_s = segment * 60.0 + segment_offset_s
+        raw_events.append({
+          "event_type": event_type,
+          "route_time_s": route_time_s,
+          "segment": segment,
+          "segment_offset_s": segment_offset_s,
+          "published_speed": "",
+          "map_speed": "",
+          "mapbox_speed": "",
+          "next_speed": "",
+        })
+
+      if event_types not in ("vision", "both") or event_type != "starpilotPlan":
+        continue
+      plan = event.starpilotPlan
+      source = str(plan.slcSpeedLimitSource)
+      if source != "Vision":
+        last_vision_speed = None
         continue
-      route_time_s = (event.logMonoTime - route_start_monotime) / 1e9
-      raw_events.append((route_time_s, event_type))
 
-  raw_events.sort(key=lambda item: item[0])
+      published_speed = round(float(plan.slcSpeedLimit) * MS_TO_MPH)
+      if published_speed <= 0 or published_speed == last_vision_speed:
+        continue
+      last_vision_speed = published_speed
+      segment_offset_s = max((event.logMonoTime - segment_start_monotime) / 1e9, 0.0)
+      raw_events.append({
+        "event_type": "visionPublish",
+        "route_time_s": segment * 60.0 + segment_offset_s,
+        "segment": segment,
+        "segment_offset_s": segment_offset_s,
+        "published_speed": published_speed,
+        "map_speed": round(float(plan.slcMapSpeedLimit) * MS_TO_MPH),
+        "mapbox_speed": round(float(plan.slcMapboxSpeedLimit) * MS_TO_MPH),
+        "next_speed": round(float(plan.slcNextSpeedLimit) * MS_TO_MPH),
+      })
+
+  raw_events.sort(key=lambda item: item["route_time_s"])
   deduped: list[dict] = []
-  for route_time_s, event_type in raw_events:
-    if deduped and abs(route_time_s - deduped[-1]["route_time_s"]) <= 0.5:
-      if event_type == "userBookmark":
-        deduped[-1]["event_type"] = event_type
-        deduped[-1]["route_time_s"] = route_time_s
-        deduped[-1]["segment"] = max(int(route_time_s // 60.0), 0)
-        deduped[-1]["segment_offset_s"] = route_time_s - deduped[-1]["segment"] * 60.0
+  for raw_event in raw_events:
+    if deduped and abs(raw_event["route_time_s"] - deduped[-1]["route_time_s"]) <= 0.5:
+      if raw_event["event_type"] == "userBookmark":
+        deduped[-1].update(raw_event)
+      elif raw_event["event_type"] == "visionPublish":
+        deduped[-1].update({
+          key: raw_event[key]
+          for key in ("published_speed", "map_speed", "mapbox_speed", "next_speed")
+        })
+        if deduped[-1]["event_type"] not in BOOKMARK_TYPES:
+          deduped[-1]["event_type"] = raw_event["event_type"]
       continue
-
-    segment = max(int(route_time_s // 60.0), 0)
-    segment_offset_s = route_time_s - segment * 60.0
-    deduped.append({
-      "event_type": event_type,
-      "route_time_s": route_time_s,
-      "segment": segment,
-      "segment_offset_s": segment_offset_s,
-    })
+    deduped.append(raw_event)
   return deduped
 
 
@@ -140,6 +185,11 @@ def write_localized_manifest(path: Path, rows: list[dict]) -> None:
       "frame_path",
       "crop_path",
       "box",
+      "event_type",
+      "published_speed",
+      "map_speed",
+      "mapbox_speed",
+      "next_speed",
     ])
     writer.writeheader()
     writer.writerows(rows)
@@ -151,6 +201,13 @@ def fmt_detection(result) -> str:
   return f"{result[0]}@{result[1]:.3f}"
 
 
+def manifest_path(path: Path, workspace: Path) -> str:
+  try:
+    return str(path.relative_to(workspace))
+  except ValueError:
+    return str(path)
+
+
 def main() -> int:
   args = parse_args()
   workspace = resolve_workspace(args.workspace)
@@ -170,12 +227,12 @@ def main() -> int:
   for raw_route in args.routes:
     dongle_id, log_id = parse_route_id(raw_route)
     session_id = f"connect_{dongle_id}_{log_id}"
-    bookmarks = load_route_bookmarks(clip_root, log_id)
+    bookmarks = load_route_bookmarks(clip_root, log_id, args.event_types)
     if not bookmarks:
       print(f"{raw_route}: no bookmark events found in downloaded rlogs")
       continue
 
-    print(f"{raw_route}: found {len(bookmarks)} bookmark(s)")
+    print(f"{raw_route}: found {len(bookmarks)} event(s)")
     for bookmark_number, bookmark in enumerate(bookmarks, start=1):
       window = BookmarkWindow(
         bookmark_number=bookmark_number,
@@ -186,13 +243,15 @@ def main() -> int:
         spans_previous_segment=float(bookmark["segment_offset_s"]) - args.lead_in < 0.0,
       )
 
-      sampled_frames = extract_window_frames({
-        "route": log_id,
-        "segment": window.segment,
-        "segmentOffsetS": window.segment_offset_s,
-        "leadinStartS": window.leadin_start_s,
-        "spansPreviousSegment": window.spans_previous_segment,
-      }, clip_root, args.sample_every, args.max_samples)
+      sampled_frames = []
+      if not args.skip_contact_sheets:
+        sampled_frames = extract_window_frames({
+          "route": log_id,
+          "segment": window.segment,
+          "segmentOffsetS": window.segment_offset_s,
+          "leadinStartS": window.leadin_start_s,
+          "spansPreviousSegment": window.spans_previous_segment,
+        }, clip_root, args.sample_every, args.max_samples)
 
       contact_sheet_frames = []
       contact_sheet_labels = []
@@ -213,8 +272,8 @@ def main() -> int:
           "segment": window.segment,
           "segment_offset_s": f"{window.segment_offset_s:.3f}",
           "sample_offset_s": f"{sample['relative_offset_s']:.3f}",
-          "frame_path": str(frame_path.relative_to(workspace)),
-          "contact_sheet_path": str(contact_sheet_path.relative_to(workspace)),
+          "frame_path": manifest_path(frame_path, workspace),
+          "contact_sheet_path": manifest_path(contact_sheet_path, workspace),
           "source_video_path": str(sample["source_video"]),
           "event_type": bookmark["event_type"],
           "route_time_s": f"{bookmark['route_time_s']:.3f}",
@@ -230,6 +289,7 @@ def main() -> int:
         args.search_before,
         args.search_after,
         args.localize_sample_every,
+        seek=args.seek_sampling,
       ):
         scored = score_frame(daemon, frame_bgr, use_ocr=not args.model_only)
         if scored is None:
@@ -271,6 +331,11 @@ def main() -> int:
           "frame_path": str(frame_path),
           "crop_path": str(crop_path),
           "box": ",".join(str(value) for value in scored["box"]),
+          "event_type": bookmark["event_type"],
+          "published_speed": bookmark["published_speed"],
+          "map_speed": bookmark["map_speed"],
+          "mapbox_speed": bookmark["mapbox_speed"],
+          "next_speed": bookmark["next_speed"],
         })
 
   ensure_dir(leadin_manifest_path.parent)