From 523f89541b791f1715f1f742754ecc5dfc627bbc Mon Sep 17 00:00:00 2001 From: Stevenson Chittumuri Date: Fri, 3 Jul 2026 12:43:04 -0400 Subject: [PATCH] Follow the coasted LOST target briefly while it's still moving Publish the locked target's coasted LOST track (instead of filtering it out) and let _drive_ptz_auto chase it while |coast velocity| >= _COAST_FOLLOW_MIN_V (1.0 px/frame). The tracker's velocity decay makes this self-limiting: a mover is followed for ~1s of coast then falls back to hold->coast->search; a stationary loss (v~=0) is never followed, matching today's behavior. Co-Authored-By: Claude Fable 5 --- autoptz/engine/camera_worker.py | 67 ++++++--- tests/test_worker_coast_follow.py | 223 ++++++++++++++++++++++++++++++ 2 files changed, 270 insertions(+), 20 deletions(-) create mode 100644 tests/test_worker_coast_follow.py diff --git a/autoptz/engine/camera_worker.py b/autoptz/engine/camera_worker.py index 43854e32..bd5129b6 100644 --- a/autoptz/engine/camera_worker.py +++ b/autoptz/engine/camera_worker.py @@ -289,6 +289,14 @@ def wrapper(self: Any, *args: Any, **kwargs: Any) -> Any: _TARGET_OVERLAP_IOU = 0.08 _TARGET_CLOSE_Y_OVERLAP = 0.45 _TARGET_CLOSE_GAP_FRAC = 0.18 + +# Minimum coasted-track speed (px/frame) worth chasing while a locked target is +# LOST. The tracker's coast velocity decays each frame (see track.py), so this +# threshold is self-limiting: a mover is followed for ~1 s of coast, then falls +# back to hold->coast->search once its velocity has decayed below this floor. +# A stationary loss (occluder steps in front of a still subject) never crosses +# it, preserving today's no-chase behaviour for that case. +_COAST_FOLLOW_MIN_V = 1.0 _POSE_BBOX_MARGIN = 0.22 _POSE_KEYPOINT_MARGIN = 0.08 _POSE_JUMP_MIN_PX = 70.0 @@ -1532,30 +1540,46 @@ def _drive_ptz_auto( # coasts→stops), even if a target track is locked. tracking_on = self._feature("tracking") target = self._resolve_target_track(tracks) - # A coasting (LOST) target is a STALE box — don't chase it. Stepping the - # controller with track_active=False lets it run its graceful coast→search - # →stop instead of driving the PTZ toward where the subject no longer is - # (the "moves the camera for no reason while the box lingers" bug). + # A coasting (LOST) target is a STALE box, but its damped pre-loss + # velocity is still meaningful prediction for a beat: while |coast + # velocity| clears _COAST_FOLLOW_MIN_V we follow that prediction so a + # brief occlusion doesn't freeze the camera; once it decays below the + # floor (or the loss was a stationary occluder, v≈0 from the start) we + # fall back to track_active=False so the controller coasts→searches→stops + # instead of driving the PTZ toward where the subject no longer is. + coasting_fast = ( + target is not None + and target.lost + and (math.hypot(target.vx, target.vy) >= _COAST_FOLLOW_MIN_V) + ) if ( target is not None - and not target.lost + and (not target.lost or coasting_fast) and frame is not None and self._tracking_enabled and tracking_on ): - fh = max(1, int(frame.shape[0])) - usable = self._target_box_usable_for_ptz(target, frame) - box_h_frac = (float(target.bbox.y2) - float(target.bbox.y1)) / fh if usable else 0.0 - occluded = usable and self._target_box_collapsed(box_h_frac) - if usable and not occluded: + if target.lost: + # The coast prediction bypasses the live-box usability/collapse + # gates below (they assume a fresh detection) — a coasted bbox is + # by definition not "fresh," that's the point of following it. err, height = self._track_error(target, frame, now, tracks=tracks) vel = self._estimate_aim_velocity(err, now) active = True else: - # Bad/partial boxes are not evidence. Do not update aim velocity - # from them; tell the controller to hold/stop until a fresh target - # box is usable again. - err, height, vel, active = (0.0, 0.0), 0.0, (0.0, 0.0), False + fh = max(1, int(frame.shape[0])) + usable = self._target_box_usable_for_ptz(target, frame) + box_h_frac = (float(target.bbox.y2) - float(target.bbox.y1)) / fh if usable else 0.0 + occluded = usable and self._target_box_collapsed(box_h_frac) + if usable and not occluded: + err, height = self._track_error(target, frame, now, tracks=tracks) + vel = self._estimate_aim_velocity(err, now) + active = True + else: + # Bad/partial boxes are not evidence. Do not update aim velocity + # from them; tell the controller to hold/stop until a fresh target + # box is usable again. + err, height, vel, active = (0.0, 0.0), 0.0, (0.0, 0.0), False try: # A box that suddenly collapsed (occlusion → only a partial body # visible) is not a trustworthy aim — coast (track_active=False) so @@ -4086,9 +4110,14 @@ def _maybe_track(self, frame: NDArray[np.uint8] | None) -> list[TrackInfo]: out: list[TrackInfo] = [] for t in tracks: + is_lost = getattr(t, "state", None) == "lost" + is_target = self._target_track_id is not None and t.track_id == self._target_track_id # LOST tracks remain inside the tracker for ReID/re-acquisition, but - # they are stale visual boxes. Do not publish them to the UI or PTZ. - if getattr(t, "state", None) == "lost": + # they are stale visual boxes. Do not publish them to the UI or PTZ — + # EXCEPT the locked target: its coasted (damped pre-loss velocity) + # prediction is published with lost=True so the PTZ layer can briefly + # follow the coast instead of freezing through the occlusion. + if is_lost and not is_target: continue # _track_identity[track_id] = (identity_id, display_name, score) ident = self._track_identity.get(t.track_id) @@ -4100,10 +4129,8 @@ def _maybe_track(self, frame: NDArray[np.uint8] | None) -> list[TrackInfo]: identity=(ident[1] if ident else None), # NAME, for display identity_id=(ident[0] if ident else None), # id, for enroll/target confidence=(ident[2] if ident else t.conf), - is_target=( - self._target_track_id is not None and t.track_id == self._target_track_id - ), - lost=False, + is_target=is_target, + lost=is_lost, vx=float(vel[0]), vy=float(vel[1]), ) diff --git a/tests/test_worker_coast_follow.py b/tests/test_worker_coast_follow.py new file mode 100644 index 00000000..1981f8b4 --- /dev/null +++ b/tests/test_worker_coast_follow.py @@ -0,0 +1,223 @@ +"""R-1 worker coast-follow: the PTZ briefly follows a coasted LOST target. + +The tracker (autoptz/engine/pipeline/track.py) already coasts LOST tracks +along a damped pre-loss velocity (see tests/test_track.py::TestPredictiveCoast). +This file covers the WORKER side that makes that coast reach the camera: + + 1. ``_maybe_track`` publishes the locked target's coasted LOST track (instead + of filtering it out like every other LOST track). + 2. ``_drive_ptz_auto`` follows that coasted target while it is still moving + fast enough to be worth chasing (``|v| >= _COAST_FOLLOW_MIN_V``), and + falls back to today's hold→coast→search once it has decayed below that. + 3. ``_apply_target_lock`` / ``_append_held_target`` do not synthesize a + second, frozen copy of the target now that the coasted one is published. + +Follows the ``_bare_worker`` harness pattern from test_camera_worker_framing.py +and the fake detect/tracker stack pattern from test_worker_crash_safety.py. +""" + +from __future__ import annotations + +import math + +import numpy as np + +from autoptz.config.models import CameraConfig +from autoptz.engine.camera_worker import _COAST_FOLLOW_MIN_V, CameraWorker +from autoptz.engine.pipeline.track import TrackState +from autoptz.engine.runtime.messages import BBox, TrackInfo + + +def _bare_worker() -> CameraWorker: + cfg = CameraConfig(id="cam-coast", name="Cam Coast") + return CameraWorker("cam-coast", cfg, on_telemetry=lambda m: None) + + +def _frame() -> np.ndarray: + return np.zeros((720, 1000, 3), dtype=np.uint8) + + +def _bbox(x1: float, y1: float, x2: float, y2: float) -> BBox: + return BBox(x1=x1, y1=y1, x2=x2, y2=y2) + + +def _track(track_id: int, bbox: BBox, *, lost: bool = False, vx: float = 0.0, vy: float = 0.0): + return TrackInfo(track_id=track_id, bbox=bbox, lost=lost, vx=vx, vy=vy) + + +class _FakeController: + """Minimal ``_ptz`` stand-in: only the methods ``_drive_ptz_auto`` touches + before handing off to ``_publish_ptz`` (which the tests monkeypatch).""" + + def set_loop_latency(self, seconds: float) -> None: + pass + + +def _worker_with_target(track_id: int = 7): + w = _bare_worker() + w._ptz = _FakeController() + w._tracking_enabled = True + w._target_track_id = track_id + w._manual_override_active = lambda now: False # type: ignore[assignment] + w._feature = lambda name: True # type: ignore[assignment] + return w + + +# ── (a)/(b)/(d): _drive_ptz_auto coast-follow ────────────────────────────────── + + +class TestDrivePtzAutoCoastFollow: + def test_lost_moving_target_drives_ptz_active(self) -> None: + """(a) lost + moving (|v| >= 1.0) → driven with track_active=True using + the coasted box error.""" + w = _worker_with_target() + calls: list[dict] = [] + w._publish_ptz = lambda ctrl, err, vel, height, **kw: calls.append( # type: ignore[assignment] + {"err": err, "vel": vel, "height": height, **kw} + ) + frame = _frame() + # Coasted box well right-of-center, moving right at 5 px/frame. + target = _track(7, _bbox(800, 300, 900, 500), lost=True, vx=5.0, vy=0.0) + w._drive_ptz_auto([target], frame, now=1000.0) + + assert calls, "no PTZ command published for a lost+moving target" + last = calls[-1] + assert last["track_active"] is True + # Coasted bbox is right of center → positive x error. + assert last["err"][0] > 0.0 + + def test_lost_stationary_target_does_not_drive(self) -> None: + """(b) lost + stationary (|v| < 1.0) → track_active=False (no drive).""" + w = _worker_with_target() + calls: list[dict] = [] + w._publish_ptz = lambda ctrl, err, vel, height, **kw: calls.append( # type: ignore[assignment] + {"err": err, "vel": vel, "height": height, **kw} + ) + frame = _frame() + target = _track(7, _bbox(800, 300, 900, 500), lost=True, vx=0.2, vy=0.1) + assert math.hypot(0.2, 0.1) < _COAST_FOLLOW_MIN_V + w._drive_ptz_auto([target], frame, now=1000.0) + + assert calls, "no PTZ command published" + assert calls[-1]["track_active"] is False + + def test_tracking_feature_disabled_stays_inactive_even_if_moving(self) -> None: + """(d) ``tracking`` feature off → inactive even with a moving lost target.""" + w = _worker_with_target() + w._feature = lambda name: False # type: ignore[assignment] + calls: list[dict] = [] + w._publish_ptz = lambda ctrl, err, vel, height, **kw: calls.append( # type: ignore[assignment] + {"err": err, "vel": vel, "height": height, **kw} + ) + frame = _frame() + target = _track(7, _bbox(800, 300, 900, 500), lost=True, vx=5.0, vy=0.0) + w._drive_ptz_auto([target], frame, now=1000.0) + + assert calls, "no PTZ command published" + assert calls[-1]["track_active"] is False + + def test_min_velocity_threshold_is_one_pixel_per_frame(self) -> None: + assert _COAST_FOLLOW_MIN_V == 1.0 + + def test_non_target_lost_tracks_still_ignored(self) -> None: + """No behaviour change for LOST tracks that are NOT the locked target — + _resolve_target_track must not pick them up.""" + w = _worker_with_target(track_id=7) + calls: list[dict] = [] + w._publish_ptz = lambda ctrl, err, vel, height, **kw: calls.append( # type: ignore[assignment] + {"err": err, "vel": vel, "height": height, **kw} + ) + frame = _frame() + other = _track(9, _bbox(100, 100, 200, 200), lost=True, vx=5.0, vy=0.0) + w._drive_ptz_auto([other], frame, now=1000.0) + + assert calls, "no PTZ command published" + assert calls[-1]["track_active"] is False + + +# ── (c): exactly one TrackInfo carries the target id while coasting ─────────── + + +class TestApplyTargetLockNoHeldDuplicate: + def test_coasted_target_present_no_held_duplicate_appended(self) -> None: + """(c) With the coasted LOST target already published, _apply_target_lock + (via _append_held_target) must NOT synthesize a second frozen copy.""" + w = _bare_worker() + w._target_track_id = 7 + # Prime the lock with a previous trusted sighting so _append_held_target + # WOULD fire if the target were missing (target is None) -- it must not + # fire here because the coasted track is present. + w._target_lock.trusted_track_id = 7 + w._target_lock.trusted_bbox = _bbox(700, 300, 800, 500) + w._target_lock.trusted_t = 999.999 + + tracks = [_track(7, _bbox(800, 300, 900, 500), lost=True, vx=5.0, vy=0.0)] + w._apply_target_lock(tracks, _frame(), now=1000.0) + + matching = [t for t in tracks if t.track_id == 7] + assert len(matching) == 1, ( + f"expected exactly one TrackInfo for the target, got {len(matching)}" + ) + assert matching[0].lost is True + + +# ── requirement 1: _maybe_track publishes the target's coasted LOST track ───── + + +class _FakeDetector: + def detect(self, frame): # noqa: ANN001, ANN202 + return [] + + +class _FakeTracker: + """Fake boxmot-wrapper tracker returning canned Track objects.""" + + def __init__(self, tracks): # noqa: ANN001 + self._tracks = tracks + + def update(self, detections, frame, fps=30.0): # noqa: ANN001, ANN202 + return self._tracks + + +class _FakeDetectStack: + def __init__(self, tracks): # noqa: ANN001 + self.detector = _FakeDetector() + self.tracker = _FakeTracker(tracks) + + +class _FakeRawTrack: + """Stand-in for autoptz.engine.pipeline.track.Track (the tracker's own + output type), shaped with the attributes _maybe_track's loop reads.""" + + def __init__(self, track_id, bbox, state, conf=0.9, velocity=(0.0, 0.0)): # noqa: ANN001 + self.track_id = track_id + self.bbox = bbox + self.state = state + self.conf = conf + self.velocity = velocity + + +def test_maybe_track_publishes_coasted_target_lost_track() -> None: + """Requirement 1: the loop keeps filtering LOST tracks EXCEPT the locked + target, which it emits with lost=True, its coasted bbox, and vx/vy from + the tracker's velocity.""" + w = _bare_worker() + w._target_track_id = 7 + target_raw = _FakeRawTrack( + 7, _bbox(800, 300, 900, 500), TrackState.LOST, conf=0.8, velocity=(5.0, -2.0) + ) + other_lost = _FakeRawTrack(9, _bbox(100, 100, 200, 200), TrackState.LOST, velocity=(1.0, 1.0)) + confirmed = _FakeRawTrack(3, _bbox(10, 10, 60, 60), TrackState.CONFIRMED, conf=0.95) + w._detect = _FakeDetectStack([target_raw, other_lost, confirmed]) # type: ignore[assignment] + + out = w._maybe_track(_frame()) + + ids = {t.track_id: t for t in out} + assert 9 not in ids, "non-target LOST tracks must still be filtered out" + assert 3 in ids and ids[3].lost is False + assert 7 in ids, "the locked target's coasted LOST track must be published" + published = ids[7] + assert published.lost is True + assert published.bbox.x1 == 800 and published.bbox.x2 == 900 + assert published.vx == 5.0 + assert published.vy == -2.0