forked from Dan4ick/Lidar_Muxa
187 lines
8.6 KiB
Python
187 lines
8.6 KiB
Python
"""Оценка на синтетике организаторов: какие из вставленных предметов найдены.
|
||
|
||
Разметки к `cloud_with_fake_obj` нет, но она и не нужна: вставленные точки не
|
||
лежат на элевациях колец (отклонение больше 0.002°, у настоящих — до 0.0001°).
|
||
Кластеры таких точек, связанные от кадра к кадру по дальности, и есть эталон:
|
||
предметы нумеруются в порядке появления, то есть в порядке из объявления
|
||
организаторов. Положение вдоль пути для связывания не годится — на остановке
|
||
одометрия замирает и соседние предметы сливаются. Методика — EXPERIMENTS п. 18.2.
|
||
|
||
python tools/eval_org_synth.py
|
||
python tools/eval_org_synth.py --set half_width=1.6 --set h_top=0
|
||
python tools/eval_org_synth.py --no-memory --out run.json
|
||
|
||
Эталон кэшируется в `data/cache/<бэг>_truth.json`.
|
||
"""
|
||
from __future__ import annotations
|
||
|
||
import argparse
|
||
import json
|
||
import time
|
||
from pathlib import Path
|
||
|
||
import numpy as np
|
||
|
||
import _bootstrap as B
|
||
import _params as PS
|
||
from flyguard.bag import Bag
|
||
from flyguard.mbon_readout import MbonReadout
|
||
from flyguard.mushroom_body import MushroomBody
|
||
from flyguard.pipeline import FlyGuard, Params
|
||
from flyguard.retina import _ring_elevations, _xyz64
|
||
|
||
# объявление организаторов, 24.09; False — предмет вне габарита, тревоги быть не должно
|
||
OBJECTS = [("2×2 посередине", True), ("0.3 посередине", True), ("0.3 на рельсе", True),
|
||
("0.3 у края", True), ("0.3 вне габарита", False), ("2×2 у края внутри", True),
|
||
("2×2 вне габарита", False), ("2×2 сверху", True), ("2×0.2 на рельсах", True),
|
||
("стержень с потолка", True)]
|
||
OFF_RING_DEG = 0.002
|
||
MATCH_D = 4.0
|
||
|
||
|
||
def truth(bag_path: Path) -> list[dict]:
|
||
"""Кластеры вставленных точек по кадрам."""
|
||
ref = [pc for _, pc in Bag(bag_path).frames(start=1100, stop=1106)]
|
||
asc = np.sort(_ring_elevations(ref))
|
||
rows, t0 = [], None
|
||
for k, (_, pc) in enumerate(Bag(bag_path).frames()):
|
||
t0 = pc.stamp if t0 is None else t0
|
||
x, y, z, ok = _xyz64(pc)
|
||
x, y, z = x[ok], y[ok], z[ok]
|
||
el = np.degrees(np.arctan2(z, np.hypot(x, y)))
|
||
i = np.clip(np.searchsorted(asc, el), 1, asc.size - 1)
|
||
off = np.minimum(np.abs(el - asc[i - 1]), np.abs(asc[i] - el)) > OFF_RING_DEG
|
||
objs = []
|
||
if off.any():
|
||
fx, fd, fz = x[off], -y[off], z[off]
|
||
o = np.argsort(fd)
|
||
fx, fd, fz = fx[o], fd[o], fz[o]
|
||
cut = np.flatnonzero(np.diff(fd) > 3.0) + 1
|
||
for a, b in zip(np.r_[0, cut], np.r_[cut, fd.size]):
|
||
objs.append(dict(d=float(np.median(fd[a:b])), x0=float(fx[a:b].min()),
|
||
x1=float(fx[a:b].max()), z0=float(fz[a:b].min()),
|
||
z1=float(fz[a:b].max()), n=int(b - a)))
|
||
rows.append(dict(k=k, t=pc.stamp - t0, objs=objs))
|
||
return rows
|
||
|
||
|
||
def link(rows: list[dict]) -> list[dict]:
|
||
"""Кластеры → предметы: связь с треком из последних кадров по дальности."""
|
||
tracks = []
|
||
for r in rows:
|
||
for o in r["objs"]:
|
||
if o["n"] < 3 or o["d"] > 130:
|
||
continue
|
||
best = None
|
||
for t in tracks:
|
||
if 0 < r["k"] - t["last_k"] <= 15 and -3.0 < t["last_d"] - o["d"] < 25.0:
|
||
if best is None or abs(t["last_d"] - o["d"]) < abs(best["last_d"] - o["d"]):
|
||
best = t
|
||
if best is None:
|
||
best = {"frames": {}}
|
||
tracks.append(best)
|
||
best["frames"][r["k"]] = o
|
||
best["last_k"], best["last_d"] = r["k"], o["d"]
|
||
tracks = [t for t in tracks if len(t["frames"]) >= 8]
|
||
tracks.sort(key=lambda t: min(t["frames"]))
|
||
return tracks
|
||
|
||
|
||
def run(bag_path: Path, params: Params, memory, readout) -> list[dict]:
|
||
fg = FlyGuard(params, memory=memory, readout=readout)
|
||
out, t0 = [], None
|
||
for k, (_, pc) in enumerate(Bag(bag_path).frames()):
|
||
t0 = pc.stamp if t0 is None else t0
|
||
res = fg.process(pc)
|
||
if res is None:
|
||
continue
|
||
out.append(dict(k=k, t=pc.stamp - t0, ms=res.total_ms,
|
||
objs=[dict(d=o.distance, u=o.lateral, h=o.height, w=o.width,
|
||
sv=o.size_v, p=o.confidence, tid=o.track_id)
|
||
for o in res.decision.objects]))
|
||
return out
|
||
|
||
|
||
def score(tracks: list[dict], det: list[dict]) -> tuple[int, list, list]:
|
||
by_k = {r["k"]: r for r in det}
|
||
hits = {i: [] for i in range(len(tracks))}
|
||
used, tid_obj = set(), {}
|
||
for i, t in enumerate(tracks):
|
||
for k, o in t["frames"].items():
|
||
for j, ob in enumerate(by_k.get(k, {}).get("objs", [])):
|
||
if abs(ob["d"] - o["d"]) < MATCH_D:
|
||
hits[i].append((k, by_k[k]["t"], ob))
|
||
used.add((k, j))
|
||
tid_obj.setdefault(ob["tid"], i)
|
||
false = []
|
||
for r in det:
|
||
for j, ob in enumerate(r["objs"]):
|
||
if (r["k"], j) in used:
|
||
continue
|
||
if ob["tid"] in tid_obj: # тот же трек вплотную, где эталон уже кончился
|
||
hits[tid_obj[ob["tid"]]].append((r["k"], r["t"], ob))
|
||
else:
|
||
false.append((r["k"], r["t"], ob))
|
||
lines, ok = [], 0
|
||
for i, t in enumerate(tracks):
|
||
name, obst = OBJECTS[i] if i < len(OBJECTS) else ("?", True)
|
||
hs = hits[i]
|
||
found = bool(hs)
|
||
ok += found == obst
|
||
verdict = ("найден" if found else "ПРОПУЩЕН") if obst else \
|
||
("ЛОЖНАЯ тревога" if found else "верно молчит")
|
||
far = max(o["d"] for o in t["frames"].values())
|
||
first = max((h[2]["d"] for h in hs), default=float("nan"))
|
||
pmax = max((h[2]["p"] for h in hs), default=0.0)
|
||
lines.append(f"{i + 1:>2} {name:20s} виден с {far:5.0f} м | кадров {len(hs):4d}, "
|
||
f"первое на {first:6.1f} м, p ≤ {pmax:.2f} | {verdict}")
|
||
return ok, lines, false
|
||
|
||
|
||
def main() -> None:
|
||
ap = argparse.ArgumentParser(description=__doc__)
|
||
ap.add_argument("--bag", default=str(B.DATA / "cloud_with_fake_obj"))
|
||
ap.add_argument("--memory", default=str(B.ARTIFACTS / "mushroom_body.npz"))
|
||
ap.add_argument("--mbon", default=str(B.ARTIFACTS / "mbon_readout.npz"))
|
||
ap.add_argument("--no-memory", action="store_true")
|
||
ap.add_argument("--out", default="", help="сохранить покадровые решения (json)")
|
||
PS.add_argument(ap)
|
||
args = ap.parse_args()
|
||
|
||
bag = Path(args.bag)
|
||
cache = B.CACHE / f"{bag.name}_truth.json"
|
||
if cache.exists():
|
||
rows = json.loads(cache.read_text(encoding="utf-8"))
|
||
else:
|
||
t = time.time()
|
||
rows = truth(bag)
|
||
cache.parent.mkdir(parents=True, exist_ok=True)
|
||
cache.write_text(json.dumps(rows), encoding="utf-8")
|
||
print(f"эталон собран за {time.time() - t:.0f} с → {cache}")
|
||
tracks = link(rows)
|
||
|
||
params = Params(**PS.apply({}, args.set, Params))
|
||
memory = None if args.no_memory else MushroomBody.load(args.memory)
|
||
readout = MbonReadout.load(args.mbon) if args.mbon else None
|
||
det = run(bag, params, memory, readout)
|
||
if args.out:
|
||
Path(args.out).write_text(json.dumps(det, ensure_ascii=False), encoding="utf-8")
|
||
|
||
ok, lines, false = score(tracks, det)
|
||
print("\n".join(lines))
|
||
print(f"верно по {ok} из {len(tracks)}")
|
||
ftr = {}
|
||
for k, t, ob in false:
|
||
ftr.setdefault(ob["tid"], []).append((t, ob))
|
||
print(f"ложных треков вне эталона: {len(ftr)} (кадров {len(false)})")
|
||
for lst in sorted(ftr.values(), key=lambda v: v[0][0]):
|
||
ds = [ob["d"] for _, ob in lst]
|
||
ob = lst[len(lst) // 2][1]
|
||
print(f" {lst[0][0]:5.1f}…{lst[-1][0]:5.1f} с, {max(ds):.0f}→{min(ds):.0f} м, "
|
||
f"u {ob['u']:+.2f} h {ob['h']:.2f}, p ≤ {max(o['p'] for _, o in lst):.2f}")
|
||
ms = np.array([r["ms"] for r in det])
|
||
print(f"кадр {np.median(ms):.1f} / {np.percentile(ms, 95):.1f} мс (медиана / p95)")
|
||
|
||
|
||
if __name__ == "__main__":
|
||
main()
|