DroneMAI/fpv_lock_bytetrack.py

import cv2
import numpy as np
import time
import threading
from collections import deque
import torch
from ultralytics import YOLO

from bytetrack_min import BYTETracker  # <-- add this file nearby


# USER SETTINGS

MODEL_PATH = r"C:\Users\Legion\PycharmProjects\YOLOTrain\.venv\runs\detect\train30\weights\best.pt"
DEVICE = 0
USE_HALF = True

USE_VIDEO_FILE = True
VIDEO_PATH = r"C:\Users\Legion\PycharmProjects\YOLOTrain\videoMAI\Высший пилотаж.mp4"
VIDEO_REALTIME = True

CAM_INDEX = 0
CAP_BACKEND = cv2.CAP_DSHOW  # or cv2.CAP_MSMF

SHOW_OUTPUT = True
WINDOW_NAME = "FPV LOCK PAL + ByteTrack"
TARGET_OUT_FPS = 20  # best-effort pacing

# Effective resolution policy (for analog PAL)
EFFECTIVE_W = 720
EFFECTIVE_H = 576
FORCE_EFFECTIVE_PAL = True

# YOLO inference settings
CONF = 0.22              # confidence used INSIDE YOLO inference call
IMG_SIZE_ROI = 384
IMG_SIZE_FULL = 640
MAX_DET = 60
TARGET_CLASS_ID = 0   # set int if you want only one class

# Filters (small drones far away)
MIN_BOX_AREA = 30
MAX_BOX_AREA = 0
ASPECT_RANGE = (0.15, 6.0)

# FSM
CONFIRM_HITS = 2
MAX_MISSES = 10
RECOVER_FULLSCAN_EVERY = 18
RECOVER_FORCED_DET_EVERY = 2

# ROI sizing (in effective pixels)
BASE_RADIUS = 150
MAX_RADIUS = 520
SPEED_RADIUS_FACTOR = 18.0
FORWARD_BIAS = 1.8
SIDE_BIAS = 1.2

# KLT optical flow tracker (effective)
KLT_MAX_CORNERS = 140
KLT_QUALITY = 0.01
KLT_MIN_DIST = 6
KLT_WIN = (21, 21)
KLT_MAX_LEVEL = 3
KLT_OUTLIER_THRESH = 10.0

# KLT validity gates (anti-drift)
KLT_OK_Q = 0.35
KLT_OK_PTS = 30
KLT_REINIT_MIN_POINTS = 30

# YOLO safety
YOLO_NO_DET_LIMIT = 3
YOLO_FRESH_SEC = 0.50
YOLO_FORCE_DET_WHEN_WEAK = True

# Appearance gate (HSV hist) in effective image
USE_HSV_GATE = False
HSV_HIST_BINS = (16, 16)
HSV_GATE_MIN_SIM = 0.25
HSV_UPDATE_EVERY = 25

# Async YOLO worker
YOLO_QUEUE_MAX = 1

# Debug / draw
DEBUG = True
DRAW_ALL_BOXES = True
DRAW_LOCK_BOX = True
DRAW_KALMAN = True
DRAW_KLT_POINTS = False
DRAW_BT_TRACKS = True
BT_DRAW_ONLY_CONFIRMED = False


# ByteTrack SETTINGS (important!)


BT_HIGH = 0.35
BT_LOW  = 0.12
BT_NEW  = 0.35
BT_MATCH_IOU = 0.70
BT_BUFFER = 50
BT_MIN_HITS = 1


# HELPERS


def clamp(x, a, b):
    return max(a, min(b, x))

def box_center(box):
    x1, y1, x2, y2 = box
    return np.array([(x1 + x2) * 0.5, (y1 + y2) * 0.5], dtype=np.float32)

def box_wh(box):
    x1, y1, x2, y2 = box
    return np.array([max(1.0, x2 - x1), max(1.0, y2 - y1)], dtype=np.float32)

def box_area(box):
    w, h = box_wh(box)
    return float(w * h)

def iou(a, b):
    ax1, ay1, ax2, ay2 = a
    bx1, by1, bx2, by2 = b
    ix1, iy1 = max(ax1, bx1), max(ay1, by1)
    ix2, iy2 = min(ax2, bx2), min(ay2, by2)
    inter = max(0.0, ix2 - ix1) * max(0.0, iy2 - iy1)
    if inter <= 0:
        return 0.0
    union = box_area(a) + box_area(b) - inter
    return 0.0 if union <= 0 else float(inter / union)

def clip_box(box, w, h):
    x1, y1, x2, y2 = box
    x1 = clamp(float(x1), 0.0, float(w - 2))
    y1 = clamp(float(y1), 0.0, float(h - 2))
    x2 = clamp(float(x2), 1.0, float(w - 1))
    y2 = clamp(float(y2), 1.0, float(h - 1))
    if x2 <= x1 + 1:
        x2 = x1 + 2
    if y2 <= y1 + 1:
        y2 = y1 + 2
    return np.array([x1, y1, x2, y2], dtype=np.float32)

def crop_roi(frame, roi_box):
    x1, y1, x2, y2 = map(int, roi_box)
    return frame[y1:y2, x1:x2], x1, y1

def compute_hsv_hist(frame, box):
    x1, y1, x2, y2 = map(int, box)
    patch = frame[y1:y2, x1:x2]
    if patch.size == 0:
        return None
    hsv = cv2.cvtColor(patch, cv2.COLOR_BGR2HSV)
    hist = cv2.calcHist([hsv], [0, 1], None, list(HSV_HIST_BINS), [0, 180, 0, 256])
    cv2.normalize(hist, hist, alpha=0, beta=1, norm_type=cv2.NORM_MINMAX)
    return hist

def hsv_sim(h1, h2):
    if h1 is None or h2 is None:
        return 0.0
    d = cv2.compareHist(h1, h2, cv2.HISTCMP_BHATTACHARYYA)
    return float(1.0 - d)

def filter_yolo_boxes_with_scores(result, offset_x=0, offset_y=0, min_conf=0.12):
    dets_out = []
    if result.boxes is None or len(result.boxes) == 0:
        return dets_out

    xyxy = result.boxes.xyxy.detach().cpu().numpy()
    confs = result.boxes.conf.detach().cpu().numpy()
    clss = result.boxes.cls.detach().cpu().numpy().astype(int)

    for b, c, cls_id in zip(xyxy, confs, clss):
        score = float(c)
        if score < float(min_conf):
            continue
        if TARGET_CLASS_ID is not None and cls_id != TARGET_CLASS_ID:
            continue

        x1, y1, x2, y2 = map(float, b)
        ww = max(0.0, x2 - x1)
        hh = max(0.0, y2 - y1)
        if ww <= 1.0 or hh <= 1.0:
            continue

        a = ww * hh
        if a < MIN_BOX_AREA:
            continue
        if MAX_BOX_AREA > 0 and a > MAX_BOX_AREA:
            continue

        ar = ww / hh
        if not (ASPECT_RANGE[0] <= ar <= ASPECT_RANGE[1]):
            continue

        dets_out.append(np.array([x1 + offset_x, y1 + offset_y, x2 + offset_x, y2 + offset_y, score], dtype=np.float32))

    return dets_out


# --- scale mapping between original and effective ---

def get_effective_frame(orig_bgr):
    oh, ow = orig_bgr.shape[:2]
    if not FORCE_EFFECTIVE_PAL:
        return orig_bgr, 1.0, 1.0
    eff = cv2.resize(orig_bgr, (EFFECTIVE_W, EFFECTIVE_H), interpolation=cv2.INTER_AREA)
    sx = EFFECTIVE_W / float(ow)
    sy = EFFECTIVE_H / float(oh)
    return eff, sx, sy

def unscale_box(box_eff, sx, sy):
    x1, y1, x2, y2 = box_eff
    return np.array([x1 / sx, y1 / sy, x2 / sx, y2 / sy], dtype=np.float32)

# =========================
# KALMAN 8D (cx,cy,w,h + velocities) with dt
# =========================

class Kalman8D:
    def __init__(self):
        self.x = np.zeros((8, 1), dtype=np.float32)
        self.P = np.eye(8, dtype=np.float32) * 50.0

        self.H = np.zeros((4, 8), dtype=np.float32)
        self.H[0, 0] = 1.0
        self.H[1, 1] = 1.0
        self.H[2, 2] = 1.0
        self.H[3, 3] = 1.0

        self.R = np.eye(4, dtype=np.float32)
        self.R[0, 0] = 16.0
        self.R[1, 1] = 16.0
        self.R[2, 2] = 25.0
        self.R[3, 3] = 25.0

        self.initialized = False
        self.q_pos = 2.0
        self.q_size = 3.0
        self.q_vel = 4.0

    def F(self, dt):
        F = np.eye(8, dtype=np.float32)
        F[0, 4] = dt
        F[1, 5] = dt
        F[2, 6] = dt
        F[3, 7] = dt
        return F

    def Q(self, dt):
        q = np.zeros((8, 8), dtype=np.float32)
        s1 = dt * dt
        s2 = dt
        q[0, 0] = self.q_pos * s1
        q[1, 1] = self.q_pos * s1
        q[2, 2] = self.q_size * s1
        q[3, 3] = self.q_size * s1
        q[4, 4] = self.q_vel * s2
        q[5, 5] = self.q_vel * s2
        q[6, 6] = self.q_vel * s2
        q[7, 7] = self.q_vel * s2
        return q

    def predict(self, dt):
        dt = float(max(1e-3, dt))
        F = self.F(dt)
        self.x = F @ self.x
        self.P = F @ self.P @ F.T + self.Q(dt)
        return self.x[:4].flatten()

    def update(self, z):
        z = np.array(z, dtype=np.float32).reshape(4, 1)
        y = z - self.H @ self.x
        S = self.H @ self.P @ self.H.T + self.R
        K = self.P @ self.H.T @ np.linalg.inv(S)
        self.x = self.x + K @ y
        I = np.eye(8, dtype=np.float32)
        self.P = (I - K @ self.H) @ self.P

    def init_from_box(self, box):
        cx, cy = box_center(box)
        w, h = box_wh(box)
        self.x[:] = 0
        self.x[0, 0] = cx
        self.x[1, 0] = cy
        self.x[2, 0] = w
        self.x[3, 0] = h
        self.P = np.eye(8, dtype=np.float32) * 50.0
        self.initialized = True

    def to_box(self):
        cx = float(self.x[0, 0])
        cy = float(self.x[1, 0])
        w = float(max(2.0, self.x[2, 0]))
        h = float(max(2.0, self.x[3, 0]))
        return np.array([cx - w * 0.5, cy - h * 0.5, cx + w * 0.5, cy + h * 0.5], dtype=np.float32)

    def uncertainty(self):
        return float(self.P[0, 0] + self.P[1, 1] + self.P[2, 2] + self.P[3, 3])

# =========================
# KLT TRACKER (effective)
# =========================

class KLTTracker:
    def __init__(self):
        self.prev_gray = None
        self.pts = None
        self.box = None
        self.good_count = 0
        self.quality = 0.0

    def reset(self):
        self.prev_gray = None
        self.pts = None
        self.box = None
        self.good_count = 0
        self.quality = 0.0

    def init(self, frame_bgr, box):
        gray = cv2.cvtColor(frame_bgr, cv2.COLOR_BGR2GRAY)
        x1, y1, x2, y2 = map(int, box)
        mask = np.zeros_like(gray)
        cv2.rectangle(mask, (x1, y1), (x2, y2), 255, -1)

        pts = cv2.goodFeaturesToTrack(
            gray,
            maxCorners=KLT_MAX_CORNERS,
            qualityLevel=KLT_QUALITY,
            minDistance=KLT_MIN_DIST,
            mask=mask
        )
        self.prev_gray = gray
        self.pts = pts
        self.box = box.copy()
        self.good_count = 0
        self.quality = 0.0

    def update(self, frame_bgr):
        if self.prev_gray is None or self.pts is None or len(self.pts) == 0 or self.box is None:
            self.quality = 0.0
            self.good_count = 0
            return None

        gray = cv2.cvtColor(frame_bgr, cv2.COLOR_BGR2GRAY)
        next_pts, st, err = cv2.calcOpticalFlowPyrLK(
            self.prev_gray, gray, self.pts, None,
            winSize=KLT_WIN,
            maxLevel=KLT_MAX_LEVEL,
            criteria=(cv2.TERM_CRITERIA_EPS | cv2.TERM_CRITERIA_COUNT, 30, 0.01)
        )

        if next_pts is None or st is None:
            self.quality = 0.0
            self.good_count = 0
            self.prev_gray = gray
            return None

        st = st.reshape(-1)
        good_old = self.pts[st == 1].reshape(-1, 2)
        good_new = next_pts[st == 1].reshape(-1, 2)

        self.good_count = int(len(good_new))
        if self.good_count < 6:
            self.quality = 0.0
            self.prev_gray = gray
            self.pts = good_new.reshape(-1, 1, 2) if self.good_count > 0 else None
            return None

        disp = good_new - good_old
        med = np.median(disp, axis=0)
        d = np.linalg.norm(disp - med[None, :], axis=1)
        inliers = d < KLT_OUTLIER_THRESH
        in_cnt = int(np.count_nonzero(inliers))

        if in_cnt < 6:
            self.quality = 0.0
            self.prev_gray = gray
            self.pts = good_new.reshape(-1, 1, 2)
            return None

        disp_in = disp[inliers]
        med = np.median(disp_in, axis=0)
        dx, dy = float(med[0]), float(med[1])

        x1, y1, x2, y2 = self.box
        new_box = np.array([x1 + dx, y1 + dy, x2 + dx, y2 + dy], dtype=np.float32)

        frac = float(in_cnt) / float(len(inliers))
        self.quality = clamp(frac * (self.good_count / max(1, KLT_MAX_CORNERS)), 0.0, 1.0)

        self.prev_gray = gray
        self.pts = good_new[inliers].reshape(-1, 1, 2)
        self.box = new_box
        return new_box

# =========================
# ASYNC YOLO WORKER (effective)
# =========================

class YOLOWorker:
    def __init__(self, model):
        self.model = model
        self.req = deque(maxlen=YOLO_QUEUE_MAX)
        self.res = deque(maxlen=1)
        self.lock = threading.Lock()
        self.running = False
        self.thread = threading.Thread(target=self._loop, daemon=True)

    def start(self):
        self.running = True
        self.thread.start()

    def stop(self):
        self.running = False
        self.thread.join(timeout=1.0)

    def submit(self, frame_eff_bgr, roi_box_eff, mode, ts):
        with self.lock:
            self.req.append((frame_eff_bgr, roi_box_eff, mode, ts))

    def try_get(self):
        with self.lock:
            if not self.res:
                return None
            return self.res.pop()

    def _loop(self):
        while self.running:
            item = None
            with self.lock:
                if self.req:
                    item = self.req.pop()
                    self.req.clear()

            if item is None:
                time.sleep(0.001)
                continue

            frame, roi_box, mode, ts = item
            h, w = frame.shape[:2]
            dets = []
            infer_ms = 0.0
            used_roi = False

            try:
                if roi_box is not None:
                    roi_box = clip_box(roi_box, w, h)
                    crop, ox, oy = crop_roi(frame, roi_box)
                    if crop.size > 0:
                        used_roi = True
                        t0 = time.perf_counter()
                        with torch.inference_mode():
                            r = self.model(crop, conf=CONF, imgsz=IMG_SIZE_ROI, verbose=False, max_det=MAX_DET)[0]
                        infer_ms = (time.perf_counter() - t0) * 1000.0
                        dets = filter_yolo_boxes_with_scores(r, offset_x=ox, offset_y=oy, min_conf=BT_LOW)

                else:
                    sh, sw = frame.shape[:2]
                    short = min(sh, sw)
                    scale = 1.0
                    target = IMG_SIZE_FULL
                    if short > target:
                        scale = target / float(short)
                        small = cv2.resize(frame, (int(sw * scale), int(sh * scale)), interpolation=cv2.INTER_AREA)
                    else:
                        small = frame

                    t0 = time.perf_counter()
                    with torch.inference_mode():
                        r = self.model(small, conf=CONF, imgsz=IMG_SIZE_FULL, verbose=False, max_det=MAX_DET)[0]
                    infer_ms = (time.perf_counter() - t0) * 1000.0

                    dets_s = filter_yolo_boxes_with_scores(r, 0, 0, min_conf=BT_LOW)
                    if scale != 1.0:
                        inv = 1.0 / scale
                        dets = [np.array([d[0]*inv, d[1]*inv, d[2]*inv, d[3]*inv, d[4]], dtype=np.float32) for d in dets_s]
                    else:
                        dets = dets_s

            except Exception:
                dets = []
                infer_ms = 0.0

            with self.lock:
                self.res.append((dets, ts, mode, infer_ms, used_roi))

# =========================
# MAIN
# =========================

def main():
    print("Loading YOLO...")
    model = YOLO(MODEL_PATH)

    if torch.cuda.is_available():
        model.to(f"cuda:{DEVICE}")
        if USE_HALF:
            try:
                model.model.half()
            except Exception:
                pass

    if torch.cuda.is_available():
        dummy = np.zeros((IMG_SIZE_ROI, IMG_SIZE_ROI, 3), dtype=np.uint8)
        with torch.inference_mode():
            for _ in range(3):
                _ = model(dummy, conf=CONF, imgsz=IMG_SIZE_ROI, verbose=False, max_det=MAX_DET)

    cap = cv2.VideoCapture(VIDEO_PATH) if USE_VIDEO_FILE else cv2.VideoCapture(CAM_INDEX, CAP_BACKEND)
    if not cap.isOpened():
        print("Capture open failed")
        return

    if SHOW_OUTPUT:
        cv2.namedWindow(WINDOW_NAME, cv2.WINDOW_NORMAL)

    yolo_worker = YOLOWorker(model)
    yolo_worker.start()

    kf = Kalman8D()
    klt = KLTTracker()

    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  # single-target lock over ByteTrack tracks

    locked_box_eff = None
    confirmed = False
    hit_streak = 0
    miss_streak = 0
    ref_hist = None

    prev_ts = None
    frame_id = 0
    last_det_frame = -999
    last_fullscan_frame = -999

    yolo_no_det = 0
    last_yolo_ok_ts = -1e9

    perf_hist = deque(maxlen=120)
    yolo_hist = deque(maxlen=120)

    print("Start")

    while True:
        ret, frame_orig = cap.read()
        ts = time.perf_counter()
        if not ret:
            break

        frame_eff, sx, sy = get_effective_frame(frame_orig)
        eh, ew = frame_eff.shape[:2]

        # dt
        if prev_ts is None:
            dt = 1.0 / 25.0
        else:
            dt = float(max(1e-3, ts - prev_ts))
        prev_ts = ts

        iter_start = time.perf_counter()

        # 1) Kalman predict
        pred_box_eff = None
        if kf.initialized:
            kf.predict(dt)
            pred_box_eff = clip_box(kf.to_box(), ew, eh)

        # speed/unc (effective px)
        speed = 0.0
        unc = 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()

        # 2) KLT update (effective) with validity gates
        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)

            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

        # 3) Adaptive detect schedule
        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)

        yolo_fresh = (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

        # 4) Submit YOLO (effective)
        if run_det:
            roi_box = None
            if (not need_fullscan) and kf.initialized:
                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

                radius = BASE_RADIUS + SPEED_RADIUS_FACTOR * vnorm
                radius = clamp(radius, BASE_RADIUS, MAX_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

                roi_box = clip_box([scx - rx, scy - ry, scx + rx, scy + ry], ew, eh)

            mode = "ROI" if roi_box is not None else "FULL"
            if need_fullscan:
                roi_box = None
                mode = "FULL"
                last_fullscan_frame = frame_id

            yolo_worker.submit(frame_eff, roi_box, mode, ts)
            last_det_frame = frame_id

        # 5) Consume YOLO result
        # 5) Consume YOLO result
        yolo = yolo_worker.try_get()
        dets_eff = []
        infer_ms = 0.0
        used_roi = False
        if yolo is not None:
            dets_eff, ts_det, mode, infer_ms, used_roi = yolo
            yolo_hist.append(infer_ms)

            if len(dets_eff) == 0:
                yolo_no_det += 1
            else:
                yolo_no_det = 0
                last_yolo_ok_ts = ts

        # ---- DEBUG: print one detection sample sometimes ----
        if DEBUG and dets_eff and (frame_id % 30 == 0):
            print("sample det:", dets_eff[0])

        # 6) ByteTrack update + choose single target
        dets_np = np.array(dets_eff, dtype=np.float32) if dets_eff else np.zeros((0, 5), dtype=np.float32)
        tracks = bt.update(dets_np, dt=dt)

        chosen = None
        target_track = None

        # keep current target_id if possible
        if target_id is not None:
            for t in tracks:
                if t.track_id == target_id:
                    target_track = t
                    break

        # reacquire if needed
        if target_track is None and len(tracks) > 0:
            if kf.initialized:
                pred = clip_box(kf.to_box(), ew, eh)
                pc = box_center(pred)
                best = None
                best_score = -1e9
                for t in tracks:
                    b = clip_box(t.tlbr, ew, eh)
                    c = box_center(b)
                    dist = float(np.linalg.norm(c - pc))
                    i = iou(b, pred)
                    s = (2.0 * i) + (1.0 / (1.0 + dist)) + 0.2 * float(t.score)
                    if s > best_score:
                        best_score = s
                        best = t
                target_track = best
            else:
                target_track = max(tracks, key=lambda tt: box_area(tt.tlbr))

        # apply chosen
        if target_track is not None:
            target_id = target_track.track_id
            chosen = clip_box(target_track.tlbr, ew, eh)

        # 7) Update Kalman / FSM using chosen (same logic)
        if chosen is not None:
            if not kf.initialized:
                kf.init_from_box(chosen)
            else:
                cx, cy = box_center(chosen)
                bw, bh = box_wh(chosen)
                kf.update([cx, cy, bw, bh])

            locked_box_eff = chosen
            hit_streak += 1
            miss_streak = 0

            if not confirmed and hit_streak >= CONFIRM_HITS:
                confirmed = True
                klt.init(frame_eff, locked_box_eff)
                ref_hist = compute_hsv_hist(frame_eff, locked_box_eff) if USE_HSV_GATE else None

        else:
            if not (confirmed and klt_valid):
                miss_streak += 1
                hit_streak = 0

            if confirmed and (yolo_no_det >= YOLO_NO_DET_LIMIT) and (not klt_valid):
                confirmed = False
                locked_box_eff = None
                ref_hist = None
                target_id = None
                klt.reset()
                miss_streak = 0
                hit_streak = 0
                yolo_no_det = 0

            elif miss_streak >= MAX_MISSES:
                confirmed = False
                locked_box_eff = None
                ref_hist = None
                target_id = None
                klt.reset()

        # 8) Draw on ORIGINAL frame (map boxes back)
        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])

        # 8.1 Draw YOLO dets (green)
        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)

        # 8.2 Draw ByteTrack tracks (cyan)
        if DRAW_BT_TRACKS and tracks:
            for t in tracks:
                if BT_DRAW_ONLY_CONFIRMED and (t.hits < BT_MIN_HITS):
                    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}",
                            (x1, max(0, y1 - 8)),
                            cv2.FONT_HERSHEY_SIMPLEX, 0.6, (255, 255, 0), 2)

        # 8.3 Draw locked target (red)
        if DRAW_LOCK_BOX and locked_box_orig is not None:
            x1, y1, x2, y2 = map(int, locked_box_orig)
            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)

        # 8.4 Draw Kalman point (blue)
        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 DRAW_ALL_BOXES and dets_eff:
            for d in dets_eff:
                b = d[:4]
                bo = unscale_box(b, sx, sy)
                x1, y1, x2, y2 = map(int, bo)
                cv2.rectangle(frame_orig, (x1, y1), (x2, y2), (0, 255, 0), 1)

        status = "CONFIRMED" if confirmed else "RECOVER"
        cv2.putText(frame_orig, status, (20, 40), cv2.FONT_HERSHEY_SIMPLEX, 1.0, (0, 255, 255), 2)

        if DEBUG:
            cv2.putText(frame_orig, f"eff={ew}x{eh} miss={miss_streak} hit={hit_streak} detEvery={det_every} yNoDet={yolo_no_det}",
                        (20, 75), cv2.FONT_HERSHEY_SIMPLEX, 0.7, (0, 255, 255), 2)
            cv2.putText(frame_orig, f"YOLO dets={len(dets_eff)} infer={infer_ms:.1f}ms mode={'ROI' if used_roi else 'FULL'}",
                        (20, 105), cv2.FONT_HERSHEY_SIMPLEX, 0.7, (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",
                        (20, 135), cv2.FONT_HERSHEY_SIMPLEX, 0.7, (0, 255, 255), 2)

        # show / pace
        if SHOW_OUTPUT:
            cv2.imshow(WINDOW_NAME, frame_orig)

            iter_ms = (time.perf_counter() - iter_start) * 1000.0
            perf_hist.append(iter_ms)

            target_dt = 1.0 / float(max(1, TARGET_OUT_FPS))
            elapsed = time.perf_counter() - iter_start
            wait_ms = 1
            if VIDEO_REALTIME and elapsed < target_dt:
                wait_ms = int((target_dt - elapsed) * 1000.0)
                wait_ms = clamp(wait_ms, 1, 50)

            if cv2.waitKey(int(wait_ms)) == 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

    yolo_worker.stop()
    cap.release()
    cv2.destroyAllWindows()

if __name__ == "__main__":
    main()