Ko'p kamera: bitta maydon, bir necha ko'z

# WILDTRACK (15-bob): 7 kameradan uchtasi (C1, C2, C6), dastlabki 150 ta belgilangan kadr, belgilar va kalibrovka.
# Arxiv 6.8 GB. HTTP Range so'rovlari bilan faqat kerakli fayllarni o'qiymiz (~1 GB).
import io
import struct
import urllib.request
import zlib


class HttpFayl(io.RawIOBase):
    """Serverdagi faylning faqat so'ralgan bayt oralig'ini (HTTP Range) yuklaydi — zipfile uni oddiy fayl deb o'qiydi."""
    def __init__(self, url):
        self.url, self.pos = url, 0
        with urllib.request.urlopen(urllib.request.Request(url, method="HEAD")) as r:
            self.size = int(r.headers["Content-Length"])

    def readable(self):
        return True

    def seekable(self):
        return True

    def tell(self):
        return self.pos

    def seek(self, offset, whence=0):
        self.pos = {0: offset, 1: self.pos + offset, 2: self.size + offset}[whence]
        return self.pos

    def readinto(self, b):
        if self.pos >= self.size:
            return 0
        oxiri = min(self.pos + len(b), self.size) - 1
        with urllib.request.urlopen(urllib.request.Request(self.url, headers={"Range": f"bytes={self.pos}-{oxiri}"})) as r:
            data = r.read()
        b[:len(data)] = data
        self.pos += len(data)
        return len(data)


def ochib_ol(fayl, info, maqsad):
    """Arxiv a'zosini chiqaradi. Bu arxivda ofsetlar 4 GB (2^32) ga siljigan — to'g'risini nom va CRC bo'yicha topamiz."""
    for k in range(3):
        ofset = info.header_offset % 2**32 + k * 2**32
        fayl.seek(ofset)
        sarlavha = fayl.read(30)
        nom_uz, qosh_uz = struct.unpack("<HH", sarlavha[26:30])
        if sarlavha[:4] != b"PK\x03\x04" or fayl.read(nom_uz).decode("utf-8", "replace") != info.filename:
            continue
        fayl.seek(ofset + 30 + nom_uz + qosh_uz)
        data = fayl.read(info.compress_size)
        if info.compress_type == zipfile.ZIP_DEFLATED:
            data = zlib.decompress(data, -15)
        if zlib.crc32(data) != info.CRC:
            raise ValueError(f"CRC mos emas: {info.filename}")
        maqsad.parent.mkdir(parents=True, exist_ok=True)
        maqsad.write_bytes(data)
        return
    raise ValueError(f"arxivda topilmadi: {info.filename}")


wt = root / "wildtrack"
kadrlar = {f"{k * 5:08d}" for k in range(150)}                               # belgilangan kadrlar: 0, 5, 10, ...
if not (wt / "Wildtrack_dataset" / "Image_subsets" / "C6" / "00000745.png").exists():
    arxiv = io.BufferedReader(HttpFayl("https://documents.epfl.ch/groups/c/cv/cvlab-unit/www/data/Wildtrack/Wildtrack_dataset_full.zip"),
                              buffer_size=1 << 20)
    for info in zipfile.ZipFile(arxiv).infolist():
        n = info.filename
        kerak = ("annotations_positions/" in n or "calibrations/" in n
                 or (Path(n).parent.name in ("C1", "C2", "C6") and Path(n).stem in kadrlar))
        if kerak and not info.is_dir() and not n.startswith("__MACOSX") and not (wt / n).exists():
            ochib_ol(arxiv, info, wt / n)

import itertools
import json
import re
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 import YOLO
from ultralytics.utils import LOGGER

LOGGER.setLevel("ERROR")
WT = Path("datasets/wildtrack/Wildtrack_dataset")
KAMERA_NOMI = {"C1": "CVLab1", "C2": "CVLab2", "C3": "CVLab3", "C4": "CVLab4", "C5": "IDIAP1", "C6": "IDIAP2", "C7": "IDIAP3"}
KAMERALAR = ["C1", "C2", "C6"]
KADRLAR = [k * 5 for k in range(150)]                                  # yuklab olingan kadrlar: 0, 5, ..., 745

def pozitsiya(pid):
    """positionID -> poldagi (X, Y), santimetrda."""
    return np.array([-300 + 2.5 * (pid % 480), -900 + 2.5 * (pid // 480)])

gt = []                                                                  # (kadr, odam, X, Y, {kamera: box})
for k in KADRLAR:
    for p in json.load(open(WT / "annotations_positions" / f"{k:08d}.json")):
        boxlar = {c: (v["xmin"], v["ymin"], v["xmax"], v["ymax"]) for c, v in zip(KAMERA_NOMI, p["views"]) if v["xmin"] >= 0}
        gt.append((k, p["personID"], *pozitsiya(p["positionID"]), boxlar))
G = {k: np.array([g[1:4] for g in gt if g[0] == k]) for k in KADRLAR}   # kadr -> (odam, X, Y)
print(f"{len(KADRLAR)} kadr | {len(gt)} odam-kadr | {len({g[1] for g in gt})} xil odam | kadrda o'rtacha {len(gt) / len(KADRLAR):.1f} odam")
show(pd.DataFrame([{"kamera": c, "kalibrovka fayli": n, "ko'rgan odam-kadr": sum(c in g[4] for g in gt),
                    "ulushi, %": 100 * sum(c in g[4] for g in gt) / len(gt)} for c, n in KAMERA_NOMI.items()]).round(1))

def kalibrovka(c):
    fs = cv2.FileStorage(str(WT / "calibrations" / "intrinsic_zero" / f"intr_{KAMERA_NOMI[c]}.xml"), cv2.FILE_STORAGE_READ)
    K = fs.getNode("camera_matrix").mat()
    matn = (WT / "calibrations" / "extrinsic" / f"extr_{KAMERA_NOMI[c]}.xml").read_text()
    rvec, t = (np.array(re.search(f"<{teg}>(.*?)</{teg}>", matn, re.S).group(1).split(), float) for teg in ("rvec", "tvec"))
    return K, cv2.Rodrigues(rvec)[0], t                                 # rvec (burish vektori) -> 3x3 matritsa

kal = {c: kalibrovka(c) for c in KAMERA_NOMI}
K, R, t = kal["C1"]
print("C1: K =\n", K.round(1), "\nt =", t.round(1), "sm")
print("C1 kamerasining dunyodagi joyi (X, Y, balandlik):", (-R.T @ t).round(0), "sm")

def rasmga(c, XYZ):
    K, R, t = kal[c]
    P = (R @ np.asarray(XYZ, float).T).T + t                            # dunyo -> kamera
    uv = (K @ P.T).T
    return uv[:, :2] / uv[:, 2:]                                        # s ga bo'lamiz

qatorlar = []
for c in KAMERA_NOMI:
    q = [(g[2], g[3], g[4][c]) for g in gt if c in g[4]]
    uv = rasmga(c, [(X, Y, 0) for X, Y, _ in q])
    oyoq = np.array([((b[0] + b[2]) / 2, b[3]) for *_, b in q])
    boy = np.array([b[3] - b[1] for *_, b in q])
    xato = np.hypot(*(uv - oyoq).T)
    qatorlar.append({"kamera": c, "box lar": len(q), "farq, px (median)": np.median(xato),
                     "box balandligiga nisbatan, % (median)": 100 * np.median(xato / boy)})
show(pd.DataFrame(qatorlar).round(1))

H = {c: np.linalg.inv(kal[c][0] @ np.c_[kal[c][1][:, :2], kal[c][2]]) for c in KAMERALAR}   # rasm -> pol

def polga(c, uv):
    p = H[c] @ np.c_[uv, np.ones(len(uv))].T
    return (p[:2] / p[2]).T

qatorlar = []
for c in KAMERALAR:
    q = [(g[2], g[3], g[4][c]) for g in gt if c in g[4]]
    xy = polga(c, np.array([((b[0] + b[2]) / 2, b[3]) for *_, b in q]))
    haqiqiy = np.array([(X, Y) for X, Y, _ in q])
    xato = np.hypot(*(xy - haqiqiy).T)
    uzoqlik = np.hypot(*(haqiqiy - (-kal[c][1].T @ kal[c][2])[:2]).T)   # odamdan kameragacha, polda
    qatorlar.append({"kamera": c, "farq, sm (median)": np.median(xato), "90% i shundan kam": np.percentile(xato, 90),
                     "kameradan 10 m gacha": np.median(xato[uzoqlik < 1000]), "10 m dan uzoq": np.median(xato[uzoqlik >= 1000])})
show(pd.DataFrame(qatorlar).round(1))

model = YOLO("weights/yolo11n.pt")
det = {}
for c in KAMERALAR:
    fayl = Path(f"out/wildtrack/{c}_yolo11n_1280.txt")
    if not fayl.exists():
        fayl.parent.mkdir(parents=True, exist_ok=True)
        qatorlar = []
        for k in KADRLAR:
            b = model.predict(WT / "Image_subsets" / c / f"{k:08d}.png", classes=[0], conf=0.25, imgsz=1280, device=0, verbose=False)[0].boxes
            qatorlar += [f"{k},{x[0]:.1f},{x[1]:.1f},{x[2]:.1f},{x[3]:.1f},{x[4]:.4f}"
                         for x in np.c_[b.xyxy.cpu().numpy(), b.conf.cpu().numpy()]]
        fayl.write_text("\n".join(qatorlar) + "\n")
    d = np.loadtxt(fayl, delimiter=",")
    det.update({(c, k): d[d[:, 0] == k][:, 1:] for k in KADRLAR})     # (n, 5): x1 y1 x2 y2 ishonch

def maydonda(xy):
    return (xy[:, 0] >= -300) & (xy[:, 0] < 900) & (xy[:, 1] >= -900) & (xy[:, 1] < 2700)

pol = {}
for (c, k), d in det.items():
    xy = polga(c, np.c_[(d[:, 0] + d[:, 2]) / 2, d[:, 3]])
    pol[c, k] = np.c_[xy, d[:, 4], np.arange(len(d))][maydonda(xy)]   # X, Y, ishonch, box raqami

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

def moslash(A, B, r=50):
    """A, B: poldagi nuqtalar (n, 2). Masofasi <= r sm bo'lgan juftlar, Venger bilan."""
    if not len(A) or not len(B):
        return []
    D = np.hypot(A[:, None, 0] - B[None, :, 0], A[:, None, 1] - B[None, :, 1])
    q, u = linear_sum_assignment(np.where(D <= r, D, 1e6))
    return [(i, j) for i, j in zip(q, u) if D[i, j] <= r]

qatorlar = []
for c in KAMERALAR:
    kor = rasmda = polda = soxta = nuqta = 0
    for k in KADRLAR:
        q = [(g[1], g[4][c]) for g in gt if g[0] == k and c in g[4]]
        kor += len(q)
        if q and len(det[c, k]):
            M = iou_matritsa([b for _, b in q], det[c, k])
            i, j = linear_sum_assignment(-M)
            rasmda += int((M[i, j] >= 0.5).sum())
        m = moslash(pol[c, k][:, :2], G[k][:, 1:])
        polda += len({int(G[k][j, 0]) for _, j in m} & {pid for pid, _ in q})
        soxta += len(pol[c, k]) - len(m)
        nuqta += len(pol[c, k])
    qatorlar.append({"kamera": c, "ko'rinadigan odam-kadr": kor, "rasmda topildi (IoU >= 0.5), %": 100 * rasmda / kor,
                     "polda topildi (<= 50 sm), %": 100 * polda / kor, "poldagi nuqtalar": nuqta, "precision, %": 100 * (1 - soxta / nuqta)})
show(pd.DataFrame(qatorlar).round(1))

bir, turli = [], []
for k in KADRLAR:
    egasi = {(c, i): int(G[k][j, 0]) for c in KAMERALAR for i, j in moslash(pol[c, k][:, :2], G[k][:, 1:])}
    for (ca, i), pa in egasi.items():
        for (cb, j), pb in egasi.items():
            if ca < cb:
                (bir if pa == pb else turli).append(np.hypot(*(pol[ca, k][i, :2] - pol[cb, k][j, :2])))
bir, turli = np.array(bir), np.array(turli)
print(f"bir odam, ikki kamera: {len(bir)} juft, median {np.median(bir):.0f} sm | turli odamlar: {len(turli)} juft, median {np.median(turli):.0f} sm")
show(pd.DataFrame([{"chegara, sm": r, "bir odam: shu masofada, %": 100 * (bir <= r).mean(),
                    "turli odamlar: shu masofada, %": 100 * (turli <= r).mean()} for r in (50, 75, 100, 150)]).round(2))

def birlashtir(k, kameralar, r=75):
    klaster = []                                                        # har biri: {kamera: pol qatori}
    for c in kameralar:
        P = pol[c, k]
        markaz = np.array([np.mean([p[:2] for p in q.values()], 0) for q in klaster]).reshape(-1, 2)
        m = moslash(markaz, P[:, :2], r)
        for i, j in m:
            klaster[i][c] = P[j]
        band = {j for _, j in m}
        klaster += [{c: P[j]} for j in range(len(P)) if j not in band]
    return klaster

def nuqtalar(klaster):
    """Klaster -> (X, Y, ball): o'rtacha joy va ishonchlar yig'indisi."""
    return np.array([[*np.mean([p[:2] for p in q.values()], 0), sum(p[2] for p in q.values())] for q in klaster]).reshape(-1, 3)

JAMI = sum(len(G[k]) for k in KADRLAR)                                   # maydondagi hamma odam-kadr

def pr_egri(kameralar):
    ballar, togri = [], []
    for k in KADRLAR:
        P = nuqtalar(birlashtir(k, kameralar))
        m = dict(moslash(P[:, :2], G[k][:, 1:]))
        ballar += list(P[:, 2])
        togri += [i in m for i in range(len(P))]
    tartib = np.argsort(-np.array(ballar))
    t = np.array(togri)[tartib]
    tp, fp = np.cumsum(t), np.cumsum(~t)
    return tp / JAMI, tp / (tp + fp), np.array(ballar)[tartib]

egri, qatorlar = {}, []
for n in (1, 2, 3):
    for kam in itertools.combinations(KAMERALAR, n):
        nom = " + ".join(kam)
        rec, prec, _ = egri[nom] = pr_egri(list(kam))
        qatorlar.append({"kameralar": nom, "eng yuqori recall, %": 100 * rec[-1],
                         **{f"recall, precision >= {p:.0%}": 100 * rec[prec >= p].max() if (prec >= p).any() else np.nan
                            for p in (0.9, 0.8, 0.7)}})
show(pd.DataFrame(qatorlar).round(1))

_, ax = plt.subplots(figsize=(8, 5))
for nom, (rec, prec, _) in egri.items():
    qalin = nom.count("+") + 1
    ax.plot(rec * 100, prec * 100, lw=0.8 * qalin, label=nom, alpha=0.5 + 0.15 * qalin)
ax.set_xlabel("recall, % (maydondagi hamma odam)")
ax.set_ylabel("precision, %")
ax.set_ylim(40, 101)
ax.legend(fontsize=8)
fig("kop_kamera_pr.png", "Poldagi nuqtalar uchun precision-recall: yupqa — bitta kamera, o'rta — ikkita, qalin — uchala kamera birga.")

k = max(KADRLAR, key=lambda k: sum(len(q) == 3 for q in birlashtir(k, KAMERALAR)))   # uch kamera birga ko'rgan odamlar eng ko'p kadr
klaster = birlashtir(k, KAMERALAR)
ranglar = [r for i, r in enumerate(plt.cm.tab20.colors[0::2] + plt.cm.tab20.colors[1::2]) if i not in (7, 17)]   # kulranglarsiz 18 rang
rang = {}
for q in klaster:
    if len(q) >= 2:
        rang[id(q)] = ranglar[len(rang) % len(ranglar)]
_, ax = plt.subplot_mosaic([KAMERALAR, ["pol"] * 3], figsize=(15, 8.5), height_ratios=[1, 1.15])
for c in KAMERALAR:
    ax[c].imshow(cv2.imread(str(WT / "Image_subsets" / c / f"{k:08d}.png"))[:, :, ::-1])
    for q in klaster:
        if c in q:
            x1, y1, x2, y2 = det[c, k][int(q[c][3]), :4]
            ax[c].add_patch(plt.Rectangle((x1, y1), x2 - x1, y2 - y1, fill=False, lw=2, color=rang.get(id(q), "lightgray")))
    ax[c].set_title(c, fontsize=10)
    ax[c].axis("off")
ax["pol"].add_patch(plt.Rectangle((-900, -300), 3600, 1200, fill=False, color="gray", ls="--"))
ax["pol"].scatter(G[k][:, 2], G[k][:, 1], marker="x", color="black", s=40, label="GT: odamning joyi")
for q in klaster:
    X, Y = np.mean([p[:2] for p in q.values()], 0)
    ax["pol"].scatter(Y, X, s=40 * len(q), color=rang.get(id(q), "lightgray"), edgecolors="black", lw=0.5)
for c in KAMERALAR:
    X, Y, _ = -kal[c][1].T @ kal[c][2]
    ax["pol"].scatter(Y, X, marker="^", s=150, color="red")
    ax["pol"].annotate(c, (Y, X), xytext=(8, -4), textcoords="offset points", color="red")
ax["pol"].set_aspect("equal")
ax["pol"].set_xlabel("Y, sm")
ax["pol"].set_ylabel("X, sm")
ax["pol"].legend(loc="upper right", fontsize=8)
fig("bir_kadr.png", f"{k}-kadr. Yuqorida: har kamerada box lar, bir xil rang — polda bitta klasterga tushgan (bitta odam deb topilgan). Kulrang — faqat bitta kamera ko'rgan. Pastda: maydon (12 x 36 m) tepadan, doira kattaligi — nechta kamera ko'rgani, qizil uchburchak — kameralar.")

yol = {}
for g in gt:
    yol.setdefault(g[1], {})[g[0]] = np.array(g[2:4])
qadam = [np.hypot(*(v[k + 5] - v[k])) for v in yol.values() for k in v if k + 5 in v]
print(f"GT: ketma-ket kadrlar orasida siljish — median {np.median(qadam):.0f} sm, 75% i {np.percentile(qadam, 75):.0f} sm gacha, "
      f"95% i {np.percentile(qadam, 95):.0f} sm gacha")

def pol_tracker(nuqta, r=100, max_yoqolish=4, tezlik=True):
    treklar, natija, oxirgi_id = [], [], 0                              # trek: [id, X, Y, vX, vY, yo'qolgan kadrlar]
    for k in KADRLAR:
        P = nuqta[k][:, :2]
        taxmin = np.array([[t[1] + t[3] * (t[5] + 1), t[2] + t[4] * (t[5] + 1)] if tezlik else t[1:3] for t in treklar]).reshape(-1, 2)
        m = moslash(taxmin, P, r)
        for i, j in m:
            t = treklar[i]
            if tezlik:
                v = (P[j] - np.array(t[1:3])) / (t[5] + 1)
                t[3:5] = list(0.5 * np.array(t[3:5]) + 0.5 * v)      # tezlikni silliqlaymiz
            t[1:3], t[5] = list(P[j]), 0
        ulangan_t, ulangan_p = {i for i, _ in m}, {j for _, j in m}
        for i, t in enumerate(treklar):
            if i not in ulangan_t:
                t[5] += 1
        treklar = [t for t in treklar if t[5] <= max_yoqolish]
        for j in range(len(P)):
            if j not in ulangan_p:
                oxirgi_id += 1
                treklar.append([oxirgi_id, *P[j], 0.0, 0.0, 0])
        natija += [(k, t[0], t[1], t[2]) for t in treklar if t[5] == 0]
    return np.array(natija)

def id_tahlili(trek):
    idlar, oxirgi, almashish, topildi = {}, {}, 0, 0
    for k in KADRLAR:
        T = trek[trek[:, 0] == k]
        for i, j in moslash(G[k][:, 1:], T[:, 2:4]):
            gid, tid = int(G[k][i, 0]), int(T[j, 1])
            topildi += 1
            idlar.setdefault(gid, set()).add(tid)
            almashish += gid in oxirgi and oxirgi[gid] != tid
            oxirgi[gid] = tid
    return {"topilgan odam-kadr, %": 100 * topildi / JAMI, "ID lar": len(np.unique(trek[:, 1])),
            "bir odamga ID": np.mean([len(v) for v in idlar.values()]), "ID almashishi": almashish,
            "almashish / topilgan, %": 100 * almashish / topildi}

qatorlar = []
for kam in (["C1"], KAMERALAR):
    rec, prec, ball = egri[" + ".join(kam)]
    chegara = ball[np.nonzero(prec >= 0.8)[0][-1]]                     # 80% precision dagi ball
    nuqta = {k: P[P[:, 2] >= chegara] for k, P in ((k, nuqtalar(birlashtir(k, kam))) for k in KADRLAR)}
    for tezlik in (False, True):
        qatorlar.append({"kameralar": " + ".join(kam), "tezlik": tezlik, **id_tahlili(pol_tracker(nuqta, tezlik=tezlik))})
show(pd.DataFrame(qatorlar).round(2))

juft = []                                                               # (detection oyog'i u, v, haqiqiy X, Y)
for k in KADRLAR:
    q = [(g[2], g[3], g[4]["C1"]) for g in gt if g[0] == k and "C1" in g[4]]
    d = det["C1", k]
    if q and len(d):
        M = iou_matritsa([b for *_, b in q], d)
        i, j = linear_sum_assignment(-M)
        juft += [((d[b, 0] + d[b, 2]) / 2, d[b, 3], q[a][0], q[a][1]) for a, b in zip(i, j) if M[a, b] >= 0.5]
D = np.array(juft)
belgilar = np.array([((g[4]["C1"][0] + g[4]["C1"][2]) / 2, g[4]["C1"][3], g[2], g[3]) for g in gt if "C1" in g[4]])
print("sinov: C1 dagi", len(D), "ta detection")

print(f"to'liq kalibrovka (K, R, t) bilan xato: median {np.median(np.hypot(*(polga('C1', D[:, :2]) - D[:, 2:]).T)):.1f} sm")

rng = np.random.default_rng(0)
qatorlar = []
for n in (4, 8, 20):
    xato = []
    for _ in range(50):
        i = rng.choice(len(belgilar), n, replace=False)
        Hn, _ = cv2.findHomography(belgilar[i, :2], belgilar[i, 2:])     # rasm -> pol, eng kichik kvadratlar
        xato.append(np.median(np.hypot(*(cv2.perspectiveTransform(D[None, :, :2], Hn)[0] - D[:, 2:]).T)))
    qatorlar.append({"pol xaritasi": f"{n} ta belgidan", "xato, sm (median)": np.median(xato), "eng yomon 10% tanlovda": np.percentile(xato, 90)})
show(pd.DataFrame(qatorlar).round(1))