Kalman filtr va Venger algoritmi: SORT ni noldan yozamiz

import time
from pathlib import Path

import cv2
import numpy as np
import pandas as pd
import torch
import torchvision
import matplotlib.pyplot as plt
from scipy.optimize import linear_sum_assignment
from ultralytics.trackers.utils.kalman_filter import KalmanFilterXYWH

MOT = Path("datasets/MOT17/train/MOT17-09-FRCNN")
d = np.loadtxt("out/mot17/MOT17-09_yolo11n_1280.txt", delimiter=",")     # 9-bobda saqlangan box lar
det = {k: np.c_[d[d[:, 0] == k][:, 2:4], d[d[:, 0] == k][:, 2:4] + d[d[:, 0] == k][:, 4:6], d[d[:, 0] == k][:, 6]]
       for k in range(1, 526)}                                           # kadr -> (n, 5): x1 y1 x2 y2 ishonch
gt = np.loadtxt(MOT / "gt" / "gt.txt", delimiter=",")
piyoda = gt[(gt[:, 7] == 1) & (gt[:, 6] == 1)]

def iou_matritsa(a, b):
    return torchvision.ops.box_iou(torch.tensor(np.asarray(a)[:, :4], dtype=torch.float64),
                                   torch.tensor(np.asarray(b)[:, :4], dtype=torch.float64)).numpy()

def xyxy_to_xywh(b):
    return np.array([(b[0] + b[2]) / 2, (b[1] + b[3]) / 2, b[2] - b[0], b[3] - b[1]])

def xywh_to_xyxy(x):
    return np.array([x[0] - x[2] / 2, x[1] - x[3] / 2, x[0] + x[2] / 2, x[1] + x[3] / 2])

print("kadrlar:", len(det), "| box lar:", len(d), "| GT piyoda box lari:", len(piyoda))

class Kalman:
    """O'zgarmas tezlikli Kalman filtr, holat [cx, cy, w, h, vx, vy, vw, vh]. Shovqinlar ultralytics dagi kabi."""
    def __init__(self, z):
        self.F = np.eye(8)
        self.F[:4, 4:] = np.eye(4)                                      # joy += tezlik (bir kadrda)
        self.H = np.eye(4, 8)                                           # faqat joy va o'lcham o'lchanadi
        w, h = z[2], z[3]
        self.x = np.r_[z, np.zeros(4)]                                  # boshida tezlik noma'lum -> 0
        self.P = np.diag(np.square([w / 10, h / 10, w / 10, h / 10, w / 16, h / 16, w / 16, h / 16]))

    def bashorat(self):
        w, h = self.x[2], self.x[3]
        Q = np.diag(np.square([w / 20, h / 20, w / 20, h / 20, w / 160, h / 160, w / 160, h / 160]))
        self.x = self.F @ self.x
        self.P = self.F @ self.P @ self.F.T + Q

    def tuzatish(self, z):
        w, h = self.x[2], self.x[3]
        R = np.diag(np.square([w / 20, h / 20, w / 20, h / 20]))
        S = self.H @ self.P @ self.H.T + R
        K = self.P @ self.H.T @ np.linalg.inv(S)
        self.x = self.x + K @ (z - self.H @ self.x)
        self.P = self.P - K @ S @ K.T

t1 = piyoda[piyoda[:, 1] == 1]
t1 = t1[np.argsort(t1[:, 0])]
olchov = {}
for q in t1:
    k = int(q[0])
    D = det[k][det[k][:, 4] >= 0.25]
    if len(D):
        m = iou_matritsa([[q[2], q[3], q[2] + q[4], q[3] + q[5]]], D)[0]
        if m.max() >= 0.5:
            olchov[k] = xyxy_to_xywh(D[m.argmax()])
kadrlar = list(range(int(t1[0, 0]), int(t1[-1, 0]) + 1))
print("trek kadrlari:", len(kadrlar), "| o'lchov bor:", len(olchov), "| o'lchovsiz 1-100 oralig'ida:",
      [k for k in kadrlar[:100] if k not in olchov])

bizniki, ultra = Kalman(olchov[kadrlar[0]]), KalmanFilterXYWH()
mean, cov = ultra.initiate(olchov[kadrlar[0]])
tarix, farq_x, farq_p = {}, 0.0, 0.0
for k in kadrlar[1:]:
    bizniki.bashorat()
    mean, cov = ultra.predict(mean, cov)
    if k in olchov:
        bizniki.tuzatish(olchov[k])
        mean, cov = ultra.update(mean, cov, olchov[k])
    tarix[k] = bizniki.x.copy()
    farq_x, farq_p = max(farq_x, np.abs(bizniki.x - mean).max()), max(farq_p, np.abs(bizniki.P - cov).max())
print(f"ultralytics bilan eng katta farq: holat {farq_x:.1e}, kovariatsiya {farq_p:.1e}")

gt_c = {int(q[0]): (q[2] + q[4] / 2, q[3] + q[5] / 2) for q in t1}
_, axes = plt.subplots(1, 2, figsize=(12, 3.8))
for ax, i, nom in zip(axes, (0, 1), ("markaz x, px", "markaz y, px")):
    ax.plot(kadrlar[1:], [gt_c[k][i] for k in kadrlar[1:]], color="gray", lw=4, alpha=0.5, label="GT")
    ax.scatter(list(olchov), [z[i] for z in olchov.values()], s=6, color="#2563eb", label="detector (o'lchov)")
    ax.plot(list(tarix), [x[i] for x in tarix.values()], color="#dc2626", lw=1.2, label="Kalman")
    ax.axvspan(34.5, 49.5, color="orange", alpha=0.15)
    ax.set_xlim(1, 100)
    oyna = [gt_c[k][i] for k in kadrlar[1:100]]                  # y o'qini shu 100 kadrga moslaymiz
    ax.set_ylim(min(oyna) - 40, max(oyna) + 40)
    ax.set_xlabel("kadr")
    ax.set_title(nom, fontsize=10)
axes[0].legend(fontsize=8)
fig("kalman_1trek.png", "1-trekning birinchi 100 kadri. To'q sariq oraliq — detector odamni ko'rmagan kadrlar: Kalman faqat bashorat qiladi.")

bosh, oxir = 34, 49                                              # oxirgi o'lchov va o'lchovsiz oxirgi kadr
print(f"{oxir}-kadrda GT markazidan uzoqlik: Kalman bashorati {np.hypot(*(tarix[oxir][:2] - gt_c[oxir])):.0f} px, "
      f"'oxirgi ko'rilgan joy' {np.hypot(*(olchov[bosh][:2] - gt_c[oxir])):.0f} px")

M = np.array([[0.60, 0.55],                                     # A trek: 1-box bilan 0.60, 2-box bilan 0.55
              [0.50, 0.00]])                                     # B trek: 1-box bilan 0.50, 2-box bilan tegmaydi
print("ochko'z: avval eng katta IoU -> A-1 (0.60). Qolgan juftlik B-2 = 0.00 -> B ulanmay qoladi, jami IoU 0.60")
r, c = linear_sum_assignment(1 - M)
print("optimal:", [f"{'AB'[i]}-{j + 1} ({M[i, j]:.2f})" for i, j in zip(r, c)], f"jami IoU {M[r, c].sum():.2f}")

def vengriya(C):
    """Minimal yig'indili tayinlash. C: (n, m), n <= m. Qaytaradi: har qator uchun tanlangan ustun."""
    n, m = C.shape
    u, v = np.zeros(n + 1), np.zeros(m + 1)                       # potensiallar
    p = np.zeros(m + 1, dtype=int)                                # p[j] — j-ustunga biriktirilgan qator (1 dan)
    yol = np.zeros(m + 1, dtype=int)                              # almashtirish yo'lini qayta tiklash uchun
    for i in range(1, n + 1):
        p[0], j0 = i, 0
        minv, band = np.full(m + 1, np.inf), np.zeros(m + 1, dtype=bool)
        while True:                                               # bo'sh ustun topilguncha yo'lni kengaytiramiz
            band[j0] = True
            i0, delta, j1 = p[j0], np.inf, 0
            for j in range(1, m + 1):
                if not band[j]:
                    narx = C[i0 - 1, j - 1] - u[i0] - v[j]
                    if narx < minv[j]:
                        minv[j], yol[j] = narx, j0
                    if minv[j] < delta:
                        delta, j1 = minv[j], j
            for j in range(m + 1):                                # potensiallarni yangilash
                if band[j]:
                    u[p[j]] += delta
                    v[j] -= delta
                else:
                    minv[j] -= delta
            j0 = j1
            if p[j0] == 0:
                break
        while j0:                                                 # yo'l bo'ylab qaytib, juftlarni almashtiramiz
            j1 = yol[j0]
            p[j0] = p[j1]
            j0 = j1
    javob = np.full(n, -1)
    for j in range(1, m + 1):
        if p[j]:
            javob[p[j] - 1] = j - 1
    return javob

rng = np.random.default_rng(0)
eng_katta_farq = 0.0
for _ in range(500):
    n = int(rng.integers(1, 12))
    C = rng.random((n, int(rng.integers(n, 16))))
    r, c = linear_sum_assignment(C)
    eng_katta_farq = max(eng_katta_farq, abs(C[np.arange(n), vengriya(C)].sum() - C[r, c].sum()))
print("500 ta tasodifiy matritsa: scipy bilan umumiy narxdagi eng katta farq:", eng_katta_farq)

C = rng.random((40, 40))
t0 = time.perf_counter(); vengriya(C); t_biz = time.perf_counter() - t0
t0 = time.perf_counter(); linear_sum_assignment(C); t_scipy = time.perf_counter() - t0
print(f"40 x 40: bizning Python kod {t_biz * 1000:.1f} ms, scipy (C da yozilgan) {t_scipy * 1000:.3f} ms")

class Trek:
    def __init__(self, tid, box):
        self.id, self.kalman, self.box = tid, Kalman(xyxy_to_xywh(box)), box
        self.tasdiq, self.yoqolgan = 1, 0                        # necha marta ulangan, necha kadr ko'rinmagan

def mini_sort(det, kalman=True, venger=True, max_yoqolish=30, min_tasdiq=3, min_ishonch=0.25, min_iou=0.3):
    treklar, natija, oxirgi_id = [], [], 0
    for k in sorted(det):
        D = det[k][det[k][:, 4] >= min_ishonch]
        for t in treklar:
            if kalman:
                t.kalman.bashorat()
        taxmin = [xywh_to_xyxy(t.kalman.x[:4]) if kalman else t.box for t in treklar]
        juftlar = []
        if treklar and len(D):
            M = iou_matritsa(taxmin, D)
            if venger:
                r, c = linear_sum_assignment(1 - M)
                juftlar = [(i, j) for i, j in zip(r, c) if M[i, j] >= min_iou]
            else:                                                 # 9-bobdagi ochko'z ulash
                band_t, band_d = set(), set()
                for i, j in sorted(zip(*np.nonzero(M >= min_iou)), key=lambda juft: -M[juft]):
                    if i not in band_t and j not in band_d:
                        juftlar.append((i, j))
                        band_t.add(i)
                        band_d.add(j)
        ulangan_t, ulangan_d = {i for i, _ in juftlar}, {j for _, j in juftlar}
        for i, j in juftlar:
            t = treklar[i]
            t.kalman.tuzatish(xyxy_to_xywh(D[j]))
            t.box, t.tasdiq, t.yoqolgan = D[j, :4], t.tasdiq + 1, 0
        for i, t in enumerate(treklar):
            if i not in ulangan_t:
                t.yoqolgan += 1
        treklar = [t for t in treklar if t.yoqolgan <= max_yoqolish]
        for j in range(len(D)):
            if j not in ulangan_d:
                oxirgi_id += 1
                treklar.append(Trek(oxirgi_id, D[j, :4]))
        for t in treklar:
            if t.yoqolgan == 0 and t.tasdiq >= min_tasdiq:
                natija.append((k, t.id, *(xywh_to_xyxy(t.kalman.x[:4]) if kalman else t.box)))
    return np.array(natija)

def id_tahlili(trek, piyoda):
    """Har GT odamga berilgan ID lar, ID almashishlari soni va topilgan GT box lar soni."""
    idlar, oxirgi, almashish, topildi = {}, {}, 0, 0
    for k in np.unique(piyoda[:, 0]).astype(int):
        G, P = piyoda[piyoda[:, 0] == k], trek[trek[:, 0] == k]
        if not len(P):
            continue
        M = iou_matritsa(np.c_[G[:, 2], G[:, 3], G[:, 2] + G[:, 4], G[:, 3] + G[:, 5]], P[:, 2:6])
        juftlar, band_g, band_p = [], set(), set()
        for a in range(len(G)):
            b = np.nonzero(P[:, 1] == oxirgi.get(int(G[a, 1]), -1))[0]
            if len(b) and b[0] not in band_p and M[a, b[0]] >= 0.5:
                juftlar.append((a, b[0]))
                band_g.add(a)
                band_p.add(b[0])
        for a, b in sorted(zip(*np.nonzero(M >= 0.5)), key=lambda juft: -M[juft]):
            if a not in band_g and b not in band_p:
                juftlar.append((a, b))
                band_g.add(a)
                band_p.add(b)
        for a, b in juftlar:
            gid, pid = int(G[a, 1]), int(P[b, 1])
            topildi += 1
            idlar.setdefault(gid, []).append((k, pid))
            if gid in oxirgi and oxirgi[gid] != pid:
                almashish += 1
            oxirgi[gid] = pid
    return idlar, almashish, topildi

bosqichlar = [("9-bob: oddiy tracker",         dict(kalman=False, venger=False, max_yoqolish=0, min_tasdiq=1)),
              ("+ Venger ulash",               dict(kalman=False, venger=True, max_yoqolish=0, min_tasdiq=1)),
              ("+ Kalman bashorati",           dict(kalman=True, venger=True, max_yoqolish=0, min_tasdiq=1)),
              ("+ xotira: 30 kadr (1 s)",      dict(kalman=True, venger=True, max_yoqolish=30, min_tasdiq=1)),
              ("+ tasdiq: 3 kadr",             dict(kalman=True, venger=True, max_yoqolish=30, min_tasdiq=3)),
              ("+ ishonch chegarasi 0.4",      dict(kalman=True, venger=True, max_yoqolish=30, min_tasdiq=3, min_ishonch=0.4))]
qatorlar, natijalar = [], {}
for nom, sozlama in bosqichlar:
    t0 = time.perf_counter()
    trek = mini_sort(det, **sozlama)
    ms = (time.perf_counter() - t0) / len(det) * 1000
    idlar, almashish, topildi = id_tahlili(trek, piyoda)
    natijalar[nom] = idlar
    qatorlar.append({"bosqich": nom, "ID lar": len(np.unique(trek[:, 1])), "ID almashishi": almashish,
                     "bir odamga ID": np.mean([len({p for _, p in v}) for v in idlar.values()]),
                     "topilgan GT, %": topildi / len(piyoda) * 100, "tracker, ms/kadr": ms})
show(pd.DataFrame(qatorlar).round(2))

idlar = natijalar[bosqichlar[-1][0]]
_, ax = plt.subplots(figsize=(11, 6))
for qator, gid in enumerate(sorted(idlar)):
    k, p = np.array(idlar[gid]).T
    ax.scatter(k, np.full(len(k), qator), c=plt.cm.tab20(p % 20), s=6, marker="s")
ax.set_yticks(range(len(idlar)))
ax.set_yticklabels([f"GT {g}" for g in sorted(idlar)], fontsize=7)
ax.set_xlabel("kadr")
ax.set_xlim(0, 526)
fig("sort_idlar.png", "Mini-SORT (oxirgi bosqich): har qator — bitta haqiqiy odam, rang — tracker bergan ID.")

yozuvlar = sorted((k, gid, pid) for gid, v in idlar.items() for k, pid in v)   # (kadr, GT odam, tracker ID)
egasi, hodisa = {}, None                                          # tracker ID si -> uni oxirgi olgan GT odam
for k, gid, pid in yozuvlar:
    if hodisa is None and pid in egasi and egasi[pid] != gid:
        hodisa = (k, pid, egasi[pid], gid)
    egasi[pid] = gid
k, pid, oldingi_gid, yangi_gid = hodisa
print(f"{k}-kadr: tracker ID {pid} oldin GT {oldingi_gid} ga tegishli edi, endi GT {yangi_gid} ga berildi")

trek = mini_sort(det, **bosqichlar[-1][1])
_, axes = plt.subplots(1, 3, figsize=(13, 4.2))
for ax, kk in zip(axes, (k - 20, k - 8, k)):
    rasm = cv2.imread(str(MOT / "img1" / f"{kk:06d}.jpg"))[:, :, ::-1]
    G = piyoda[(piyoda[:, 0] == k) & np.isin(piyoda[:, 1], [oldingi_gid, yangi_gid])]   # hodisa kadridagi ikki odam
    x1, y1 = G[:, 2].min() - 150, G[:, 3].min() - 60
    x2, y2 = (G[:, 2] + G[:, 4]).max() + 150, (G[:, 3] + G[:, 5]).max() + 60
    ax.imshow(rasm)
    for q in trek[trek[:, 0] == kk]:
        ax.add_patch(plt.Rectangle(q[2:4], q[4] - q[2], q[5] - q[3], fill=False, color="yellow", lw=2))
        ax.text(q[2], q[3], f"ID {int(q[1])}", color="black", fontsize=9, bbox=dict(fc="yellow", ec="none", pad=1), clip_on=True)
    ax.set_xlim(max(x1, 0), min(x2, rasm.shape[1]))
    ax.set_ylim(min(y2, rasm.shape[0]), max(y1, 0))
    ax.set_title(f"{kk}-kadr", fontsize=10)
    ax.axis("off")
fig("almashish.png", "Tracker ID lari (sariq) almashish hodisasidan oldin va keyin.")