YaroslavKartashov
/
MAI
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity
You cannot select more than 25 topics Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
main
Branches Tags
${ item.name }
Create tag ${ searchTerm }
Create branch ${ searchTerm }
from '4340aa0790'
${ noResults }
MAI/main.py

2554 lines
110 KiB
Python
Raw Blame History Unescape Escape

This file contains ambiguous Unicode characters!

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

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()
Reference in New Issue View Git Blame Copy Permalink