import json import os import signal import sys import queue import threading from pathlib import Path import cv2 import numpy as np import time from collections import deque import torch from ultralytics import YOLO import cbam_register # noqa: F401 # Keep compatibility with checkpoints that reference __main__.CBAM. ChannelAttentionDyn = cbam_register.ChannelAttentionDyn SpatialAttention = cbam_register.SpatialAttention CBAM = cbam_register.CBAM from config import * from helpers import * from trackers import Kalman8D from trackers_hybrid import HybridTracker from trackers_safe import SafeKalman8D from yolo_worker import YOLOWorker from autogaze_runner import AutoGazeROIWorker from camera_motion import ( estimate_global_affine, estimate_global_affine_ex, apply_affine_to_point, affine_is_plausible, ) from motion_saliency import MotionSaliency from stationary_killer import StationaryKiller from decision_logger import TrackingDecisionLogger from guidance import ScreenGuidanceController from error_output import ErrorOutputSender from udp_dump_capture import LiveMikUdpCapture, UdpDumpCapture from delimited_frame_capture import DelimitedFrameCapture from configurable_udp_capture import ConfigurableUdpCapture from target_physics import analyze_motion_group, match_motion_evidence from ballistic_trajectory import predict_ballistic STOP_REQUESTED = False def request_stop(_signum=None, _frame=None): global STOP_REQUESTED STOP_REQUESTED = True from template_matching import tm_update_template, tm_search from track_score_policy import initial_candidate_score, track_passes_score_gate from target_handoff import ( compute_fast_handoff_hits, evaluate_stale_lock, pick_guidance_override_box, should_override_guidance, update_guidance_override_latch, ) # Allow importing ByteTrack implementation from parent TEST directory. PARENT_TEST = Path(__file__).resolve().parent.parent if str(PARENT_TEST) not in sys.path: sys.path.insert(0, str(PARENT_TEST)) try: from bytetrack_min_aggressive import BYTETracker except Exception: from bytetrack_min_aggressive import BYTETracker def build_unique_out_video_path(base_path: str) -> str: base = Path(base_path) suffix = base.suffix or ".mp4" stem = base.stem if base.suffix else base.name parent = base.parent if str(base.parent) not in ("", ".") else Path.cwd() parent.mkdir(parents=True, exist_ok=True) stamp = time.strftime("%Y%m%d_%H%M%S") candidate = parent / f"{stem}_{stamp}{suffix}" attempt = 1 while candidate.exists(): candidate = parent / f"{stem}_{stamp}_{attempt:02d}{suffix}" attempt += 1 return str(candidate) def export_ui_frame(frame_bgr, path: Path, quality: int) -> bool: path.parent.mkdir(parents=True, exist_ok=True) temp = path.with_name(path.name + ".tmp.jpg") if not cv2.imwrite(str(temp), frame_bgr, [int(cv2.IMWRITE_JPEG_QUALITY), int(quality)]): return False temp.replace(path) return True def draw_cached_detection_overlay(frame_bgr, overlay, guidance_ctrl) -> None: if not overlay: return if DRAW_RAW_YOLO_BOXES: for box, score in overlay.get("raw_boxes", ()): x1, y1, x2, y2 = map(int, clip_box(box, frame_bgr.shape[1], frame_bgr.shape[0])) cv2.rectangle(frame_bgr, (x1, y1), (x2, y2), (80, 170, 255), 1) cv2.putText( frame_bgr, f"YOLO {score:.2f}", (x1, min(frame_bgr.shape[0] - 4, y2 + 16)), cv2.FONT_HERSHEY_SIMPLEX, 0.5, (80, 170, 255), 1, ) if DRAW_ALL_BOXES: for box, score in overlay.get("accepted_boxes", ()): x1, y1, x2, y2 = map(int, clip_box(box, frame_bgr.shape[1], frame_bgr.shape[0])) cv2.rectangle(frame_bgr, (x1, y1), (x2, y2), (0, 255, 0), 1) cv2.putText( frame_bgr, f"{score:.2f}", (x1, max(0, y1 - 6)), cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 255, 0), 2, ) verified_box = overlay.get("verified_drone_box") if DRAW_LOCK_BOX and verified_box is not None: x1, y1, x2, y2 = map(int, clip_box(verified_box, frame_bgr.shape[1], frame_bgr.shape[0])) cv2.rectangle(frame_bgr, (x1, y1), (x2, y2), (0, 0, 255), 2) cv2.putText( frame_bgr, f"DRONE ID={overlay.get('target_id')}", (x1, max(0, y1 - 10)), cv2.FONT_HERSHEY_SIMPLEX, 0.7, (0, 0, 255), 2, ) cv2.putText( frame_bgr, overlay.get("status", "RECOVER"), (20, 40), cv2.FONT_HERSHEY_SIMPLEX, 1.0, (0, 255, 255), 2, ) guidance_state = overlay.get("guidance_state") if guidance_state is not None: guidance_ctrl.draw_overlay( frame_bgr, guidance_state, overlay.get("sx", 1.0), overlay.get("sy", 1.0), ) class LatestFrameExporter: def __init__(self, path, quality, max_fps): self.path = path self.quality = quality self.period = 1.0 / max(1.0, float(max_fps)) self.frames = queue.Queue(maxsize=1) self.stop_event = threading.Event() self.thread = threading.Thread(target=self._run, name="ui-frame-export", daemon=True) def start(self): self.thread.start() def submit(self, frame_bgr): try: self.frames.put_nowait(frame_bgr.copy()) except queue.Full: try: self.frames.get_nowait() except queue.Empty: pass self.frames.put_nowait(frame_bgr.copy()) def _run(self): next_export_at = time.perf_counter() while not self.stop_event.is_set() or not self.frames.empty(): try: frame = self.frames.get(timeout=0.1) except queue.Empty: continue wait_sec = next_export_at - time.perf_counter() if wait_sec > 0.0: self.stop_event.wait(wait_sec) try: while True: frame = self.frames.get_nowait() except queue.Empty: pass export_ui_frame(frame, self.path, self.quality) next_export_at = max(next_export_at + self.period, time.perf_counter()) def stop(self): self.stop_event.set() if self.thread.is_alive(): self.thread.join(timeout=5.0) class RealtimeFramePump: def __init__( self, cap, source_kind, input_fps, target_fps, overlay_getter, guidance_ctrl, publish_frame, ): self.cap = cap self.source_kind = source_kind self.input_fps = input_fps self.target_fps = target_fps self.overlay_getter = overlay_getter self.guidance_ctrl = guidance_ctrl self.publish_frame = publish_frame self.frames = queue.Queue(maxsize=1) self.stop_event = threading.Event() self.thread = threading.Thread(target=self._run, name="realtime-capture", daemon=True) self.finished = False self.error = None self.read_frames = 0 self.dropped_analysis_frames = 0 self.output_times = deque(maxlen=120) def start(self): self.thread.start() def get(self, timeout=0.2): try: return self.frames.get(timeout=timeout) except queue.Empty: return None def output_fps(self): if len(self.output_times) < 2: return 0.0 return (len(self.output_times) - 1) / max( self.output_times[-1] - self.output_times[0], 1e-6, ) def _run(self): started_at = None frame_id = 0 try: while not self.stop_event.is_set() and not STOP_REQUESTED: ret, frame_orig = self.cap.read() loop_ts = time.perf_counter() if not ret: break frame_ts, dt_source = get_frame_timestamp_seconds( self.cap, self.source_kind, frame_id, self.input_fps, loop_ts, ) item = (frame_orig, frame_id, frame_ts, loop_ts, dt_source) try: self.frames.put_nowait(item) except queue.Full: try: self.frames.get_nowait() self.dropped_analysis_frames += 1 except queue.Empty: pass self.frames.put_nowait(item) display_frame = frame_orig.copy() draw_cached_detection_overlay( display_frame, self.overlay_getter(), self.guidance_ctrl, ) self.publish_frame(display_frame, frame_ts, frame_id) self.read_frames += 1 self.output_times.append(time.perf_counter()) if self.target_fps > 0.0: if started_at is None: started_at = loop_ts - (frame_id / self.target_fps) deadline = started_at + ((frame_id + 1) / self.target_fps) lag = time.perf_counter() - deadline if lag > 0.5: started_at = time.perf_counter() - (frame_id / self.target_fps) deadline = started_at + ((frame_id + 1) / self.target_fps) wait_sec = max(0.0, deadline - time.perf_counter()) if wait_sec > 0.0: self.stop_event.wait(wait_sec) frame_id += 1 except Exception as exc: self.error = exc finally: self.finished = True def stop(self): self.stop_event.set() try: self.cap.release() except Exception as exc: print(f"WARN: capture release failed: {exc}") if self.thread.is_alive(): self.thread.join(timeout=2.0) if self.thread.is_alive(): print("WARN: realtime capture thread did not stop") # ─── Motion saliency re-weighting ──────────────────────────── # Параметры тюнинга (подбираются по логам): MS_BOOST_WEIGHT = 0.6 # сила буста для движущихся детектов MS_FLOOR = 0.12 # ниже этого ms_score — confidence штрафуется MS_FACTOR_MIN = 0.15 # clamp factor снизу (до -85% confidence для статики) MS_FACTOR_MAX = 1.80 # clamp factor сверху def reweight_dets_by_motion(dets, motion_sal): """ Корректирует confidence детектов в зависимости от motion saliency внутри bbox каждого детекта. Движущиеся цели получают буст, статичные (на горизонте, ЛЭП, деревьях) — штраф. Защищает от wrong target lock на статических объектах сцены. """ if motion_sal is None or motion_sal.saliency is None or not dets: return dets out = [] for d in dets: box = d[:4] conf = float(d[4]) ms_score = motion_sal.score_box(box) delta = ms_score - float(MS_FLOOR) factor = 1.0 + float(MS_BOOST_WEIGHT) * delta factor = max(float(MS_FACTOR_MIN), min(float(MS_FACTOR_MAX), factor)) new_conf = float(max(0.0, min(1.0, conf * factor))) new_d = d.copy() if isinstance(d, np.ndarray) else np.array(d, dtype=np.float32) new_d[4] = new_conf out.append(new_d) return out def pick_soft_yolo_handoff_det( dets, ref_box, ew, eh, *, min_score, dist_diag, dist_min, iou_floor, max_area_ratio, max_aspect_ratio, ): """Pick a YOLO detection that is spatially consistent with the current lock. This is intentionally independent from ByteTrack id. It lets Kalman/KLT accept a fresh detector measurement when ByteTrack keeps creating new ids for a tiny fast target. """ if ref_box is None or not dets: return None, 0.0 ref = clip_box(ref_box, ew, eh) rc = box_center(ref) rw, rh = box_wh(ref) rdiag = max(1.0, float(np.hypot(float(rw), float(rh)))) rarea = max(1.0, float(box_area(ref))) max_dist = max(float(dist_min), float(dist_diag) * rdiag) best_box = None best_conf = 0.0 best_score = -1e9 for det in dets: arr = np.asarray(det, dtype=np.float32).reshape(-1) if arr.size < 5: continue conf = float(arr[4]) if conf < float(min_score): continue b = clip_box(arr[:4], ew, eh) if box_area(b) <= 1.0: continue cc = box_center(b) dist = float(np.linalg.norm(cc - rc)) ov = float(iou(b, ref)) if dist > max_dist and ov < float(iou_floor): continue carea = max(1.0, float(box_area(b))) area_ratio = max(carea / rarea, rarea / carea) if float(max_area_ratio) > 0.0 and area_ratio > float(max_area_ratio): continue ar_ratio = safe_ratio(box_ar(b), box_ar(ref)) if float(max_aspect_ratio) > 0.0 and ar_ratio > float(max_aspect_ratio): continue # Prefer overlap/near-center, but keep confidence meaningful. score = (2.5 * ov) + (0.6 * conf) + (1.0 / (1.0 + dist)) - (0.015 * dist / rdiag) if score > best_score: best_score = score best_box = b best_conf = conf return best_box, best_conf def match_fresh_track_to_box(fresh_tracks, box, ew, eh, *, min_iou, max_center_dist): if box is None or not fresh_tracks: return None bc = box_center(box) best_track = None best_score = -1e9 for t in fresh_tracks: tb = clip_box(t.tlbr, ew, eh) ov = float(iou(tb, box)) dist = float(np.linalg.norm(box_center(tb) - bc)) if ov < float(min_iou) and dist > float(max_center_dist): continue score = (3.0 * ov) + (0.5 * float(t.score)) + (1.0 / (1.0 + dist)) if score > best_score: best_score = score best_track = t return best_track def candidate_box_is_similar(candidate_box, prev_box, ew, eh, *, dist_min, dist_diag): if candidate_box is None or prev_box is None: return False cand = clip_box(candidate_box, ew, eh) prev = clip_box(prev_box, ew, eh) pc = box_center(prev) cc = box_center(cand) pw, ph = box_wh(prev) cw, ch = box_wh(cand) diag = max(1.0, float(np.hypot(max(float(pw), float(cw)), max(float(ph), float(ch))))) limit = max(float(dist_min), float(dist_diag) * diag) return float(np.linalg.norm(cc - pc)) <= limit def make_box_from_center_wh(cx, cy, bw, bh, ew, eh): bw = max(2.0, float(bw)) bh = max(2.0, float(bh)) return clip_box([float(cx) - 0.5 * bw, float(cy) - 0.5 * bh, float(cx) + 0.5 * bw, float(cy) + 0.5 * bh], ew, eh) def make_union_roi_from_boxes(boxes, ew, eh, *, margin, min_side): valid = [] for b in boxes or []: if b is None: continue bb = clip_box(b, ew, eh) if box_area(bb) > 1.0: valid.append(bb) if not valid: return None arr = np.asarray(valid, dtype=np.float32) x1 = float(np.min(arr[:, 0])) - float(margin) y1 = float(np.min(arr[:, 1])) - float(margin) x2 = float(np.max(arr[:, 2])) + float(margin) y2 = float(np.max(arr[:, 3])) + float(margin) side = max(float(min_side), x2 - x1, y2 - y1) cx = 0.5 * (x1 + x2) cy = 0.5 * (y1 + y2) return clip_box([cx - 0.5 * side, cy - 0.5 * side, cx + 0.5 * side, cy + 0.5 * side], ew, eh) def _trajectory_velocity_from_history(obs_hist, fallback_vx, fallback_vy): if obs_hist is None or len(obs_hist) < 2: return float(fallback_vx), float(fallback_vy), 0.0, 0.0 recent = list(obs_hist)[-min(len(obs_hist), int(max(2, TRAJ_HISTORY_LOOKBACK))):] p0 = np.asarray(recent[0]["center"], dtype=np.float32) p1 = np.asarray(recent[-1]["center"], dtype=np.float32) t0 = float(recent[0]["ts"]) t1 = float(recent[-1]["ts"]) dt_hist = max(1e-3, t1 - t0) hv = (p1 - p0) / dt_hist vx = float(TRAJ_VEL_HIST_WEIGHT) * float(hv[0]) + (1.0 - float(TRAJ_VEL_HIST_WEIGHT)) * float(fallback_vx) vy = float(TRAJ_VEL_HIST_WEIGHT) * float(hv[1]) + (1.0 - float(TRAJ_VEL_HIST_WEIGHT)) * float(fallback_vy) ax = 0.0 ay = 0.0 if len(recent) >= 4: mid = len(recent) // 2 pa = np.asarray(recent[0]["center"], dtype=np.float32) pb = np.asarray(recent[mid]["center"], dtype=np.float32) pc = np.asarray(recent[-1]["center"], dtype=np.float32) ta = float(recent[0]["ts"]) tb = float(recent[mid]["ts"]) tc = float(recent[-1]["ts"]) v1 = (pb - pa) / max(1e-3, tb - ta) v2 = (pc - pb) / max(1e-3, tc - tb) acc = (v2 - v1) / max(1e-3, tc - ta) ax = float(np.clip(acc[0], -float(TRAJ_MAX_ACCEL_PX_S2), float(TRAJ_MAX_ACCEL_PX_S2))) ay = float(np.clip(acc[1], -float(TRAJ_MAX_ACCEL_PX_S2), float(TRAJ_MAX_ACCEL_PX_S2))) return vx, vy, ax, ay def build_maneuver_hypotheses( obs_hist, ref_box, kf, miss_streak, dt, ew, eh, now_ts=None ): """Build short-horizon trajectory hypotheses for detector loss.""" if ref_box is None or kf is None or (not getattr(kf, "initialized", False)): return [] ref = clip_box(ref_box, ew, eh) if box_area(ref) <= 1.0: return [] cx, cy = box_center(ref) bw, bh = box_wh(ref) vx_kf = float(kf.x[4, 0]) if getattr(kf, "x", None) is not None else 0.0 vy_kf = float(kf.x[5, 0]) if getattr(kf, "x", None) is not None else 0.0 vx, vy, ax, ay = _trajectory_velocity_from_history(obs_hist, vx_kf, vy_kf) speed = float(np.hypot(vx, vy)) horizon = float(dt) * float(max(1, int(miss_streak) + int(TRAJ_HORIZON_MISS_OFFSET))) horizon = float(np.clip(horizon, float(TRAJ_HORIZON_MIN_SEC), float(TRAJ_HORIZON_MAX_SEC))) grow = min(1.0 + float(TRAJ_BOX_GROW_PER_MISS) * float(max(0, int(miss_streak))), float(TRAJ_BOX_GROW_MAX)) bw2 = float(bw) * grow bh2 = float(bh) * grow hypotheses = [] def add(label, px, py, weight, width=bw2, height=bh2, uncertainty=0.0): b = make_box_from_center_wh(px, py, width, height, ew, eh) hypotheses.append({ "label": str(label), "box": b, "center": box_center(b), "weight": float(weight), "uncertainty": float(uncertainty), }) ballistic = None if BALLISTIC_PREDICT_ENABLE and now_ts is not None: ballistic = predict_ballistic( obs_hist, now_ts, ew, eh, lookback=TRAJ_HISTORY_LOOKBACK, min_observations=BALLISTIC_MIN_OBSERVATIONS, min_span_sec=BALLISTIC_MIN_SPAN_SEC, max_horizon_sec=TRAJ_HORIZON_MAX_SEC, max_speed=BALLISTIC_MAX_SPEED_PX_S, max_accel=TRAJ_MAX_ACCEL_PX_S2, max_size_rate=BALLISTIC_MAX_SIZE_RATE_S, max_uncertainty=BALLISTIC_MAX_UNCERTAINTY_PX, ) if ( ballistic is not None and float(ballistic["confidence"]) < float(BALLISTIC_MIN_CONFIDENCE) ): ballistic = None if ballistic is not None: ballistic_box = ballistic["box"] ballistic_center = ballistic["center"] ballistic_w, ballistic_h = box_wh(ballistic_box) vx, vy = map(float, ballistic["velocity"]) ax, ay = map(float, ballistic["acceleration"]) speed = float(np.hypot(vx, vy)) horizon = float(ballistic["horizon"]) cx, cy = map(float, ballistic_center) bw2, bh2 = float(ballistic_w), float(ballistic_h) add( "ballistic", cx, cy, 1.15 * float(ballistic["confidence"]), bw2, bh2, ballistic["uncertainty"], ) corridor = 0.55 * float(ballistic["uncertainty"]) if corridor >= float(TRAJ_DUP_CENTER_DIST): add("corrL", cx - corridor, cy, 0.58, bw2, bh2, corridor) add("corrR", cx + corridor, cy, 0.58, bw2, bh2, corridor) add("corrU", cx, cy - corridor, 0.52, bw2, bh2, corridor) add("corrD", cx, cy + corridor, 0.52, bw2, bh2, corridor) else: add("cv", cx + vx * horizon, cy + vy * horizon, 1.00) if TRAJ_USE_ACCEL: add("ca", cx + vx * horizon + 0.5 * ax * horizon * horizon, cy + vy * horizon + 0.5 * ay * horizon * horizon, 0.95) add("damp", cx + 0.55 * vx * horizon, cy + 0.55 * vy * horizon, 0.72) if speed >= float(TRAJ_MIN_SPEED_FOR_MANEUVER): ux = vx / max(speed, 1e-6) uy = vy / max(speed, 1e-6) px1, py1 = -uy, ux px2, py2 = uy, -ux lat = min(float(TRAJ_LATERAL_ACCEL_MAX), max(float(TRAJ_LATERAL_ACCEL_MIN), speed * float(TRAJ_LATERAL_ACCEL_SPEED_GAIN))) shift = 0.5 * lat * horizon * horizon add("turnL", cx + vx * horizon + px1 * shift, cy + vy * horizon + py1 * shift, 0.82) add("turnR", cx + vx * horizon + px2 * shift, cy + vy * horizon + py2 * shift, 0.82) vert = min(float(TRAJ_VERTICAL_ACCEL_MAX), max(float(TRAJ_VERTICAL_ACCEL_MIN), speed * float(TRAJ_VERTICAL_ACCEL_SPEED_GAIN))) vshift = 0.5 * vert * horizon * horizon add("up", cx + vx * horizon, cy + vy * horizon - vshift, 0.65) add("down", cx + vx * horizon, cy + vy * horizon + vshift, 0.65) filtered = [] for h in hypotheses: hc = np.asarray(h["center"], dtype=np.float32) if any(float(np.linalg.norm(hc - np.asarray(old["center"], dtype=np.float32))) < float(TRAJ_DUP_CENTER_DIST) for old in filtered): continue filtered.append(h) if len(filtered) >= int(TRAJ_MAX_HYPOTHESES): break return filtered def pick_det_near_trajectory_hypotheses(dets, hypotheses, ew, eh, *, min_score, dist_min, dist_diag): if not dets or not hypotheses: return None, 0.0, "-", -1e9 best_box = None best_conf = 0.0 best_label = "-" best_score = -1e9 for det in dets: arr = np.asarray(det, dtype=np.float32).reshape(-1) if arr.size < 5: continue conf = float(arr[4]) if conf < float(min_score): continue b = clip_box(arr[:4], ew, eh) if box_area(b) <= 1.0: continue bc = box_center(b) for h in hypotheses: hb = clip_box(h["box"], ew, eh) hc = np.asarray(h["center"], dtype=np.float32) hw, hh = box_wh(hb) hdiag = max(1.0, float(np.hypot(float(hw), float(hh)))) lim = max(float(dist_min), float(dist_diag) * hdiag) dist = float(np.linalg.norm(bc - hc)) ov = float(iou(b, hb)) if dist > lim and ov < 0.01: continue score = (float(h.get("weight", 1.0)) * 0.35) + conf + 2.0 * ov + (1.0 / (1.0 + dist)) - 0.01 * (dist / hdiag) if score > best_score: best_score = score best_box = b best_conf = conf best_label = str(h.get("label", "traj")) return best_box, best_conf, best_label, best_score def pick_yolo_reanchor_candidate( dets, *, ew, eh, pred_ref, prev_candidate_box, fresh_tracks, min_score, repeat_dist_min, repeat_dist_diag, pred_dist_min, pred_dist_diag, max_area_ratio, max_aspect_ratio, osd_reject, osd_min_score, trajectory_hypotheses=None, traj_dist_min=0.0, traj_dist_diag=0.0, ): """Pick a detector candidate for stale-KLT re-anchoring. This path is intentionally separate from the normal KLT-anchor guard. It is only allowed to adopt a far box after a short M/N persistence check, so a single false positive cannot pull Kalman away from a real lock. """ if not dets: return None, 0.0, None, False, False pred = clip_box(pred_ref, ew, eh) if pred_ref is not None else None pred_c = box_center(pred) if pred is not None else None pred_diag = 40.0 pred_area = 1.0 if pred is not None: pw, ph = box_wh(pred) pred_diag = max(1.0, float(np.hypot(float(pw), float(ph)))) pred_area = max(1.0, float(box_area(pred))) best_box = None best_conf = 0.0 best_track = None best_same = False best_near_pred = False best_score = -1e9 for det in dets: arr = np.asarray(det, dtype=np.float32).reshape(-1) if arr.size < 5: continue conf = float(arr[4]) if conf < float(min_score): continue b = clip_box(arr[:4], ew, eh) if box_area(b) <= 1.0: continue c = box_center(b) if osd_reject and in_osd_zone(float(c[0]), float(c[1]), ew, eh) and conf < float(osd_min_score): continue if pred is not None: carea = max(1.0, float(box_area(b))) area_ratio = max(carea / pred_area, pred_area / carea) if float(max_area_ratio) > 0.0 and area_ratio > float(max_area_ratio): continue ar_ratio = safe_ratio(box_ar(b), box_ar(pred)) if float(max_aspect_ratio) > 0.0 and ar_ratio > float(max_aspect_ratio): continue near_pred = False near_traj = False traj_bonus = 0.0 dist_pred = 0.0 if pred_c is not None: dist_pred = float(np.linalg.norm(c - pred_c)) pred_limit = max(float(pred_dist_min), float(pred_dist_diag) * pred_diag) near_pred = bool(dist_pred <= pred_limit) if trajectory_hypotheses: best_traj_score = -1e9 for h in trajectory_hypotheses: hb = clip_box(h["box"], ew, eh) hc = np.asarray(h["center"], dtype=np.float32) hw, hh = box_wh(hb) hdiag = max(1.0, float(np.hypot(float(hw), float(hh)))) lim = max(float(traj_dist_min), float(traj_dist_diag) * hdiag) dtraj = float(np.linalg.norm(c - hc)) ovtraj = float(iou(b, hb)) if dtraj <= lim or ovtraj >= 0.01: near_traj = True best_traj_score = max(best_traj_score, (2.0 * ovtraj) + (1.0 / (1.0 + dtraj)) + 0.25 * float(h.get("weight", 1.0))) if near_traj: traj_bonus = max(0.0, best_traj_score) same_prev = candidate_box_is_similar( b, prev_candidate_box, ew, eh, dist_min=float(repeat_dist_min), dist_diag=float(repeat_dist_diag), ) fresh_track = match_fresh_track_to_box( fresh_tracks, b, ew, eh, min_iou=float(SOFT_YOLO_TRACK_IOU), max_center_dist=max(float(SOFT_YOLO_TRACK_DIST_MIN), float(SOFT_YOLO_TRACK_DIST_DIAG) * pred_diag), ) # For the first far candidate we still keep memory, but we do not adopt # it until it repeats. Give repeated/fresh/near-pred boxes priority. score = 1.0 * conf if same_prev: score += 0.80 if fresh_track is not None: score += 0.35 + 0.25 * float(fresh_track.score) if near_pred: score += 0.25 + (1.0 / (1.0 + dist_pred)) if near_traj: score += 0.45 + traj_bonus if pred_c is not None and (not near_traj): score -= 0.0025 * dist_pred if score > best_score: best_score = score best_box = b best_conf = conf best_track = fresh_track best_same = bool(same_prev) best_near_pred = bool(near_pred or near_traj) return best_box, best_conf, best_track, best_same, best_near_pred def klt_anchor_accepts_box( candidate_box, anchor_box, ew, eh, *, dist_diag, dist_min, iou_floor, max_area_ratio, max_aspect_ratio, ): """Reject one-frame ByteTrack/YOLO jumps when KLT still has a strong anchor.""" if candidate_box is None or anchor_box is None: return True, "no_anchor" cand = clip_box(candidate_box, ew, eh) anch = clip_box(anchor_box, ew, eh) if box_area(cand) <= 1.0 or box_area(anch) <= 1.0: return False, "empty_box" ac = box_center(anch) cc = box_center(cand) aw, ah = box_wh(anch) adiag = max(1.0, float(np.hypot(float(aw), float(ah)))) max_dist = max(float(dist_min), float(dist_diag) * adiag) dist = float(np.linalg.norm(cc - ac)) ov = float(iou(cand, anch)) if dist > max_dist and ov < float(iou_floor): return False, f"far_from_klt:dist={dist:.1f}>lim={max_dist:.1f},iou={ov:.3f}" aarea = max(1.0, float(box_area(anch))) carea = max(1.0, float(box_area(cand))) area_ratio = max(carea / aarea, aarea / carea) if float(max_area_ratio) > 0.0 and area_ratio > float(max_area_ratio): return False, f"area_jump:{area_ratio:.1f}" ar_ratio = safe_ratio(box_ar(cand), box_ar(anch)) if float(max_aspect_ratio) > 0.0 and ar_ratio > float(max_aspect_ratio): return False, f"aspect_jump:{ar_ratio:.1f}" return True, "ok" def main(): global STOP_REQUESTED STOP_REQUESTED = False signal.signal(signal.SIGTERM, request_stop) signal.signal(signal.SIGINT, request_stop) if hasattr(signal, "SIGBREAK"): signal.signal(signal.SIGBREAK, request_stop) show_output = bool(SHOW_OUTPUT) if show_output and os.name != "nt" and not ( os.environ.get("DISPLAY") or os.environ.get("WAYLAND_DISPLAY") ): show_output = False print("Headless runtime detected: OpenCV window disabled") print("Loading YOLO...") model = YOLO(MODEL_PATH) if torch.cuda.is_available() and int(DEVICE) >= 0: model.to(f"cuda:{DEVICE}") source_mode = os.environ.get("FPV_SOURCE_MODE", "").strip().lower() custom_udp_dump = False udp_input_host = os.environ.get("FPV_UDP_INPUT_HOST", "0.0.0.0").strip() or "0.0.0.0" udp_input_port = int(os.environ.get("FPV_UDP_INPUT_PORT", "59004")) separator_byte = int(os.environ.get("FPV_FRAME_SEPARATOR_BYTE", "0"), 0) & 0xFF frame_encoding = os.environ.get("FPV_FRAME_ENCODING", "auto").strip().lower() or "auto" try: packet_schema = json.loads(os.environ.get("FPV_UDP_PACKET_SCHEMA", "{}")) except json.JSONDecodeError as exc: print(f"Invalid FPV_UDP_PACKET_SCHEMA, defaults used: {exc}") packet_schema = {} if source_mode == "udp_dump": dump_cap = UdpDumpCapture(SOURCE, fps=CAMERA_FPS or INPUT_FPS_FALLBACK) if dump_cap.isOpened(): cap, source_kind = dump_cap, "file" custom_udp_dump = True else: cap, source_kind = open_source(SOURCE, CAP_BACKEND) elif source_mode == "udp_mik_live": cap = LiveMikUdpCapture( host=udp_input_host, port=udp_input_port, fps=CAMERA_FPS or INPUT_FPS_FALLBACK, width=CAMERA_WIDTH, height=CAMERA_HEIGHT, ) source_kind = "stream" elif source_mode == "udp_custom_live": cap = ConfigurableUdpCapture( host=udp_input_host, port=udp_input_port, fps=CAMERA_FPS or INPUT_FPS_FALLBACK, width=CAMERA_WIDTH, height=CAMERA_HEIGHT, encoding=frame_encoding, separator=separator_byte, schema=packet_schema, ) source_kind = "stream" elif source_mode in {"udp_delimited_live", "udp_delimited_file"}: cap = DelimitedFrameCapture( source=SOURCE if source_mode == "udp_delimited_file" else None, host=udp_input_host, port=udp_input_port, separator=separator_byte, encoding=frame_encoding, width=CAMERA_WIDTH, height=CAMERA_HEIGHT, fps=CAMERA_FPS or INPUT_FPS_FALLBACK, ) source_kind = "file" if source_mode == "udp_delimited_file" else "stream" else: cap, source_kind = open_source(SOURCE, CAP_BACKEND) if not cap.isOpened(): print(f"Capture open failed: {SOURCE}") return if source_kind == "camera": fourcc = str(CAMERA_FOURCC or "").strip() if len(fourcc) >= 4: cap.set(cv2.CAP_PROP_FOURCC, cv2.VideoWriter_fourcc(*fourcc[:4])) if int(CAMERA_WIDTH) > 0: cap.set(cv2.CAP_PROP_FRAME_WIDTH, int(CAMERA_WIDTH)) if int(CAMERA_HEIGHT) > 0: cap.set(cv2.CAP_PROP_FRAME_HEIGHT, int(CAMERA_HEIGHT)) if int(CAMERA_FPS) > 0: cap.set(cv2.CAP_PROP_FPS, int(CAMERA_FPS)) try: cap.set(cv2.CAP_PROP_BUFFERSIZE, 1) except Exception: pass source_labels = { "udp_mik_live": f"udp_mik_live://{udp_input_host}:{udp_input_port}", "udp_delimited_live": ( f"udp_delimited_live://{udp_input_host}:{udp_input_port}" f"?separator={separator_byte}&encoding={frame_encoding}" ), "udp_custom_live": ( f"udp_custom_live://{udp_input_host}:{udp_input_port}" f"?assembly={packet_schema.get('assembly', 'fragmented')}" f"&encoding={frame_encoding}" ), "udp_delimited_file": ( f"udp_delimited_file?separator={separator_byte}&encoding={frame_encoding}" ), } source_label = ( "udp_dump_mik" if custom_udp_dump else source_labels.get(source_mode, source_kind) ) print(f"Opened source: {SOURCE} ({source_label})") input_fps = float(cap.get(cv2.CAP_PROP_FPS)) if (not np.isfinite(input_fps)) or (input_fps <= 1.0): input_fps = 0.0 if TARGET_OUT_FPS > 0: target_out_fps = float(TARGET_OUT_FPS) elif source_kind == "file" and input_fps > 0.0: target_out_fps = input_fps else: target_out_fps = 0.0 if VIDEO_REALTIME and target_out_fps > 0.0: print(f"Pacing output at ~{target_out_fps:.2f} FPS") else: print("Pacing disabled (show as fast as processing allows)") if show_output: cv2.namedWindow(WINDOW_NAME, cv2.WINDOW_NORMAL) writer = None out_video_path = None active_video_marker = None archive_mode = str(ARCHIVE_RECORD_MODE).strip().lower() if archive_mode not in {"full", "fragments"}: archive_mode = "full" archive_gap = max(0.0, float(DETECTION_CLIP_MAX_GAP_SEC)) last_archive_hit_ts = -1e9 archive_written_frames = 0 if SAVE_INFER_VIDEO: ow = int(cap.get(cv2.CAP_PROP_FRAME_WIDTH)) oh = int(cap.get(cv2.CAP_PROP_FRAME_HEIGHT)) fpsw = float(cap.get(cv2.CAP_PROP_FPS)) if (not np.isfinite(fpsw)) or fpsw <= 1.0: fpsw = target_out_fps if target_out_fps > 0 else 30.0 fourcc = cv2.VideoWriter_fourcc(*"mp4v") out_video_path = build_unique_out_video_path(OUT_VIDEO_PATH) active_video_marker = Path(out_video_path).parent / ".active_video" active_video_marker.unlink(missing_ok=True) writer = cv2.VideoWriter(out_video_path, fourcc, fpsw, (ow, oh)) if not writer.isOpened(): print(f"WARN: video writer open failed: {out_video_path}") writer = None else: active_video_marker.write_text(Path(out_video_path).name, encoding="utf-8") print(f"Saving inference video to: {out_video_path}") print(f"Archive mode: {archive_mode}, detection gap: {archive_gap:.1f}s") ui_frame_path = Path(UI_FRAME_EXPORT_PATH) ui_frame_every = max(1, int(UI_FRAME_EXPORT_EVERY)) ui_jpeg_quality = int(np.clip(UI_FRAME_EXPORT_JPEG_QUALITY, 1, 100)) ui_exporter = None if UI_FRAME_EXPORT_ENABLE: ui_exporter = LatestFrameExporter( ui_frame_path, ui_jpeg_quality, UI_FRAME_EXPORT_MAX_FPS, ) ui_exporter.start() def publish_frame(frame_bgr, timestamp_sec, current_frame_id): nonlocal archive_written_frames if writer is not None and ( archive_mode == "full" or (float(timestamp_sec) - last_archive_hit_ts) <= archive_gap ): writer.write(frame_bgr) archive_written_frames += 1 if ui_exporter is not None and current_frame_id % ui_frame_every == 0: ui_exporter.submit(frame_bgr) full_shape = ( (int(EFFECTIVE_H), int(EFFECTIVE_W)) if FORCE_EFFECTIVE_PAL else (int(cap.get(cv2.CAP_PROP_FRAME_HEIGHT)), int(cap.get(cv2.CAP_PROP_FRAME_WIDTH))) ) yolo_worker = YOLOWorker(model, full_frame_shape=full_shape) yolo_worker.start() autogaze_worker = AutoGazeROIWorker() autogaze_worker.start() print(autogaze_worker.status_line()) track_logger = TrackingDecisionLogger( TRACK_LOG_ENABLE, TRACK_LOG_PATH, TRACK_SUMMARY_PATH, flush_every=TRACK_LOG_FLUSH_EVERY, ) print(track_logger.status_line()) guidance_ctrl = ScreenGuidanceController() print(guidance_ctrl.status_line()) error_output = ErrorOutputSender() error_output.start() print(error_output.status_line()) kf = SafeKalman8D() klt = HybridTracker() bt = BYTETracker( track_high_thresh=BT_HIGH, track_low_thresh=BT_LOW, new_track_thresh=BT_NEW, match_thresh=BT_MATCH_IOU, track_buffer=BT_BUFFER, min_hits=BT_MIN_HITS, ) target_id = None target_absent_frames = 0 switch_candidate_id = None switch_candidate_hits = 0 locked_box_eff = None confirmed = False hit_streak = 0 miss_streak = 0 acquire_score = 0 acquire_miss = 0 ref_hist = None nominal_dt = 1.0 / float(input_fps if input_fps > 1.0 else INPUT_FPS_FALLBACK) prev_frame_ts = None frame_id = 0 last_det_frame = -999 last_fullscan_frame = -999 last_klt_init_frame = -999 yolo_no_det = 0 last_yolo_ok_ts = -1e9 perf_hist = deque(maxlen=120) yolo_hist = deque(maxlen=120) analysis_frames = 0 last_perf_log_ts = -1e9 latest_detection_overlay = None verified_drone_latched = False realtime_analysis_every = max(1, int(REALTIME_ANALYSIS_EVERY)) stream_fps = target_out_fps or input_fps or float(CAMERA_FPS) or float(INPUT_FPS_FALLBACK) analysis_period = realtime_analysis_every / max(1.0, stream_fps) next_analysis_at = 0.0 traj = deque(maxlen=max(10, int(TRAIL_SECONDS * (input_fps if input_fps > 0 else 60.0)))) traj_frame_i = 0 guidance_override_memory_box_eff = None guidance_override_memory_ttl = 0 prev_gray_global = None physics_bad_lock_streak = 0 template_gray = None # --- Motion saliency --- motion_sal = MotionSaliency() print(motion_sal.status_line()) ms_outliers = np.zeros((0, 2), dtype=np.float32) # --- Stationary clutter killer --- killer = StationaryKiller() print(killer.status_line()) motion_roi_eff = None motion_active_zones = 0 motion_mask = None flash_roi_eff = None flash_ttl = 0 wavelet_roi_eff = None wavelet_roi_ttl = 0 wavelet_roi_peak = 0.0 wavelet_track_hist = {} wavelet_track_last_seen = {} track_center_hist = {} track_center_last_seen = {} preacq_hist = deque(maxlen=max(1, int(PREACQ_WINDOW))) preacq_hits = 0 autogaze_cooldown = 0 wavelet_cooldown = 0 adaptive_chase_stage = "far" # M/N memory for safe YOLO re-anchor when KLT/Kalman are stale. yolo_reanchor_memory_box_eff = None yolo_reanchor_memory_hits = 0 yolo_reanchor_memory_last_frame = -999 yolo_reanchor_last_reason = "" # Short history of accepted target centers for multi-hypothesis prediction. trajectory_obs_hist = deque(maxlen=max(6, int(TRAJ_HISTORY_LEN))) realtime_pump = None if VIDEO_REALTIME: realtime_pump = RealtimeFramePump( cap=cap, source_kind=source_kind, input_fps=input_fps, target_fps=target_out_fps, overlay_getter=lambda: latest_detection_overlay, guidance_ctrl=guidance_ctrl, publish_frame=publish_frame, ) realtime_pump.start() print("Realtime capture active: latest-frame analysis queue") print("Start") while True: if STOP_REQUESTED: print("Stop requested; finalizing outputs...") break if realtime_pump is not None: wait_for_analysis = next_analysis_at - time.perf_counter() if wait_for_analysis > 0.0: time.sleep(wait_for_analysis) item = realtime_pump.get() if item is None: if realtime_pump.finished: if realtime_pump.error is not None: print(f"Realtime capture failed: {realtime_pump.error}") break continue frame_orig, frame_id, frame_ts, loop_ts, dt_source = item else: ret, frame_orig = cap.read() loop_ts = time.perf_counter() if not ret: break frame_ts, dt_source = get_frame_timestamp_seconds( cap, source_kind, frame_id, input_fps, loop_ts, ) iter_start = time.perf_counter() frame_eff, sx, sy = get_effective_frame(frame_orig) eh, ew = frame_eff.shape[:2] dt = sanitize_dt(frame_ts, prev_frame_ts, nominal_dt) prev_frame_ts = frame_ts if autogaze_cooldown > 0: autogaze_cooldown -= 1 if wavelet_cooldown > 0: wavelet_cooldown -= 1 if autogaze_cooldown <= 0: autogaze_worker.submit(frame_eff, frame_id) autogaze_roi_eff = None autogaze_rois_eff = [] autogaze_info = None gray_now = cv2.cvtColor(frame_eff, cv2.COLOR_BGR2GRAY) frame_frozen = bool( prev_gray_global is not None and float(cv2.mean(cv2.absdiff(prev_gray_global, gray_now))[0]) <= float(TRAJ_FREEZE_MEAN_ABS_MAX) ) target_track = None pred_box_eff = None best_score = None have_yolo = False yolo_mode = "IDLE" yolo_raw_count = 0 merged_part_count = 0 infer_ms = 0.0 klt_valid = False speed = 0.0 det_every = 0 approach_active = False close_force_fullscan = False fast_maneuver_guard = False wavelet_active = False best_wavelet_energy = 0.0 best_wavelet_bonus = 0.0 best_wavelet_hits = 0 best_dist = 0.0 best_residual_ok = False guidance_candidate_box_eff = None guidance_candidate_track_id = None guidance_raw_yolo_box_eff = None guidance_override_box_eff = None used_prediction_hold = False stale_lock_active = False stale_lock_reason = "" fast_handoff_active = False guidance_reset_event = "" guidance_force_neutral = False soft_yolo_box_eff = None soft_yolo_score = 0.0 soft_yolo_track = None soft_yolo_adopted = False yolo_reanchor_box_eff = None yolo_reanchor_score = 0.0 yolo_reanchor_track = None yolo_reanchor_adopted = False yolo_reanchor_near_pred = False yolo_reanchor_reason = "" trajectory_hypotheses = [] trajectory_roi_eff = None trajectory_primary_box_eff = None trajectory_uncertainty = 0.0 trajectory_det_label = "-" trajectory_reanchor_used = False physics_prev_gray = prev_gray_global physics_entries = [] A = None ms_outliers = np.zeros((0, 2), dtype=np.float32) if prev_gray_global is not None and USE_CAM_MOTION_COMP: A, _ms_inliers, ms_outliers = estimate_global_affine_ex( prev_gray_global, gray_now ) if A is not None and (not affine_is_plausible(A)): A = None ms_outliers = np.zeros((0, 2), dtype=np.float32) if A is not None and kf.initialized: cx0 = float(kf.x[0, 0]) cy0 = float(kf.x[1, 0]) ncx, ncy = apply_affine_to_point(A, cx0, cy0) kf.x[0, 0] = ncx kf.x[1, 0] = ncy if locked_box_eff is not None: x1, y1, x2, y2 = locked_box_eff p1x, p1y = apply_affine_to_point(A, float(x1), float(y1)) p2x, p2y = apply_affine_to_point(A, float(x2), float(y2)) locked_box_eff[:] = np.array([p1x, p1y, p2x, p2y], dtype=np.float32) locked_box_eff[:] = clip_box(locked_box_eff, ew, eh) motion_roi_eff = None motion_active_zones = 0 motion_mask = None if flash_ttl > 0: flash_ttl -= 1 else: flash_roi_eff = None if wavelet_roi_ttl > 0: wavelet_roi_ttl -= 1 else: wavelet_roi_eff = None wavelet_roi_peak = 0.0 if MOTION_ZONE_ENABLE and prev_gray_global is not None: motion_mask = build_motion_mask( prev_gray_global, gray_now, affine=A if USE_CAM_MOTION_COMP else None ) motion_roi_eff, motion_active_zones = motion_zones_to_roi(motion_mask, ew, eh) # --- Update motion saliency map --- if prev_gray_global is not None: motion_sal.update( gray_now, prev_gray_global, A, kind="affine" if A is not None else "none", outlier_pts=ms_outliers, ) prev_gray_global = gray_now pred_box_eff = None if kf.initialized: kf_q_scale = 1.0 if KALMAN_Q_DT_BOOST_ENABLE and dt > float(KALMAN_Q_DT_BOOST_START_SEC): kf_q_scale += float(KALMAN_Q_DT_BOOST_SLOPE) * (dt - float(KALMAN_Q_DT_BOOST_START_SEC)) kf_q_scale = min(kf_q_scale, float(KALMAN_Q_DT_BOOST_MAX)) kf.predict(dt, q_scale=kf_q_scale) vmax = float(KALMAN_CLAMP_V_PX_S) kf.x[4, 0] = float(np.clip(kf.x[4, 0], -vmax, vmax)) kf.x[5, 0] = float(np.clip(kf.x[5, 0], -vmax, vmax)) pred_box_eff = clip_box(kf.to_box(), ew, eh) unc = 0.0 if TURN_SAFE_ENABLE and pred_box_eff is not None: unc = kf.uncertainty() speed = 0.0 if kf.initialized: vx = float(kf.x[4, 0]) vy = float(kf.x[5, 0]) speed = (vx * vx + vy * vy) ** 0.5 unc = kf.uncertainty() pred_area_eff = 0.0 if pred_box_eff is not None: pred_area_eff = box_area(pred_box_eff) elif locked_box_eff is not None: pred_area_eff = box_area(locked_box_eff) if ADAPTIVE_CHASE_ENABLE: hyst = float(max(0.0, ADAPTIVE_STAGE_AREA_HYST)) mid_on = float(ADAPTIVE_STAGE_MID_AREA) close_on = float(APPROACH_CLOSE_AREA) if adaptive_chase_stage == "close": if pred_area_eff < (close_on - hyst): adaptive_chase_stage = "mid" if pred_area_eff >= (mid_on - hyst) else "far" elif adaptive_chase_stage == "mid": if pred_area_eff >= (close_on + hyst): adaptive_chase_stage = "close" elif pred_area_eff < (mid_on - hyst): adaptive_chase_stage = "far" else: if pred_area_eff >= (close_on + hyst): adaptive_chase_stage = "close" elif pred_area_eff >= (mid_on + hyst): adaptive_chase_stage = "mid" else: adaptive_chase_stage = "close" if (APPROACH_MODE_ENABLE and (pred_area_eff >= float(APPROACH_FORCE_DET_AREA))) else "far" approach_active = APPROACH_MODE_ENABLE and (adaptive_chase_stage in ("mid", "close")) approach_force_det_every = None approach_kf_roi_scale = 1.0 approach_max_area_ratio = float(TURN_MAX_AREA_RATIO) approach_near_dist_diag = float(APPROACH_NEAR_DIST_DIAG) approach_hsv_min_scale = 1.0 approach_score_weight = 0.0 approach_score_clip = 0.0 approach_switch_extra_miss = 0 approach_switch_extra_hits = 0 if approach_active: if adaptive_chase_stage == "mid": approach_force_det_every = int(max(1, APPROACH_MID_FORCE_DET_EVERY)) approach_kf_roi_scale = float(APPROACH_MID_KF_ROI_SCALE) approach_max_area_ratio = float(APPROACH_MID_MAX_AREA_RATIO) approach_near_dist_diag = float(APPROACH_MID_NEAR_DIST_DIAG) approach_hsv_min_scale = float(APPROACH_MID_HSV_MIN_SCALE) approach_score_weight = float(APPROACH_MID_SCORE_WEIGHT) approach_score_clip = float(APPROACH_MID_SCORE_CLIP) approach_switch_extra_miss = int(max(0, APPROACH_MID_SWITCH_EXTRA_MISS)) approach_switch_extra_hits = int(max(0, APPROACH_MID_SWITCH_EXTRA_HITS)) else: approach_force_det_every = int(max(1, APPROACH_FORCE_DET_EVERY)) approach_kf_roi_scale = float(APPROACH_KF_ROI_SCALE) approach_max_area_ratio = float(APPROACH_MAX_AREA_RATIO) approach_near_dist_diag = float(APPROACH_NEAR_DIST_DIAG) approach_hsv_min_scale = float(APPROACH_HSV_MIN_SCALE) approach_score_weight = float(APPROACH_SCORE_WEIGHT) approach_score_clip = float(APPROACH_SCORE_CLIP) approach_switch_extra_miss = int(max(0, APPROACH_CLOSE_SWITCH_EXTRA_MISS)) approach_switch_extra_hits = int(max(0, APPROACH_CLOSE_SWITCH_EXTRA_HITS)) klt_valid = False if confirmed and locked_box_eff is not None: if klt.prev_gray is None or klt.pts is None or len(klt.pts) < KLT_REINIT_MIN_POINTS: klt.init(frame_eff, locked_box_eff) last_klt_init_frame = frame_id klt_box = klt.update(frame_eff) if klt_box is not None: klt_box = clip_box(klt_box, ew, eh) klt_valid = (klt.quality >= KLT_OK_Q) and (klt.good_count >= KLT_OK_PTS) if klt_valid: cx, cy = box_center(klt_box) bw, bh = box_wh(klt_box) kf.update([cx, cy, bw, bh]) locked_box_eff = klt_box if ( TRAJ_PREDICT_ENABLE and confirmed and kf.initialized and (miss_streak >= int(TRAJ_PREDICT_MISS_GE)) ): traj_ref_box = pred_box_eff if pred_box_eff is not None else locked_box_eff trajectory_hypotheses = build_maneuver_hypotheses( trajectory_obs_hist, traj_ref_box, kf, miss_streak, dt, ew, eh, now_ts=frame_ts, ) if trajectory_hypotheses: trajectory_primary_box_eff = trajectory_hypotheses[0]["box"] trajectory_uncertainty = max( float(h.get("uncertainty", 0.0)) for h in trajectory_hypotheses ) trajectory_roi_eff = make_union_roi_from_boxes( [h["box"] for h in trajectory_hypotheses], ew, eh, margin=max(float(TRAJ_ROI_MARGIN), trajectory_uncertainty), min_side=TRAJ_ROI_MIN_SIDE, ) wavelet_roi_allowed = ( (wavelet_cooldown <= 0) and WAVELET_ROI_ENABLE and ( ((not confirmed) and (miss_streak >= int(WAVELET_ROI_MISS_GE))) or (confirmed and (miss_streak >= int(WAVELET_ROI_CONF_MISS_GE))) ) ) if wavelet_roi_allowed: wv_every = int(max(1, WAVELET_ROI_EVERY_N)) compute_wavelet_now = (wavelet_roi_ttl <= 0) or ((frame_id % wv_every) == 0) if compute_wavelet_now: wv_t0 = time.perf_counter() wave_pred_ref = pred_box_eff if pred_box_eff is not None else locked_box_eff wroi, wpeak = wavelet_hot_roi( gray_now, ew, eh, motion_mask=motion_mask, pred_ref=wave_pred_ref, margin=WAVELET_ROI_MARGIN, min_side=WAVELET_ROI_MIN_SIDE, pred_scale=WAVELET_ROI_PRED_SCALE, min_peak_ratio=WAVELET_ROI_MIN_PEAK_RATIO, ) wv_ms = (time.perf_counter() - wv_t0) * 1000.0 if wv_ms > float(MODULE_TIME_BUDGET_MS): wavelet_cooldown = max(int(wavelet_cooldown), int(WAVELET_COOLDOWN_FRAMES)) if wroi is not None: wavelet_roi_eff = wroi wavelet_roi_peak = float(wpeak) wavelet_roi_ttl = int(max(int(wavelet_roi_ttl), int(max(1, WAVELET_ROI_TTL)))) base = 6 if confirmed else RECOVER_FORCED_DET_EVERY fast_bonus = int(clamp(speed / 28.0, 0, 4)) unc_bonus = int(clamp(unc / 160.0, 0, 4)) miss_bonus = int(clamp(miss_streak, 0, 3)) klt_bonus = 2 if (confirmed and not klt_valid) else 0 det_every = clamp(base - (fast_bonus + unc_bonus + miss_bonus + klt_bonus), 1, 10) if confirmed and approach_active and (approach_force_det_every is not None): det_every = min(int(det_every), int(approach_force_det_every)) yolo_fresh = (frame_ts - last_yolo_ok_ts) <= YOLO_FRESH_SEC if YOLO_FORCE_DET_WHEN_WEAK and confirmed and (not klt_valid) and (not yolo_fresh): det_every = 1 run_det = (frame_id - last_det_frame) >= det_every need_fullscan = (not confirmed) and (frame_id - last_fullscan_frame) >= RECOVER_FULLSCAN_EVERY if UNVERIFIED_FORCE_FULLSCAN and not verified_drone_latched: need_fullscan = True # Close-stage detector policy: # older builds forced FULL-CLOSE on every detector pass while ch=close. # That is safe but noisy: ByteTrack sees every false positive on the screen, # even when KLT/Kalman are locked on the real target. Now close mode uses # ROI-KF/ROI-TRAJ most of the time and falls back to FULL-CLOSE only when # the lock is weak or on a periodic health-check. close_periodic_due = bool( CLOSE_PERIODIC_FULLSCAN_ENABLE and confirmed and (adaptive_chase_stage == "close") and ((frame_id - last_fullscan_frame) >= int(CLOSE_PERIODIC_FULLSCAN_EVERY)) ) close_force_fullscan = bool( confirmed and CLOSE_FORCE_FULLSCAN and (adaptive_chase_stage == "close") and ( (miss_streak >= int(CLOSE_FULLSCAN_MISS_GE)) or (CLOSE_FULLSCAN_WHEN_KLT_INVALID and (not klt_valid)) or (target_absent_frames >= int(CLOSE_FULLSCAN_TARGET_ABSENT_GE)) or close_periodic_due ) ) if TURN_SAFE_ENABLE and TURN_SAFE_FORCE_FULLSCAN and confirmed and (miss_streak >= FULLSCAN_WHEN_MISS_GE): need_fullscan = True if ( confirmed and (miss_streak >= int(MOTION_CONF_MISS_GE)) and ((frame_id - last_fullscan_frame) >= int(CONF_MISS_FORCE_FULLSCAN_EVERY)) ): need_fullscan = True if (not confirmed) and (miss_streak >= int(RECOVER_FORCE_FULLSCAN_MISS_GE)): need_fullscan = True if close_force_fullscan: need_fullscan = True autogaze_ref = pred_box_eff if pred_box_eff is not None else locked_box_eff autogaze_roi_eff, autogaze_rois_eff, autogaze_info = autogaze_worker.get_latest( frame_id, ew, eh, pred_ref_box=autogaze_ref, ) autogaze_roi_allowed = ( (autogaze_cooldown <= 0) and autogaze_roi_eff is not None and ( ((not confirmed) and (miss_streak >= int(AUTOGAZE_MISS_GE))) or (confirmed and (miss_streak >= int(AUTOGAZE_CONF_MISS_GE))) ) ) if ( autogaze_info is not None and int(autogaze_info.get("age", -1)) == 0 and float(autogaze_info.get("infer_ms", 0.0)) > float(MODULE_TIME_BUDGET_MS) ): autogaze_cooldown = max(int(autogaze_cooldown), int(AUTOGAZE_COOLDOWN_FRAMES)) autogaze_roi_allowed = False if run_det: roi_box = None mode = "FULL" force_motion_first = (not confirmed) and (miss_streak >= int(RECOVER_FORCE_MOTION_FIRST_MISS_GE)) allow_kf_roi = confirmed or (miss_streak < int(RECOVER_ROI_KF_DISABLE_MISS_GE)) prefer_motion_confirmed = ( MOTION_CONF_ENABLE and confirmed and (miss_streak >= int(MOTION_CONF_MISS_GE)) and (motion_roi_eff is not None) and ((not MOTION_CONF_KLT_INVALID_ONLY) or (not klt_valid)) ) if ( (not need_fullscan) and prefer_motion_confirmed ): roi_box = motion_roi_eff.copy() mode = "ROI-MOTION-C" elif ( (not need_fullscan) and FLASH_ROI_ENABLE and (flash_ttl > 0) and (flash_roi_eff is not None) and ((not confirmed) or (target_id is None) or (not klt_valid) or (miss_streak >= 1)) ): roi_box = clip_box(flash_roi_eff, ew, eh) mode = "ROI-FLASH" elif ( (not need_fullscan) and autogaze_roi_allowed ): roi_box = clip_box(autogaze_roi_eff, ew, eh) mode = "ROI-GAZE" elif ( (not need_fullscan) and wavelet_roi_allowed and WAVELET_ROI_ENABLE and (wavelet_roi_ttl > 0) and (wavelet_roi_eff is not None) ): roi_box = clip_box(wavelet_roi_eff, ew, eh) mode = "ROI-WAVE" elif (not need_fullscan) and force_motion_first and MOTION_ZONE_ENABLE and (motion_roi_eff is not None): roi_box = motion_roi_eff.copy() mode = "ROI-MOTION" elif ( TRAJ_ROI_ENABLE and (not need_fullscan) and confirmed and (miss_streak >= int(TRAJ_ROI_MISS_GE)) and (trajectory_roi_eff is not None) ): roi_box = clip_box(trajectory_roi_eff, ew, eh) mode = "ROI-TRAJ" elif (not need_fullscan) and kf.initialized and allow_kf_roi: cx = float(kf.x[0, 0]) cy = float(kf.x[1, 0]) vx = float(kf.x[4, 0]) vy = float(kf.x[5, 0]) vnorm = (vx * vx + vy * vy) ** 0.5 + 1e-6 ux, uy = vx / vnorm, vy / vnorm # Адаптивный ROI: для мелких целей сохраняем # запас на ошибку Kalman + контекст для YOLO. # Для bbox 15×10 ROI ≈ 280px (после resize 640 # объект занимает ~15% — оптимум для YOLO recall). # Для bbox 30×20 ROI ≈ 550px (почти не сужается). box_diag = 0.0 if locked_box_eff is not None: bw = float(locked_box_eff[2] - locked_box_eff[0]) bh = float(locked_box_eff[3] - locked_box_eff[1]) box_diag = (bw * bw + bh * bh) ** 0.5 if box_diag >= 8.0: target_roi_side = box_diag / 0.065 target_roi_side = max(260.0, min(520.0, target_roi_side)) else: target_roi_side = float(BASE_RADIUS) radius_speed = BASE_RADIUS + SPEED_RADIUS_FACTOR * vnorm radius_speed = clamp(radius_speed, BASE_RADIUS, MAX_RADIUS) # Берём максимум — даём запас на смещение Kalman radius = max(radius_speed, target_roi_side * 0.5) radius = min(radius, MAX_RADIUS) radius = max(radius, BASE_RADIUS) fx = ux * radius * (FORWARD_BIAS - 1.0) fy = uy * radius * (FORWARD_BIAS - 1.0) scx = cx + fx * 0.35 scy = cy + fy * 0.35 rx = radius * FORWARD_BIAS ry = radius * SIDE_BIAS if approach_active: rx *= float(approach_kf_roi_scale) ry *= float(approach_kf_roi_scale) roi_box = clip_box([scx - rx, scy - ry, scx + rx, scy + ry], ew, eh) mode = "ROI-KF" elif (not need_fullscan) and (not confirmed) and MOTION_ZONE_ENABLE and (motion_roi_eff is not None): roi_box = motion_roi_eff.copy() mode = "ROI-MOTION" if need_fullscan: roi_box = None mode = "FULL-CLOSE" if close_force_fullscan else "FULL" last_fullscan_frame = frame_id yolo_worker.submit(frame_eff, roi_box, mode, frame_ts) last_det_frame = frame_id yolo = yolo_worker.try_get() dets_eff = [] infer_ms = 0.0 used_roi = True have_yolo = False yolo_mode = "NONE" yolo_raw_count = 0 yolo_ts = None raw_yolo_dets_eff = [] merged_part_count = 0 if yolo is not None: have_yolo = True if len(yolo) >= 6: dets_eff, yolo_ts, yolo_mode, infer_ms, used_roi, raw_yolo_dets_eff = yolo else: dets_eff, yolo_ts, yolo_mode, infer_ms, used_roi = yolo # Motion-aware re-weighting: буст движущимся детектам, # штраф статичным (горизонт, ЛЭП, деревья). Защита # от wrong target lock на статических объектах. dets_eff = reweight_dets_by_motion(dets_eff, motion_sal) yolo_raw_count = len(dets_eff) dets_eff = filter_drone_candidates( dets_eff, motion_mask, ew, eh, reference_box=locked_box_eff if confirmed else None, ) if PHYSICS_GATE_ENABLE and physics_prev_gray is not None: physical_dets = [] for det in dets_eff: det_box = clip_box(det[:4], ew, eh) evidence = analyze_motion_group( physics_prev_gray, gray_now, det_box, affine=A, dt=dt, ) physics_entries.append((det_box.copy(), evidence)) near_lock = bool( confirmed and locked_box_eff is not None and ( iou(det_box, locked_box_eff) >= float(HARD_TARGET_LATCH_IOU_FLOOR) or np.linalg.norm(box_center(det_box) - box_center(locked_box_eff)) <= max( float(HARD_TARGET_LATCH_DIST_MIN), float(HARD_TARGET_LATCH_DIST_DIAG) * max(1.0, float(np.linalg.norm(box_wh(locked_box_eff)))), ) ) ) raw_score = float(det[4]) if ( evidence.reliable and not evidence.valid and not near_lock ): continue physical_det = np.asarray(det, dtype=np.float32).copy() if evidence.valid: physical_det[4] = min( 0.99, raw_score + float(PHYSICS_VALID_SCORE_BOOST) * evidence.score, ) physical_dets.append(physical_det) dets_eff = physical_dets if RECOVER_FILTER_ENABLE and (not confirmed) and (yolo_mode in ("ROI-MOTION", "ROI-WAVE", "ROI-MOTION-C", "ROI-GAZE")) and yolo_raw_count > 0: raw_dets = dets_eff filtered = [d for d in raw_dets if recover_det_is_valid(d, motion_mask)] if len(filtered) > 0: dets_eff = filtered else: raw_sorted = sorted(raw_dets, key=lambda dd: float(dd[4]), reverse=True) dets_eff = raw_sorted[:min(3, len(raw_sorted))] merge_ref = pred_box_eff if pred_box_eff is not None else locked_box_eff if confirmed and (merge_ref is not None) and len(dets_eff) >= int(max(2, PART_MERGE_MIN_PARTS)): dets_eff, merged_part_count = merge_close_part_dets(dets_eff, merge_ref, ew, eh) if FLASH_ROI_ENABLE and len(dets_eff) > 0: best_d = max(dets_eff, key=lambda dd: float(dd[4])) if float(best_d[4]) >= float(FLASH_ROI_MIN_SCORE): flash_roi_eff = make_focus_roi_from_box( best_d[:4], ew, eh, margin=FLASH_ROI_MARGIN, min_side=FLASH_ROI_MIN_SIDE, ) flash_ttl = int(max(1, FLASH_ROI_TTL)) yolo_hist.append(infer_ms) if len(dets_eff) == 0: yolo_no_det += 1 else: yolo_no_det = 0 if yolo_ts is not None: last_yolo_ok_ts = float(yolo_ts) else: last_yolo_ok_ts = frame_ts preacq_det = None if PREACQ_ENABLE and (not confirmed): pre_ref = pred_box_eff if pred_box_eff is not None else locked_box_eff if have_yolo: preacq_det = pick_preacq_det(dets_eff, pre_ref, ew, eh) preacq_hist.append(1 if preacq_det is not None else 0) else: preacq_hist.append(0) preacq_hits = int(sum(preacq_hist)) if preacq_det is not None: acquire_score = int(clamp(acquire_score + int(PREACQ_ACQ_BONUS), 0, 999)) acquire_miss = 0 if PREACQ_INIT_KF_ON_MN and preacq_hits >= int(PREACQ_MIN_HITS): pb = clip_box(preacq_det[:4], ew, eh) if not kf.initialized: kf.init_from_box(pb) else: cxp, cyp = box_center(pb) bwp, bhp = box_wh(pb) kf.update([cxp, cyp, bwp, bhp]) locked_box_eff = pb hit_streak = max(int(hit_streak), 1) flash_roi_eff = make_focus_roi_from_box( pb, ew, eh, margin=FLASH_ROI_MARGIN, min_side=FLASH_ROI_MIN_SIDE ) flash_ttl = int(max(int(flash_ttl), int(max(1, FLASH_ROI_TTL)))) else: preacq_hist.clear() preacq_hits = 0 if DEBUG and dets_eff and (frame_id % 30 == 0): print("sample det:", dets_eff[0]) if have_yolo and dets_eff: guidance_raw_yolo_box_eff = pick_best_det(dets_eff, ew, eh) if have_yolo: dets_np = np.array(dets_eff, dtype=np.float32) if dets_eff else np.zeros((0, 5), dtype=np.float32) else: dets_np = None tracks = bt.update(dets_np, dt=dt) tracks_for_select = [t for t in tracks if t.time_since_update <= BT_MAX_PREDICT_AGE] fresh_tracks = [t for t in tracks_for_select if t.time_since_update == 0] if SWITCH_STATIC_GUARD_ENABLE: hist_window = max(2, int(SWITCH_TRACK_HIST_WINDOW)) stale_thr = max(12, 3 * hist_window) if track_center_last_seen: stale_ids = [ tid for tid, last_f in track_center_last_seen.items() if (frame_id - int(last_f)) > int(stale_thr) ] for tid in stale_ids: track_center_last_seen.pop(tid, None) track_center_hist.pop(tid, None) for t in fresh_tracks: tid = int(t.track_id) b_hist = clip_box(t.tlbr, ew, eh) cc = box_center(b_hist) hist = track_center_hist.get(tid) if hist is None or hist.maxlen != hist_window: hist = deque(maxlen=hist_window) track_center_hist[tid] = hist hist.append(np.array([float(cc[0]), float(cc[1])], dtype=np.float32)) track_center_last_seen[tid] = int(frame_id) chosen = None target_track = None pred_ref = pred_box_eff if pred_box_eff is not None else (locked_box_eff if locked_box_eff is not None else None) pred_diag = float(np.linalg.norm(box_wh(pred_ref))) if pred_ref is not None else 40.0 if SOFT_YOLO_HANDOFF_ENABLE and confirmed and have_yolo and dets_eff: soft_ref = locked_box_eff if (klt_valid and locked_box_eff is not None) else pred_ref if soft_ref is not None and ( klt_valid or miss_streak >= int(SOFT_YOLO_MIN_MISS) or target_absent_frames >= int(SOFT_YOLO_MIN_ABSENT) ): soft_has_strong_klt_anchor = bool( klt_valid and locked_box_eff is not None and float(klt.quality) >= float(SOFT_YOLO_KLT_MIN_Q) ) soft_yolo_box_eff, soft_yolo_score = pick_soft_yolo_handoff_det( dets_eff, soft_ref, ew, eh, min_score=float(SOFT_YOLO_MIN_SCORE), dist_diag=float(SOFT_YOLO_KLT_DIST_DIAG if soft_has_strong_klt_anchor else SOFT_YOLO_LOST_DIST_DIAG), dist_min=float(SOFT_YOLO_KLT_DIST_MIN if soft_has_strong_klt_anchor else SOFT_YOLO_LOST_DIST_MIN), iou_floor=float(SOFT_YOLO_KLT_IOU_FLOOR if soft_has_strong_klt_anchor else SOFT_YOLO_IOU_FLOOR), max_area_ratio=float(SOFT_YOLO_KLT_MAX_AREA_RATIO if soft_has_strong_klt_anchor else SOFT_YOLO_MAX_AREA_RATIO), max_aspect_ratio=float(SOFT_YOLO_MAX_ASPECT_RATIO), ) if soft_yolo_box_eff is not None: match_dist = max(float(SOFT_YOLO_TRACK_DIST_MIN), float(SOFT_YOLO_TRACK_DIST_DIAG) * pred_diag) soft_yolo_track = match_fresh_track_to_box( fresh_tracks, soft_yolo_box_eff, ew, eh, min_iou=float(SOFT_YOLO_TRACK_IOU), max_center_dist=match_dist, ) if (soft_yolo_track is not None) and ((target_id is None) or (soft_yolo_track.track_id != target_id)): guidance_candidate_box_eff = soft_yolo_box_eff guidance_candidate_track_id = int(soft_yolo_track.track_id) elif target_id is not None: guidance_candidate_box_eff = soft_yolo_box_eff if target_id is not None: for t in tracks_for_select: if t.track_id == target_id: target_track = t break if target_track is None: target_absent_frames += 1 else: if have_yolo and target_track.time_since_update > 0 and len(fresh_tracks) > 0: target_track = None target_absent_frames = TARGET_SWITCH_MISS_FRAMES else: target_absent_frames = 0 else: target_absent_frames = 0 current_target_track = target_track current_target_has_fresh_update = bool( current_target_track is not None and current_target_track.time_since_update == 0 ) current_target_motion_ok = False if ( current_target_track is not None and EGO_RESIDUAL_GATE_ENABLE and motion_mask is not None ): current_target_motion_ok = track_residual_motion_ok( current_target_track, motion_mask, ew, eh, ) # If KLT/Kalman are holding an old point while YOLO repeatedly sees a # candidate elsewhere, allow a controlled re-anchor. This fixes stale # KLT locks without reintroducing one-frame false-positive jumps. reanchor_context = bool( YOLO_REANCHOR_ENABLE and confirmed and have_yolo and dets_eff and ( miss_streak >= int(YOLO_REANCHOR_MISS_GE) or (not current_target_has_fresh_update) or target_absent_frames >= int(YOLO_REANCHOR_ABSENT_GE) ) ) if reanchor_context: reanchor_ref = pred_box_eff if pred_box_eff is not None else locked_box_eff ( yolo_reanchor_box_eff, yolo_reanchor_score, yolo_reanchor_track, yolo_reanchor_same, yolo_reanchor_near_pred, ) = pick_yolo_reanchor_candidate( dets_eff, ew=ew, eh=eh, pred_ref=reanchor_ref, prev_candidate_box=yolo_reanchor_memory_box_eff, fresh_tracks=fresh_tracks, min_score=float(YOLO_REANCHOR_MIN_SCORE), repeat_dist_min=float(YOLO_REANCHOR_REPEAT_DIST_MIN), repeat_dist_diag=float(YOLO_REANCHOR_REPEAT_DIST_DIAG), pred_dist_min=float(YOLO_REANCHOR_PRED_DIST_MIN), pred_dist_diag=float(YOLO_REANCHOR_PRED_DIST_DIAG), max_area_ratio=float(YOLO_REANCHOR_MAX_AREA_RATIO), max_aspect_ratio=float(YOLO_REANCHOR_MAX_ASPECT_RATIO), osd_reject=bool(YOLO_REANCHOR_OSD_REJECT), osd_min_score=float(YOLO_REANCHOR_OSD_MIN_SCORE), trajectory_hypotheses=trajectory_hypotheses, traj_dist_min=float(TRAJ_REANCHOR_DIST_MIN), traj_dist_diag=float(TRAJ_REANCHOR_DIST_DIAG), ) if ( TRAJ_REANCHOR_ENABLE and (yolo_reanchor_box_eff is None) and trajectory_hypotheses ): (traj_box, traj_score, trajectory_det_label, _traj_pick_score) = pick_det_near_trajectory_hypotheses( dets_eff, trajectory_hypotheses, ew, eh, min_score=float(TRAJ_REANCHOR_MIN_SCORE), dist_min=float(TRAJ_REANCHOR_DIST_MIN), dist_diag=float(TRAJ_REANCHOR_DIST_DIAG), ) if traj_box is not None: yolo_reanchor_box_eff = traj_box yolo_reanchor_score = float(traj_score) yolo_reanchor_near_pred = True yolo_reanchor_track = match_fresh_track_to_box( fresh_tracks, yolo_reanchor_box_eff, ew, eh, min_iou=float(SOFT_YOLO_TRACK_IOU), max_center_dist=max(float(SOFT_YOLO_TRACK_DIST_MIN), float(SOFT_YOLO_TRACK_DIST_DIAG) * 40.0), ) if yolo_reanchor_box_eff is not None: recent_same = bool( yolo_reanchor_same and ((frame_id - int(yolo_reanchor_memory_last_frame)) <= int(YOLO_REANCHOR_WINDOW)) ) if recent_same: yolo_reanchor_memory_hits += 1 else: yolo_reanchor_memory_hits = 1 yolo_reanchor_memory_box_eff = yolo_reanchor_box_eff.copy() yolo_reanchor_memory_last_frame = int(frame_id) req_reanchor_hits = int(YOLO_REANCHOR_HITS) if yolo_reanchor_track is not None: req_reanchor_hits = min(req_reanchor_hits, int(YOLO_REANCHOR_TRACK_HITS)) if ( yolo_reanchor_near_pred and float(yolo_reanchor_score) >= float(YOLO_REANCHOR_NEAR_PRED_ONE_SHOT_SCORE) ): req_reanchor_hits = 1 if yolo_reanchor_memory_hits >= req_reanchor_hits: yolo_reanchor_reason = ( f"yolo_reanchor:hits={yolo_reanchor_memory_hits}/{req_reanchor_hits}," f"score={float(yolo_reanchor_score):.2f}," f"nearPred={int(yolo_reanchor_near_pred)}," f"traj={trajectory_det_label}," f"trk={int(yolo_reanchor_track is not None)}" ) guidance_candidate_box_eff = yolo_reanchor_box_eff if yolo_reanchor_track is not None: guidance_candidate_track_id = int(yolo_reanchor_track.track_id) elif (frame_id - int(yolo_reanchor_memory_last_frame)) > int(YOLO_REANCHOR_WINDOW): yolo_reanchor_memory_box_eff = None yolo_reanchor_memory_hits = 0 elif (frame_id - int(yolo_reanchor_memory_last_frame)) > int(YOLO_REANCHOR_WINDOW): yolo_reanchor_memory_box_eff = None yolo_reanchor_memory_hits = 0 motion_switch_mode = confirmed and (yolo_mode in ("ROI-MOTION-C", "ROI-WAVE")) fast_maneuver_guard = ( SWITCH_FAST_MANEUVER_ENABLE and confirmed and (speed >= float(SWITCH_FAST_MANEUVER_SPEED)) ) switch_miss_need = int(TARGET_SWITCH_MISS_FRAMES) if motion_switch_mode: switch_miss_need = max(switch_miss_need, int(MOTION_CONF_SWITCH_MISS_FRAMES)) if approach_active: switch_miss_need += int(approach_switch_extra_miss) if fast_maneuver_guard: switch_miss_need += int(max(0, SWITCH_FAST_MANEUVER_EXTRA_MISS)) stale_switch_ready = bool( STALE_LOCK_BREAK_ENABLE and confirmed and (target_track is None) and (len(fresh_tracks) > 0) and ( (miss_streak >= int(STALE_LOCK_BREAK_MIN_MISS)) or (not klt_valid) or (float(klt.quality) < float(STALE_LOCK_BREAK_KLT_QUALITY)) ) ) can_switch = target_track is None and ( (target_id is None) or (not confirmed) or (target_absent_frames >= switch_miss_need) or stale_switch_ready ) candidate_tracks = fresh_tracks if (have_yolo and len(fresh_tracks) > 0) else tracks_for_select eligible_tracks = candidate_tracks if (not confirmed) and RECOVER_FILTER_ENABLE and (yolo_mode in ("ROI-MOTION", "ROI-WAVE", "ROI-MOTION-C", "ROI-GAZE")): eligible_tracks = [t for t in candidate_tracks if t.hits >= int(RECOVER_TARGET_MIN_HITS)] if len(eligible_tracks) == 0: eligible_tracks = candidate_tracks if REJECT_OSD_ZONES: eligible_tracks = [ t for t in eligible_tracks if ( confirmed and target_id is not None and int(t.track_id) == int(target_id) ) or not box_is_osd_candidate(t.tlbr, ew, eh) ] track_physics = { int(t.track_id): match_motion_evidence( clip_box(t.tlbr, ew, eh), physics_entries, ) for t in eligible_tracks } if PHYSICS_GATE_ENABLE: eligible_tracks = [ t for t in eligible_tracks if not ( (track_physics.get(int(t.track_id)) is not None) and track_physics[int(t.track_id)].reliable and not track_physics[int(t.track_id)].valid and ((not confirmed) or (target_id is None) or (int(t.track_id) != int(target_id))) ) ] weak_reacq_guard = ( WEAK_REACQ_GUARD_ENABLE and confirmed and (not klt_valid) and (miss_streak >= int(WEAK_REACQ_MISS_GE)) ) if weak_reacq_guard: eligible_tracks = [ t for t in eligible_tracks if (t.hits >= int(WEAK_REACQ_MIN_HITS)) and (float(t.score) >= float(WEAK_REACQ_MIN_SCORE)) ] if EGO_RESIDUAL_GATE_ENABLE and miss_streak >= int(EGO_RESIDUAL_MISS_GE) and motion_mask is not None: motion_tracks = [t for t in eligible_tracks if track_residual_motion_ok(t, motion_mask, ew, eh)] if len(motion_tracks) > 0: eligible_tracks = motion_tracks wavelet_active = ( (wavelet_cooldown <= 0) and WAVELET_ASSIST_ENABLE and (miss_streak >= int(WAVELET_ACTIVE_MISS_GE)) and ((not WAVELET_ONLY_UNCONFIRMED) or (not confirmed)) ) if wavelet_active and WAVELET_ONLY_RECOVER_PHASE: wavelet_active = ((not confirmed) or (miss_streak > 0)) best_wavelet_energy = 0.0 best_wavelet_bonus = 0.0 best_wavelet_hits = 0 if WAVELET_MN_ENABLE and wavelet_track_last_seen: stale = [ tid for tid, last_f in wavelet_track_last_seen.items() if (frame_id - int(last_f)) > int(max(12, 3 * int(WAVELET_MN_WINDOW))) ] for tid in stale: wavelet_track_last_seen.pop(tid, None) wavelet_track_hist.pop(tid, None) if can_switch and len(eligible_tracks) > 0: best = None best_score = -1e9 best_hist = None for t in eligible_tracks: b = clip_box(t.tlbr, ew, eh) c = box_center(b) physics_evidence = track_physics.get(int(t.track_id)) physics_bonus = ( float(PHYSICS_SELECTION_BONUS) * physics_evidence.score if physics_evidence is not None and physics_evidence.valid else 0.0 ) approach_bonus = 0.0 is_switch_candidate = confirmed and (target_id is not None) and (t.track_id != target_id) if not track_passes_score_gate( track_score=float(t.score), confirmed=confirmed, is_switch_candidate=is_switch_candidate, weak_reacq_guard=weak_reacq_guard, acquire_floor=float(TRACK_SCORE_MIN_ACQUIRE), reacquire_floor=float(TRACK_SCORE_MIN_REACQUIRE), switch_floor=float(TRACK_SCORE_MIN_SWITCH), ): continue candidate_motion_ok = bool( EGO_RESIDUAL_GATE_ENABLE and motion_mask is not None and track_residual_motion_ok(t, motion_mask, ew, eh) ) residual_ok = bool(is_switch_candidate and candidate_motion_ok) dist = 0.0 i = 0.0 if pred_ref is not None: pc = box_center(pred_ref) dist = float(np.linalg.norm(c - pc)) max_dist = max(40.0, TARGET_REACQ_DIST_FACTOR * pred_diag) if confirmed and (target_id is None): noid_max = max(float(CONFIRMED_NO_ID_NEAR_MIN), float(CONFIRMED_NO_ID_NEAR_FACTOR) * pred_diag) max_dist = min(max_dist, noid_max) if not confirmed: near_max = max(float(RECOVER_NEAR_DIST_MIN), float(RECOVER_NEAR_DIST_FACTOR) * pred_diag) max_dist = min(max_dist, near_max) if weak_reacq_guard: weak_max = max(float(WEAK_REACQ_MAX_DIST_MIN), float(WEAK_REACQ_MAX_DIST_FACTOR) * pred_diag) max_dist = min(max_dist, weak_max) if dist > max_dist: continue i = iou(b, pred_ref) pred_area = box_area(pred_ref) cand_area = box_area(b) ar_ratio = safe_ratio(box_ar(b), box_ar(pred_ref)) grow_ratio = cand_area / max(pred_area, 1e-3) shrink_ratio = pred_area / max(cand_area, 1e-3) allow_grow_ratio = float(TURN_MAX_AREA_RATIO) if approach_active and cand_area >= pred_area: close_growth = ( dist <= (float(approach_near_dist_diag) * pred_diag) or (cand_area >= float(APPROACH_CLOSE_AREA)) ) if close_growth: allow_grow_ratio = max(allow_grow_ratio, float(approach_max_area_ratio)) if grow_ratio > 1.0: approach_bonus = float( clamp( float(approach_score_weight) * np.log2(grow_ratio), 0.0, float(approach_score_clip), ) ) if ar_ratio > TURN_MAX_ASPECT_RATIO: continue if cand_area >= pred_area: if grow_ratio > allow_grow_ratio: continue else: if shrink_ratio > TURN_MAX_AREA_RATIO: continue if target_id is not None and t.track_id != target_id: if i < TARGET_SWITCH_IOU_FLOOR and dist > (1.2 * pred_diag): continue if ( is_switch_candidate and SWITCH_TRAJ_GATE_ENABLE and (miss_streak >= int(SWITCH_TRAJ_MISS_GE)) and (pred_ref is not None) and kf.initialized ): vx = float(kf.x[4, 0]) vy = float(kf.x[5, 0]) vnorm = float(np.hypot(vx, vy)) if vnorm >= float(SWITCH_TRAJ_MIN_SPEED): dvec = c - box_center(pred_ref) dnorm = float(np.linalg.norm(dvec)) if dnorm > 1e-3: ux = vx / vnorm uy = vy / vnorm cos_v = float((dvec[0] * ux + dvec[1] * uy) / dnorm) perp = float(abs(dvec[0] * uy - dvec[1] * ux)) perp_lim = max(float(SWITCH_TRAJ_PERP_MIN), float(SWITCH_TRAJ_PERP_DIAG) * pred_diag) traj_ok = (cos_v >= float(SWITCH_TRAJ_COS_MIN)) and (perp <= perp_lim) if (not traj_ok) and (not (SWITCH_TRAJ_REQUIRE_RESIDUAL_BYPASS and residual_ok)): continue if is_switch_candidate and SWITCH_STATIC_GUARD_ENABLE: hist = track_center_hist.get(int(t.track_id)) min_hist = max(3, int(max(2, int(SWITCH_TRACK_HIST_WINDOW)) // 2)) if hist is not None and len(hist) >= min_hist: disp = float(np.linalg.norm(hist[-1] - hist[0])) disp_lim = max(float(SWITCH_STATIC_MIN_DISP), float(SWITCH_STATIC_MIN_DIAG) * pred_diag) if (disp < disp_lim) and (not (SWITCH_STATIC_REQUIRE_RESIDUAL_BYPASS and residual_ok)): continue if motion_switch_mode and is_switch_candidate: if int(t.hits) < int(MOTION_CONF_SWITCH_MIN_HITS): continue if float(t.score) < float(MOTION_CONF_SWITCH_MIN_SCORE): continue if pred_ref is not None: if (i < float(MOTION_CONF_SWITCH_IOU_FLOOR)) and (dist > (float(MOTION_CONF_SWITCH_DIST_DIAG) * pred_diag)): continue if ( MOTION_CONF_SWITCH_REQUIRE_RESIDUAL and EGO_RESIDUAL_GATE_ENABLE and motion_mask is not None and (not residual_ok) ): continue if ( confirmed and (target_id is not None) and (t.track_id != target_id) and OSD_SWITCH_BLOCK_ENABLE and REJECT_OSD_ZONES ): in_osd = in_osd_zone(float(c[0]), float(c[1]), ew, eh) if in_osd: if pred_ref is None: continue if dist > (float(OSD_SWITCH_BLOCK_DIST_DIAG) * pred_diag): continue app = 0.0 cand_hist = None if USE_HSV_GATE and ref_hist is not None: cand_hist = compute_hsv_hist(frame_eff, b) app = hsv_sim(ref_hist, cand_hist) min_app = float(HSV_GATE_MIN_SIM) if approach_active and pred_ref is not None: pred_area = box_area(pred_ref) cand_area = box_area(b) grow_ratio = cand_area / max(pred_area, 1e-3) if (cand_area >= float(APPROACH_CLOSE_AREA)) or (grow_ratio >= float(APPROACH_HSV_RELAX_GROW_RATIO)): min_app *= float(approach_hsv_min_scale) if app < min_app: continue wv_energy = 0.0 wv_bonus = 0.0 wv_hits = 0 if wavelet_active: wv_energy = wavelet_energy_haar( gray_now, b, margin=WAVELET_BOX_MARGIN, min_side=WAVELET_MIN_SIDE, ) motion_ok = ( EGO_RESIDUAL_GATE_ENABLE and motion_mask is not None and track_residual_motion_ok(t, motion_mask, ew, eh) ) tiny_box = box_area(b) <= float(WAVELET_GATE_TINY_AREA_MAX) if tiny_box and (miss_streak >= int(WAVELET_GATE_MISS_GE)): if (wv_energy < float(WAVELET_GATE_MIN_ENERGY)) and (not motion_ok): continue if WAVELET_MN_ENABLE: tid = int(t.track_id) hist = wavelet_track_hist.get(tid) if hist is None: hist = deque(maxlen=max(1, int(WAVELET_MN_WINDOW))) wavelet_track_hist[tid] = hist wv_pass = (wv_energy >= float(WAVELET_GATE_MIN_ENERGY)) or motion_ok hist.append(1 if wv_pass else 0) wavelet_track_last_seen[tid] = int(frame_id) wv_hits = int(sum(hist)) if tiny_box and (miss_streak >= int(WAVELET_MN_MISS_GE)) and (wv_hits < int(WAVELET_MN_MIN_HITS)) and (not motion_ok): continue wv_norm = (wv_energy - float(WAVELET_ENERGY_BASE)) / max(1e-6, float(WAVELET_ENERGY_BASE)) wv_bonus = float(clamp(float(WAVELET_SCORE_WEIGHT) * wv_norm, -float(WAVELET_SCORE_CLIP), float(WAVELET_SCORE_CLIP))) if WAVELET_MN_ENABLE and int(WAVELET_MN_WINDOW) > 0: mn_ratio = float(wv_hits) / float(max(1, int(WAVELET_MN_WINDOW))) wv_bonus += 0.08 * mn_ratio s = (2.2 * i) + (1.0 / (1.0 + dist)) + (0.25 * float(t.score)) + (0.9 * app) + wv_bonus + approach_bonus + physics_bonus if pred_ref is None: s = initial_candidate_score( track_score=t.score, track_hits=t.hits, residual_motion=candidate_motion_ok, appearance=app, wavelet_bonus=wv_bonus, physics_bonus=physics_bonus, ) if target_id is not None and t.track_id == target_id: s += TARGET_STICKY_SCORE_BONUS if s > best_score: best_score = s best = t best_hist = cand_hist best_wavelet_energy = wv_energy best_wavelet_bonus = wv_bonus best_wavelet_hits = wv_hits best_dist = float(dist) best_residual_ok = bool(residual_ok) if best is not None and best_score >= TARGET_PICK_MIN_SCORE: if ( confirmed and (target_id is not None) and (best.track_id != target_id) and (best.time_since_update == 0) ): guidance_candidate_box_eff = clip_box(best.tlbr, ew, eh) guidance_candidate_track_id = int(best.track_id) if ( STALE_LOCK_BREAK_ENABLE and confirmed and (target_id is not None) and (best.track_id != target_id) ): klt_iou_now = 0.0 if (klt.box is not None) and (locked_box_eff is not None): klt_iou_now = float(iou(klt.box, locked_box_eff)) handoff_decision = evaluate_stale_lock( confirmed=confirmed, have_fresh_candidate=(best.time_since_update == 0), target_has_fresh_update=current_target_has_fresh_update, candidate_dist_px=float(best_dist), pred_diag_px=float(pred_diag), miss_streak=int(miss_streak), klt_valid=bool(klt_valid), klt_quality=float(klt.quality), klt_iou=klt_iou_now, candidate_motion_ok=bool(best_residual_ok), target_motion_ok=bool(current_target_motion_ok), stale_break_dist_diag=float(STALE_LOCK_BREAK_DIST_DIAG), stale_break_min_miss=int(STALE_LOCK_BREAK_MIN_MISS), stale_break_klt_quality=float(STALE_LOCK_BREAK_KLT_QUALITY), stale_break_klt_iou=float(STALE_LOCK_BREAK_KLT_IOU), ) stale_lock_active = bool(handoff_decision.stale_lock_active) stale_lock_reason = handoff_decision.reason_text() if confirmed and weak_reacq_guard and ((target_id is None) or (best.track_id != target_id)): if switch_candidate_id == best.track_id: switch_candidate_hits += 1 else: switch_candidate_id = best.track_id switch_candidate_hits = 1 if switch_candidate_hits < int(WEAK_REACQ_ADOPT_HITS): best = None elif target_id is not None and confirmed and best.track_id != target_id: req_switch_hits = compute_fast_handoff_hits( stale_lock_active=bool(FAST_HANDOFF_ENABLE and stale_lock_active), motion_switch_mode=bool(motion_switch_mode), approach_extra_hits=int(approach_switch_extra_hits) if approach_active else 0, fast_maneuver_extra_hits=int(max(0, SWITCH_FAST_MANEUVER_EXTRA_HITS)) if fast_maneuver_guard else 0, default_switch_hits=( int(TARGET_SWITCH_CONFIRM_HITS) if verified_drone_latched else int(UNVERIFIED_TARGET_SWITCH_CONFIRM_HITS) ), fast_handoff_hits=int(FAST_HANDOFF_CONFIRM_HITS), motion_switch_hits=int(MOTION_CONF_SWITCH_CONFIRM_HITS), ) fast_handoff_active = bool( FAST_HANDOFF_ENABLE and stale_lock_active and (req_switch_hits == int(FAST_HANDOFF_CONFIRM_HITS)) ) if switch_candidate_id == best.track_id: switch_candidate_hits += 1 else: switch_candidate_id = best.track_id switch_candidate_hits = 1 if switch_candidate_hits < req_switch_hits: best = None else: switch_candidate_id = None switch_candidate_hits = 0 if best is not None and best_score >= TARGET_PICK_MIN_SCORE: target_track = best if USE_HSV_GATE: ref_hist = blend_hist(ref_hist, best_hist, alpha=0.85) target_absent_frames = 0 if target_track is None and target_id is None and len(eligible_tracks) > 0: eligible_tracks = [ t for t in eligible_tracks if track_passes_score_gate( track_score=float(t.score), confirmed=confirmed, is_switch_candidate=False, weak_reacq_guard=weak_reacq_guard, acquire_floor=float(TRACK_SCORE_MIN_ACQUIRE), reacquire_floor=float(TRACK_SCORE_MIN_REACQUIRE), switch_floor=float(TRACK_SCORE_MIN_SWITCH), ) ] picked = None if len(eligible_tracks) == 0: picked = None elif pred_ref is None: picked = max(eligible_tracks, key=lambda tt: (float(tt.score), box_area(tt.tlbr))) elif not confirmed: pc = box_center(pred_ref) near_lim = max(float(RECOVER_NEAR_DIST_MIN), float(RECOVER_NEAR_DIST_FACTOR) * pred_diag) near_tracks = [] for tt in eligible_tracks: cc = box_center(clip_box(tt.tlbr, ew, eh)) if float(np.linalg.norm(cc - pc)) <= near_lim: near_tracks.append(tt) if len(near_tracks) > 0: picked = max(near_tracks, key=lambda tt: (float(tt.score), box_area(tt.tlbr))) else: if not weak_reacq_guard: pc = box_center(pred_ref) near_lim = max(float(CONFIRMED_NO_ID_NEAR_MIN), float(CONFIRMED_NO_ID_NEAR_FACTOR) * pred_diag) near_tracks = [] for tt in eligible_tracks: cc = box_center(clip_box(tt.tlbr, ew, eh)) if float(np.linalg.norm(cc - pc)) <= near_lim: near_tracks.append(tt) if len(near_tracks) > 0: picked = max(near_tracks, key=lambda tt: (float(tt.score), box_area(tt.tlbr))) if picked is not None: target_track = picked target_absent_frames = 0 switch_candidate_id = None switch_candidate_hits = 0 if ( SOFT_YOLO_HANDOFF_ENABLE and confirmed and (soft_yolo_box_eff is not None) and ( target_track is None or miss_streak >= int(SOFT_YOLO_MIN_MISS) or target_absent_frames >= int(SOFT_YOLO_MIN_ABSENT) ) ): if soft_yolo_track is not None: target_track = soft_yolo_track target_absent_frames = 0 else: chosen = soft_yolo_box_eff target_absent_frames = 0 switch_candidate_id = None switch_candidate_hits = 0 soft_yolo_adopted = True if ( YOLO_REANCHOR_ENABLE and confirmed and (yolo_reanchor_box_eff is not None) and yolo_reanchor_reason ): # Bypass the normal KLT-anchor rejection only after M/N detector # persistence says the old KLT anchor is stale. if yolo_reanchor_track is not None: target_track = yolo_reanchor_track target_absent_frames = 0 else: chosen = yolo_reanchor_box_eff target_absent_frames = 0 switch_candidate_id = None switch_candidate_hits = 0 soft_yolo_adopted = False yolo_reanchor_adopted = True trajectory_reanchor_used = bool(trajectory_det_label != "-") yolo_reanchor_last_reason = yolo_reanchor_reason suppress_target_id_update = False if ( HARD_TARGET_LATCH_ENABLE and verified_drone_latched and confirmed and locked_box_eff is not None and miss_streak < int(HARD_TARGET_LATCH_RELEASE_MISSES) ): proposed_latch_box = None if target_track is not None: proposed_latch_box = clip_box(target_track.tlbr, ew, eh) elif chosen is not None: proposed_latch_box = clip_box(chosen, ew, eh) if proposed_latch_box is not None: latch_ok, _ = klt_anchor_accepts_box( proposed_latch_box, locked_box_eff, ew, eh, dist_diag=float(HARD_TARGET_LATCH_DIST_DIAG), dist_min=float(HARD_TARGET_LATCH_DIST_MIN), iou_floor=float(HARD_TARGET_LATCH_IOU_FLOOR), max_area_ratio=float(HARD_TARGET_LATCH_MAX_AREA_RATIO), max_aspect_ratio=float(HARD_TARGET_LATCH_MAX_ASPECT_RATIO), ) if not latch_ok: target_track = None chosen = None soft_yolo_adopted = False yolo_reanchor_adopted = False switch_candidate_id = None switch_candidate_hits = 0 elif ( target_track is not None and target_id is not None and int(target_track.track_id) != int(target_id) ): suppress_target_id_update = True klt_anchor_reject_reason = "" if ( BT_KLT_ANCHOR_GUARD_ENABLE and (not yolo_reanchor_adopted) and confirmed and klt_valid and locked_box_eff is not None and float(klt.quality) >= float(BT_KLT_ANCHOR_MIN_Q) ): proposed_box_for_anchor = None if target_track is not None: proposed_box_for_anchor = clip_box(target_track.tlbr, ew, eh) elif chosen is not None: proposed_box_for_anchor = clip_box(chosen, ew, eh) if proposed_box_for_anchor is not None: anchor_ok, klt_anchor_reject_reason = klt_anchor_accepts_box( proposed_box_for_anchor, locked_box_eff, ew, eh, dist_diag=float(BT_KLT_ANCHOR_DIST_DIAG), dist_min=float(BT_KLT_ANCHOR_DIST_MIN), iou_floor=float(BT_KLT_ANCHOR_IOU_FLOOR), max_area_ratio=float(BT_KLT_ANCHOR_MAX_AREA_RATIO), max_aspect_ratio=float(BT_KLT_ANCHOR_MAX_ASPECT_RATIO), ) if not anchor_ok: # KLT is still confidently sitting on the physical target. # Do not let a one-frame ByteTrack/Yolo false positive drag # Kalman/guidance away. Keep the KLT anchor as measurement. target_track = None soft_yolo_adopted = False target_absent_frames = 0 switch_candidate_id = None switch_candidate_hits = 0 if BT_KLT_ANCHOR_HOLD_ON_REJECT: chosen = clip_box(locked_box_eff, ew, eh) else: chosen = None if DEBUG and (frame_id % max(1, int(BT_KLT_ANCHOR_DEBUG_EVERY)) == 0): print(f"[klt_anchor_guard] reject BT/Yolo box frame={frame_id} reason={klt_anchor_reject_reason}") elif ( BT_ID_STABILIZE_WHEN_KLT_VALID and target_track is not None and target_id is not None and int(target_track.track_id) != int(target_id) ): # ByteTrack often creates a new id every frame for tiny FPV targets. # Use its box as a detector measurement, but do not treat this as a # physical target switch while KLT is strong. suppress_target_id_update = True target_absent_frames = 0 switch_candidate_id = None switch_candidate_hits = 0 prev_target_id = target_id if target_track is not None: if not suppress_target_id_update: target_id = target_track.track_id chosen = clip_box(target_track.tlbr, ew, eh) if ( not confirmed and locked_box_eff is not None and not acquisition_step_is_plausible( locked_box_eff, chosen, A, dt, ew, eh, ) ): chosen = None target_track = None target_id = None hit_streak = 0 acquire_score = max(0, acquire_score - int(ACQUIRE_MISS_PENALTY)) switch_candidate_id = None switch_candidate_hits = 0 chosen_valid = chosen is not None if chosen_valid: if not kf.initialized: kf.init_from_box(chosen) else: cx, cy = box_center(chosen) bw, bh = box_wh(chosen) if pred_box_eff is not None: pcx, pcy = box_center(pred_box_eff) dist = float(np.hypot(cx - pcx, cy - pcy)) diag = float(np.linalg.norm(box_wh(pred_box_eff))) if dist > 10.0 * max(15.0, diag): chosen = None if chosen is None and kf.initialized and TM_ENABLE and template_gray is not None: pcx, pcy = box_center(pred_box_eff) if pred_box_eff is not None else (kf.x[0, 0], kf.x[1, 0]) tm = tm_search(gray_now, template_gray, (pcx, pcy), miss_streak) if tm is not None: mcx, mcy, _, tw, th = tm if pred_box_eff is not None: bw, bh = box_wh(pred_box_eff) else: bw, bh = float(tw), float(th) kf.update([float(mcx), float(mcy), float(bw), float(bh)]) if chosen is not None: kf.update([cx, cy, bw, bh]) chosen_valid = chosen is not None if chosen_valid: locked_box_eff = chosen hit_streak += 1 miss_streak = 0 if not confirmed: bonus = int(ACQUIRE_HIT_BONUS) if target_track is not None and float(target_track.score) >= float(BT_HIGH): bonus += 1 acquire_score = int(clamp(acquire_score + bonus, 0, 999)) acquire_miss = 0 temporal_confirmed = ( hit_streak >= CONFIRM_HITS and acquire_score >= ACQUIRE_CONFIRM_SCORE ) physics_confirmed = True if PHYSICS_GATE_ENABLE and target_track is not None: evidence = track_physics.get(int(target_track.track_id)) track_hits = int(getattr(target_track, "hits", 0)) if evidence is not None and evidence.reliable: physics_confirmed = bool( evidence.valid and track_hits >= int(PHYSICS_VALID_CONFIRM_HITS) ) else: physics_confirmed = bool( track_hits >= int(PHYSICS_UNKNOWN_CONFIRM_HITS) and track_residual_motion_ok( target_track, motion_mask, ew, eh, ) ) if not confirmed and temporal_confirmed and physics_confirmed: confirmed = True if locked_box_eff is not None: klt.init(frame_eff, locked_box_eff) last_klt_init_frame = frame_id ref_hist = compute_hsv_hist(frame_eff, locked_box_eff) if USE_HSV_GATE else None template_gray = tm_update_template(gray_now, locked_box_eff) acquire_score = 0 acquire_miss = 0 preacq_hist.clear() preacq_hits = 0 if USE_HSV_GATE and have_yolo: cur_hist = compute_hsv_hist(frame_eff, locked_box_eff) if ref_hist is None: ref_hist = cur_hist else: if (frame_id % HSV_UPDATE_EVERY == 0) or (target_track is not None and float(target_track.score) >= BT_HIGH): ref_hist = blend_hist(ref_hist, cur_hist, alpha=0.80) if TM_ENABLE and (have_yolo or frame_id % 5 == 0): template_gray = tm_update_template(gray_now, locked_box_eff) if soft_yolo_adopted and SOFT_YOLO_REFRESH_KLT and confirmed and locked_box_eff is not None: klt.init(frame_eff, locked_box_eff) last_klt_init_frame = frame_id if yolo_reanchor_adopted and YOLO_REANCHOR_RESET_KLT and confirmed and locked_box_eff is not None: klt.reset() klt.init(frame_eff, locked_box_eff) last_klt_init_frame = frame_id yolo_reanchor_memory_box_eff = None yolo_reanchor_memory_hits = 0 if ( KLT_REFRESH_WITH_YOLO and confirmed and have_yolo and (target_track is not None) and (target_track.time_since_update == 0) and (locked_box_eff is not None) and (float(target_track.score) >= float(KLT_REFRESH_MIN_SCORE)) ): klt_age = int(frame_id - last_klt_init_frame) klt_iou = iou(klt.box, locked_box_eff) if klt.box is not None else 0.0 weak_anchor = ( (not klt_valid) or (klt.good_count < int(KLT_REINIT_MIN_POINTS)) or (klt.quality < float(KLT_REFRESH_MIN_QUALITY)) ) stale_anchor = klt_age >= int(max(1, KLT_REFRESH_EVERY)) drifted_anchor = (klt.box is None) or (klt_iou < float(KLT_REFRESH_MIN_IOU)) if (klt_age > 0) and (weak_anchor or stale_anchor or drifted_anchor): klt.init(frame_eff, locked_box_eff) last_klt_init_frame = frame_id else: used_prediction_hold = False if confirmed and stale_lock_active: guidance_ctrl.reset_for_target_switch( np.array([0.5 * float(ew), 0.5 * float(eh)], dtype=np.float32), cmd_damp=float(FAST_HANDOFF_GUIDANCE_CMD_DAMP), ) guidance_reset_event = "stale_lock_break" guidance_force_neutral = True if ( TURN_SAFE_ENABLE and confirmed and have_yolo and dets_eff and (miss_streak >= TURN_SAFE_MISS_BEFORE_RESET) ): best_det, best_conf = pick_best_det_with_score(dets_eff, ew, eh) if best_det is not None and float(best_conf) >= float(TURN_SAFE_MIN_SCORE): adopted_track_id = None adopted_track = None best_iou = 0.0 for tt in tracks_for_select: if tt.time_since_update > 0: continue btt = clip_box(tt.tlbr, ew, eh) i = iou(btt, best_det) if i > best_iou: best_iou = i adopted_track = tt if adopted_track is not None and best_iou >= 0.15: adopted_track_id = adopted_track.track_id if (not TURN_SAFE_REQUIRE_TRACK_ID) or (adopted_track_id is not None): kf.init_from_box(best_det) locked_box_eff = best_det confirmed = True miss_streak = 0 hit_streak = CONFIRM_HITS target_id = adopted_track_id target_absent_frames = 0 switch_candidate_id = None switch_candidate_hits = 0 klt.reset() if locked_box_eff is not None: klt.init(frame_eff, locked_box_eff) last_klt_init_frame = frame_id if USE_HSV_GATE: ref_hist = compute_hsv_hist(frame_eff, locked_box_eff) if TM_ENABLE: template_gray = tm_update_template(gray_now, locked_box_eff) acquire_score = 0 acquire_miss = 0 preacq_hist.clear() preacq_hits = 0 if confirmed and KLT_VALID_HOLD_ENABLE and klt_valid and locked_box_eff is not None: miss_streak = min(int(miss_streak) + 1, int(KLT_VALID_HOLD_MAX_MISS)) hit_streak = 0 used_prediction_hold = True elif confirmed and pred_box_eff is not None and (not stale_lock_active) and miss_streak < KALMAN_HOLD_MAX: hold_box_eff = pred_box_eff if ( TRAJ_USE_PRIMARY_FOR_HOLD and trajectory_primary_box_eff is not None and (miss_streak >= int(TRAJ_HOLD_MISS_GE)) ): hold_box_eff = trajectory_primary_box_eff locked_box_eff = hold_box_eff miss_streak += 1 hit_streak = 0 used_prediction_hold = True elif (not confirmed) and PROVISIONAL_HOLD_ENABLE and kf.initialized and pred_box_eff is not None and miss_streak < int(PROVISIONAL_HOLD_MAX): locked_box_eff = pred_box_eff miss_streak += 1 hit_streak = max(0, int(hit_streak) - int(max(1, PROVISIONAL_HIT_DECAY))) acquire_miss += 1 acquire_score = max(0, int(acquire_score) - int(max(1, ACQUIRE_MISS_PENALTY))) used_prediction_hold = True elif not (confirmed and klt_valid): miss_streak += 1 if confirmed: hit_streak = 0 else: hit_streak = max(0, int(hit_streak) - int(max(1, PROVISIONAL_HIT_DECAY))) acquire_miss += 1 acquire_score = max(0, int(acquire_score) - int(max(1, ACQUIRE_MISS_PENALTY))) if acquire_miss >= int(ACQUIRE_MAX_MISS): acquire_score = 0 if (not used_prediction_hold) and confirmed and (yolo_no_det >= YOLO_NO_DET_LIMIT) and (not klt_valid): confirmed = False locked_box_eff = None ref_hist = None template_gray = None target_id = None target_absent_frames = 0 switch_candidate_id = None switch_candidate_hits = 0 klt.reset() miss_streak = 0 hit_streak = 0 yolo_no_det = 0 acquire_score = 0 acquire_miss = 0 preacq_hist.clear() preacq_hits = 0 elif confirmed and (target_id is None) and (miss_streak >= int(CONFIRMED_NO_ID_MAX_MISS)): confirmed = False locked_box_eff = None ref_hist = None template_gray = None target_id = None target_absent_frames = 0 switch_candidate_id = None switch_candidate_hits = 0 klt.reset() hit_streak = 0 acquire_score = 0 acquire_miss = 0 preacq_hist.clear() preacq_hits = 0 elif miss_streak >= MAX_MISSES: confirmed = False locked_box_eff = None ref_hist = None template_gray = None target_id = None target_absent_frames = 0 switch_candidate_id = None switch_candidate_hits = 0 klt.reset() acquire_score = 0 acquire_miss = 0 preacq_hist.clear() preacq_hits = 0 # ─── Stationary clutter killer ────────────────────── # Независимый "полицейский": следит за движением центра # confirmed трека и motion saliency в его зоне. Форсит # сброс если трек прилип к статичному объекту (снег, # горизонт, ЛЭП). killer.update( frame_id=frame_id, target_id=target_id, locked_box_eff=locked_box_eff, motion_sal=motion_sal, confirmed=confirmed, frame_h=eh, ) if killer.should_kill(frame_id): print(f"[stationary_killer] kill tid={target_id} " f"reason={killer.last_kill_reason}") killer.notify_kill_done(frame_id) confirmed = False locked_box_eff = None ref_hist = None template_gray = None target_id = None target_absent_frames = 0 switch_candidate_id = None switch_candidate_hits = 0 klt.reset() miss_streak = 0 hit_streak = 0 acquire_score = 0 acquire_miss = 0 preacq_hist.clear() preacq_hits = 0 if PHYSICS_GATE_ENABLE and confirmed and locked_box_eff is not None: lock_physics = match_motion_evidence(locked_box_eff, physics_entries) if lock_physics is None: lock_physics = analyze_motion_group( physics_prev_gray, gray_now, locked_box_eff, affine=A, dt=dt, ) if lock_physics.reliable: hard_violation = bool( lock_physics.edge_violation or lock_physics.speed_violation ) physics_bad_lock_streak = ( physics_bad_lock_streak + 1 if hard_violation else 0 ) if physics_bad_lock_streak >= int(PHYSICS_BAD_LOCK_MAX): print( f"[physics_gate] release tid={target_id} " f"coherence={lock_physics.coherence:.2f} " f"residual={lock_physics.residual_px:.2f}px " f"scale={lock_physics.scale_ratio:.3f} " f"speed={lock_physics.speed_norm_s:.3f} edge={int(lock_physics.edge_violation)}" ) confirmed = False verified_drone_latched = False locked_box_eff = None ref_hist = None template_gray = None target_id = None target_absent_frames = 0 switch_candidate_id = None switch_candidate_hits = 0 klt.reset() kf = SafeKalman8D() miss_streak = 0 hit_streak = 0 acquire_score = 0 acquire_miss = 0 preacq_hist.clear() preacq_hits = 0 traj.clear() physics_bad_lock_streak = 0 else: physics_bad_lock_streak = 0 locked_box_orig = None if locked_box_eff is not None: locked_box_orig = unscale_box(locked_box_eff, sx, sy) locked_box_orig = clip_box(locked_box_orig, frame_orig.shape[1], frame_orig.shape[0]) verified_drone_box_eff = None verified_drone_box_orig = None verified_drone_fresh = False if confirmed: verified_drone_box_eff = verified_drone_track_box( target_track, motion_mask, ew, eh, ) if verified_drone_box_eff is not None: verified_drone_fresh = True verified_drone_latched = True elif ( verified_drone_latched and locked_box_eff is not None and miss_streak <= int(DRONE_RED_HOLD_MAX_MISS) ): verified_drone_box_eff = clip_box(locked_box_eff, ew, eh) else: verified_drone_latched = False if verified_drone_box_eff is not None: verified_drone_box_orig = clip_box( unscale_box(verified_drone_box_eff, sx, sy), frame_orig.shape[1], frame_orig.shape[0], ) else: verified_drone_latched = False if GUIDANCE_OVERRIDE_ENABLE: guidance_override_candidate_box_eff = pick_guidance_override_box( track_candidate_box=guidance_candidate_box_eff, raw_yolo_box=guidance_raw_yolo_box_eff, ) guidance_override_candidate_box_eff, guidance_override_memory_ttl = update_guidance_override_latch( new_box=guidance_override_candidate_box_eff, prev_box=guidance_override_memory_box_eff, prev_ttl=guidance_override_memory_ttl, max_ttl=int(GUIDANCE_OVERRIDE_TTL), ) guidance_override_memory_box_eff = guidance_override_candidate_box_eff guidance_override_active = should_override_guidance( confirmed=confirmed, target_track_missing=(target_track is None), have_fresh_candidate=(guidance_override_candidate_box_eff is not None), candidate_matches_target=( guidance_candidate_track_id is not None and target_id is not None and int(guidance_candidate_track_id) == int(target_id) ), miss_streak=int(miss_streak), override_miss_ge=int(GUIDANCE_OVERRIDE_MISS_GE), ) if guidance_override_active: guidance_override_box_eff = guidance_override_candidate_box_eff else: guidance_override_memory_box_eff = None guidance_override_memory_ttl = 0 if DRAW_RAW_YOLO_BOXES and raw_yolo_dets_eff: for d in raw_yolo_dets_eff: b_orig = clip_box(unscale_box(d[:4], sx, sy), frame_orig.shape[1], frame_orig.shape[0]) x1, y1, x2, y2 = map(int, b_orig) cv2.rectangle(frame_orig, (x1, y1), (x2, y2), (80, 170, 255), 1) cv2.putText( frame_orig, f"YOLO {float(d[4]):.2f}", (x1, min(frame_orig.shape[0] - 4, y2 + 16)), cv2.FONT_HERSHEY_SIMPLEX, 0.5, (80, 170, 255), 1, ) if DRAW_ALL_BOXES and dets_eff: for d in dets_eff: b_eff = d[:4] b_orig = unscale_box(b_eff, sx, sy) b_orig = clip_box(b_orig, frame_orig.shape[1], frame_orig.shape[0]) x1, y1, x2, y2 = map(int, b_orig) cv2.rectangle(frame_orig, (x1, y1), (x2, y2), (0, 255, 0), 1) cv2.putText(frame_orig, f"{float(d[4]):.2f}", (x1, max(0, y1 - 6)), cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 255, 0), 2) if DRAW_BT_TRACKS and tracks: for t in tracks: if BT_DRAW_ONLY_CONFIRMED and (t.hits < BT_MIN_HITS): continue if DRAW_BT_ONLY_FRESH and (int(t.time_since_update) > 0): continue b_eff = clip_box(t.tlbr, ew, eh) b_orig = unscale_box(b_eff, sx, sy) b_orig = clip_box(b_orig, frame_orig.shape[1], frame_orig.shape[0]) x1, y1, x2, y2 = map(int, b_orig) cv2.rectangle(frame_orig, (x1, y1), (x2, y2), (255, 255, 0), 2) cv2.putText(frame_orig, f"T{t.track_id}:{t.score:.2f} a={int(t.time_since_update)}", (x1, max(0, y1 - 8)), cv2.FONT_HERSHEY_SIMPLEX, 0.6, (255, 255, 0), 2) if DRAW_LOCK_BOX and verified_drone_box_orig is not None: x1, y1, x2, y2 = map(int, verified_drone_box_orig) cv2.rectangle(frame_orig, (x1, y1), (x2, y2), (0, 0, 255), 2) cv2.putText(frame_orig, f"DRONE ID={target_id}", (x1, max(0, y1 - 10)), cv2.FONT_HERSHEY_SIMPLEX, 0.7, (0, 0, 255), 2) if DRAW_KALMAN and kf.initialized: pb_eff = clip_box(kf.to_box(), ew, eh) pb_orig = unscale_box(pb_eff, sx, sy) cx, cy = box_center(pb_orig) cv2.circle(frame_orig, (int(cx), int(cy)), 5, (255, 0, 0), -1) if DEBUG and DRAW_MOTION_ROI and (not confirmed) and (motion_roi_eff is not None): m_orig = unscale_box(motion_roi_eff, sx, sy) m_orig = clip_box(m_orig, frame_orig.shape[1], frame_orig.shape[0]) mx1, my1, mx2, my2 = map(int, m_orig) cv2.rectangle(frame_orig, (mx1, my1), (mx2, my2), (0, 165, 255), 1) cv2.putText( frame_orig, f"MOTION zones={motion_active_zones}", (mx1, max(0, my1 - 6)), cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 165, 255), 2, ) if DEBUG and DRAW_AUTOGAZE_ROI and len(autogaze_rois_eff) > 0: for gi, g_eff in enumerate(autogaze_rois_eff): g_orig = unscale_box(g_eff, sx, sy) g_orig = clip_box(g_orig, frame_orig.shape[1], frame_orig.shape[0]) gx1, gy1, gx2, gy2 = map(int, g_orig) color = (255, 80, 20) if gi == 0 else (255, 160, 80) cv2.rectangle(frame_orig, (gx1, gy1), (gx2, gy2), color, 1) if gi == 0: cv2.putText( frame_orig, "GAZE", (gx1, max(0, gy1 - 6)), cv2.FONT_HERSHEY_SIMPLEX, 0.5, color, 2, ) if DRAW_TRAJ: if CLEAR_TRAJ_ON_RECOVER and (not confirmed): traj.clear() if (not DRAW_TRAJ_ONLY_WHEN_LOCKED) or (locked_box_eff is not None and confirmed): traj_frame_i += 1 if traj_frame_i % max(1, int(TRAIL_DRAW_EVERY_N)) == 0: src_eff = locked_box_eff if locked_box_eff is not None else pred_box_eff if src_eff is not None: c_eff = box_center(src_eff) c_orig = c_eff / np.array([sx, sy], dtype=np.float32) pt = (int(c_orig[0]), int(c_orig[1])) if (not traj) or (abs(pt[0] - traj[-1][0]) + abs(pt[1] - traj[-1][1]) >= int(TRAIL_MIN_STEP_PX)): traj.append(pt) if len(traj) >= 2: overlay = frame_orig.copy() for i in range(1, len(traj)): p0 = traj[i - 1] p1 = traj[i] age = i / max(1, (len(traj) - 1)) thickness = 1 if age < 0.85 else 2 cv2.line(overlay, p0, p1, (0, 0, 255), thickness, cv2.LINE_AA) a = float(clamp(TRAIL_ALPHA, 0.05, 0.6)) frame_orig[:] = cv2.addWeighted(overlay, a, frame_orig, 1.0 - a, 0) if DEBUG and DRAW_TRAJ_PREDICTIONS and trajectory_hypotheses: src_eff = locked_box_eff if locked_box_eff is not None else pred_box_eff if src_eff is not None: src_c = box_center(src_eff) / np.array([sx, sy], dtype=np.float32) for h in trajectory_hypotheses: hb_orig = unscale_box(h["box"], sx, sy) hb_orig = clip_box(hb_orig, frame_orig.shape[1], frame_orig.shape[0]) hx1, hy1, hx2, hy2 = map(int, hb_orig) hc = box_center(h["box"]) / np.array([sx, sy], dtype=np.float32) cv2.rectangle(frame_orig, (hx1, hy1), (hx2, hy2), (180, 0, 255), 1) cv2.line(frame_orig, (int(src_c[0]), int(src_c[1])), (int(hc[0]), int(hc[1])), (180, 0, 255), 1, cv2.LINE_AA) cv2.putText(frame_orig, str(h.get("label", "tr")), (hx1, max(0, hy1 - 4)), cv2.FONT_HERSHEY_SIMPLEX, 0.4, (180, 0, 255), 1) status = "CONFIRMED" if verified_drone_box_eff is not None else ("TRACKING" if confirmed else "RECOVER") cv2.putText(frame_orig, status, (20, 40), cv2.FONT_HERSHEY_SIMPLEX, 1.0, (0, 255, 255), 2) ag_ready = int(getattr(autogaze_worker, "ready", False)) ag_stale = int(autogaze_info["stale"]) if autogaze_info is not None else 1 ag_age = int(autogaze_info["age"]) if autogaze_info is not None else -1 ag_cells = int(autogaze_info["active_cells"]) if autogaze_info is not None else 0 ag_ms = float(autogaze_info["infer_ms"]) if autogaze_info is not None else 0.0 ag_rois = len(autogaze_rois_eff) target_track_score = float(target_track.score) if target_track is not None else None vx_guid = float(kf.x[4, 0]) if kf.initialized else 0.0 vy_guid = float(kf.x[5, 0]) if kf.initialized else 0.0 guidance_state = guidance_ctrl.update( frame_id=frame_id, frame_w=ew, frame_h=eh, confirmed=bool(verified_drone_box_eff is not None and (not guidance_force_neutral)), locked_box=None if guidance_force_neutral else verified_drone_box_eff, pred_box=None, override_box=None, target_id=target_id, target_track_score=target_track_score, klt_valid=klt_valid, klt_quality=float(klt.quality), miss_streak=miss_streak, vx=vx_guid, vy=vy_guid, ) guidance_state["det_count"] = 1 if guidance_state["active"] and verified_drone_fresh else 0 error_output.send(guidance_state) if guidance_state["active"] or yolo_raw_count > 0: last_archive_hit_ts = float(frame_ts) if DEBUG: cv2.putText(frame_orig, f"src={source_kind} p={ACTIVE_ANTI_FP_PROFILE} eff={ew}x{eh} miss={miss_streak} hit={hit_streak} acq={acquire_score} pH={preacq_hits} tAbs={target_absent_frames} swHit={switch_candidate_hits} rHit={yolo_reanchor_memory_hits} rAd={int(yolo_reanchor_adopted)} trH={len(trajectory_hypotheses)} trA={int(trajectory_reanchor_used)} trL={trajectory_det_label} stale={int(stale_lock_active)} fastH={int(fast_handoff_active)} detEvery={det_every} yNoDet={yolo_no_det} mZones={motion_active_zones} fT={flash_ttl} wRT={wavelet_roi_ttl} wRP={wavelet_roi_peak:.2f} aR={ag_ready} aS={ag_stale} aAge={ag_age} aC={ag_cells} aK={ag_rois}", (20, 75), cv2.FONT_HERSHEY_SIMPLEX, 0.65, (0, 255, 255), 2) cv2.putText(frame_orig, f"YOLO new={int(have_yolo)} dets={yolo_raw_count}->{len(dets_eff)} merge={merged_part_count} infer={infer_ms:.1f}ms mode={yolo_mode} gReset={guidance_reset_event or '-'}", (20, 105), cv2.FONT_HERSHEY_SIMPLEX, 0.65, (0, 255, 255), 2) cv2.putText(frame_orig, f"KLT valid={int(klt_valid)} q={klt.quality:.2f} pts={klt.good_count} speed={speed:.1f} dt={dt * 1000:.1f}ms dtSrc={dt_source} ch={adaptive_chase_stage} appr={int(approach_active)} fClose={int(close_force_fullscan)} swFast={int(fast_maneuver_guard)} wA={int(wavelet_active)} wCd={wavelet_cooldown} wE={best_wavelet_energy:.3f} wB={best_wavelet_bonus:.2f} wH={best_wavelet_hits} aMS={ag_ms:.1f} aCd={autogaze_cooldown}", (20, 135), cv2.FONT_HERSHEY_SIMPLEX, 0.65, (0, 255, 255), 2) guidance_ctrl.draw_overlay(frame_orig, guidance_state, sx, sy) if have_yolo: raw_boxes_orig = [ ( clip_box( unscale_box(d[:4], sx, sy), frame_orig.shape[1], frame_orig.shape[0], ).copy(), float(d[4]), ) for d in raw_yolo_dets_eff ] elif latest_detection_overlay is not None: raw_boxes_orig = latest_detection_overlay.get("raw_boxes", []) else: raw_boxes_orig = [] if have_yolo: accepted_boxes_orig = [ ( clip_box( unscale_box(d[:4], sx, sy), frame_orig.shape[1], frame_orig.shape[0], ).copy(), float(d[4]), ) for d in dets_eff ] elif latest_detection_overlay is not None: accepted_boxes_orig = latest_detection_overlay.get("accepted_boxes", []) else: accepted_boxes_orig = [] latest_detection_overlay = { "raw_boxes": raw_boxes_orig, "accepted_boxes": accepted_boxes_orig, "verified_drone_box": ( None if verified_drone_box_orig is None else verified_drone_box_orig.copy() ), "target_id": target_id, "status": status, "guidance_state": guidance_state.copy(), "sx": float(sx), "sy": float(sy), } track_logger.log_frame( frame_id=frame_id, timestamp_sec=frame_ts, dt_sec=dt, source_kind=source_kind, status=status, confirmed=confirmed, target_id=target_id, target_track_score=target_track_score, locked=(locked_box_eff is not None), locked_box=locked_box_eff, pred_box=pred_box_eff, hit_streak=hit_streak, miss_streak=miss_streak, acquire_score=acquire_score, preacq_hits=preacq_hits, switch_candidate_hits=switch_candidate_hits, best_score=best_score, have_yolo=have_yolo, yolo_mode=yolo_mode, yolo_raw_count=yolo_raw_count, det_count=len(dets_eff) if dets_eff else 0, merged_part_count=merged_part_count, infer_ms=infer_ms, stale_lock_active=stale_lock_active, stale_lock_reason=stale_lock_reason, fast_handoff_active=fast_handoff_active, guidance_reset_event=guidance_reset_event, used_prediction_hold=used_prediction_hold, klt_valid=klt_valid, klt_quality=klt.quality, klt_points=klt.good_count, speed=speed, adaptive_chase_stage=adaptive_chase_stage, approach_active=approach_active, close_force_fullscan=close_force_fullscan, fast_maneuver_guard=fast_maneuver_guard, motion_active_zones=motion_active_zones, wavelet_active=wavelet_active, wavelet_energy=best_wavelet_energy, wavelet_bonus=best_wavelet_bonus, wavelet_hits=best_wavelet_hits, autogaze_ready=ag_ready, autogaze_stale=ag_stale, autogaze_age=ag_age, autogaze_cells=ag_cells, autogaze_ms=ag_ms, guidance_active=guidance_state["active"], guidance_status=guidance_state["status"], guidance_confidence=guidance_state["confidence"], guidance_error_x=guidance_state["error_x"], guidance_error_y=guidance_state["error_y"], guidance_cmd_x=guidance_state["cmd_x"], guidance_cmd_y=guidance_state["cmd_y"], guidance_on_target=guidance_state["on_target"], ) if TRAJ_PREDICT_ENABLE: if confirmed and locked_box_eff is not None: fresh_track_measurement = bool( target_track is not None and int(getattr(target_track, "time_since_update", 1)) == 0 ) src_ok = bool( (not frame_frozen) and ( fresh_track_measurement or klt_valid or soft_yolo_adopted or yolo_reanchor_adopted ) and ( verified_drone_box_eff is not None or klt_valid ) ) if src_ok: trajectory_obs_hist.append({ "frame": int(frame_id), "ts": float(frame_ts), "center": box_center(locked_box_eff).astype(np.float32), "box": clip_box(locked_box_eff, ew, eh).copy(), }) else: trajectory_obs_hist.clear() if realtime_pump is None: publish_frame(frame_orig, frame_ts, frame_id) iter_ms = (time.perf_counter() - iter_start) * 1000.0 perf_hist.append(iter_ms) analysis_frames += 1 if realtime_pump is not None: next_analysis_at = time.perf_counter() + max(0.0, analysis_period - iter_ms / 1000.0) if show_output: cv2.imshow(WINDOW_NAME, frame_orig) key = cv2.waitKey(1) & 0xFF if key == 27: break now = time.perf_counter() if now - last_perf_log_ts >= 2.0 and perf_hist: last_perf_log_ts = now p50 = np.percentile(perf_hist, 50) p95 = np.percentile(perf_hist, 95) if realtime_pump is not None: fps = realtime_pump.output_fps() skipped = realtime_pump.dropped_analysis_frames passed = max(0, realtime_pump.read_frames - analysis_frames) analysis_every = max( 1, round(realtime_pump.read_frames / max(1, analysis_frames)), ) else: fps = 1000.0 / max(float(np.mean(perf_hist)), 1e-6) skipped = 0 passed = 0 analysis_every = 1 if yolo_hist: yp50 = np.percentile(yolo_hist, 50) yp95 = np.percentile(yolo_hist, 95) else: yp50, yp95 = 0.0, 0.0 print( f"[perf] fps~{fps:.1f} iter p50={p50:.1f} p95={p95:.1f} | " f"yolo p50={yp50:.1f} p95={yp95:.1f} " f"skip={skipped} pass={passed} analysisEvery={analysis_every}" ) if realtime_pump is None: frame_id += 1 if realtime_pump is not None: realtime_pump.stop() autogaze_worker.stop() yolo_worker.stop() error_output.close() track_summary = track_logger.close() if track_summary is not None: print( f"[track-log] confirmed={track_summary['confirmed_frames']}/{track_summary['frames']} " f"switches={track_summary['target_switches']} " f"csv={track_summary['csv_path']}" ) if writer is not None: writer.release() if archive_written_frames > 0: print(f"Saved inference video: {out_video_path}, frames={archive_written_frames}") else: try: Path(out_video_path).unlink(missing_ok=True) except OSError as exc: print(f"WARN: empty recording cleanup failed: {exc}") else: print(f"Skipped empty inference video: {out_video_path}") if ui_exporter is not None: ui_exporter.stop() if active_video_marker is not None: active_video_marker.unlink(missing_ok=True) cap.release() if show_output: cv2.destroyAllWindows() if __name__ == "__main__": main()