From f898f64f088f34a2ddd2eb442b9222b7d62f55ac Mon Sep 17 00:00:00 2001 From: KRMeeag Date: Thu, 30 Jul 2026 20:06:00 +0800 Subject: [PATCH 01/28] feat(control-panel): intial mvp for computer vision type --- src/second_vision/main.py | 18 +- src/second_vision/mock/mode_cycler.py | 73 +++++ src/second_vision/pipeline/app.py | 429 +++++++++++++++++++++++--- tests/test_pipeline_modes.py | 305 ++++++++++++++++++ 4 files changed, 781 insertions(+), 44 deletions(-) create mode 100644 src/second_vision/mock/mode_cycler.py create mode 100644 tests/test_pipeline_modes.py diff --git a/src/second_vision/main.py b/src/second_vision/main.py index cbea63e..67321ec 100644 --- a/src/second_vision/main.py +++ b/src/second_vision/main.py @@ -170,7 +170,23 @@ def _run_pipeline_mode(user_data, config, workers, cli_args): ) config_thread.start() workers.append(config_thread) - + + # DEBUG ONLY (--cycle-modes): stand in for the not-yet-built Arduino mode + # switch by cycling pipeline_mode on a timer. Without the flag this block is + # skipped entirely and the pipeline stays in "both". + cycle_seconds = getattr(app.options_menu, "cycle_modes", 0.0) + if cycle_seconds: + from second_vision.mock.mode_cycler import mode_cycler_worker + + cycler_thread = threading.Thread( + target=mode_cycler_worker, + args=(user_data, config, app, cycle_seconds), + daemon=True, name="mode-cycler" + ) + cycler_thread.start() + workers.append(cycler_thread) + app.start_mode_monitor() + print("[MAIN] Starting pipeline...") try: app.run() diff --git a/src/second_vision/mock/mode_cycler.py b/src/second_vision/mock/mode_cycler.py new file mode 100644 index 0000000..9e4a11b --- /dev/null +++ b/src/second_vision/mock/mode_cycler.py @@ -0,0 +1,73 @@ +""" +Mode Cycler — TEMPORARY debug driver. Delete once the Arduino control panel works. + +This is a stand-in for the physical mode switch: a timer that walks +pipeline_mode through both -> depth -> detection -> both so hot-swapping can be +exercised before the control panel hardware exists. + +It deliberately does exactly what config_reader_worker's "M:" branch does, +in the same order and from a worker thread, so the demo rehearses the real path +rather than a shortcut: + + announce over TTS -> config.update(pipeline_mode=...) -> app.trigger_rebuild() + +The announcement goes first on purpose. Speaking the mode is a UX requirement — +the user must be told the system is changing — and it doubles as audible warning +before the rebuild blackout (DECISIONS D21). + +Activated only by --cycle-modes; without that flag this module is never imported +into a run and the pipeline stays in "both" exactly as before. +""" + +import time + +# ============================================================ +# INTERFACE CONTRACT (matches workers/config_reader.py's M: branch): +# Input: wall-clock timer +# Output: config.pipeline_mode updates + app.trigger_rebuild() +# + {"announce": str} offered to user_data.tts_queue +# ============================================================ + +# Order matters: starting from "both" (the boot mode), each step changes which +# models are loaded, and the final step returns to "both" so a full cycle both +# tears down and restores the dual pipeline. +MODE_CYCLE = ("depth", "detection", "both") + + +def mode_cycler_worker(user_data, config, app, interval_seconds: float) -> None: + """ + Cycle pipeline_mode every `interval_seconds` until shutdown. + + Args: + user_data: shared user data (needs tts_queue and shutdown_event) + config: SystemConfig — the authority the pipeline builder reads + app: SecondVisionApp — provides trigger_rebuild() + interval_seconds: dwell time in each mode before switching + """ + print(f"[CYCLER] DEBUG mode cycling every {interval_seconds:g}s: " + f"both -> {' -> '.join(MODE_CYCLE)}") + + index = 0 + while not user_data.shutdown_event.is_set(): + # wait() rather than sleep() so Ctrl+C is acted on immediately instead of + # after the full interval. + if user_data.shutdown_event.wait(interval_seconds): + break + + try: + new_mode = MODE_CYCLE[index % len(MODE_CYCLE)] + index += 1 + + # Announcements carry no priority/tier: item_priority()/item_tier() + # treat them as +inf / urgent, so this is heard even mid-utterance. + user_data.tts_queue.offer({"announce": f"{new_mode} mode"}) + + config.update(pipeline_mode=new_mode) + print(f"[CYCLER] switching to {new_mode} at {time.strftime('%H:%M:%S')}") + + # Deferred onto the GLib main loop — never rebuild from this thread. + app.trigger_rebuild() + except Exception as e: # never let the worker thread die + print(f"[CYCLER] Error during mode switch: {e}") + + print("[CYCLER] stopped") diff --git a/src/second_vision/pipeline/app.py b/src/second_vision/pipeline/app.py index 04bdb2b..431002a 100644 --- a/src/second_vision/pipeline/app.py +++ b/src/second_vision/pipeline/app.py @@ -2,11 +2,14 @@ import queue import sys import threading +import time from pathlib import Path os.environ["GST_PLUGIN_FEATURE_RANK"] = "vaapidecodebin:NONE" +import cv2 import gi +import numpy as np import setproctitle gi.require_version("Gst", "1.0") @@ -56,6 +59,35 @@ hailo_logger = get_logger(__name__) +# Pipeline modes. These are the values SystemConfig.pipeline_mode takes and the ones the +# Arduino sends as "M:" (see workers/config_reader.py). +MODE_BOTH = "both" +MODE_DETECTION = "detection" +MODE_DEPTH = "depth" +VALID_MODES = (MODE_BOTH, MODE_DETECTION, MODE_DEPTH) + +# A mode swap is considered healthy if the new pipeline delivers its first frame within +# this long. Not enforced — exceeding it is logged as SLOW so it gets reported, since the +# cost of unloading/reloading a HEF on the Hailo device is the one genuinely unknown +# quantity in the swap. +SWAP_BUDGET_SECONDS = 3.0 + +# How often the --cycle-modes heartbeat reports frame flow / refreshes the debug banner. +HEARTBEAT_SECONDS = 2 + +# Give up waiting for the first frame of a swapped-in pipeline after this long, so a +# genuinely stalled swap is reported instead of polling forever. +_SWAP_POLL_TIMEOUT_SECONDS = 15.0 + +# Floor on how often the pipeline may be torn down and rebuilt. A rebuild takes roughly +# 0.4-1.1s (measured on the Pi 5 + Hailo-8), and tearing the v4l2 source down again while +# the previous teardown is still settling crashes GStreamer inside gst_object_unref. +# Requests arriving sooner are deferred, not dropped: trigger_rebuild() coalesces them and +# _rebuild_pipeline() re-checks the mode afterwards, so the pipeline always converges on +# the most recently requested mode. +MIN_REBUILD_INTERVAL_SECONDS = 2.0 + + class SecondVisionApp(GStreamerApp): def __init__(self, app_callback, user_data, config=None, parser=None): if parser is None: @@ -73,7 +105,23 @@ def __init__(self, app_callback, user_data, config=None, parser=None): help="Specific HEF model to use for detection (default: yolov8s.hef)", ) - # Handle list models flags for both + # DEBUG ONLY — temporary. Cycles pipeline_mode on a timer so mode switching can be + # exercised before the Arduino control panel exists. Omit the flag and nothing + # changes: no cycler thread, mode stays "both". See mock/mode_cycler.py. + parser.add_argument( + "--cycle-modes", + nargs="?", + type=float, + const=10.0, + default=0.0, + metavar="SECONDS", + help="DEBUG: cycle both->depth->detection every SECONDS " + "(bare flag = 10s, omitted = disabled)", + ) + + # Must come AFTER every add_argument above: handle_list_models_flag uses + # parse_known_args internally, so arguments registered after it never appear in + # --help (hailo-apps .hailo/memory/common_pitfalls.md). handle_list_models_flag(parser, DEPTH_PIPELINE) handle_list_models_flag(parser, DETECTION_PIPELINE) @@ -95,6 +143,23 @@ def __init__(self, app_callback, user_data, config=None, parser=None): self.config = config setproctitle.setproctitle("Parallel-Depth-Detection-V4") + # Set once shutdown begins, so a rebuild already queued on the idle loop can't + # resurrect a torn-down pipeline while the app is exiting. See shutdown(). + self._shutting_down = False + + # Rebuild serialization. _built_mode is the mode the live pipeline was + # actually built for, which is how a rebuild detects that the requested + # mode moved on while it was running. + self._rebuild_in_flight = False + self._last_rebuild_at = 0.0 + self._built_mode = None + + # Swap timing state (only meaningful once a rebuild has been requested). + self._swap_started_at = None + self._swap_frame_baseline = 0 + self._swap_target_mode = None + self._hb_last_count = 0 + # ---- Depth App Parameters ---- self.depth_hef_path = resolve_hef_path( self.hef_path, app_name=DEPTH_PIPELINE, arch=self.arch @@ -140,10 +205,18 @@ def __init__(self, app_callback, user_data, config=None, parser=None): self.create_pipeline() hailo_logger.debug("Pipeline created successfully") - def get_pipeline_string(self): - source_pipeline = self.get_source_pipeline(no_webcam_compression=True) - - # 1. Depth Branch + # ------------------------------------------------------------------ + # Pipeline construction + # + # get_pipeline_string() is called fresh by the framework on every + # _rebuild_pipeline(), so branching it on config.pipeline_mode is all that's + # needed to hot-swap which models run. The branch fragments below are shared + # by all three builders so a single-mode pipeline is always the exact same + # branch the dual pipeline uses, just without the tee. + # ------------------------------------------------------------------ + + def _depth_branch(self): + """Depth branch fragments: (wrapper, callback, sink).""" depth_pipeline = INFERENCE_PIPELINE( hef_path=self.depth_hef_path, post_process_so=self.depth_post_process_so, @@ -159,8 +232,10 @@ def get_pipeline_string(self): # already draws (via use_frame/set_frame), which was showing up as two # redundant video windows. Display is handled by cv2 in callbacks.py. depth_sink = "fakesink name=depth_sink sync=false" + return depth_pipeline_wrapper, depth_callback, depth_sink - # 2. Detection Branch + def _detection_branch(self): + """Detection branch fragments: (wrapper, tracker, callback, sink).""" detection_pipeline = INFERENCE_PIPELINE( hef_path=self.det_hef_path, post_process_so=self.det_post_process_so, @@ -185,65 +260,333 @@ def get_pipeline_string(self): ) det_callback = USER_CALLBACK_PIPELINE(name="det_callback") det_sink = "fakesink name=det_sink sync=false" + return detection_pipeline_wrapper, tracker_pipeline, det_callback, det_sink + + def _build_dual(self): + """Both models in parallel off a tee — the default, full-system pipeline.""" + source_pipeline = self.get_source_pipeline(no_webcam_compression=True) + depth_pipeline_wrapper, depth_callback, depth_sink = self._depth_branch() + detection_pipeline_wrapper, tracker_pipeline, det_callback, det_sink = self._detection_branch() - # 3. Parallel tee architecture (display handled by cv2 in callbacks.py) - pipeline_str = ( + # Parallel tee architecture (display handled by cv2 in callbacks.py) + return ( f"{source_pipeline} ! tee name=t " f"t. ! {QUEUE(name='depth_branch_q', leaky='downstream')} ! {depth_pipeline_wrapper} ! {depth_callback} ! {depth_sink} " f"t. ! {QUEUE(name='det_branch_q', leaky='downstream')} ! {detection_pipeline_wrapper} ! {tracker_pipeline} ! {det_callback} ! {det_sink}" ) - hailo_logger.info("Generated Pipeline string:\n%s", pipeline_str) + def _build_detection_only(self): + """Detection alone — no tee, no depth inference on the device.""" + source_pipeline = self.get_source_pipeline(no_webcam_compression=True) + detection_pipeline_wrapper, tracker_pipeline, det_callback, det_sink = self._detection_branch() + + # The leaky branch queues exist only to decouple the two parallel branches + # from each other, so a single-branch pipeline doesn't need them. + return ( + f"{source_pipeline} ! {detection_pipeline_wrapper} ! {tracker_pipeline} " + f"! {det_callback} ! {det_sink}" + ) + + def _build_depth_only(self): + """Depth alone — no tee, no detection inference on the device.""" + source_pipeline = self.get_source_pipeline(no_webcam_compression=True) + depth_pipeline_wrapper, depth_callback, depth_sink = self._depth_branch() + + return f"{source_pipeline} ! {depth_pipeline_wrapper} ! {depth_callback} ! {depth_sink}" + + def current_mode(self) -> str: + """ + The pipeline mode to build for. + + Falls back to "both" when there is no config (app.py's own main() and + main2.py both construct SecondVisionApp without one) or when the mode is + unrecognised, so an unexpected value can never leave the device with no + pipeline at all. + """ + mode = self.config.get("pipeline_mode") if self.config is not None else None + if mode is None: + return MODE_BOTH + if mode not in VALID_MODES: + hailo_logger.warning("Unknown pipeline_mode %r — falling back to %r", mode, MODE_BOTH) + return MODE_BOTH + return mode + + def get_pipeline_string(self): + mode = self.current_mode() + if mode == MODE_DETECTION: + pipeline_str = self._build_detection_only() + elif mode == MODE_DEPTH: + pipeline_str = self._build_depth_only() + else: + pipeline_str = self._build_dual() + + # Record what the live pipeline is actually running, so _rebuild_pipeline() + # can tell whether the requested mode moved on while it was rebuilding. + self._built_mode = mode + hailo_logger.info("Generated Pipeline string (mode=%s):\n%s", mode, pipeline_str) return pipeline_str def _connect_callback(self): """ - Wire the detection and depth branches to their callbacks.py handlers. + Wire the branches present in the current mode to their callbacks.py handlers. This pipeline exposes two USER_CALLBACK_PIPELINE identities ("det_callback" and "depth_callback") instead of the single "identity_callback" the base GStreamerApp expects, so the default _connect_callback can't find either one — this override replaces it. + + Exactly one branch is routed through _internal_callback_wrapper, which is + what increments user_data's frame counter (feeding get_det_fps() and the + --enable-watchdog stall detector). In "both" mode that's the detection + branch, and depth connects directly: the tee gives both branches every + frame, so wrapping both would double-count. In a single-branch mode the + one branch present has to be the wrapped one, otherwise the frame counter + freezes and the watchdog reads a healthy pipeline as a stall. """ disable_callback = self.options_menu.disable_callback + mode = self.current_mode() + + wire_det = mode in (MODE_BOTH, MODE_DETECTION) + wire_depth = mode in (MODE_BOTH, MODE_DEPTH) + # Depth is the frame-counting branch only when detection isn't there to do it. + depth_counts_frames = mode == MODE_DEPTH + + if wire_det: + det_identity = self.pipeline.get_by_name("det_callback") + if det_identity: + det_identity.set_property("signal-handoffs", True) + det_identity.connect( + "handoff", _internal_callback_wrapper, self.user_data, callbacks.on_det_frame, disable_callback + ) + hailo_logger.debug("Connected detection callback.") + else: + hailo_logger.warning("det_callback identity not found in pipeline") + + if wire_depth: + depth_identity = self.pipeline.get_by_name("depth_callback") + if depth_identity: + depth_identity.set_property("signal-handoffs", True) + if depth_counts_frames: + depth_identity.connect( + "handoff", _internal_callback_wrapper, self.user_data, callbacks.on_depth_frame, disable_callback + ) + elif not disable_callback: + depth_identity.connect("handoff", callbacks.on_depth_frame, self.user_data) + hailo_logger.debug("Connected depth callback.") + else: + hailo_logger.warning("depth_callback identity not found in pipeline") + + def _on_pipeline_rebuilt(self): + """ + Clear per-pipeline state after a rebuild. - # Detection branch goes through the internal wrapper for frame - # counting/watchdog support. - det_identity = self.pipeline.get_by_name("det_callback") - if det_identity: - det_identity.set_property("signal-handoffs", True) - det_identity.connect( - "handoff", _internal_callback_wrapper, self.user_data, callbacks.on_det_frame, disable_callback - ) - hailo_logger.debug("Connected detection callback.") - else: - hailo_logger.warning("det_callback identity not found in pipeline") - - # Depth branch connects directly — the tee means both branches see - # every frame, so routing depth through the wrapper too would - # double-increment the shared frame counter. - depth_identity = self.pipeline.get_by_name("depth_callback") - if depth_identity: - depth_identity.set_property("signal-handoffs", True) - if not disable_callback: - depth_identity.connect("handoff", callbacks.on_depth_frame, self.user_data) - hailo_logger.debug("Connected depth callback.") - else: - hailo_logger.warning("depth_callback identity not found in pipeline") + The rebuilt pipeline contains a brand-new hailotracker whose IDs restart + from scratch, but user_data survives the swap. Without this, a fresh + track_id can land on a dead track's entry and inherit its zone, + last_announced and first_seen — callbacks.py's prev["label"] == label + guard doesn't help when the label repeats, which for "person" is most of + the time. Symptom would be a just-appeared object announced as + "still ", or silently suppressed by an inherited repeat floor. + + Also resets the FPS windows so each mode's reported rate reflects that + mode rather than being averaged with the previous one. + + Guarded with getattr/hasattr: StandaloneUserData and the mock user_data + don't carry the detection tracking attributes. + """ + user_data = self.user_data + + if hasattr(user_data, "track_history"): + user_data.track_history.clear() + if hasattr(user_data, "IDs_changed_zones"): + user_data.IDs_changed_zones.clear() + if hasattr(user_data, "head_turn_cooldown_until"): + user_data.head_turn_cooldown_until = 0.0 + + now = time.monotonic() + if hasattr(user_data, "fps_start_time"): + user_data.fps_start_time = now + if hasattr(user_data, "depth_fps_start_time"): + user_data.depth_fps_start_time = now + if hasattr(user_data, "depth_frame_count"): + user_data.depth_frame_count = 0 + + # NOTE for the depth owner: this is where DepthPostProcessor.reset() belongs + # once depth post-processing lands, so EMA-smoothed readings from the previous + # mode don't bleed into the first frames of the new one. + + hailo_logger.info("Pipeline rebuilt — mode=%s, per-pipeline state reset", self.current_mode()) + + # Time the swap from here (main loop) rather than from the requesting thread. + if self._swap_started_at is not None: + GLib.timeout_add(50, self._poll_swap_complete) def trigger_rebuild(self): """ - Schedule a pipeline rebuild, called by config_reader_worker after a - mode change. Rebuilds must go through GLib.idle_add — the config - reader runs on its own thread, and GStreamer state changes aren't - safe to make directly from a thread other than the main loop's. - - Note: get_pipeline_string() isn't mode-aware yet, so this currently - just tears down and rebuilds the same fixed dual-branch pipeline — - it doesn't yet switch to a detection-only/depth-only pipeline based - on self.config.pipeline_mode. + Schedule a pipeline rebuild after a mode change. + + Called by config_reader_worker (and, for debugging, mock/mode_cycler.py) + from their own threads. Rebuilds must go through GLib.idle_add — GStreamer + state changes aren't safe to make directly from a thread other than the + main loop's. + + get_pipeline_string() is mode-aware, so this genuinely swaps which models + run, based on config.pipeline_mode. + """ + if self._shutting_down: + hailo_logger.debug("Rebuild requested during shutdown — ignoring") + return + + if self._rebuild_in_flight: + # Don't stack teardowns. The in-flight rebuild re-reads the mode when + # it finishes and rebuilds again if it changed, so this request is + # coalesced rather than lost. + hailo_logger.debug("Rebuild already in flight — request coalesced") + return + + self._rebuild_in_flight = True + self._swap_started_at = time.monotonic() + self._swap_frame_baseline = self.user_data.get_count() + self._swap_target_mode = self.current_mode() + + # Hold off if the previous rebuild only just finished — see + # MIN_REBUILD_INTERVAL_SECONDS. + since_last = time.monotonic() - self._last_rebuild_at + delay_ms = max(1, int((MIN_REBUILD_INTERVAL_SECONDS - since_last) * 1000)) + GLib.timeout_add(delay_ms, self._rebuild_pipeline) + + def _rebuild_pipeline(self): + """ + Serialize rebuilds and converge on the latest requested mode. + + Guards the framework's rebuild two ways: one queued while the app is + exiting must not resurrect a torn-down pipeline, and two rebuilds must + never overlap — tearing the v4l2 source down again mid-teardown segfaults + GStreamer in gst_object_unref. + """ + if self._shutting_down: + hailo_logger.debug("Skipping rebuild — shutdown in progress") + self._rebuild_in_flight = False + return False + + try: + return super()._rebuild_pipeline() + finally: + self._last_rebuild_at = time.monotonic() + self._rebuild_in_flight = False + # The mode may have changed again while this rebuild was running + # (coalesced above). Converge on it now. + if not self._shutting_down and self.current_mode() != self._built_mode: + hailo_logger.debug("Mode changed during rebuild — rebuilding again") + self.trigger_rebuild() + + def shutdown(self, signum=None, frame=None): + """ + Shut down cleanly even if the signal arrives mid-swap. + + The framework's shutdown() calls self.pipeline.set_state(...), but + _rebuild_pipeline() sets self.pipeline = None while tearing the old + pipeline down. A Ctrl+C landing in that window raises AttributeError — + rare normally, but mode cycling passes through that window on every + swap. Handled here in the subclass; the library is not patched. + """ + self._shutting_down = True + + if self.pipeline is None: + hailo_logger.warning("Shutdown during pipeline rebuild — quitting main loop directly") + if self.loop is not None: + GLib.idle_add(self.loop.quit) + return + + super().shutdown(signum, frame) + + # ------------------------------------------------------------------ + # DEBUG ONLY — mode monitoring, active only under --cycle-modes. + # Everything below is observability for the mode-switching demo and can be + # removed with mock/mode_cycler.py once the Arduino control panel drives + # mode changes for real. + # ------------------------------------------------------------------ + + def start_mode_monitor(self): + """Begin the periodic mode heartbeat. Registered on the GLib main loop.""" + self._hb_last_count = self.user_data.get_count() + GLib.timeout_add_seconds(HEARTBEAT_SECONDS, self._mode_heartbeat) + hailo_logger.info("Mode monitor started (every %ss)", HEARTBEAT_SECONDS) + + def _mode_heartbeat(self): + """ + Report frame flow for whichever mode is active. + + Depth-only mode produces no console output of its own — the depth + callback is still a placeholder — so this is the only evidence that its + pipeline is genuinely running rather than silently stalled. + """ + if self._shutting_down: + return False + + mode = self.current_mode() + count = self.user_data.get_count() + fps = (count - self._hb_last_count) / HEARTBEAT_SECONDS + self._hb_last_count = count + + print(f"[MODE] {mode} pipeline active — {count} frames, {fps:.1f} FPS") + self._push_debug_banner(mode) + return True + + def _poll_swap_complete(self): + """ + Time a swap by when the new pipeline delivers its first frame — the point + the device is actually useful again, rather than when GStreamer reports + PLAYING. Registered from _on_pipeline_rebuilt(), so it runs on the main loop. + """ + if self._shutting_down or self._swap_started_at is None: + return False + + elapsed = time.monotonic() - self._swap_started_at + + if self.user_data.get_count() > self._swap_frame_baseline: + verdict = "OK" if elapsed <= SWAP_BUDGET_SECONDS else "SLOW" + print(f"[SWAP] -> {self._swap_target_mode}: first frame after {elapsed:.2f}s " + f"({verdict}, budget {SWAP_BUDGET_SECONDS:.0f}s)") + self._swap_started_at = None + return False + + # Safety cap: stop polling rather than spin forever if frames never resume. + if elapsed > _SWAP_POLL_TIMEOUT_SECONDS: + print(f"[SWAP] -> {self._swap_target_mode}: NO FRAMES after {elapsed:.1f}s " + f"— pipeline appears stalled") + self._swap_started_at = None + return False + + return True + + def _push_debug_banner(self, mode): + """ + Explain a frozen cv2 window. + + The debug overlay is drawn by the detection callback, so in depth-only + mode nothing calls set_frame() and the display process keeps showing its + last detection frame — indistinguishable from a hang. Push a labelled + frame instead. set_frame() is a non-blocking put and the display process + holds the last imshow, so one push per heartbeat is enough. """ - GLib.idle_add(self._rebuild_pipeline) + if mode != MODE_DEPTH: + return # detection overlay is live in the other modes + if not getattr(self.options_menu, "use_frame", False): + return # no display process running + + canvas = np.zeros((self.video_height, self.video_width, 3), dtype=np.uint8) + lines = [ + "[DEBUG]: depth-only pipeline active.", + "Detection visual debug frozen", + ] + y = max(40, self.video_height // 2 - 20) + for line in lines: + cv2.putText(canvas, line, (20, y), cv2.FONT_HERSHEY_SIMPLEX, + 0.7, (255, 255, 255), 2) + y += 40 + + self.user_data.set_frame(canvas) class StandaloneUserData(callbacks.user_app_callback_class): """ diff --git a/tests/test_pipeline_modes.py b/tests/test_pipeline_modes.py new file mode 100644 index 0000000..49e61de --- /dev/null +++ b/tests/test_pipeline_modes.py @@ -0,0 +1,305 @@ +""" +Unit tests for mode-aware pipeline construction (detection / depth / both). + +The point of these tests is the guarantee that mode switching did NOT change the +normal run: `_build_dual()` must still produce the same dual-branch pipeline it +always did, and the single-mode pipelines must reuse the *same* branch fragments +rather than diverging copies. + +Hardware-independent. `gi`, `hailo`, and the two hailo_apps modules that need +them are stubbed below (the mocked-hailo harness this project already uses for +callback logic); everything else — including the real INFERENCE_PIPELINE / +TRACKER_PIPELINE / QUEUE helpers that actually compose the strings — is imported +for real, so these assert against genuine GStreamer output. Run with: + + poetry run pytest tests/test_pipeline_modes.py -v +""" + +import sys +import types +from pathlib import Path +from types import SimpleNamespace + +# No conftest/package install — put src/ on the path so `second_vision.*` resolves. +sys.path.insert(0, str(Path(__file__).resolve().parent.parent / "src")) + + +# ============================================================ +# Mocked-hailo harness — stub only what genuinely needs the GStreamer runtime. +# The pipeline-string helpers are NOT stubbed; they are the code under test. +# ============================================================ +def _install_hailo_stubs(): + gi = types.ModuleType("gi") + gi.require_version = lambda *args, **kwargs: None + repository = types.ModuleType("gi.repository") + repository.Gst = SimpleNamespace(init=lambda *a: None) + repository.GLib = SimpleNamespace( + idle_add=lambda *a, **k: None, + timeout_add=lambda *a, **k: None, + timeout_add_seconds=lambda *a, **k: None, + ) + gi.repository = repository + sys.modules.setdefault("gi", gi) + sys.modules.setdefault("gi.repository", repository) + + sys.modules.setdefault("hailo", types.ModuleType("hailo")) + + buffer_utils = types.ModuleType("hailo_apps.python.core.common.buffer_utils") + buffer_utils.get_caps_from_pad = lambda pad: (None, None, None) + buffer_utils.get_numpy_from_buffer = lambda *a: None + sys.modules.setdefault("hailo_apps.python.core.common.buffer_utils", buffer_utils) + + gstreamer_app = types.ModuleType("hailo_apps.python.core.gstreamer.gstreamer_app") + + class _AppCallbackClass: + def __init__(self): + self.frame_count = 0 + self.use_frame = False + self.running = True + + def increment(self): + self.frame_count += 1 + + def get_count(self): + return self.frame_count + + def set_frame(self, frame): + pass + + class _GStreamerApp: + """Stand-in base. The builders under test never call up into it.""" + + gstreamer_app.app_callback_class = _AppCallbackClass + gstreamer_app.GStreamerApp = _GStreamerApp + gstreamer_app._internal_callback_wrapper = lambda *a, **k: None + sys.modules.setdefault("hailo_apps.python.core.gstreamer.gstreamer_app", gstreamer_app) + + +_install_hailo_stubs() + +from second_vision.pipeline import app as app_mod # noqa: E402 + + +# ============================================================ +# Fixtures +# ============================================================ +class _FakeConfig: + """Minimal stand-in for SystemConfig — only pipeline_mode is read here.""" + + def __init__(self, mode): + self._mode = mode + + def get(self, key): + return self._mode if key == "pipeline_mode" else None + + +def _fake_app(mode="both", config=...): + """ + A real SecondVisionApp instance with __init__ skipped. + + SecondVisionApp.__init__ parses argv and creates a live GStreamer pipeline, so + it cannot be constructed under pytest. __new__ gives a genuine instance — real + methods, real dispatch — onto which only the attributes the builders read are + set. `get_source_pipeline` is shadowed with a stub since it belongs to the + GStreamer base class. + """ + app = app_mod.SecondVisionApp.__new__(app_mod.SecondVisionApp) + app.depth_hef_path = "/fake/models/scdepthv3.hef" + app.depth_post_process_so = "/fake/so/libdepth_postprocess.so" + app.depth_post_function_name = "filter_scdepth" + app.det_hef_path = "/fake/models/yolov8s.hef" + app.det_post_process_so = "/fake/so/libyolo_hailortpp_postprocess.so" + app.det_post_function_name = "filter_letterbox" + app.batch_size = 2 + app.labels_json = None + app.thresholds_str = "nms-score-threshold=0.3 nms-iou-threshold=0.45" + app.config = _FakeConfig(mode) if config is ... else config + app.get_source_pipeline = lambda **kwargs: "FAKE_SOURCE" + app.video_width = 640 + app.video_height = 640 + return app + + +def _dual(app=None): + return (app or _fake_app())._build_dual() + + +def _detection_only(app=None): + return (app or _fake_app())._build_detection_only() + + +def _depth_only(app=None): + return (app or _fake_app())._build_depth_only() + + +# ============================================================ +# The dual pipeline is unchanged — normal runs are unaffected +# ============================================================ +def test_dual_pipeline_has_tee_and_both_branches(): + pipeline = _dual() + + assert "tee name=t" in pipeline + assert "identity name=det_callback" in pipeline + assert "identity name=depth_callback" in pipeline + assert "hailotracker" in pipeline + assert "inference_wrapper_depth" in pipeline + assert "inference_wrapper_det" in pipeline + assert "fakesink name=depth_sink" in pipeline + assert "fakesink name=det_sink" in pipeline + + +def test_dual_pipeline_keeps_leaky_branch_queues(): + # The leaky branch queues decouple the two parallel branches; losing them + # would let a slow branch stall the other. + pipeline = _dual() + assert "queue name=depth_branch_q leaky=downstream" in pipeline + assert "queue name=det_branch_q leaky=downstream" in pipeline + + +def test_depth_branch_disables_letterbox_in_every_mode(): + # The depth wrapper must not letterbox — it would distort the depth map. + for pipeline in (_dual(), _depth_only()): + assert "use-letterbox=false" in pipeline + assert "use-letterbox=true" not in pipeline.split("inference_wrapper_det")[0] + + +# ============================================================ +# Single-mode pipelines: one model, no tee +# ============================================================ +def test_dual_loads_two_networks_single_modes_load_one(): + # "hailonet name=" is the element declaration itself — plain "hailonet" also + # matches the queue feeding it (`..._hailonet_q`), which would overcount. + assert _dual().count("hailonet name=") == 2 + assert _detection_only().count("hailonet name=") == 1 + assert _depth_only().count("hailonet name=") == 1 + + +def test_single_modes_have_no_tee(): + assert "tee name=t" not in _detection_only() + assert "tee name=t" not in _depth_only() + + +def test_detection_only_omits_the_depth_branch(): + pipeline = _detection_only() + assert "identity name=det_callback" in pipeline + assert "hailotracker" in pipeline + assert "identity name=depth_callback" not in pipeline + assert "depth_inference" not in pipeline + + +def test_depth_only_omits_the_detection_branch(): + pipeline = _depth_only() + assert "identity name=depth_callback" in pipeline + assert "identity name=det_callback" not in pipeline + assert "hailotracker" not in pipeline + assert "det_inference" not in pipeline + + +def test_single_modes_reuse_the_dual_branch_fragments(): + """ + A single-mode pipeline must be the *same* branch the dual pipeline uses, so + tuning (tracker params, letterbox, thresholds) can never drift between modes. + """ + app = _fake_app() + dual = _dual(app) + + wrapper, tracker, callback, _sink = app._detection_branch() + assert wrapper in dual and tracker in dual and callback in dual + assert wrapper in _detection_only(app) + + depth_wrapper, depth_callback, _depth_sink = app._depth_branch() + assert depth_wrapper in dual and depth_callback in dual + assert depth_wrapper in _depth_only(app) + + +# ============================================================ +# Mode dispatch and its fallbacks +# ============================================================ +def test_dispatch_selects_the_matching_builder(): + both = _fake_app("both") + assert both.get_pipeline_string() == _dual(both) + + det = _fake_app("detection") + assert det.get_pipeline_string() == _detection_only(det) + + depth = _fake_app("depth") + assert depth.get_pipeline_string() == _depth_only(depth) + + +def test_missing_config_falls_back_to_dual(): + # app.py's own main() and main2.py both construct SecondVisionApp without a + # config; that must not leave the device with no pipeline. + app = _fake_app(config=None) + assert app.current_mode() == app_mod.MODE_BOTH + assert app.get_pipeline_string() == _dual(app) + + +def test_unknown_mode_falls_back_to_dual(): + app = _fake_app("sideways") + assert app.current_mode() == app_mod.MODE_BOTH + assert app.get_pipeline_string() == _dual(app) + + +def test_none_mode_falls_back_to_dual(): + app = _fake_app(None) + assert app.current_mode() == app_mod.MODE_BOTH + + +# ============================================================ +# Each mode exposes exactly the callback identities _connect_callback() wires +# ============================================================ +class _RecordingUserData: + """Captures set_frame() calls the way the display process would consume them.""" + + def __init__(self): + self.frames = [] + + def set_frame(self, frame): + self.frames.append(frame) + + +def _banner_app(mode, use_frame): + app = _fake_app(mode) + app.user_data = _RecordingUserData() + app.options_menu = SimpleNamespace(use_frame=use_frame) + return app + + +def test_debug_banner_only_in_depth_mode(): + # Detection and both modes draw a live overlay from the detection callback, + # so a banner would overwrite real video. + for mode in (app_mod.MODE_BOTH, app_mod.MODE_DETECTION): + app = _banner_app(mode, use_frame=True) + app._push_debug_banner(mode) + assert app.user_data.frames == [], mode + + app = _banner_app(app_mod.MODE_DEPTH, use_frame=True) + app._push_debug_banner(app_mod.MODE_DEPTH) + assert len(app.user_data.frames) == 1 + + +def test_debug_banner_frame_matches_video_dimensions(): + app = _banner_app(app_mod.MODE_DEPTH, use_frame=True) + app._push_debug_banner(app_mod.MODE_DEPTH) + frame = app.user_data.frames[0] + assert frame.shape == (app.video_height, app.video_width, 3) + assert frame.any(), "banner text should have been drawn onto the canvas" + + +def test_debug_banner_skipped_without_use_frame(): + # No --use-frame means no display process is running to consume it. + app = _banner_app(app_mod.MODE_DEPTH, use_frame=False) + app._push_debug_banner(app_mod.MODE_DEPTH) + assert app.user_data.frames == [] + + +def test_each_mode_exposes_only_its_own_callback_identities(): + expected = { + app_mod.MODE_BOTH: (True, True), + app_mod.MODE_DETECTION: (True, False), + app_mod.MODE_DEPTH: (False, True), + } + for mode, (has_det, has_depth) in expected.items(): + pipeline = _fake_app(mode).get_pipeline_string() + assert ("identity name=det_callback" in pipeline) is has_det, mode + assert ("identity name=depth_callback" in pipeline) is has_depth, mode From fdb92a2e5d65a8fe9b989a1631b29db2bfd613b3 Mon Sep 17 00:00:00 2001 From: KRMeeag Date: Thu, 30 Jul 2026 20:25:51 +0800 Subject: [PATCH 02/28] feat(depth-callback): add real depth post-processing and preview --- src/second_vision/core/depth_utils.py | 838 +++++++++++++++++- src/second_vision/core/depth_view.py | 217 +++++ .../pipeline/TBR-callback-depth-est.py | 0 3 files changed, 1014 insertions(+), 41 deletions(-) create mode 100644 src/second_vision/core/depth_view.py create mode 100644 src/second_vision/pipeline/TBR-callback-depth-est.py diff --git a/src/second_vision/core/depth_utils.py b/src/second_vision/core/depth_utils.py index ea3dbd5..e3bc193 100644 --- a/src/second_vision/core/depth_utils.py +++ b/src/second_vision/core/depth_utils.py @@ -1,87 +1,843 @@ """ Depth Utilities — Zone splitting, proximity curves, and ground hazard detection. -Pure functions, no I/O — safe to unit test without hardware. +Post-processing pipeline (per SV-Docu/depth_estimation_handoff.md): + thin-structure ridge pass [cables/poles/branches, at native resolution] -> + downsample -> zone split -> per zone max(sub-grid pooling [near clusters], + blank-wall detection [flat surfaces], floor-to-wall [walls read as far], + thin structures) -> safe-zone threshold -> non-linear curve -> EMA smoothing + -> 0-255 motor intensity. + +All math is vectorized NumPy on a downsampled grid; safe to run every frame +inside the GStreamer callback. Pure functions except DepthPostProcessor, +which holds the EMA state — all unit-testable without hardware. """ from typing import Tuple +import cv2 import numpy as np -MAX_DEPTH_M = 5.0 -MIN_DEPTH_M = 0.3 +# NOTE on units: the scdepthv3 postprocess (.so) emits RELATIVE depth, not +# meters. Measured on live runs, the nearest representable value is ~18.4 and +# far field reads 50+. These cutoffs are in those model units. +MAX_DEPTH_M = 30.0 # safe-zone cutoff: at/beyond this output is 0.0 +MIN_DEPTH_M = 15.0 # near-end floor of the model output range — measured: + # live captures consistently show p1 in the 15.0-15.5 + # range (was 18.4, which clamped the whole 15-18.4 + # "about to collide" band to one flat max reading) +NEAR_PERCENTILE = 10.0 # aggregate the closest ~10% pixel cluster per zone +CURVE_EXPONENT = 2.0 # intensity spikes only when critically close +EMA_ALPHA = 0.3 # ~0.2 s settle time at 30 FPS +DOWNSAMPLE_SIZE = (64, 48) # (width, height) grid for all zone math +BORDER_CROP_RATIO = 0.04 # trim 4% off each edge to drop conv border artifacts +SUBGRID_SHAPE = (4, 4) # per-zone (rows, cols) pooling grid for thin objects +DANGER_CELL = 0.5 # per-cell proximity (0-1) at/above which a cell is "danger" +# Fraction of a zone's sub-cells in danger above which the near reading is a +# BROAD surface (wall/door/furniture face) rather than a thin/localized object. +# A thin object physically cannot exceed ~a column of cells (~0.25); the live +# wall's centre zone measured ~0.35 and was mislabelled "thin" at 0.40, so the +# boundary sits between those. PLACEHOLDER pending calibration refinement. +BROAD_COVERAGE_MIN = 0.30 +# Blank-wall detection: a textureless surface is LOCALLY smooth even when the +# model renders it as a washed-out gradient (live Pi walls read as a dome: +# ~15 at the edges -> ~30 mid-wall, so a zone-global std is useless). +# WALL_VARIANCE_MAX is the median per-sub-cell std (model units) below which a +# zone is treated as a solid surface. PLACEHOLDER — set from calibration +# (real wall cells ~1-1.5 here; open/textured scenes read several times that). +WALL_VARIANCE_MAX = 3.0 +# Flatness is a graded CONFIDENCE, not a pass/fail gate: confidence is full up to +# WALL_VARIANCE_MAX and fades to zero at WALL_VARIANCE_MAX * WALL_VARIANCE_SOFT. +# A hard gate made the wall correction flip fully on/off on a hair of texture, +# and live walls sit at ~2.0 against a limit of 3.0 — far too thin a margin for +# an on/off decision worth 100+ points of motor intensity. PLACEHOLDER. +WALL_VARIANCE_SOFT = 1.5 +# Flatness is judged PER SUB-CELL, not over the whole zone, so an object standing +# in front of a wall cannot disqualify the wall behind it (live finding: putting +# anything in frame dropped a saturating wall back to the raw curve — a wall with +# a chair against it is a worse hazard than a bare one, not a lesser one). A real +# surface still has to cover a substantial part of the zone, or a single smooth +# patch — a book cover, a table top — would pass as a wall. PLACEHOLDER. +WALL_MIN_CELLS = 4 # of SUBGRID_SHAPE's 16 cells +# Once a zone is CONFIRMED as a solid wall, a graded proximity at/above this +# level saturates to 1.0: a solid surface whose nearest credible part already +# reads strong-warning range is a collision about to happen, and the model's +# washed-out mid-wall guesses must not soften the buzz (live finding: a +# point-blank wall read only 186/255). PLACEHOLDER pending calibration. +WALL_CONFIRM_SATURATE = 0.5 +# Where the confirmed-wall correction STARTS. Below this the reading is passed +# through untouched, so open floors and distant walls behave exactly as they did +# before; between here and WALL_CONFIRM_SATURATE it ramps smoothly to full +# warning. This replaced a hard `if value >= 0.5: return 1.0` step that made a +# wall at 20.5 model units output 119 and the same wall at 20.0 output 255 — the +# 255-vs-135 flip reported from the field. PLACEHOLDER pending calibration. +WALL_BOOST_FROM = 0.30 +# Floor-to-wall intersection: as the floor recedes upward its depth grows in +# small steps; where it meets a wall the model jumps to "far". A row-to-row jump +# larger than this ratio x the typical floor step marks that break. PLACEHOLDER. +FLOOR_WALL_JUMP_RATIO = 3.0 +# Absolute minimum break size (model units). Without it, a smooth surface's +# near-zero baseline let ~0.8-unit sensor wiggles register as walls with full +# confidence (live Pi false positive). Same medicine as HAZARD_MIN_STEP. +# PLACEHOLDER pending calibration. +FLOOR_WALL_MIN_JUMP = 2.0 +MIN_FLOOR_ROWS = 3 # need this many receding-floor rows below the break to trust it GROUND_STRIP_FRACTION = 0.25 HAZARD_THRESHOLD_RATIO = 5.0 +# Absolute row-to-row jump (model units) below which a ground break is noise. +# Needed because the ratio test alone is meaningless when the strip is flat: the +# baseline collapses toward 0, making the ratio either explode on specks or be +# skipped entirely. PLACEHOLDER pending calibration. +HAZARD_MIN_STEP = 2.0 +# Break size (model units) that grades as maximum severity (255). Severity is +# graded on the ABSOLUTE jump, not the ratio: the ratio's denominator is scene +# noise, which made severity flap 26<->255 on a completely static scene (live +# finding). PLACEHOLDER pending calibration. +HAZARD_MAX_JUMP = 20.0 +# Thin-structure (ridge) detection — cables, cords, ropes, poles, branches, +# railing bars. Measured on the live Pi: these do NOT read as "near" at all. A +# cable lying 1-8 model units in front of the surface behind it moved the zone's +# sub-grid reading from 0.14 (empty scene) to 0.14-0.30 — i.e. inside the +# background's own spread, so every magnitude-based detector is blind to it by +# construction, at any resolution. +# +# What separates a cable from its background is not HOW NEAR it is but its +# SHAPE: a narrow structure standing in front of a wider surround. That is a +# morphological top-hat. A grey-scale CLOSING with a kernel wider than the +# structure erases it (near = small values, so a cable is a thin dark line); +# closed - depth therefore isolates exactly the structures narrower than the +# kernel and leaves broad surfaces at zero. Measured separation on synthetic +# scenes at the model's native 320x256: cable 1.7-8.6, empty scene 1.25, blank +# wall 1.08, large near box 0.00. +# +# Kernel width in NATIVE depth-map pixels. Anything narrower is a candidate; +# anything wider is a surface and belongs to the wall/sub-grid detectors. 15 px +# at 320 px across a ~60 deg HFOV covers the target list with margin: an 8 mm +# power cord at 0.3 m (~9 px), a 25 mm pole at 0.5 m (~14 px), a 15 mm railing +# bar at 1 m (~4 px). PLACEHOLDER pending calibration. +THIN_MAX_WIDTH_PX = 15 +# Model units a structure must stand out from its local surround. The empty-scene +# noise floor measured 1.25, so 2.0 clears it with margin. The cost of raising it +# is real: a cable with less than ~1.5 units of local contrast stays invisible. +# PLACEHOLDER pending calibration. +THIN_MIN_CONTRAST = 2.0 +# Fraction of a zone that must survive the ridge test before it counts. A cable +# crossing a zone covers ~4-6%; speckle surviving the 3x3 opening is ~0.1%. +# PLACEHOLDER pending calibration. +THIN_MIN_AREA_FRAC = 0.005 +THIN_MIN_AREA_PX = 4 # absolute floor, so tiny grids still need >1 pixel + +ZONE_NAMES = ("left", "center", "right") + +def _zone_bounds(width: int) -> dict[str, Tuple[int, int]]: + """The 25/50/25 left/center/right split, shared by every zone-wise stage.""" + return {"left": (0, width // 4), + "center": (width // 4, 3 * width // 4), + "right": (3 * width // 4, width)} -def _filter_outliers(values: np.ndarray, low_pct: float = 5.0, high_pct: float = 95.0) -> np.ndarray: - """Drop values outside the [low_pct, high_pct] percentile band.""" - lo, hi = np.percentile(values, [low_pct, high_pct]) - filtered = values[(values >= lo) & (values <= hi)] - return filtered if filtered.size else values +def downsample_depth(depth_map: np.ndarray, size: Tuple[int, int] = DOWNSAMPLE_SIZE) -> np.ndarray: + """Shrink the depth map to a small grid so all subsequent math is cheap.""" + target_w, target_h = size + h, w = depth_map.shape + if h <= target_h and w <= target_w: + return depth_map + return cv2.resize(depth_map, (target_w, target_h), interpolation=cv2.INTER_NEAREST) -def compute_proximity(zone_slice: np.ndarray, max_depth: float = MAX_DEPTH_M, min_depth: float = MIN_DEPTH_M) -> int: + +def crop_border(depth_map: np.ndarray, ratio: float = BORDER_CROP_RATIO) -> np.ndarray: """ - Convert a zone's raw depth values into a 0-255 motor intensity. + Trim a border margin off the depth map to remove convolutional edge + artifacts. - Uses an inverse-square falloff (closer = disproportionately stronger) - instead of a linear mapping, which feels unnatural to users. Percentile - filtering removes sensor noise/reflection outliers before averaging. + SC-DepthV3 produces unreliable — usually spuriously-near — values in the + outermost rows/columns because the convolutions run against zero-padding + with a truncated receptive field there. Those pixels would otherwise pull + the low-percentile zone aggregation toward "near" (false side intensities) + and paint a red ring in the visualization. Cropping before any aggregation + keeps them out of both. Returns the map unchanged if it is too small to + crop safely. """ - filtered = _filter_outliers(zone_slice.ravel()) - avg_depth = float(np.mean(filtered)) + h, w = depth_map.shape + mh, mw = int(h * ratio), int(w * ratio) + if mh == 0 and mw == 0: + return depth_map + if h - 2 * mh < 1 or w - 2 * mw < 1: + return depth_map + return depth_map[mh:h - mh, mw:w - mw] + - clamped = max(min_depth, min(avg_depth, max_depth)) +def _proximity_curve(near_depth: float, max_depth: float = MAX_DEPTH_M, min_depth: float = MIN_DEPTH_M) -> float: + """ + Map a single near-depth value to a 0.0-1.0 warning intensity: 0 at/beyond + the safe-zone cutoff, rising along the non-linear curve as it gets closer. + Shared by the percentile and blank-wall aggregations. + """ + if near_depth >= max_depth: + return 0.0 + clamped = max(min_depth, near_depth) falloff = (max_depth - clamped) / (max_depth - min_depth) - return int(round(255 * falloff ** 2)) + return float(falloff ** CURVE_EXPONENT) -def compute_zone_intensities(depth_data: np.ndarray, width: int) -> dict[str, int]: +def compute_proximity(zone_slice: np.ndarray, max_depth: float = MAX_DEPTH_M, min_depth: float = MIN_DEPTH_M) -> float: """ - Split a depth frame into left/center/right zones (25/50/25) and - return each zone's motor intensity (0-255). + Convert a zone's depth values into a 0.0-1.0 warning intensity. + + Aggregates with a low percentile (the closest pixel cluster) rather than + the zone mean, so a single nearby obstacle in an otherwise open zone is + not averaged away, while still rejecting single-pixel noise. + """ + near_depth = float(np.percentile(zone_slice, NEAR_PERCENTILE)) + return _proximity_curve(near_depth, max_depth, min_depth) + + +def subgrid_cell_stds(zone_slice: np.ndarray, grid_shape: Tuple[int, int] = SUBGRID_SHAPE) -> np.ndarray: + """ + Per-cell standard deviation over the sub-grid, as a (rows, cols) array. + + The per-cell quantity behind both `local_flatness` (which takes its median) + and `detect_blank_wall` (which judges each cell separately, so that an + object standing in front of a wall cannot disqualify the whole zone). + """ + rows, cols = grid_shape + h, w = zone_slice.shape + if h < rows or w < cols: + return np.full((1, 1), float(np.std(zone_slice))) + if h % rows == 0 and w % cols == 0: + cells = (zone_slice.reshape(rows, h // rows, cols, w // cols) + .transpose(0, 2, 1, 3) + .reshape(rows * cols, -1)) + return np.std(cells, axis=1).reshape(rows, cols) + out = np.zeros((rows, cols), dtype=float) + for r, band in enumerate(np.array_split(zone_slice, rows, axis=0)): + for c, cell in enumerate(np.array_split(band, cols, axis=1)): + if cell.size: + out[r, c] = float(np.std(cell)) + return out + + +def local_flatness(zone_slice: np.ndarray) -> float: + """ + The zone's LOCAL smoothness: median std across the sub-grid cells. + + Exposed so calibration measures what the detectors actually use — recording + a zone-global std instead reads ~2x higher on a dome-rendered wall and would + calibrate the threshold to the wrong side. """ - left = depth_data[:, :width // 4] - center = depth_data[:, width // 4: 3 * width // 4] - right = depth_data[:, 3 * width // 4:] + rows, cols = SUBGRID_SHAPE + h, w = zone_slice.shape + if h < rows or w < cols: + return float(np.std(zone_slice)) + if h % rows == 0 and w % cols == 0: + cells = (zone_slice.reshape(rows, h // rows, cols, w // cols) + .transpose(0, 2, 1, 3) + .reshape(rows * cols, -1)) + return float(np.median(np.std(cells, axis=1))) + stds = [float(np.std(c)) + for band in np.array_split(zone_slice, rows, axis=0) + for c in np.array_split(band, cols, axis=1) if c.size] + return float(np.median(stds)) + +def wall_confidence(flatness: float, variance_max: float = WALL_VARIANCE_MAX) -> float: + """ + How confident are we that this zone is a solid, textureless surface? 1.0 = + certain, 0.0 = too much structure to call it a wall. + + Deliberately a RAMP, not the pass/fail test this used to be. A hard gate at + `variance_max` meant a hair more texture flipped the wall correction from + fully on to fully off, and on real walls the margin is thin (live captures + measured flatness ~2.0 against a limit of 3.0). That turned an imperceptible + change in the scene — a light going on, someone stepping into frame — into a + 100+ point swing in what the motors did. + + Full confidence up to `variance_max`, fading to none at + `variance_max * WALL_VARIANCE_SOFT`, so the correction is withdrawn + gradually as a scene stops looking like a wall. + """ + if flatness <= variance_max: + return 1.0 + span = variance_max * (WALL_VARIANCE_SOFT - 1.0) + if span <= 0: + return 0.0 + return float(np.clip(1.0 - (flatness - variance_max) / span, 0.0, 1.0)) + + +def ground_break_stats(depth_map: np.ndarray, frame_height: int) -> dict: + """ + Row-gradient statistics of the ground strip — the raw quantities + detect_ground_hazard thresholds on (HAZARD_MIN_STEP, HAZARD_MAX_JUMP) and + the floor-profile break size behind FLOOR_WALL_MIN_JUMP. + + Diagnostics/calibration only; returns zeros when the strip is too thin. + """ + strip_start = int(frame_height * (1 - GROUND_STRIP_FRACTION)) + strip = depth_map[strip_start:, :] + if strip.shape[0] < 2: + return {"max_grad": 0.0, "median_grad": 0.0, "signed_break": 0.0} + profile = np.mean(strip, axis=1)[::-1] # bottom row first + diffs = np.diff(profile) + gradient = np.abs(diffs) + break_i = int(np.argmax(gradient)) return { - "left": compute_proximity(left), - "center": compute_proximity(center), - "right": compute_proximity(right), + "max_grad": float(gradient[break_i]), + "median_grad": float(np.median(gradient)), + "signed_break": float(diffs[break_i]), # + = drop-off, - = step-up } +def detect_blank_wall(zone_slice: np.ndarray, variance_max: float = WALL_VARIANCE_MAX) -> float: + """ + Warning intensity for a textureless close surface (a blank wall). + + Per the handoff, walls render as "washed-out, uncertain gradients" — and live + Pi captures confirm it: a real wall reads as a smooth DOME (near at the + edges, "far" mid-wall), not a flat plane. So "low variance" must be judged + LOCALLY: the flatness test is the median of per-sub-cell std over the 4x4 + grid. A wall is smooth in every small patch even when it bows globally; + genuinely open/textured scenes are rough in every patch. (The old zone-global + std failed on exactly this — the dome's spread pushed it over the limit and + real walls went undetected outside the flattest zone.) + + When the zone is locally smooth — i.e. CONFIRMED as a solid surface — the + warning is graded from its NEAREST credible cluster (the NEAR_PERCENTILE of + the zone), not the median: a wall is one connected surface, so its collision + distance is its nearest part, and the washed-out mid-wall readings are + precisely where scale ambiguity inflates the distance. Grading from the + median let that corruption soften the warning (live finding: point-blank + wall buzzed 186/255 because the dome's middle dragged the median "far"). + + A confirmed wall's reading is then CORRECTED UPWARD, reaching full warning at + WALL_CONFIRM_SATURATE proximity — a solid surface that near IS the collision + case this edge case exists for, and the model's under-reading must not cap + the buzz. + + Both steps are CONTINUOUS, and that matters more than it sounds. This used to + be a pass/fail flatness gate followed by a hard `if value >= 0.5: return 1.0`, + which gave the device exactly two behaviours on a wall: 255, or the raw curve + (~100-135). Measured: a wall at 20.5 model units output 119 and the same wall + at 20.0 output 255 — a 136-point jump for a few centimetres. Any scene change + that nudged the estimate across that line (a light switching on, someone + stepping into frame) flipped the motors between the two. Now the response is + monotonic in distance and degrades smoothly as a surface stops looking flat. + + Returns 0.0 for scenes too textured to call a wall and for smooth-but-far + surfaces (near cluster beyond the safe-zone cutoff). Combined via max() with + the other detectors, so it can only raise a warning, never suppress one — + fail-safe. (At the confidence boundary the returned value equals the plain + curve, which the sub-grid detector already meets or exceeds, so dropping it + to 0.0 there cannot change what the device does.) + + NOTE: variance_max is in relative model units and is a PLACEHOLDER pending + the metric-calibration measurement. Walls whose readings land entirely + beyond the cutoff (scale ambiguity) still need the separate floor-to-wall + strategy. + """ + stds = subgrid_cell_stds(zone_slice) + conf = np.array([[wall_confidence(float(s), variance_max) for s in row] for row in stds]) + if not np.any(conf > 0.0) or int((conf > 0.0).sum()) < min(WALL_MIN_CELLS, conf.size): + return 0.0 + + base = subgrid_cell_proximities(zone_slice) + corrected = base + conf * (_wall_boost(base) - base) + return float(corrected[conf > 0.0].max()) + + +def _wall_boost(base: np.ndarray | float): + """ + The confirmed-surface correction: identity below WALL_BOOST_FROM, ramping to + full warning at WALL_CONFIRM_SATURATE, flat at 1.0 above it. + + Shaped this way deliberately. The correction exists because scale ambiguity + makes the model UNDER-read a washed-out wall that is about to be hit, so it + belongs near collision range and nowhere else. An earlier attempt at this fix + used a plain gain (`base / WALL_CONFIRM_SATURATE`) which was continuous but + lifted every smooth surface — an open receding floor read twice as near, and + that inflated floor reading then MASKED the thin-structure detector, undoing + the cable fix in exactly the scene it was built for. Below WALL_BOOST_FROM + the reading is now left exactly as it was. + """ + span = WALL_CONFIRM_SATURATE - WALL_BOOST_FROM + if span <= 0: + return np.minimum(base, 1.0) if isinstance(base, np.ndarray) else min(base, 1.0) + ramp = WALL_BOOST_FROM + (base - WALL_BOOST_FROM) / span * (1.0 - WALL_BOOST_FROM) + return np.clip(np.where(base <= WALL_BOOST_FROM, base, ramp), 0.0, 1.0) + + +def detect_floor_to_wall(zone_slice: np.ndarray, jump_ratio: float = FLOOR_WALL_JUMP_RATIO) -> float: + """ + Warning intensity for a near wall the model renders as FAR (scale ambiguity). + + The wall's own depth is unreliable, but the textured floor leading up to it + is not. Scanning a zone from the bottom up, the floor depth grows in small + steps as it recedes; where it runs into the wall the model jumps to "far". + We take the floor depth just BELOW that jump — the true distance to the wall + base — and extrapolate it to the washed-out region above. + + Fires only when the floor terminates against something NEAR: + - open corridor -> floor recedes smoothly, no jump -> 0.0 + - distant dropoff -> break happens at a far depth -> ~0.0 (curve) + - near wall/edge -> floor stops short at a near depth -> graded warning + + So it can only add a warning for a genuinely close obstruction, never + suppress one. Thresholds are relative-unit PLACEHOLDERS pending calibration. + """ + h = zone_slice.shape[0] + if h < MIN_FLOOR_ROWS + 1: + return 0.0 + + # Row-wise median, bottom row first (median across columns rejects speckle). + prof = np.median(zone_slice, axis=1).astype(float)[::-1] + + if prof[0] >= MAX_DEPTH_M: + return 0.0 # no near floor visible -> method N/A + + diffs = np.diff(prof) + + # Candidate break = the single largest FARTHER jump in the profile. + break_i = int(np.argmax(diffs)) + jump = float(diffs[break_i]) + + # Absolute noise floor FIRST: on a smooth surface the relative baseline + # collapses toward zero, so without this a ~0.8-unit sensor wiggle scored as + # a full-confidence wall (live Pi false positive, no floor in frame at all). + if jump < FLOOR_WALL_MIN_JUMP: + return 0.0 + + if break_i < MIN_FLOOR_ROWS: + return 0.0 # break too close to the bottom -> no real floor run below it + + # Baseline from the FLOOR rows only (below the break). Rows above it belong + # to the wall, whose dozens of near-zero noise steps used to drag the median + # down and corrupt the ratio test in both directions. + floor_steps = diffs[:break_i] + rising = floor_steps[floor_steps > 0] + if rising.size == 0: + return 0.0 # nothing receding below the break -> not a floor + typical_step = max(float(np.median(rising)), 1e-6) + + if jump <= jump_ratio * typical_step: + return 0.0 # break not distinct from the floor's own slope -> open space + + floor_reach = float(prof[break_i]) # last reliable floor depth before the break + return _proximity_curve(floor_reach) # near -> warn, far -> ~0 + + +def subgrid_proximity(zone_slice: np.ndarray, grid_shape: Tuple[int, int] = SUBGRID_SHAPE) -> float: + """ + Sub-grid pooling for small / thin objects (poles, cables, chair legs). + + A thin obstacle can occupy far less than the NEAR_PERCENTILE fraction of a + whole zone, so the zone-wide percentile averages it into the background and + misses it. Splitting the zone into a small grid (default 4x4) and taking the + *worst* (nearest) cell means an object that dominates even one cell still + raises the alarm. Each cell is still aggregated with compute_proximity, so a + single noisy pixel inside a cell is rejected rather than triggering a false + max. + + For blind navigation, missing a thin hazard is worse than an occasional + over-warn, so this deliberately errs toward higher sensitivity. It is never + less sensitive than the whole-zone reading (the nearest cell's near-cluster + is at least as close as the whole zone's), so it supersedes it. + + np.array_split tolerates zones that don't divide evenly; the 16-cell loop is + not a per-pixel loop, so it stays cheap on the 64x48 grid. + """ + return float(subgrid_cell_proximities(zone_slice, grid_shape).max()) + + +def _curve_array(near: np.ndarray, max_depth: float = MAX_DEPTH_M, min_depth: float = MIN_DEPTH_M) -> np.ndarray: + """Vectorized _proximity_curve over an array of near-depth values.""" + clamped = np.maximum(near, min_depth) + falloff = (max_depth - clamped) / (max_depth - min_depth) + return np.where(near >= max_depth, 0.0, falloff ** CURVE_EXPONENT) + + +def subgrid_cell_edges(h: int, w: int, grid_shape: Tuple[int, int] = SUBGRID_SHAPE): + """Row/column edge indices matching subgrid_cell_proximities' split.""" + rows, cols = grid_shape + if h % rows == 0 and w % cols == 0: + return np.arange(rows + 1) * (h // rows), np.arange(cols + 1) * (w // cols) + row_edges = np.cumsum([0] + [len(x) for x in np.array_split(np.arange(h), rows)]) + col_edges = np.cumsum([0] + [len(x) for x in np.array_split(np.arange(w), cols)]) + return row_edges, col_edges + + +def subgrid_cell_proximities(zone_slice: np.ndarray, grid_shape: Tuple[int, int] = SUBGRID_SHAPE) -> np.ndarray: + """ + Per-cell proximity for a zone's sub-grid as a (rows, cols) array. + + PERF: when the zone divides evenly (the normal 64x48 case) this reshapes all + cells into one 2-D block and takes a SINGLE np.percentile along an axis, + instead of one call per cell — the streaming thread runs this every frame, so + the 16x reduction in percentile calls matters. Falls back to array_split for + odd sizes. The visualization reuses this instead of recomputing. + """ + rows, cols = grid_shape + h, w = zone_slice.shape + if h < rows or w < cols: + return np.full((1, 1), compute_proximity(zone_slice)) + + if h % rows == 0 and w % cols == 0: + cells = (zone_slice.reshape(rows, h // rows, cols, w // cols) + .transpose(0, 2, 1, 3) + .reshape(rows * cols, -1)) + # Percentile in the array's own dtype, curve widened to float64 — the same + # order the per-cell scalar path used, so results match bit-for-bit. + near = np.percentile(cells, NEAR_PERCENTILE, axis=1).astype(np.float64) + return _curve_array(near).reshape(rows, cols) + + out = np.zeros((rows, cols), dtype=float) + for r, row_band in enumerate(np.array_split(zone_slice, rows, axis=0)): + for c, cell in enumerate(np.array_split(row_band, cols, axis=1)): + if cell.size: + out[r, c] = compute_proximity(cell) + return out + + +def thin_structure_mask( + depth_map: np.ndarray, + max_width: int = THIN_MAX_WIDTH_PX, + min_contrast: float = THIN_MIN_CONTRAST, +) -> np.ndarray: + """ + Boolean mask of NARROW structures standing in front of their surround — + cables, power cords, extension leads, ropes, thin poles, branches, railing + bars. + + These are the hazards every other detector in this module is structurally + blind to. `subgrid_proximity`, `detect_blank_wall` and `detect_floor_to_wall` + all ask "how near is this?", and a cable is barely nearer than the floor or + wall behind it — the whole zone reads as one moderately-near surface (live + finding: a Pi power lead across the frame left the centre zone at s=0.29, + tagged broad, indistinguishable from the empty scene). Raising sensitivity + cannot fix that: the cable's magnitude signal is genuinely inside the + background's spread. + + The signal that IS there is shape. A grey-scale morphological CLOSING with a + kernel wider than the structure erases it (near = SMALL depth values, so a + cable is a thin dark line, and closing fills dark thin structures); a wall, + door, box or floor is wider than the kernel and survives untouched. + `closed - depth` is therefore a top-hat that keeps only things narrower than + `max_width` and is exactly 0.0 on broad surfaces — the selectivity is + geometric, not a tuned threshold. + + A 3x3 opening on the resulting mask drops isolated speckle, so a single noisy + pixel cannot invent a cable (the same reasoning that made every other + aggregation here a percentile rather than a min). + + Run this on the depth map at its NATIVE resolution, before + `downsample_depth`: a 3 px cable does not survive a 5x decimation to the + 64x48 grid intact, and the whole point is to catch it while it is still + resolved. + """ + depth = depth_map.astype(np.float32, copy=False) + kernel = cv2.getStructuringElement(cv2.MORPH_RECT, (max_width, max_width)) + closed = cv2.morphologyEx(depth, cv2.MORPH_CLOSE, kernel) + ridge = closed - depth # > 0 only where locally nearer + mask = (ridge >= min_contrast).astype(np.uint8) + mask = cv2.morphologyEx(mask, cv2.MORPH_OPEN, np.ones((3, 3), np.uint8)) + return mask.astype(bool) + + +def _thin_grade(structure_depths: np.ndarray, zone_area: int) -> float: + """ + Grade one zone from the depths of its ridge pixels alone. + + Uses the NEAR_PERCENTILE of the structure's own pixels — the background it + sits against is already excluded by the mask — then the same proximity curve + as every other detector, so a cable across the doorway warns and a washing + line at the end of the garden does not. + + Requires the ridge to cover THIN_MIN_AREA_FRAC of the zone: a real cable + crosses a zone and covers several percent, while speckle surviving the 3x3 + opening is an order of magnitude below that. + """ + if structure_depths.size < max(THIN_MIN_AREA_PX, int(zone_area * THIN_MIN_AREA_FRAC)): + return 0.0 + return _proximity_curve(float(np.percentile(structure_depths, NEAR_PERCENTILE))) + + +def thin_zone_warning(zone_slice: np.ndarray, zone_mask: np.ndarray) -> float: + """Thin-structure warning for a single zone and its slice of the mask.""" + if zone_mask.shape != zone_slice.shape: + return 0.0 + return _thin_grade(zone_slice[zone_mask], zone_mask.size) + + +def thin_zone_intensities(depth_map: np.ndarray, mask: np.ndarray) -> dict[str, float]: + """ + Split an already-computed thin mask into zones and grade each one. + + The mask is computed ONCE over the full frame and only then split, never + computed per zone: the closing needs the surround on both sides of a + structure to judge its width, and a zone boundary cutting through a cable + would otherwise make it look like the edge of a surface. + + PERF: gathers the ridge pixels with a single fancy-index over the frame and + splits them by column, rather than boolean-indexing each zone separately. + The mask is sparse by construction (a cable is a few percent of the frame), + so this touches thousands of pixels instead of re-scanning tens of thousands + three times — it runs on the streaming thread, where the budget is the whole + reason rendering was moved to another process. + """ + h, w = depth_map.shape + zone_area = h * (w // 4) + ys, xs = np.nonzero(mask) + if xs.size == 0: + return {zone: 0.0 for zone in ZONE_NAMES} + depths = depth_map[ys, xs] + return {zone: _thin_grade(depths[(xs >= x0) & (xs < x1)], + zone_area if zone != "center" else 2 * zone_area) + for zone, (x0, x1) in _zone_bounds(w).items()} + + +def compute_thin_intensities(depth_map: np.ndarray) -> dict[str, float]: + """Per-zone thin-structure warning for a whole depth frame (mask + grading).""" + return thin_zone_intensities(depth_map, thin_structure_mask(depth_map)) + + +def thin_overlay_mask(mask: np.ndarray, size: Tuple[int, int] = DOWNSAMPLE_SIZE) -> np.ndarray: + """ + Shrink a native-resolution thin mask onto the display grid, keeping every + cell a structure touched. + + A cable is one or two pixels wide, so INTER_NEAREST would sample most of it + away and the overlay would show gaps where the detector actually fired. + INTER_AREA averages the block, so any coverage at all survives as non-zero — + the overlay then shows what was detected rather than a decimated guess. + """ + target_w, target_h = size + if mask.shape == (target_h, target_w): + return mask.astype(bool) + shrunk = cv2.resize(mask.astype(np.uint8) * 255, (target_w, target_h), + interpolation=cv2.INTER_AREA) + return shrunk > 0 + + +def zone_warning(zone_slice: np.ndarray) -> float: + """ + Combined per-zone warning: the worst of sub-grid pooling (thin objects), + blank-wall detection (flat close surfaces), and floor-to-wall intersection + (near walls the model reads as far). Each can only raise the warning, never + lower it, so the device stays fail-safe. + """ + return max( + subgrid_proximity(zone_slice), + detect_blank_wall(zone_slice), + detect_floor_to_wall(zone_slice), + ) + + +def zone_warning_breakdown(zone_slice: np.ndarray, thin_value: float | None = None) -> dict: + """ + Itemised per-detector contributions for one zone — the same numbers + zone_warning() takes the max of, but broken out so the live view can show + WHICH edge-case detector is driving a zone. + + `thin_value` is the thin-structure reading the hot path already computed at + native resolution. Pass it whenever it is available: recomputing it here + from the 64x48 zone is strictly less sensitive, and a HUD reporting "no thin + object" while the motors buzz on one would be worse than no HUD at all. + Falls back to computing it from the zone when not supplied, so off-device + tools (verify_scene) still show the tag. + + Also classifies the SHAPE of the near reading, because the sub-grid detector + fires on anything close, not just thin objects (a wall at arm's length maxes + it out too — live Pi finding). Coverage = fraction of sub-cells in danger: + "thin" — few cells fire (localized object: pole, cable, box edge) + "broad" — cells blanket the zone (large close surface: wall, door) + "none" — sub-grid quiet. + + Diagnostics/visualisation only; the hot path uses zone_warning(). Runs in the + display process, so it costs the streaming thread nothing. + """ + cells = subgrid_cell_proximities(zone_slice) + if thin_value is None: + thin_value = thin_zone_warning(zone_slice, thin_structure_mask(zone_slice)) + parts = { + "sub": float(cells.max()), # nearest cluster of anything + "wall": detect_blank_wall(zone_slice), # locally-smooth close surface + "f2w": detect_floor_to_wall(zone_slice), # wall the model reads as far + "thin": float(thin_value), # cable / pole / branch / railing + } + # Coverage is judged RELATIVE to the zone's strongest cell, not an absolute + # threshold: a moderate-distance wall reads ~0.4 in EVERY cell (widespread, + # so broad) while a pole reads ~1.0 in one column and ~0 elsewhere + # (concentrated, so thin). An absolute cutoff called that wall "thin". + peak = parts["sub"] + if peak <= 0.01: + coverage, shape = 0.0, "none" + else: + coverage = float((cells >= 0.5 * peak).mean()) + shape = "broad" if coverage > BROAD_COVERAGE_MIN else "thin" + winner = max(parts, key=parts.get) + return {"parts": parts, "winner": winner, "value": parts[winner], + "coverage": coverage, "shape": shape} + + +def compute_zone_intensities(depth_data: np.ndarray, width: int) -> dict[str, float]: + """ + Split a depth frame into left/center/right zones (25/50/25) and + return each zone's warning intensity (0.0-1.0). + + Each zone combines sub-grid pooling (a thin obstacle filling only part of the + zone still triggers) with blank-wall detection (a flat, textureless close + surface still triggers even when its per-cluster depth reads washed out). + """ + return {zone: zone_warning(depth_data[:, x0:x1]) + for zone, (x0, x1) in _zone_bounds(width).items()} + + +def to_motor_intensity(value: float) -> int: + """Convert a 0.0-1.0 intensity to the 0-255 int expected on serial_queue.""" + return int(round(255 * float(np.clip(value, 0.0, 1.0)))) + + +class DepthPostProcessor: + """ + Stateful frame-to-frame processor: runs the full post-processing pipeline + and EMA-smooths the per-zone intensities so haptics don't flicker. + + Call process() once per frame from the depth callback. + """ + + def __init__(self, alpha: float = EMA_ALPHA): + self.alpha = alpha + self._smoothed: dict[str, float] | None = None + self.last_thin: dict[str, float] = {zone: 0.0 for zone in ZONE_NAMES} + self._last_mask: np.ndarray | None = None + + def process(self, depth_map: np.ndarray) -> dict[str, int]: + """ + Full pipeline: thin-structure pass (native resolution) -> downsample -> + zone intensities -> EMA -> 0-255 ints. + + Returns {"left": int, "center": int, "right": int} per the + serial_queue contract. + + Hand this the CROPPED but NOT yet downsampled map. The thin-structure + pass has to run before the 64x48 decimation — a 3 px cable does not + survive it — while everything after is unchanged, because + downsample_depth() is a no-op on an already-downsampled grid. The thin + reading is combined with max(), so like every other detector here it can + only raise a warning, never soften one. + """ + mask = thin_structure_mask(depth_map) + thin = thin_zone_intensities(depth_map, mask) + self.last_thin = thin + # Kept at native resolution; shrinking it is display-only work, so the + # caller pays for it on preview frames via overlay_mask() rather than + # every frame on the streaming thread. + self._last_mask = mask + + small = downsample_depth(depth_map) + raw = compute_zone_intensities(small, small.shape[1]) + raw = {zone: max(raw[zone], thin[zone]) for zone in ZONE_NAMES} + + if self._smoothed is None: + self._smoothed = dict(raw) + else: + for zone in ZONE_NAMES: + self._smoothed[zone] = ( + self.alpha * raw[zone] + (1.0 - self.alpha) * self._smoothed[zone] + ) + + return {zone: to_motor_intensity(self._smoothed[zone]) for zone in ZONE_NAMES} + + def overlay_mask(self, size: Tuple[int, int] = DOWNSAMPLE_SIZE) -> np.ndarray | None: + """ + The last frame's thin mask, shrunk to the display grid. + + Zones the detector did NOT act on are cleared first, so the outline means + "this raised a warning" and nothing else. The raw mask keeps every ridge + pixel that clears THIN_MIN_CONTRAST, but a zone only counts once the + ridge also covers THIN_MIN_AREA_FRAC of it — without this the view drew + cyan around specks and texture fragments while the HUD read t0.00 and the + motors did nothing, which is exactly the HUD-disagrees-with-the-device + trap this module already learned once. + + Display-only, and small enough to hand to another process (~3 KB vs the + ~70 KB native mask). Call it only on frames you actually preview. + """ + if self._last_mask is None: + return None + mask = self._last_mask + if any(v <= 0.0 for v in self.last_thin.values()): + mask = mask.copy() + for zone, (x0, x1) in _zone_bounds(mask.shape[1]).items(): + if self.last_thin.get(zone, 0.0) <= 0.0: + mask[:, x0:x1] = False + return thin_overlay_mask(mask, size) + + def reset(self) -> None: + """Clear EMA state (e.g., after a pipeline rebuild or mode switch).""" + self._smoothed = None + self.last_thin = {zone: 0.0 for zone in ZONE_NAMES} + self._last_mask = None + + def detect_ground_hazard( depth_map: np.ndarray, frame_height: int, threshold_ratio: float = HAZARD_THRESHOLD_RATIO, -) -> Tuple[bool, int]: +) -> Tuple[bool, int, str]: """ - Detect a ground-plane departure (stairs, ledge) in the bottom strip of the frame. + Detect a ground-plane departure (stairs, ledge, curb) in the bottom strip of + the frame, and WHICH WAY the ground breaks. Monocular depth reads a drop-off as "far" rather than "down", so this looks for a sudden gradient spike in row-wise ground depth instead of absolute - distance. Returns (hazard_detected, severity), with severity pre-scaled to - 0-255 for the serial_queue "hazard_severity" field. + distance. The SIGN of that spike (walking-forward order, i.e. bottom row + first) distinguishes the two opposite dangers: + + depth jumps FARTHER -> "down" (drop-off / descending stairs — fall hazard) + depth jumps NEARER -> "up" (curb / step-up / riser — trip hazard) + + Returns (hazard_detected, severity, direction) with severity pre-scaled to + 0-255 for the serial_queue "hazard_severity" field and direction one of + "down" / "up" / "none". A step-up shows as a weaker signature than a + drop-off (a riser plateaus more than it spikes), so treat "up" as + best-effort until live calibration. """ strip_start = int(frame_height * (1 - GROUND_STRIP_FRACTION)) ground_strip = depth_map[strip_start:, :] if ground_strip.shape[0] < 2: - return False, 0 + return False, 0, "none" - row_depths = np.mean(ground_strip, axis=1) - gradient = np.abs(np.diff(row_depths)) + # Bottom row first = walking-forward order, so the diff signs read naturally: + # positive step = ground receding, big positive break = void of a drop-off. + profile = np.mean(ground_strip, axis=1)[::-1] + diffs = np.diff(profile) + gradient = np.abs(diffs) + break_i = int(np.argmax(gradient)) + max_gradient = float(gradient[break_i]) - median_gradient = np.median(gradient) - if median_gradient <= 1e-6: - return False, 0 + # Absolute floor first: a step smaller than this is sensor noise no matter how + # it compares to the baseline. Also stops a near-flat strip (tiny baseline) + # from turning specks into hazards. + if max_gradient < HAZARD_MIN_STEP: + return False, 0, "none" - ratio = float(np.max(gradient) / median_gradient) - if ratio <= threshold_ratio: - return False, 0 + median_gradient = float(np.median(gradient)) + if median_gradient > 1e-6: + # Ratio test is the DETECTION GATE only: is this break distinct from the + # strip's own texture? + if max_gradient / median_gradient <= threshold_ratio: + return False, 0, "none" + # else: the strip is flat apart from this one break (e.g. a large drop-off + # whose void drives the median to exactly 0) — trivially distinct, gate + # passes. (Bailing out here used to silently miss big, close drop-offs.) - severity = int(np.clip((ratio / threshold_ratio) * 255, 0, 255)) - return True, severity + direction = "down" if diffs[break_i] > 0 else "up" + # Severity is graded on the ABSOLUTE break size, never the ratio: the + # ratio's denominator is scene noise, so a static desk-edge scene flapped + # between sev 26 and 255 as the noise median wandered (live finding — the + # third instance of the unstable-median-denominator bug family). Jump size + # is stable frame-to-frame and physically meaningful: how far the ground + # falls (or rises). + severity = int(np.interp(max_gradient, [HAZARD_MIN_STEP, HAZARD_MAX_JUMP], [1, 255])) + return True, severity, direction \ No newline at end of file diff --git a/src/second_vision/core/depth_view.py b/src/second_vision/core/depth_view.py new file mode 100644 index 0000000..8376ccb --- /dev/null +++ b/src/second_vision/core/depth_view.py @@ -0,0 +1,217 @@ +""" +Depth post-processing visualization — pure rendering, no hardware dependencies. + +Deliberately imports only numpy/cv2/depth_utils (NO gi, hailo, GStreamer), so the +exact same view can be produced: + * on the Pi, inside the display process (see sv_dual_callback_withdepth), and + * on any laptop, from a saved depth frame or a hand-drawn scene + (see verify_scene.py) — no Hailo device required. + +Keeping one implementation means what you verify off-device is what actually runs. +""" +import os + +import cv2 +import numpy as np + +from hailo_apps.python.pipeline_apps.custom_depth_detection.depth_utils import ( + DANGER_CELL, + MIN_DEPTH_M, + SUBGRID_SHAPE, + subgrid_cell_edges, + subgrid_cell_proximities, + zone_warning_breakdown, +) + +# Sub-grid overlay: 4x4 grid per zone, shaded danger cells, white ring on the +# worst (nearest) cell. Rendering happens off the inference thread, so it is ON by +# default to keep the edge cases verifiable. Disable with SV_SUBGRID_OVERLAY=0. +SHOW_SUBGRID_OVERLAY = os.environ.get("SV_SUBGRID_OVERLAY", "1") != "0" +# (DANGER_CELL is imported from depth_utils so overlay and breakdown agree) + +# Color scale: FIXED by default (red = at/below MIN_DEPTH_M, blue = at/beyond +# COLOR_FAR), the same ruler the demos use, so colors mean the same thing in +# every frame and every room. The old per-frame grading painted the farthest +# part of ANY scene blue — even the middle of a close wall — which read as +# "safe" when it wasn't. SV_COLOR_RELATIVE=1 restores per-frame contrast. +COLOR_FAR = 50.0 # model units; tighten after calibration measures open scenes +USE_RELATIVE_COLOR = os.environ.get("SV_COLOR_RELATIVE", "0") == "1" + +VIEW_W, VIEW_H = 640, 480 + + +def zone_grid_cells(small, x0, x1, grid=SUBGRID_SHAPE): + """ + Yield (gx0, gy0, gx1, gy1, proximity) in grid coordinates for each sub-cell + of a zone. + + Reuses depth_utils.subgrid_cell_proximities (one vectorized percentile per + zone) and its matching cell edges, so the overlay is an exact reflection of + the real computation rather than a recomputed approximation. + """ + zone = small[:, x0:x1] + prox = subgrid_cell_proximities(zone, grid) + row_edges, col_edges = subgrid_cell_edges(zone.shape[0], zone.shape[1], grid) + if prox.shape != (len(row_edges) - 1, len(col_edges) - 1): + return # tiny-zone fallback shape; nothing meaningful to outline + for r in range(prox.shape[0]): + for c in range(prox.shape[1]): + yield (x0 + int(col_edges[c]), int(row_edges[r]), + x0 + int(col_edges[c + 1]), int(row_edges[r + 1]), float(prox[r, c])) + + +def draw_subgrid_overlay(frame, small, view_w=VIEW_W, view_h=VIEW_H): + """ + Draw the 4x4 sub-grid on each zone: faint grid lines, red-shaded danger cells, + and a white ring on the worst (nearest) cell per zone — the cell that actually + drives that zone's intensity. One overlay copy + one blend keeps it cheap. + """ + h, w = small.shape + sx, sy = view_w / w, view_h / h + zone_bounds = {"left": (0, w // 4), "center": (w // 4, 3 * w // 4), "right": (3 * w // 4, w)} + + danger_rects, rings = [], [] + for x0, x1 in zone_bounds.values(): + best_box, best_val = None, 0.0 + for gx0, gy0, gx1, gy1, val in zone_grid_cells(small, x0, x1): + p0 = (int(gx0 * sx), int(gy0 * sy)) + p1 = (int(gx1 * sx), int(gy1 * sy)) + cv2.rectangle(frame, p0, p1, (70, 70, 70), 1) # faint grid (opaque, cheap) + if val >= DANGER_CELL: + danger_rects.append((p0, p1)) + if val > best_val: + best_val, best_box = val, (p0, p1) + if best_box and best_val >= DANGER_CELL: + rings.append(best_box) + + if danger_rects: + overlay = frame.copy() + for p0, p1 in danger_rects: + cv2.rectangle(overlay, p0, p1, (0, 0, 255), -1) + frame = cv2.addWeighted(overlay, 0.30, frame, 0.70, 0) + for p0, p1 in rings: + cv2.rectangle(frame, p0, p1, (255, 255, 255), 2) + + cv2.putText(frame, "subgrid 4x4: red cell=danger ring=worst", (8, view_h - 40), + cv2.FONT_HERSHEY_SIMPLEX, 0.5, (255, 255, 255), 1, cv2.LINE_AA) + return frame + + +def draw_thin_mask(frame, thin_mask, view_w=VIEW_W, view_h=VIEW_H): + """ + Outline the pixels the thin-structure detector fired on, in cyan. + + Without this the detector is unfalsifiable by eye: a cable is invisible in + the colourized depth map (that is exactly why the magnitude detectors miss + it), so "did it see the wire?" could only be answered from numbers. Drawn as + contours rather than a fill so a 1-2 px structure stays visible. + """ + mask = cv2.resize(thin_mask.astype(np.uint8) * 255, (view_w, view_h), + interpolation=cv2.INTER_NEAREST) + contours, _ = cv2.findContours(mask, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE) + cv2.drawContours(frame, contours, -1, (255, 255, 0), 2) + return frame + + +def cv2_draw_depth(small, intensities, hazard_detected, severity, direction="none", + thin=None, thin_mask=None): + """ + Render the post-processing state as a BGR image: colorized depth map + (hot = close), zone dividers, optional sub-grid overlay, per-zone intensity + bars tagged with the active detectors, ground-hazard strip (with step + direction: DOWN = fall hazard, UP = trip hazard), and raw depth stats for + calibration. + """ + view_w, view_h = VIEW_W, VIEW_H + + d_lo, d_med, d_hi = np.percentile(small, [1, 50, 99]) + # Invert so near = hot (red), far = cold (blue) + if USE_RELATIVE_COLOR: + span = max(d_hi - d_lo, 1e-6) + norm = np.clip((d_hi - small) / span, 0.0, 1.0) + else: + norm = np.clip((COLOR_FAR - small) / (COLOR_FAR - MIN_DEPTH_M), 0.0, 1.0) + colored = cv2.applyColorMap((norm * 255).astype(np.uint8), cv2.COLORMAP_JET) + frame = cv2.resize(colored, (view_w, view_h), interpolation=cv2.INTER_NEAREST) + + if SHOW_SUBGRID_OVERLAY: + frame = draw_subgrid_overlay(frame, small, view_w, view_h) + + if thin_mask is not None and np.any(thin_mask): + frame = draw_thin_mask(frame, thin_mask, view_w, view_h) + + # Zone dividers (25/50/25 split used by compute_zone_intensities) + q1, q3 = view_w // 4, 3 * view_w // 4 + cv2.line(frame, (q1, 0), (q1, view_h), (255, 255, 255), 2) + cv2.line(frame, (q3, 0), (q3, view_h), (255, 255, 255), 2) + + # Ground-hazard strip boundary (bottom 25% of the frame). + # Red = drop-off (fall), orange = step-up (trip), yellow = clear. + strip_y = int(view_h * 0.75) + if hazard_detected: + strip_color = (0, 140, 255) if direction == "up" else (0, 0, 255) + else: + strip_color = (0, 255, 255) + cv2.line(frame, (0, strip_y), (view_w, strip_y), strip_color, 2) + + # Per-zone intensity bars, tagged with WHICH edge-case detector is active, so + # blank-wall / floor-to-wall are verifiable too, not just thin objects. + gw = small.shape[1] + grid_zones = { + "left": small[:, :gw // 4], + "center": small[:, gw // 4: 3 * gw // 4], + "right": small[:, 3 * gw // 4:], + } + bar_max_h = view_h // 3 + zone_spans = {"left": (0, q1), "center": (q1, q3), "right": (q3, view_w)} + for zone, (x0, x1) in zone_spans.items(): + val = intensities[zone] + bar_h = int(bar_max_h * val / 255) + if bar_h > 0: + overlay = frame.copy() + cv2.rectangle(overlay, (x0 + 4, view_h - bar_h), (x1 - 4, view_h), (0, 0, 255), -1) + frame = cv2.addWeighted(overlay, 0.5, frame, 0.5, 0) + + # Show every ACTIVE detector, not just the winner. T now comes from the + # real thin-structure detector (cable/pole/branch/railing), which is the + # only one that can tell a narrow object from a surface. The near-cluster + # detector keeps its own shape label — C (concentrated) vs N (broad near + # surface) — because it fires on any close thing, wall included; it used + # to claim "thin object" on a wall, which was misleading (live Pi finding). + bd = zone_warning_breakdown(grid_zones[zone], + None if thin is None else thin.get(zone)) + p = bd["parts"] + active = "" + if p["thin"] > 0.01: + active += "T" + if p["sub"] > 0.01: + active += "N" if bd["shape"] == "broad" else "C" + if p["wall"] > 0.01: + active += "W" + if p["f2w"] > 0.01: + active += "F" + cv2.putText(frame, f"{zone[0].upper()}={val} [{active or '-'}]", (x0 + 8, view_h - 30), + cv2.FONT_HERSHEY_SIMPLEX, 0.6, (255, 255, 255), 2) + cv2.putText(frame, + f"t{p['thin']:.2f} s{p['sub']:.2f} w{p['wall']:.2f} f{p['f2w']:.2f} " + f"cov{bd['coverage']:.0%}", + (x0 + 8, view_h - 10), + cv2.FONT_HERSHEY_SIMPLEX, 0.42, (230, 230, 230), 1, cv2.LINE_AA) + + # Raw depth stats — the numbers needed to calibrate MIN/MAX_DEPTH_M + cv2.putText(frame, f"depth p1/p50/p99: {d_lo:.2f} / {d_med:.2f} / {d_hi:.2f}", + (8, 24), cv2.FONT_HERSHEY_SIMPLEX, 0.6, (255, 255, 255), 2) + if hazard_detected: + # DOWN = drop-off / descending stairs (fall) — red. + # UP = curb / step-up (trip) — orange. + label = {"down": "HAZARD: DROP-OFF", "up": "HAZARD: STEP-UP"}.get(direction, "HAZARD") + color = (0, 140, 255) if direction == "up" else (0, 0, 255) + cv2.putText(frame, f"{label} sev={severity}", (8, 52), + cv2.FONT_HERSHEY_SIMPLEX, 0.8, color, 2) + + cv2.putText(frame, + "active: T=thin obj (cyan outline) C=concentrated near N=near surface " + "W=blank-wall F=floor-to-wall", + (8, view_h - 48), cv2.FONT_HERSHEY_SIMPLEX, 0.42, (255, 255, 255), 1, cv2.LINE_AA) + + return frame \ No newline at end of file diff --git a/src/second_vision/pipeline/TBR-callback-depth-est.py b/src/second_vision/pipeline/TBR-callback-depth-est.py new file mode 100644 index 0000000..e69de29 From 26ad5c04e3ce5e7085b4dada3f50a28abf755292 Mon Sep 17 00:00:00 2001 From: KRMeeag Date: Thu, 30 Jul 2026 20:28:26 +0800 Subject: [PATCH 03/28] feat(depth-callback): add depth calibration capture and analysis --- src/second_vision/core/calibration.py | 0 1 file changed, 0 insertions(+), 0 deletions(-) create mode 100644 src/second_vision/core/calibration.py diff --git a/src/second_vision/core/calibration.py b/src/second_vision/core/calibration.py new file mode 100644 index 0000000..e69de29 From 67c580b86e91f75cc9ee0e4a0cf849959cde8205 Mon Sep 17 00:00:00 2001 From: Kenzhu-A Date: Thu, 30 Jul 2026 20:56:03 +0800 Subject: [PATCH 04/28] feat(depth-callback): added depth estimation callback and calibration file --- src/second_vision/core/calibration.py | 312 ++++++++++++++ .../pipeline/TBR-callback-depth-est.py | 401 ++++++++++++++++++ 2 files changed, 713 insertions(+) diff --git a/src/second_vision/core/calibration.py b/src/second_vision/core/calibration.py index e69de29..a2fe587 100644 --- a/src/second_vision/core/calibration.py +++ b/src/second_vision/core/calibration.py @@ -0,0 +1,312 @@ +""" +Depth calibration capture + analysis (Second Vision, depth post-processing). + +The depth post-processing thresholds (MIN_DEPTH_M, MAX_DEPTH_M, WALL_VARIANCE_MAX) +are in the model's RELATIVE units, not meters, so they are placeholders until +measured on the real device. This module turns that measurement into a repeatable +procedure: + + CAPTURE (runs inside the live pipeline, headless — no video window, so it works + even while the VNC preview is frozen): + 1) SV_CALIBRATE=1 python -m ...sv_dual_callback_withdepth + 2) place an object at a known distance, then in a SECOND ssh terminal: + echo "0.5m" > calib_label.txt # tag the current samples + move it and retag. Capture ALL of these to resolve every threshold: + 0.5m 1m 2m 3m — object centred at measured distances + wall — camera ~1 m from a blank wall, filling the view + open — open corridor, nothing close + pole — broomstick/cane at ~1 m (thin object) + stairs (or curb)— floor visible, real ground break in the strip + Rows stream to depth_calibration.csv, throttled to the terminal. + + ANALYSE (offline, no hardware): + python -m ...calibration --analyze depth_calibration.csv + Groups rows by label and prints per-distance stats plus suggested threshold + values you can drop into depth_utils.py. + +Nothing here runs on the normal (non-calibration) path. +""" +import argparse +import csv +import os +import queue +import threading +import time +from collections import defaultdict + +import numpy as np + +from hailo_apps.python.pipeline_apps.custom_depth_detection.depth_utils import ( + ground_break_stats, + local_flatness, + zone_warning_breakdown, +) + +# Per-scope stats recorded for every frame. Kept small and fixed so the CSV +# schema is stable and easy to analyse. +# +# IMPORTANT: the per-zone measures come from depth_utils (local_flatness, +# zone_warning_breakdown) rather than being reimplemented here. A previous +# version recorded a zone-GLOBAL std while the detector compared a median +# per-cell std — ~2x apart on a real wall, and on opposite sides of the +# threshold, so calibrating from it would have degraded the detector. +_STAT_NAMES = ("p10", "p50", "std", "min", "max", "flatness", "coverage") +_SCOPES = ("frame", "left", "center", "right") +_GROUND_NAMES = ("max_grad", "median_grad", "signed_break") + + +def _scope_stats(a: np.ndarray) -> dict: + """Depth distribution + the exact measures the detectors threshold on.""" + stats = { + "p10": float(np.percentile(a, 10)), + "p50": float(np.percentile(a, 50)), + "std": float(np.std(a)), # kept for reference; NOT the wall metric + "min": float(np.min(a)), + "max": float(np.max(a)), + # what detect_blank_wall actually compares to WALL_VARIANCE_MAX + "flatness": local_flatness(a), + } + # what the T/N shape split compares to BROAD_COVERAGE_MIN + stats["coverage"] = float(zone_warning_breakdown(a)["coverage"]) + return stats + + +def zone_depth_stats(depth_map: np.ndarray) -> dict: + """ + Flat {scope_stat: value} dict for the whole frame and the L/C/R zones, + using the same 25/50/25 split as compute_zone_intensities, plus the ground + strip's row-gradient stats (for the hazard / floor-to-wall thresholds). + """ + w = depth_map.shape[1] + regions = { + "frame": depth_map, + "left": depth_map[:, :w // 4], + "center": depth_map[:, w // 4: 3 * w // 4], + "right": depth_map[:, 3 * w // 4:], + } + out = {} + for scope, region in regions.items(): + for name, val in _scope_stats(region).items(): + out[f"{scope}_{name}"] = val + for name, val in ground_break_stats(depth_map, depth_map.shape[0]).items(): + out[f"ground_{name}"] = val + return out + + +def _columns() -> list: + cols = ["timestamp", "frame", "label"] + for scope in _SCOPES: + for name in _STAT_NAMES: + cols.append(f"{scope}_{name}") + cols += [f"ground_{n}" for n in _GROUND_NAMES] + return cols + + +class CalibrationLogger: + """ + Streams per-frame depth stats to a CSV and a throttled terminal line, tagging + each row with a distance label read live from a small text file (so you can + change it from another ssh session without touching the running pipeline). + + THREADING: log() runs on the GStreamer streaming thread and must stay near + free — it only copies the small (12 KB) array onto a queue. The stats math + (three detectors x four regions) and the CSV/SD-card writes happen in a + background worker thread; flushes are batched. Doing all of that inline + starved the video branch and blurred the camera preview (live finding). + Frames are dropped when the worker is busy — calibration averages over many + frames, so sampling is fine and the pipeline must never wait. + """ + + def __init__(self, csv_path="depth_calibration.csv", label_file="calib_label.txt", + print_every=8, sample_every=2, flush_every=20): + self.csv_path = csv_path + self.label_file = label_file + self.print_every = max(int(print_every), 1) + self.sample_every = max(int(sample_every), 1) + self.flush_every = max(int(flush_every), 1) + self._cols = _columns() + new_file = not os.path.exists(csv_path) or os.path.getsize(csv_path) == 0 + self._fh = open(csv_path, "a", newline="") + self._writer = csv.DictWriter(self._fh, fieldnames=self._cols) + if new_file: + self._writer.writeheader() + self._fh.flush() + self._rows = 0 + self._queue: queue.Queue = queue.Queue(maxsize=8) + self._worker_thread = threading.Thread( + target=self._worker, name="calib-logger", daemon=True) + self._worker_thread.start() + print(f"[CALIB] logging to {csv_path} | set distance with: echo \"0.5m\" > {label_file}") + + def _current_label(self) -> str: + try: + with open(self.label_file) as f: + label = f.readline().strip() + if label: + return label + except OSError: + pass + return os.environ.get("SV_CALIB_LABEL", "unlabeled") + + def log(self, small: np.ndarray, frame_count: int) -> None: + """Streaming-thread side: copy + hand off. Never computes, never blocks.""" + if frame_count % self.sample_every: + return + try: + self._queue.put_nowait((small.copy(), frame_count, time.time())) + except queue.Full: + pass # worker busy (e.g. SD flush) — drop rather than stall the pipeline + + def _worker(self) -> None: + while True: + item = self._queue.get() + if item is None: + return + small, frame_count, ts = item + try: + label = self._current_label() + stats = zone_depth_stats(small) + self._writer.writerow( + {"timestamp": ts, "frame": frame_count, "label": label, **stats}) + self._rows += 1 + if self._rows % self.flush_every == 0: + self._fh.flush() + if self._rows % self.print_every == 0: + print(f"[CALIB] frame {frame_count} label={label!r} | " + f"p10/p50={stats['frame_p10']:.2f}/{stats['frame_p50']:.2f} " + f"flat={stats['center_flatness']:.2f} cov={stats['center_coverage']:.0%} | " + f"L/C/R p10={stats['left_p10']:.1f}/{stats['center_p10']:.1f}/{stats['right_p10']:.1f}") + except Exception as e: # never let the logger kill the session + print(f"[CALIB] worker error: {e}") + + def close(self) -> None: + try: + self._queue.put(None, timeout=1.0) + self._worker_thread.join(timeout=5.0) + except Exception: + pass + try: + self._fh.flush() + self._fh.close() + except Exception: + pass + + +# --------------------------------------------------------------------------- # +# Offline analysis +# --------------------------------------------------------------------------- # + +def _grouped_means(csv_path: str): + """Return {label: {col: mean}} and the ordered list of labels seen.""" + sums, counts, order = defaultdict(lambda: defaultdict(float)), defaultdict(int), [] + with open(csv_path, newline="") as f: + for row in csv.DictReader(f): + label = row["label"] + if label not in order: + order.append(label) + counts[label] += 1 + for col, val in row.items(): + if col in ("timestamp", "frame", "label"): + continue + try: + sums[label][col] += float(val) + except (TypeError, ValueError): + pass + means = {lbl: {c: sums[lbl][c] / counts[lbl] for c in sums[lbl]} for lbl in order} + return means, order, counts + + +def analyze_csv(csv_path: str) -> None: + means, order, counts = _grouped_means(csv_path) + if not order: + print("No rows found in", csv_path) + return + + print(f"\n=== Calibration summary: {csv_path} ===") + print(f"{'label':<12}{'n':>6}{'p10':>8}{'p50':>8}{'flatness':>10}{'coverage':>10}" + f"{'g_max':>8}{'g_med':>8}{'break':>9}") + for lbl in order: + m = means[lbl] + print(f"{lbl:<12}{counts[lbl]:>6}{m['frame_p10']:>8.2f}{m['frame_p50']:>8.2f}" + f"{m['center_flatness']:>10.2f}{m['center_coverage']:>10.0%}" + f"{m['ground_max_grad']:>8.2f}{m['ground_median_grad']:>8.2f}" + f"{m['ground_signed_break']:>9.2f}") + print(" (flatness/coverage are CENTRE-zone; break: + = drop-off, - = step-up)") + + def pick(*names): + for n in names: + for l in order: + if l.lower() == n or n in l.lower(): + return l + return None + + print("\n--- Suggested thresholds (depth_utils.py) ---") + + # MIN_DEPTH_M: nearest reading the model emits -> smallest frame_p10 seen. + near_lbl = min(order, key=lambda l: means[l]["frame_p10"]) + print(f"MIN_DEPTH_M ~ {means[near_lbl]['frame_p10']:.1f}" + f" (nearest p10, from {near_lbl!r})") + + # MAX_DEPTH_M: safe-zone cutoff -> the p10 of the open / clear scene. + far_lbl = pick("open", "clear", "far") or max(order, key=lambda l: means[l]["frame_p10"]) + print(f"MAX_DEPTH_M ~ {means[far_lbl]['frame_p10']:.1f}" + f" (safe-zone cutoff, from {far_lbl!r})") + + # WALL_VARIANCE_MAX: between a wall's local flatness and an open scene's. + wall_lbl, open_lbl = pick("wall"), pick("open", "clear", "far") + if wall_lbl and open_lbl: + wf, of = means[wall_lbl]["center_flatness"], means[open_lbl]["center_flatness"] + print(f"WALL_VARIANCE_MAX ~ {(wf + of) / 2:.2f}" + f" (between wall {wf:.2f} and open {of:.2f})") + else: + print("WALL_VARIANCE_MAX : need a 'wall' label and an 'open' label") + + # BROAD_COVERAGE_MIN: between a thin object's coverage and a wall's. + pole_lbl = pick("pole", "thin", "stick") + if pole_lbl and wall_lbl: + pc, wc = means[pole_lbl]["center_coverage"], means[wall_lbl]["center_coverage"] + print(f"BROAD_COVERAGE_MIN ~ {(pc + wc) / 2:.2f}" + f" (between pole {pc:.0%} and wall {wc:.0%})") + else: + print("BROAD_COVERAGE_MIN : need a 'pole' label and a 'wall' label") + + # HAZARD_MIN_STEP: above the largest gradient seen on SAFE ground. + safe_lbls = [l for l in order if l.lower() in ("open", "clear", "far", "wall")] + if safe_lbls: + worst_safe = max(means[l]["ground_max_grad"] for l in safe_lbls) + print(f"HAZARD_MIN_STEP ~ {worst_safe * 1.5:.2f}" + f" (1.5x the worst safe-ground gradient {worst_safe:.2f})") + else: + print("HAZARD_MIN_STEP : need an 'open' or 'wall' label") + + # HAZARD_MAX_JUMP / FLOOR_WALL_MIN_JUMP: from a real ground break. + haz_lbl = pick("stair", "dropoff", "drop", "curb", "step") + if haz_lbl: + g = means[haz_lbl]["ground_max_grad"] + d = "drop-off" if means[haz_lbl]["ground_signed_break"] > 0 else "step-up" + print(f"HAZARD_MAX_JUMP ~ {g:.2f} (saturate at the {haz_lbl!r} break, a {d})") + print(f"FLOOR_WALL_MIN_JUMP ~ {max(g * 0.3, 1.0):.2f}" + f" (0.3x that break; confirm against a floor+wall capture)") + else: + print("HAZARD_MAX_JUMP : need a 'stairs' or 'curb' label") + print("FLOOR_WALL_MIN_JUMP : need a 'stairs' or 'curb' label") + + # WALL_CONFIRM_SATURATE: feel decision, informed by the point-blank wall. + if wall_lbl: + print(f"WALL_CONFIRM_SATURATE : feel decision — a confirmed wall at " + f"p10 {means[wall_lbl]['frame_p10']:.1f} should read max buzz; " + f"agree the cut-in distance with the haptics owner") + + print("\nNote: starting points from the captured means — sanity-check against " + "the per-label table above, then re-run the unit tests.\n") + + +def main(): + ap = argparse.ArgumentParser(description="Depth calibration analysis") + ap.add_argument("--analyze", metavar="CSV", required=True, help="calibration CSV to summarise") + args = ap.parse_args() + analyze_csv(args.analyze) + + +if __name__ == "__main__": + main() \ No newline at end of file diff --git a/src/second_vision/pipeline/TBR-callback-depth-est.py b/src/second_vision/pipeline/TBR-callback-depth-est.py index e69de29..78034d0 100644 --- a/src/second_vision/pipeline/TBR-callback-depth-est.py +++ b/src/second_vision/pipeline/TBR-callback-depth-est.py @@ -0,0 +1,401 @@ +import multiprocessing +import os +import queue as queue_module +import time +from pathlib import Path +import cv2 +os.environ["GST_PLUGIN_FEATURE_RANK"] = "vaapidecodebin:NONE" + +import gi +gi.require_version("Gst", "1.0") +from gi.repository import Gst + +import hailo +import numpy as np + +from hailo_apps.python.pipeline_apps.custom_depth_detection.sv_pipeline_v4 import GStreamerParallelApp +from hailo_apps.python.core.gstreamer.gstreamer_app import app_callback_class, _internal_callback_wrapper +from hailo_apps.python.core.common.hailo_logger import get_logger + +from hailo_apps.python.core.common.buffer_utils import get_caps_from_pad, get_numpy_from_buffer +from hailo_apps.python.pipeline_apps.custom_depth_detection.depth_utils import ( + DepthPostProcessor, + SUBGRID_SHAPE, + crop_border, + detect_ground_hazard, + downsample_depth, +) +from hailo_apps.python.pipeline_apps.custom_depth_detection.calibration import CalibrationLogger +from hailo_apps.python.pipeline_apps.custom_depth_detection.capture import FrameCapture +# Rendering lives in depth_view (no hardware deps) so the identical view can be +# reproduced off-device by verify_scene.py. +from hailo_apps.python.pipeline_apps.custom_depth_detection.depth_view import cv2_draw_depth + +# Object detection version +# LEFT_BOUNDARY = 0.22 +# CENTER_LEFT_BOUNDARY = 0.25 +# CENTER_RIGHT_BOUNDARY = 0.75 +# RIGHT_BOUNDARY = 0.78 + +# Depth estimation version (used to make depth estimation work) +LEFT_BOUNDARY = 0.22 +CENTER_LEFT_BOUNDARY = 0.25 +CENTER_RIGHT_BOUNDARY = 0.75 +RIGHT_BOUNDARY = 0.78 + +CONFIDENCE_THRESHOLD = 0.70 + +PREVIEW_EVERY = 2 # only send every Nth frame to the display process +# (overlay settings now live in depth_view.py — SV_SUBGRID_OVERLAY=0 disables) + +hailo_logger = get_logger(__name__) + +class user_app_callback_class(app_callback_class): + def __init__(self): + super().__init__() + # Structure: {track_id: {'direction': str, 'label': str, 'last_frame': int}} + self.track_history = {} + self.fps_start_time = time.monotonic() + # Set of track_ids that have moved from center to a side zone + self.IDs_changed_zones = set() + self.depth_count = 0 + self.depth_processor = DepthPostProcessor() + # Calibration capture mode (SV_CALIBRATE=1): logs raw per-zone stats to + # CSV + terminal and skips the overlay, so it runs headless even while the + # VNC preview is frozen. Off for normal runs. + self.calibration = CalibrationLogger() if os.environ.get("SV_CALIBRATE") == "1" else None + # Corpus capture mode (SV_CAPTURE=1): dumps labelled raw depth frames to + # .npy for offline re-scoring by score_corpus.py. Independent of the + # overlay and of calibration mode — both can run at once, and neither + # needs a display, so this works even if the preview is frozen. + self.capture = FrameCapture() if os.environ.get("SV_CAPTURE") == "1" else None + # Small queue feeding the depth visualization process; frames are + # dropped when full so the streaming thread never blocks. + self.depth_view_queue = multiprocessing.Queue(maxsize=2) + + def get_depth_stats(self, depth_mat): + depth_values = np.array(depth_mat).flatten() + try: + m_depth_values = depth_values[depth_values <= np.percentile(depth_values, 95)] + except Exception: + m_depth_values = np.array([]) + + if len(m_depth_values) > 0: + avg_d = np.mean(m_depth_values) + min_d = np.min(m_depth_values) + max_d = np.max(m_depth_values) + return avg_d, min_d, max_d + return 0, 0, 0 + + def get_fps(self): + elapsed = time.monotonic() - self.fps_start_time + if elapsed> 0: + return self.get_count() / elapsed + return 0.0 + +def depth_view_worker(view_queue): + """ + Runs in a separate process: RENDERS and shows depth visualization frames. + + PERF: rendering happens here, not in the GStreamer callback. The callback + only ships the small (64x48) depth array plus the already-computed values, so + the streaming thread stays free for inference (depth + YOLO share the CPU). + """ + while True: + try: + payload = view_queue.get(timeout=1.0) + except queue_module.Empty: + continue + small, intensities, hazard_detected, severity, hazard_dir, thin, thin_mask = payload + cv2.imshow("Depth Post-Processing", + cv2_draw_depth(small, intensities, hazard_detected, severity, hazard_dir, + thin, thin_mask)) + cv2.waitKey(1) + + +def on_depth_frame(element, buffer, user_data): + if buffer is None: + return + + # Process every frame so the EMA stays smooth; only printing is throttled. + user_data.depth_count += 1 + + roi = hailo.get_roi_from_buffer(buffer) + depth_mask = roi.get_objects_typed(hailo.HAILO_DEPTH_MASK) + + if len(depth_mask) > 0: + mask = depth_mask[0] + depth_mat = mask.get_data() + + try: + h = mask.get_height() + w = mask.get_width() + depth_data = np.array(depth_mat).reshape((h, w)) + except Exception as e: + # Fallback if get_height/width fails + depth_data = np.array(depth_mat) + + # We expect a 2D array representing the depth map + if len(depth_data.shape) == 2: + # Capture the RAW frame first — before crop/downsample — so the + # corpus holds exactly what the model emitted and stays valid if the + # post-processing chain changes. Non-blocking (drops when the disk + # can't keep up). + if user_data.capture is not None: + user_data.capture.maybe_save(depth_data, user_data.depth_count) + + # Drop the unreliable border ring before any aggregation so the + # zone intensities, hazard check, and visualization all see clean data. + depth_data = crop_border(depth_data) + small = downsample_depth(depth_data) + + # Calibration mode: log raw stats headless and skip all drawing. + if user_data.calibration is not None: + user_data.calibration.log(small, user_data.depth_count) + return + + # Hand the processor the CROPPED but full-resolution map: its + # thin-structure pass must see cables/poles before the 64x48 + # decimation destroys them. It downsamples internally, so every + # other detector sees exactly the same `small` grid as before. + intensities = user_data.depth_processor.process(depth_data) + hazard_detected, severity, hazard_dir = detect_ground_hazard(small, small.shape[0]) + + # Ship raw values; the display process does the drawing (see + # depth_view_worker). Throttled — the EMA-smoothed values barely + # change frame to frame, so previewing every 2nd frame is free. + # The thin readings ride along so the HUD tags what the motors got, + # rather than a weaker value recomputed from the 64x48 grid. + if user_data.depth_count % PREVIEW_EVERY == 0: + try: + user_data.depth_view_queue.put_nowait( + (small, intensities, hazard_detected, severity, hazard_dir, + user_data.depth_processor.last_thin, + user_data.depth_processor.overlay_mask()) + ) + except queue_module.Full: + pass + + if user_data.depth_count % 15 == 0: + d_lo, d_med, d_hi = np.percentile(small, [1, 50, 99]) + print(f"[DEPTH] Frame {user_data.depth_count} | Intensities: L={intensities['left']} C={intensities['center']} R={intensities['right']} | Hazard: {hazard_detected} ({hazard_dir}, sev: {severity}) | raw p1/p50/p99: {d_lo:.2f}/{d_med:.2f}/{d_hi:.2f}") + elif user_data.depth_count % 15 == 0: + # Fallback if the data isn't a 2D array + avg_d, min_d, max_d = user_data.get_depth_stats(depth_data) + print(f"[DEPTH] Frame {user_data.depth_count} | Avg: {avg_d:.2f} | Min: {min_d:.2f} | Max: {max_d:.2f}") + + +def cv2_draw_det(frame_bgr, active_zones, det_labels, width, height, user_data): + left_line_x = int(width * LEFT_BOUNDARY) + center_left_line_x = int(width * CENTER_LEFT_BOUNDARY) + center_right_line_x = int(width * CENTER_RIGHT_BOUNDARY) + right_line_x = int(width * RIGHT_BOUNDARY) + + # Draw semi-transparent red tint on active zones + overlay = frame_bgr.copy() + tint_color = (0, 0, 255) # Red in BGR + + if "left" in active_zones: + cv2.rectangle(overlay, (0, 0), (left_line_x, height), tint_color, -1) + if "center" in active_zones: + cv2.rectangle(overlay, (center_left_line_x, 0), (center_right_line_x, height), tint_color, -1) + if "right" in active_zones: + cv2.rectangle(overlay, (right_line_x, 0), (width, height), tint_color, -1) + + # Blend: 30% tint + 70% original + if active_zones: + frame_bgr = cv2.addWeighted(overlay, 0.3, frame_bgr, 0.7, 0) + + # Draw zone divider lines on top of the blended frame + cv2.line(frame_bgr, (left_line_x, 0), (left_line_x, height), (255, 0, 0), 2) + cv2.line(frame_bgr, (center_left_line_x, 0), (center_left_line_x, height), (0, 255, 0), 2) + cv2.line(frame_bgr, (center_right_line_x, 0), (center_right_line_x, height), (0, 255, 0), 2) + cv2.line(frame_bgr, (right_line_x, 0), (right_line_x, height), (0, 0, 255), 2) + + # Draw bounding boxes and text labels for each person detection + for (direction, label), (display_text, bbox, area, track_id) in det_labels.items(): + x1 = int(bbox.xmin() * width) + y1 = int(bbox.ymin() * height) + x2 = int(bbox.xmax() * width) + y2 = int(bbox.ymax() * height) + + if track_id in user_data.IDs_changed_zones: + bbox_color = (0, 165, 255) # Orange for objects that left the center + else: + bbox_color = (0, 255, 0) # Green for standard detections + + cv2.rectangle(frame_bgr, (x1, y1), (x2, y2), bbox_color, 2) + cv2.putText( + img=frame_bgr, + text=display_text, + org=(x1, y1 - 10), + fontFace=cv2.FONT_HERSHEY_SIMPLEX, + fontScale=0.7, + color=(0, 0, 0), + thickness=2 + ) + cv2.putText( + img=frame_bgr, + text=f"ID: {track_id}", + org=(x1, y1 + 15), + fontFace=cv2.FONT_HERSHEY_SIMPLEX, + fontScale=0.6, + color=(0, 255, 0), + thickness=2 + ) + + user_data.set_frame(frame_bgr) + +def on_det_frame(element, buffer, user_data): + if buffer is None: + return + + frame_count = user_data.get_count() + + roi = hailo.get_roi_from_buffer(buffer) + detections = roi.get_objects_typed(hailo.HAILO_DETECTION) + + # Get the frame from the GStreamer buffer (not from the queue!) + pad = element.get_static_pad("src") + fmt, width, height = get_caps_from_pad(pad) + + if not user_data.use_frame or fmt is None or width is None or height is None: + # No frame available — still print detection info below + frame_bgr = None + else: + frame = get_numpy_from_buffer(buffer, fmt, width, height) + # Hailo provides RGB, but CV2 expects BGR + frame_bgr = cv2.cvtColor(frame, cv2.COLOR_RGB2BGR) + + # --- Pass 1: Collect active zones and detection info --- + active_zones = set() + # Structure: {(direction, label): (display_text, bbox, area, track_id)} + det_labels = {} + + for det in detections: + # Get object properties from detection + label = det.get_label() + confidence = det.get_confidence() + bbox = det.get_bbox() + track = det.get_objects_typed(hailo.HAILO_UNIQUE_ID) + track_id = (track[0].get_id() if len(track) == 1 else 0) + + # Get the center x coordinate of the bounding box + center_x = bbox.xmin() + (bbox.width() / 2.0) + + # Get the label for the detection + direction_text = f"{label} direction cannot be determined!" + # print(f"[DET] Confidence value of {label} is {confidence*100:.0f}%") + + # Filter detections based on confidence threshold + if confidence >= CONFIDENCE_THRESHOLD: + # Direction Assignment + if center_x <= LEFT_BOUNDARY: + direction_text = f"{label} on left!" + direction = "left" + elif center_x >= CENTER_LEFT_BOUNDARY and center_x <= CENTER_RIGHT_BOUNDARY: + direction_text = f"{label} in front!" + direction = "center" + elif center_x >= RIGHT_BOUNDARY: + direction_text = f"{label} on right!" + direction = "right" + else: + direction_text = f"{label} not identifiable!" + continue + + # ID Tracking - Check if ID has changed direction + if track_id in user_data.track_history: + track_hist = user_data.track_history[track_id] + + # If the object was seen before, and its zone changed + if track_hist["label"] == label and track_hist["direction"] == "center" and direction != "center": + direction_text = f"center {label} leaving {direction}!" + user_data.IDs_changed_zones.add(track_id) + elif track_hist["label"] == label and track_hist["direction"] != "center" and direction == "center": + # If it comes back to the center, remove it from the hazard set + user_data.IDs_changed_zones.discard(track_id) + + user_data.track_history[track_id] = { + "direction": direction, + "label": label, + "last_frame": frame_count + } + + # Add or update detection in the list + area = bbox.width() * bbox.height() + if (direction, label) in det_labels: + prev_text, prev_bbox, prev_area, prev_track_id = det_labels[(direction, label)] + + # Make sure the current text says "multiple" + if not prev_text.startswith("multiple"): + prev_text = f"multiple {prev_text}" + + if area > prev_area: + det_labels[(direction, label)] = (f"multiple {direction_text}", bbox, area, track_id) + else: + det_labels[(direction, label)] = (prev_text, prev_bbox, prev_area, prev_track_id) + else: + det_labels[(direction, label)] = (direction_text, bbox, area, track_id) + + active_zones.add(direction) + + # --- Pass 2: Stale ID Related Logic --- + stale_ids = [] + for track_id in user_data.track_history: + if frame_count - user_data.track_history[track_id]["last_frame"] > 15: + stale_ids.append(track_id) + + for track_id in stale_ids: + user_data.track_history.pop(track_id, None) + user_data.IDs_changed_zones.discard(track_id) + + # --- Pass 3: Draw zone tints, lines, and text --- + if frame_bgr is not None: + cv2_draw_det(frame_bgr, active_zones, det_labels, width, height, user_data) + +class GStreamerDualApp(GStreamerParallelApp): + def _connect_callback(self): + disable_callback = self.options_menu.disable_callback + + # Connect Detection branch using the internal wrapper to handle frame counting and watchdog + det_identity = self.pipeline.get_by_name("det_callback") + if det_identity: + det_identity.set_property("signal-handoffs", True) + det_identity.connect( + "handoff", _internal_callback_wrapper, self.user_data, on_det_frame, disable_callback + ) + hailo_logger.debug("Connected detection callback.") + + # Connect Depth branch directly (to avoid double-incrementing the frame counter) + depth_identity = self.pipeline.get_by_name("depth_callback") + if depth_identity: + depth_identity.set_property("signal-handoffs", True) + if not disable_callback: + depth_identity.connect("handoff", on_depth_frame, self.user_data) + hailo_logger.debug("Connected depth callback.") + +def main(): + hailo_logger.info("Starting SV Dual Pipeline App") + user_data = user_app_callback_class() + + # Depth visualization window runs in its own process (mirrors the + # framework's display_user_data_frame pattern); daemon so it dies with us. + viewer = multiprocessing.Process( + target=depth_view_worker, args=(user_data.depth_view_queue,), daemon=True + ) + viewer.start() + + # Pass None for app_callback because we explicitly connect them in _connect_callback override + app = GStreamerDualApp(None, user_data) + try: + app.run() + finally: + # Flush queued captures / close the CSV on Ctrl-C too, so a capture run + # ended by hand still leaves a complete corpus and manifest. + if user_data.capture is not None: + user_data.capture.close() + if user_data.calibration is not None: + user_data.calibration.close() + +if __name__ == "__main__": + main() \ No newline at end of file From ae302d88d5a376420b35cf9bc8ee56fdd2446b10 Mon Sep 17 00:00:00 2001 From: Kenzhu-A Date: Thu, 30 Jul 2026 21:08:27 +0800 Subject: [PATCH 05/28] feat(depth-callback): added v4 of the sv pipeline --- src/custom_depth_detection/sv_pipeline_v4.py | 193 +++++++++++++++++++ src/second_vision/core/capture.py | 0 2 files changed, 193 insertions(+) create mode 100644 src/custom_depth_detection/sv_pipeline_v4.py create mode 100644 src/second_vision/core/capture.py diff --git a/src/custom_depth_detection/sv_pipeline_v4.py b/src/custom_depth_detection/sv_pipeline_v4.py new file mode 100644 index 0000000..6705606 --- /dev/null +++ b/src/custom_depth_detection/sv_pipeline_v4.py @@ -0,0 +1,193 @@ +import os +from pathlib import Path + +os.environ["GST_PLUGIN_FEATURE_RANK"] = "vaapidecodebin:NONE" + +import gi +import setproctitle + +gi.require_version("Gst", "1.0") +from gi.repository import Gst + +from hailo_apps.python.core.common.core import ( + get_pipeline_parser, + get_resource_path, + handle_list_models_flag, + resolve_hef_path, +) +from hailo_apps.python.core.common.defines import ( + DEPTH_APP_TITLE, + DEPTH_PIPELINE, + DEPTH_POSTPROCESS_FUNCTION, + DEPTH_POSTPROCESS_SO_FILENAME, + RESOURCES_SO_DIR_NAME, + RESOURCES_VIDEOS_DIR_NAME, + DETECTION_APP_TITLE, + DETECTION_PIPELINE, + DETECTION_POSTPROCESS_FUNCTION, + DETECTION_POSTPROCESS_SO_FILENAME, +) +from hailo_apps.python.core.common.hef_utils import get_hef_labels_json + +from hailo_apps.python.core.common.hailo_logger import get_logger +from hailo_apps.python.core.gstreamer.gstreamer_app import ( + GStreamerApp, + app_callback_class, + dummy_callback, +) +from hailo_apps.python.core.gstreamer.gstreamer_helper_pipelines import ( + + INFERENCE_PIPELINE, + INFERENCE_PIPELINE_WRAPPER, + USER_CALLBACK_PIPELINE, + TRACKER_PIPELINE, + QUEUE, +) + +hailo_logger = get_logger(__name__) + +class GStreamerParallelApp(GStreamerApp): + def __init__(self, app_callback, user_data, parser=None): + if parser is None: + parser = get_pipeline_parser() + + parser.add_argument( + "--labels-json", + default=None, + help="Path to custom labels JSON file", + ) + + parser.add_argument( + "--det-hef-path", + default="yolov8n.hef", + help="Specific HEF model to use for detection (default: yolov8n.hef)", + ) + + # Handle list models flags for both + handle_list_models_flag(parser, DEPTH_PIPELINE) + handle_list_models_flag(parser, DETECTION_PIPELINE) + + hailo_logger.info("Initializing Parallel Depth & Detection App V4...") + + super().__init__(parser, user_data) + + # Adjust dimensions for detection defaults + if self.video_width == 1280: + self.video_width = 640 + if self.video_height == 720: + self.video_height = 640 + + # Adjust batch size for detection defaults + if self.batch_size == 1: + self.batch_size = 2 + + self.app_callback = app_callback + setproctitle.setproctitle("Parallel-Depth-Detection-V4") + + # ---- Depth App Parameters ---- + self.depth_hef_path = resolve_hef_path( + self.hef_path, app_name=DEPTH_PIPELINE, arch=self.arch + ) + self.depth_post_process_so = get_resource_path( + DEPTH_PIPELINE, RESOURCES_SO_DIR_NAME, self.arch, DEPTH_POSTPROCESS_SO_FILENAME + ) + self.depth_post_function_name = DEPTH_POSTPROCESS_FUNCTION + + # ---- Detection Parameters ---- + self.det_hef_path = resolve_hef_path( + self.options_menu.det_hef_path, app_name=DETECTION_PIPELINE, arch=self.arch + ) + self.det_post_process_so = get_resource_path( + DETECTION_PIPELINE, RESOURCES_SO_DIR_NAME, self.arch, DETECTION_POSTPROCESS_SO_FILENAME + ) + self.det_post_function_name = DETECTION_POSTPROCESS_FUNCTION + + self.labels_json = self.options_menu.labels_json + if self.labels_json is None: # if no labels JSON file is provided, try auto-detect it from the HEF file + self.labels_json = get_hef_labels_json(self.det_hef_path) + if self.labels_json is not None: + hailo_logger.info("Auto detected Labels JSON: %s", self.labels_json) + + nms_score_threshold = 0.3 + nms_iou_threshold = 0.45 + self.thresholds_str = ( + f"nms-score-threshold={nms_score_threshold} " + f"nms-iou-threshold={nms_iou_threshold} " + f"output-format-type=HAILO_FORMAT_TYPE_FLOAT32" + ) + + # Validate resource paths + for path, name in [ + (self.depth_hef_path, "Depth HEF"), + (self.depth_post_process_so, "Depth Postprocess SO"), + (self.det_hef_path, "Detection HEF"), + (self.det_post_process_so, "Detection Postprocess SO") + ]: + if path is None or not Path(path).exists(): + hailo_logger.error(f"{name} path is invalid or missing: %s", path) + + self.create_pipeline() + hailo_logger.debug("Pipeline created successfully") + + def get_pipeline_string(self): + source_pipeline = self.get_source_pipeline(no_webcam_compression=True) + + # 1. Depth Branch + depth_pipeline = INFERENCE_PIPELINE( + hef_path=self.depth_hef_path, + post_process_so=self.depth_post_process_so, + post_function_name=self.depth_post_function_name, + name="depth_inference", + ) + depth_pipeline_wrapper = INFERENCE_PIPELINE_WRAPPER( + depth_pipeline, name="inference_wrapper_depth" + ).replace("use-letterbox=true", "use-letterbox=false") + depth_callback = USER_CALLBACK_PIPELINE(name="depth_callback") + depth_sink = f"fakesink name=depth_sink sync=false" + + # 2. Detection Branch + detection_pipeline = INFERENCE_PIPELINE( + hef_path=self.det_hef_path, + post_process_so=self.det_post_process_so, + post_function_name=self.det_post_function_name, + batch_size=self.batch_size, + config_json=self.labels_json, + additional_params=self.thresholds_str, + name="det_inference" + ) + detection_pipeline_wrapper = INFERENCE_PIPELINE_WRAPPER( + detection_pipeline, name="inference_wrapper_det" + ) + tracker_pipeline = TRACKER_PIPELINE( + class_id=-1, + kalman_dist_thr=0.8, # Increased to allow for larger velocity predictions (default: 0.7) + iou_thr=0.6, # Lowered so fast objects with less overlap still match (default: 0.9) + init_iou_thr=0.6, # Pickier about new object matching to reduce phantom IDs (default: 0.7) + keep_new_frames=3, # 100ms grace period for new detections to stabilize (default: 2) + keep_tracked_frames=10, # 333ms before tracked→lost, reduces ghost duration (default: 15) + keep_lost_frames=8, # 266ms grace for fast objects blurring out of YOLO (default: 2) + name="det_tracker" + ) + det_callback = USER_CALLBACK_PIPELINE(name="det_callback") + det_sink = f"fakesink name=det_sink sync=false" + + # 3. Parallel tee architecture (display handled by cv2 in callbacks) + pipeline_str = ( + f"{source_pipeline} ! tee name=t " + f"t. ! {QUEUE(name='depth_branch_q')} ! {depth_pipeline_wrapper} ! {depth_callback} ! {depth_sink} " + f"t. ! {QUEUE(name='det_branch_q')} ! {detection_pipeline_wrapper} ! {tracker_pipeline} ! {det_callback} ! {det_sink}" + ) + + hailo_logger.info("Generated Pipeline string:\n%s", pipeline_str) + return pipeline_str + +def main(): + hailo_logger.info("Creating user data for the app callback...") + user_data = app_callback_class() + app_callback = dummy_callback + app = GStreamerParallelApp(app_callback, user_data) + app.run() + +if __name__ == "__main__": + hailo_logger.info("Starting Parallel Depth & Detection App V4...") + main() \ No newline at end of file diff --git a/src/second_vision/core/capture.py b/src/second_vision/core/capture.py new file mode 100644 index 0000000..e69de29 From a8ee3e3cf2c7072c5ddb72a9c04c884b8dffd8be Mon Sep 17 00:00:00 2001 From: Kenzhu-A Date: Thu, 30 Jul 2026 22:26:42 +0800 Subject: [PATCH 06/28] feat(depth-callback): added depth post processing features in callbacks --- src/second_vision/core/calibration.py | 6 +- src/second_vision/core/capture.py | 196 +++++++++++++++++ src/second_vision/core/depth_view.py | 34 ++- src/second_vision/pipeline/callbacks.py | 276 ++++++++++++++++++++++-- 4 files changed, 482 insertions(+), 30 deletions(-) diff --git a/src/second_vision/core/calibration.py b/src/second_vision/core/calibration.py index a2fe587..e049e0d 100644 --- a/src/second_vision/core/calibration.py +++ b/src/second_vision/core/calibration.py @@ -8,7 +8,7 @@ CAPTURE (runs inside the live pipeline, headless — no video window, so it works even while the VNC preview is frozen): - 1) SV_CALIBRATE=1 python -m ...sv_dual_callback_withdepth + 1) SV_CALIBRATE=1 ./scripts/sv-main.sh (or ./scripts/run.sh --input usb) 2) place an object at a known distance, then in a SECOND ssh terminal: echo "0.5m" > calib_label.txt # tag the current samples move it and retag. Capture ALL of these to resolve every threshold: @@ -20,7 +20,7 @@ Rows stream to depth_calibration.csv, throttled to the terminal. ANALYSE (offline, no hardware): - python -m ...calibration --analyze depth_calibration.csv + python3 -m second_vision.core.calibration --analyze depth_calibration.csv Groups rows by label and prints per-distance stats plus suggested threshold values you can drop into depth_utils.py. @@ -36,7 +36,7 @@ import numpy as np -from hailo_apps.python.pipeline_apps.custom_depth_detection.depth_utils import ( +from second_vision.core.depth_utils import ( ground_break_stats, local_flatness, zone_warning_breakdown, diff --git a/src/second_vision/core/capture.py b/src/second_vision/core/capture.py index e69de29..d544f24 100644 --- a/src/second_vision/core/capture.py +++ b/src/second_vision/core/capture.py @@ -0,0 +1,196 @@ +""" +Depth frame capture — build a labelled corpus of REAL model output on the Pi. + +Why this exists: every edge-case detector in depth_utils.py was written and +unit-tested against SYNTHETIC arrays. Real SC-DepthV3 output on the OV2640 does +not look like those arrays — the "wall reads as a dome" finding +(depth_utils.detect_blank_wall) proved that the hard way, live, by eye. This +module turns live scenes into .npy files so the detectors can be re-scored +offline, repeatably, as many times as thresholds change. + +It is the missing PRODUCER for verify_scene.py, which has always been able to +replay a "captured on the Pi" .npy but had nothing to write one. + + CAPTURE (headless — no video window, works even if the preview is frozen): + 1) SV_CAPTURE=1 ./scripts/sv-main.sh (or ./scripts/run.sh --input usb) + 2) point the camera at a scene, then in a SECOND ssh terminal: + echo "wall_far" > calib_label.txt + re-tag as you move ("open", "thin_pole", "stairs", ...). Frames stream + to depth_corpus/