You cannot select more than 25 topics Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
Chameleon/integration/ptz_predictive_zoom/zoom_controller.py

525 lines
23 KiB
Python

This file contains ambiguous Unicode characters!

This file contains ambiguous Unicode characters that may be confused with others in your current locale. If your use case is intentional and legitimate, you can safely ignore this warning. Use the Escape button to highlight these characters.

"""Predictive TRACK zoom policy. All time inputs are seconds on the same clock."""
from collections import deque
from dataclasses import dataclass, replace
import json
import math
from pathlib import Path
from queue import Full, Queue
from threading import Lock, Thread
@dataclass(frozen=True)
class ZoomConfig:
# Fit per camera and native command. Gains multiply ln(252/6)/full_travel_s.
nominal_gain: float
upper_gain: float
wide_fov: float = 59.0
tele_fov: float = 2.0
full_travel_s: float = 5.5
native: int = 10
entry_fov: float = 35.0
entry_center: float = 0.15 # fraction of full frame, 30% of half-width
repeat_center: float = 0.09
edge_margin: float = 0.16
tracker_score: float = 0.55
tracker_frames: int = 3
frame_age_s: float = 0.25
detector_age_s: float = 0.65
status_age_s: float = 0.65
evidence_gap_s: float = 0.20
max_outward_deg_s: float = 0.15
trajectory_slack_deg: float = 0.6
edge_accel: float = 0.55 # full frame / sВІ, outward
edge_watch_margin: float = 0.24
steering_delay_s: float = 0.18
max_residual_deg_s: float = 12.0
wide_pulse_fov: float = 25.0
tele_pulse_fov: float = 15.0
wide_pulse_s: float = 0.34
tele_pulse_s: float = 0.12
stop_slack_s: float = 0.06
stop_ack_slack_s: float = 0.08
target_px: float = 48.0
status_gain: float = 0.7
status_error_log: float = 0.08
ff_gain: float = 0.7
ff_max_deg_s: float = 3.0
def __post_init__(self):
for name, value in vars(self).items():
if not math.isfinite(value) or value <= 0:
raise ValueError(f"invalid zoom setting {name}")
if not (
self.native == 10 and 0 < self.nominal_gain <= self.upper_gain
and 0 < self.tele_fov < self.tele_pulse_fov < self.wide_pulse_fov
< self.entry_fov < self.wide_fov < 90
and 0 < self.repeat_center <= self.entry_center <= 0.15
and 0 < self.edge_margin <= self.edge_watch_margin < 0.5
and isinstance(self.tracker_frames, int) and 2 <= self.tracker_frames <= 12
and self.tracker_score <= 1 and self.status_gain <= 1
and self.frame_age_s <= 0.25 and self.detector_age_s <= 0.65
and self.status_age_s <= 0.65
and 0.10 <= self.tele_pulse_s <= self.wide_pulse_s <= 0.34
and self.wide_pulse_s + self.stop_slack_s <= 0.40 + 1e-9
and self.stop_ack_slack_s <= 0.12
):
raise ValueError("zoom settings exceed the native/geometry/watchdog limits")
@property
def log_rate(self):
return self.nominal_gain * math.log(252 / 6) / self.full_travel_s
@property
def upper_log_rate(self):
return self.upper_gain * math.log(252 / 6) / self.full_travel_s
def window(self, fov):
scale = min(1.0, max(0.0, (fov - self.tele_pulse_fov)
/ (self.wide_pulse_fov - self.tele_pulse_fov)))
nominal = self.tele_pulse_s + scale * (self.wide_pulse_s - self.tele_pulse_s)
return nominal, min(0.40, nominal + self.stop_slack_s)
def _log_focal(fov):
return -math.log(math.tan(math.radians(fov / 2)))
def _fov(log_focal):
return math.degrees(2 * math.atan(math.exp(-log_focal)))
class OpticalStateEstimator:
"""Integrate accepted native-10 windows, then correct with timestamped status."""
def __init__(self, config):
self.cfg = config
self.actual = None
self.status_at = 0.0
self.nominal = self.upper = 0.0
self.spans = deque()
self.samples = deque()
self.active = None
self.valid = False
def _seconds_since_status(self, at, status_at=None):
status_at = self.status_at if status_at is None else status_at
spans = self.spans
if self.active:
start, hard_end = self.active
spans = (*spans, (start, hard_end))
return sum(max(0.0, min(end, at) - max(start, status_at))
for start, end in spans)
def estimate(self, at):
for status_at, nominal, upper, _ in reversed(self.samples):
if status_at <= at:
elapsed = self._seconds_since_status(at, status_at)
tele = _log_focal(self.cfg.tele_fov)
return (_fov(min(tele, nominal + self.cfg.log_rate * elapsed)),
_fov(min(tele, upper + self.cfg.upper_log_rate * elapsed)))
return None, None
def observe(self, at, hfov, now):
if not (self.status_at < at <= now and math.isfinite(hfov)
and self.cfg.tele_fov - 0.1 <= hfov <= self.cfg.wide_fov + 1.0):
return
measured = _log_focal(hfov)
estimated, upper = self.estimate(at)
had_motion = self._seconds_since_status(at) > 0.0
discrepancy = (had_motion and estimated is not None
and (measured > _log_focal(upper) + self.cfg.status_error_log
or measured < _log_focal(self.actual) - self.cfg.status_error_log))
if discrepancy:
self.valid = False
elif not self.active:
self.valid = True
if not had_motion or estimated is None or discrepancy:
self.nominal = measured
else:
prediction = _log_focal(estimated)
self.nominal = prediction + self.cfg.status_gain * (measured - prediction)
# The filtered estimate never reduces the conservative edge envelope.
self.upper = max(self.nominal, measured + self.cfg.status_error_log)
self.actual, self.status_at = hfov, at
self.samples.append((at, self.nominal, self.upper, measured))
cutoff = at - self.cfg.status_age_s - self.cfg.frame_age_s
while len(self.samples) > 1 and self.samples[1][0] <= cutoff:
self.samples.popleft()
while self.spans and self.spans[0][1] <= self.samples[0][0]:
self.spans.popleft()
def start(self, put_at, ack_at, hard_end):
if self.active or not self.fresh(ack_at) or not put_at <= ack_at < hard_end:
self.valid = False
self.active = (put_at, hard_end)
def stop(self, at, *, clean):
if self.active:
start, hard_end = self.active
if at > hard_end + self.cfg.stop_ack_slack_s or at < start or not clean:
self.valid = False
if at > start:
self.spans.append((start, min(at, hard_end + self.cfg.stop_ack_slack_s)))
self.active = None
def fresh(self, now):
return self.valid and 0 <= now - self.status_at <= self.cfg.status_age_s
@dataclass(frozen=True)
class ZoomFrame:
at: float
seq: int
offset: tuple[float, float] # positive right/down; fractions of full frame
bbox_frac: tuple[float, float]
bbox_px: tuple[float, float]
angles: tuple[float, float] # camera-signed LOS error in degrees
rates: tuple[float, float] # camera-signed LOS residual, deg/s
aspect: float # image height / width
tracker_score: float | None
detector_at: float
detector_conf: float | None
geometry_ok: bool
los_valid: bool
@dataclass(frozen=True)
class ZoomDecision:
reason: str
window: tuple[float, float] | None = None
stop: bool = False
ff_deg_s: tuple[float, float] = (0.0, 0.0)
margin: float | None = None
class ZoomController:
"""Per TRACK epoch; the existing worker retains ownership and all STOPs."""
def __init__(self, config, owner):
self.cfg = config
self.owner = owner
self.optics = OpticalStateEstimator(config)
self.history = deque(maxlen=config.tracker_frames)
self.request = None
self.first_ack_at = None
self.confirmed_stop = None
self.completed_request = None
self.stop_cooldowns = None
def accepted(self, request, put_at, ack_at, hard_end):
if self.request == request:
return
self.request = request
self.confirmed_stop = None
self.completed_request = None
self.stop_cooldowns = None
if self.first_ack_at is None:
self.first_ack_at = ack_at
self.optics.start(put_at, ack_at, hard_end)
def stopped(self, request, at, *, confirmed, clean):
if self.request != request:
return
self.optics.stop(at, clean=clean)
self.request = None
if clean and self.optics.fresh(at):
self.completed_request = request
if confirmed:
self.confirmed_stop = (at, self.optics.status_at)
def optical_confirmation(self, request, at):
if self.completed_request == request and self.request is None and self.optics.fresh(at):
self.confirmed_stop = (at, self.optics.status_at)
def can_reuse_feedback(self, now, feedback_after):
return (self.confirmed_stop is not None and self.request is None
and self.confirmed_stop[0] == feedback_after and self.optics.fresh(now)
and self.optics.status_at >= self.confirmed_stop[1])
def owns(self, st):
return (st.get("predictive_zoom_controller") is self and _owner(st) == self.owner
and st.get("mode") == "TRACK" and st.get("tracking_active"))
def repeat_after_stop(self, st, now):
return bool(
self.owns(st) and self.can_reuse_feedback(
now, float(st.get("zoom_feedback_required_after", 0.0) or 0.0)
)
and st.get("zoom_step_last_delivery_result") == "confirmed"
and not any(st.get(key) for key in (
"zoom_step_feedback_pending", "zoom_step_pending",
"zoom_stop_obligation_active", "zoom_no_effect_blocked",
"zoom_step_cancel_requested", "zoom_rearm_required", "lens_move_stop_required",
"lens_move_put_inflight",
))
and self.stop_cooldowns == _cooldowns(st)
and tuple(st.get("last_ptz_physical_cmd", (None, None, None)))[2] == 0
)
def _record(self, frame, fresh):
history = self.history
if (not fresh or frame.tracker_score is None
or frame.tracker_score < self.cfg.tracker_score):
history.clear()
return
if history:
prev = history[-1]
if frame.seq <= prev.seq or frame.at <= prev.at:
history.clear()
return
dt = frame.at - prev.at
if (dt > self.cfg.evidence_gap_s
or any(abs(b - a - rate * dt) > self.cfg.trajectory_slack_deg
for a, b, rate in zip(prev.angles, frame.angles, prev.rates))):
history.clear()
history.append(frame)
def decide(self, frame, now, *, first, cooldown, transport_clear):
cfg = self.cfg
values = (*frame.offset, *frame.bbox_frac, *frame.bbox_px,
*frame.angles, *frame.rates, frame.at, frame.aspect)
fresh = (frame.geometry_ok and frame.los_valid and frame.seq > 0
and all(math.isfinite(x) for x in values)
and 0 < frame.aspect <= 4 and min(frame.bbox_frac + frame.bbox_px) > 0
and 0 <= now - frame.at <= cfg.frame_age_s)
prev = self.history[-1] if self.history else None
moving = self.request is not None
if frame.tracker_score is not None and not (
math.isfinite(frame.tracker_score) and frame.tracker_score >= cfg.tracker_score
):
self.history.clear()
return ZoomDecision("low_tracker_score", stop=moving)
if not fresh or (prev is not None and (frame.at <= prev.at or frame.seq <= prev.seq)):
self.history.clear()
return ZoomDecision("stale_or_geometry", stop=moving)
if not self.optics.fresh(now):
return ZoomDecision("fov_cooldown", stop=moving)
fov, min_fov = self.optics.estimate(now)
captured_fov, captured_min = self.optics.estimate(frame.at)
if captured_fov is None:
return ZoomDecision("fov_cooldown", stop=moving)
captured_base = next(measured for at, _, _, measured in reversed(self.optics.samples)
if at <= frame.at)
def projection(hfov):
horizontal = math.tan(math.radians(hfov / 2))
return horizontal, horizontal * frame.aspect
axes_at_capture = projection(captured_fov)
angles = tuple(math.degrees(math.atan(2 * offset * axis))
for offset, axis in zip(frame.offset, axes_at_capture))
if prev and 0 < frame.at - prev.at <= cfg.evidence_gap_s:
dt = frame.at - prev.at
rates = tuple((value - old) / dt for value, old in zip(angles, prev.angles))
frame = replace(frame, rates=rates)
frame = replace(frame, angles=angles)
self._record(frame, fresh)
if max(map(abs, frame.rates)) > cfg.max_residual_deg_s:
return ZoomDecision("edge_margin_low", stop=moving)
window = cfg.window(min_fov)
if moving:
remaining = max(0.0, self.optics.active[1] - now)
else:
remaining = window[1]
remaining += cfg.stop_ack_slack_s
horizon = cfg.steering_delay_s + (now - frame.at) + remaining
# Residual is signed in camera coordinates. Use its magnitude for the
# edge projection, so a sign flip or overshoot cannot shrink the bound.
narrowed = min(_log_focal(cfg.tele_fov),
_log_focal(min_fov) + cfg.upper_log_rate * remaining)
# Use the widest supported captured view as the reference so a
# narrower prediction never understates the pixel magnification.
growth = math.exp(max(0.0, narrowed - captured_base + cfg.status_error_log))
half_tan = projection(captured_min)
motion = [math.tan(math.radians(min(80, abs(rate) * horizon))) / (2 * axis)
for rate, axis in zip(frame.rates, half_tan)]
margin = min(0.5 - (abs(off) + size / 2 + move) * growth
for off, size, move in zip(frame.offset, frame.bbox_frac, motion))
if margin < cfg.edge_margin:
return ZoomDecision("edge_margin_low", stop=moving, margin=margin)
if moving and prev and 0 < frame.at - prev.at <= cfg.evidence_gap_s:
dt = frame.at - prev.at
for off, rate, old_rate, axis in zip(frame.angles, frame.rates, prev.rates, half_tan):
outward_accel = math.copysign(1, off) * (rate - old_rate) / (dt * 2 * math.degrees(axis))
outward_rate = math.copysign(1, off) * rate
if margin < cfg.edge_watch_margin and outward_rate > 0 and outward_accel > cfg.edge_accel:
return ZoomDecision("edge_acceleration", stop=True, margin=margin)
norm = math.hypot(*frame.angles)
outward = sum(a * r for a, r in zip(frame.angles, frame.rates)) / norm if norm else 0.0
wide_fraction = max(0.0, (fov - cfg.entry_fov) / (cfg.wide_fov - cfg.entry_fov))
center_limit = (cfg.repeat_center + min(1.0, wide_fraction)
* (cfg.entry_center - cfg.repeat_center)) if first else cfg.repeat_center
detector_fresh = (0 <= frame.at - frame.detector_at <= cfg.detector_age_s
and 0 <= now - frame.detector_at <= cfg.detector_age_s)
if moving:
if not detector_fresh:
return ZoomDecision("waiting_yolo", stop=True, margin=margin)
if min(frame.bbox_px) >= cfg.target_px:
return ZoomDecision("size_reached", stop=True, margin=margin)
if now >= self.optics.active[1]:
return ZoomDecision("fov_cooldown", stop=True, margin=margin)
# d(theta)/dt needed to cancel (df/f)*tan(theta) on each axis.
ff = tuple(max(-cfg.ff_max_deg_s, min(cfg.ff_max_deg_s,
math.degrees(cfg.ff_gain * cfg.log_rate * math.sin(math.radians(a))
* math.cos(math.radians(a))))) for a in frame.angles)
return ZoomDecision("pulse_active", ff_deg_s=ff, margin=margin)
if min(frame.bbox_px) >= cfg.target_px:
return ZoomDecision("size_reached", margin=margin)
if not transport_clear or cooldown or min_fov <= cfg.tele_fov + 0.05:
return ZoomDecision("fov_cooldown", margin=margin)
if not detector_fresh:
return ZoomDecision("waiting_yolo", margin=margin)
if frame.tracker_score is None or len(self.history) < cfg.tracker_frames:
return ZoomDecision("low_tracker_score", margin=margin)
if max(map(abs, frame.offset)) > center_limit or outward > cfg.max_outward_deg_s:
return ZoomDecision("not_centered", margin=margin)
return ZoomDecision("fast_start" if first else "burst", window=window, margin=margin)
class EpisodeTelemetryLogger:
"""Write JSONL off the control/STOP threads, with a bounded queue."""
def __init__(self, directory, capacity=4096):
self.directory = Path(directory)
self.queue = Queue(maxsize=capacity)
self.dropped = 0
Thread(target=self._write, name="ptz-zoom-telemetry", daemon=True).start()
def submit(self, camera, owner, event, at, **fields):
generation, started = owner
try:
self.queue.put_nowait((camera, generation, started, {
"cam": camera, "generation": generation, "track_start_utc": started,
"utc": at, "elapsed_ms": round((at - started) * 1000, 1),
"event": event, **fields,
}))
except Full:
self.dropped += 1
def _write(self):
import logging
log = logging.getLogger("PTZTracker")
while True:
camera, generation, started, row = self.queue.get()
try:
self.directory.mkdir(parents=True, exist_ok=True)
path = self.directory / f"ptz-{camera}-{generation}-{started:.6f}.jsonl"
with path.open("a", encoding="utf-8") as out:
out.write(json.dumps(row, allow_nan=False) + "\n")
except (OSError, ValueError):
log.exception("Unable to write PTZ zoom telemetry")
finally:
self.queue.task_done()
def _owner(st):
return int(st.get("ptz_generation", 0) or 0), float(st.get("track_started_at", 0.0) or 0.0)
def controller_for_track(st, camera_config):
"""Called only from TRACK, under the same lock used by status and the worker."""
if not st.get("tracking_active") or st.get("mode") != "TRACK" or _owner(st)[1] <= 0:
return None
current = st.get("predictive_zoom_controller")
if current is None or not current.owns(st):
try:
settings = ZoomConfig(**camera_config["predictive_zoom"])
except (TypeError, ValueError):
# A malformed per-camera calibration must leave legacy control available.
return None
current = ZoomController(settings, _owner(st))
st["predictive_zoom_controller"] = current
sample = st.get("predictive_zoom_hfov")
if sample is not None:
current.optics.observe(*sample, now=sample[0])
return current
def transport_accepted(config, camera, st, request, native, put_at, ack_at, hard_end):
controller = st.get("predictive_zoom_controller")
if controller is None or not controller.owns(st):
return
if controller.first_ack_at is None:
controller.first_ack_at = ack_at
_telemetry(config, camera, controller, "first_zoom", ack_at,
t_to_first_zoom_ms=round((ack_at - controller.owner[1]) * 1000, 1),
t_to_first_zoom_put_ms=round((put_at - controller.owner[1]) * 1000, 1),
request=request, native=native)
if (st.get("predictive_zoom_request") == request
and native == controller.cfg.native):
controller.accepted(request, put_at, ack_at, hard_end)
def _cooldowns(st):
return (float(st.get("zoom_step_cooldown_until", 0.0) or 0.0),
float(st.get("zoom_recenter_until", 0.0) or 0.0))
def _reusable_optics(controller, st, at):
before = st.get("zoom_step_start_hfov_deg")
actual = controller.optics.actual
return bool(before and actual and actual < before
and controller.optics.status_at == st.get("zoom_status_at")
and float(st.get("zoom_step_started_at", 0.0)) < controller.optics.status_at <= at)
def _normal_stop_cooldowns(config, st, at):
expected = (getattr(config, "ZOOM_STEP_COOLDOWN_SEC", 0.14),
getattr(config, "ZOOM_STEP_RECENTER_SEC", 0.14))
return all(observed <= at + delay for observed, delay in zip(_cooldowns(st), expected))
def transport_confirmed(config, st, request, at):
controller = st.get("predictive_zoom_controller")
if (controller is not None and controller.owns(st)
and _reusable_optics(controller, st, at) and _normal_stop_cooldowns(config, st, at)):
controller.optical_confirmation(request, at)
controller.stop_cooldowns = _cooldowns(st)
def transport_stopped(config, camera, st, request, at, *, reason, confirmed, clean):
controller = st.get("predictive_zoom_controller")
if controller is None or not controller.owns(st) or controller.request != request:
return
clean = clean and st.get("predictive_zoom_early_stop") != request
reusable = confirmed and _reusable_optics(controller, st, at)
controller.stopped(request, at, clean=clean, confirmed=reusable and _normal_stop_cooldowns(config, st, at))
controller.stop_cooldowns = _cooldowns(st)
_telemetry(config, camera, controller, "stop", at,
request=request, reason=reason, confirmed=confirmed, clean=clean)
_telemetry_loggers = {}
_telemetry_lock = Lock()
def _telemetry(config, camera, controller, event, at, **fields):
directory = getattr(config, "PREDICTIVE_ZOOM_TELEMETRY_DIR", None)
if directory is None:
return
with _telemetry_lock:
log = _telemetry_loggers.get(directory)
if log is None:
log = EpisodeTelemetryLogger(directory)
_telemetry_loggers[directory] = log
log.submit(camera, controller.owner, event, at, **fields, dropped_before=log.dropped)
def telemetry_tick(config, camera, controller, frame, decision, zoom, legacy_reason, at):
estimated, narrowest = controller.optics.estimate(at)
reason = decision.reason
if zoom <= 0.0 and decision.window is not None:
reason = legacy_reason
_telemetry(config, camera, controller, "tick", at, seq=frame.seq,
frame_utc=frame.at, zoom_cmd=zoom, reason=reason,
stop_requested=decision.stop, FOV_actual=controller.optics.actual,
FOV_status_utc=controller.optics.status_at or None,
FOV_estimated=estimated, FOV_min_estimated=narrowest,
bbox_px=frame.bbox_px, tracker_score=frame.tracker_score,
detector_conf=frame.detector_conf, detector_utc=frame.detector_at or None,
margin=decision.margin, ff_deg_s=decision.ff_deg_s)