import sys 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 template_matching import tm_update_template, tm_search from track_score_policy import 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) # ─── 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): """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): b = make_box_from_center_wh(px, py, bw2, bh2, ew, eh) hypotheses.append({"label": str(label), "box": b, "center": box_center(b), "weight": float(weight)}) 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(): print("Loading YOLO...") model = YOLO(MODEL_PATH) if torch.cuda.is_available(): model.to(f"cuda:{DEVICE}") if torch.cuda.is_available(): # Warmup на обоих размерах — критично для dynamic CBAM # чтобы MLP построились на обоих scales до реального инференса. dummy_roi = np.zeros((IMG_SIZE_ROI, IMG_SIZE_ROI, 3), dtype=np.uint8) dummy_full = np.zeros((IMG_SIZE_FULL, IMG_SIZE_FULL, 3), dtype=np.uint8) with torch.inference_mode(): for _ in range(3): _ = model( dummy_roi, conf=YOLO_CONF_EFFECTIVE, imgsz=IMG_SIZE_ROI, verbose=False, max_det=MAX_DET, device=DEVICE, half=USE_HALF ) for _ in range(2): _ = model( dummy_full, conf=YOLO_CONF_EFFECTIVE, imgsz=IMG_SIZE_FULL, verbose=False, max_det=MAX_DET, device=DEVICE, half=USE_HALF ) print(f"Model warmed up at {IMG_SIZE_ROI} and {IMG_SIZE_FULL}") cap, source_kind = open_source(SOURCE, CAP_BACKEND) if not cap.isOpened(): print(f"Capture open failed: {SOURCE}") return try: cap.set(cv2.CAP_PROP_BUFFERSIZE, 1) except Exception: pass print(f"Opened source: {SOURCE} ({source_kind})") 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 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) 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: print(f"Saving inference video to: {out_video_path}") yolo_worker = YOLOWorker(model) 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()) 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) 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 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))) print("Start") while True: ret, frame_orig = cap.read() loop_ts = time.perf_counter() if not ret: break frame_eff, sx, sy = get_effective_frame(frame_orig) eh, ew = frame_eff.shape[:2] frame_ts, dt_source = get_frame_timestamp_seconds(cap, source_kind, frame_id, input_fps, loop_ts) 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) 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_det_label = "-" trajectory_reanchor_used = False 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 iter_start = time.perf_counter() 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 ) if trajectory_hypotheses: trajectory_primary_box_eff = trajectory_hypotheses[0]["box"] trajectory_roi_eff = make_union_roi_from_boxes( [h["box"] for h in trajectory_hypotheses], ew, eh, margin=TRAJ_ROI_MARGIN, 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 # 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 = False have_yolo = False yolo_mode = "NONE" yolo_raw_count = 0 yolo_ts = None merged_part_count = 0 if yolo is not None: have_yolo = True 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) 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 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) 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 residual_ok = False if is_switch_candidate and EGO_RESIDUAL_GATE_ENABLE and motion_mask is not None: residual_ok = track_residual_motion_ok(t, motion_mask, ew, eh) 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 if pred_ref is None: s = (0.35 * float(t.score)) + (0.0005 * box_area(b)) + (0.9 * app) + wv_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), 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 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) 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 if not confirmed and ((hit_streak >= CONFIRM_HITS) or (acquire_score >= ACQUIRE_CONFIRM_SCORE)): 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 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 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]) 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_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 locked_box_orig is not None: x1, y1, x2, y2 = map(int, locked_box_orig) if miss_streak <= int(DRAW_KALMAN_WHEN_MISS_LE): cv2.rectangle(frame_orig, (x1, y1), (x2, y2), (0, 0, 255), 2) cv2.putText(frame_orig, f"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 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(confirmed and (not guidance_force_neutral)), locked_box=None if guidance_force_neutral else locked_box_eff, pred_box=None if guidance_force_neutral else pred_box_eff, override_box=None if guidance_force_neutral else guidance_override_box_eff, 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, ) 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) 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: src_ok = bool( (hit_streak > 0) or klt_valid or soft_yolo_adopted or yolo_reanchor_adopted or (miss_streak <= int(TRAJ_HISTORY_KEEP_MISS_LE)) ) 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 writer is not None: writer.write(frame_orig) if SHOW_OUTPUT: cv2.imshow(WINDOW_NAME, frame_orig) iter_ms = (time.perf_counter() - iter_start) * 1000.0 perf_hist.append(iter_ms) elapsed = time.perf_counter() - iter_start wait_ms = 1 if VIDEO_REALTIME and target_out_fps > 0.0: target_dt = 1.0 / target_out_fps if elapsed < target_dt: wait_ms = int((target_dt - elapsed) * 1000.0) wait_ms = clamp(wait_ms, 1, 50) key = cv2.waitKey(int(wait_ms)) & 0xFF if key == 27: break if frame_id % 60 == 0 and perf_hist: p50 = np.percentile(perf_hist, 50) p95 = np.percentile(perf_hist, 95) fps = 1000.0 / max(np.mean(perf_hist), 1e-6) 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} | yolo p50={yp50:.1f} p95={yp95:.1f}") frame_id += 1 autogaze_worker.stop() yolo_worker.stop() 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() cap.release() cv2.destroyAllWindows() if __name__ == "__main__": main()