|
|
from __future__ import annotations
|
|
|
import time
|
|
|
import logging
|
|
|
from collections import deque
|
|
|
from typing import Deque, Tuple, Optional, Dict, List
|
|
|
|
|
|
import numpy as np
|
|
|
from pathlib import Path
|
|
|
import os
|
|
|
|
|
|
from . import config, state
|
|
|
from .utils import wrap_deg
|
|
|
|
|
|
logger = logging.getLogger("PTZTracker.GEO")
|
|
|
|
|
|
# Скользящее окно сэмплов: (cxn, pan_deg, t)
|
|
|
_samples: Dict[int, Deque[Tuple[float, float, float]]] = {}
|
|
|
# Последняя корректная оценка (north, hfov, rms)
|
|
|
_estimate: Dict[int, Tuple[float, float, float]] = {}
|
|
|
|
|
|
# Параметры автокалибровки
|
|
|
MIN_SAMPLES = 30 # минимум наблюдений
|
|
|
MAX_SAMPLES = 400 # размер окна
|
|
|
MIN_SPREAD = 0.20 # cxn должен «расходиться» как минимум на 0.20
|
|
|
RMS_OK = 4.0 # приемлемое СКО по азимуту (в градусах)
|
|
|
REESTIMATE_EVERY = 10.0 # не переобучать чаще, чем раз в N секунд на камеру
|
|
|
|
|
|
# Условия и дебаунс автосохранения
|
|
|
SAVE_DEBOUNCE_SEC = 60.0 # писать файл не чаще раз в минуту на камеру
|
|
|
MIN_CHANGE_NORTH_DEG = 1.0 # насколько должен отличаться north, чтобы писать
|
|
|
MIN_CHANGE_HFOV_DEG = 1.0 # насколько должен отличаться hfov, чтобы писать
|
|
|
|
|
|
_last_fit_at: Dict[int, float] = {}
|
|
|
_last_saved_at: Dict[int, float] = {}
|
|
|
_last_saved_vals: Dict[int, Tuple[float, float]] = {} # pano_id -> (north, hfov)
|
|
|
|
|
|
|
|
|
def _unwrap_deg_seq(degs: np.ndarray) -> np.ndarray:
|
|
|
"""Разворачивает углы из [-180,180) в непрерывную последовательность для ЛСК."""
|
|
|
r = np.deg2rad(degs)
|
|
|
r_unw = np.unwrap(r)
|
|
|
return np.rad2deg(r_unw)
|
|
|
|
|
|
|
|
|
def _fit_linear(cxn: np.ndarray, pan_deg: np.ndarray) -> Tuple[float, float, float]:
|
|
|
"""
|
|
|
ЛСК: pan_deg ~ north + hfov*(cxn-0.5)
|
|
|
Возвращает (north_deg_wrapped[-180,180), hfov_deg[>0], rms_err_deg).
|
|
|
"""
|
|
|
x = cxn - 0.5
|
|
|
y = _unwrap_deg_seq(pan_deg)
|
|
|
|
|
|
A = np.column_stack([np.ones_like(x), x])
|
|
|
try:
|
|
|
coef, *_ = np.linalg.lstsq(A, y, rcond=None)
|
|
|
north_raw, hfov = float(coef[0]), float(coef[1])
|
|
|
|
|
|
resid = y - (north_raw + hfov * x)
|
|
|
rms = float(np.sqrt(np.mean(resid**2))) if resid.size else 999.0
|
|
|
|
|
|
north_wrapped = wrap_deg(north_raw)
|
|
|
# Если наклон отрицательный — перевернём модель (эквивалент сдвигу на 180°)
|
|
|
if hfov < 0:
|
|
|
hfov = abs(hfov)
|
|
|
north_wrapped = wrap_deg(north_wrapped + 180.0)
|
|
|
|
|
|
return north_wrapped, hfov, rms
|
|
|
except Exception as e:
|
|
|
logger.debug("[GEO] fit failed: %s", e)
|
|
|
return 0.0, 90.0, 999.0
|
|
|
|
|
|
|
|
|
def _cameras_toml_path() -> Path:
|
|
|
"""
|
|
|
Совместимо с логикой config._load_cameras_from_file():
|
|
|
- CAMERAS_FILE из окружения
|
|
|
- cameras.toml рядом с модулем config
|
|
|
"""
|
|
|
env = os.getenv("CAMERAS_FILE")
|
|
|
if env:
|
|
|
return Path(env).resolve()
|
|
|
cfg_path = Path(config.__file__).resolve().parent
|
|
|
return (cfg_path / "cameras.toml").resolve()
|
|
|
|
|
|
|
|
|
def _rewrite_all_cameras_from_runtime(path: Path) -> None:
|
|
|
"""
|
|
|
Перезаписывает cameras.toml на основе текущего in-memory config.CAMERA_CONFIG.
|
|
|
Пишем только основные поля (как в streaming._rewrite_all_cameras).
|
|
|
"""
|
|
|
lines: List[str] = []
|
|
|
for cid in sorted(config.CAMERA_CONFIG.keys()):
|
|
|
c = config.CAMERA_CONFIG[cid]
|
|
|
lines.append("[[camera]]")
|
|
|
lines.append(f"id = {int(cid)}")
|
|
|
lines.append(f"ip = \"{c['ip']}\"")
|
|
|
lines.append(f"username = \"{c.get('username', 'admin')}\"")
|
|
|
lines.append(f"password = \"{c.get('password', '')}\"")
|
|
|
lines.append(f"ptz_channel = {int(c.get('ptz_channel', 1))}")
|
|
|
lines.append("wiper_channel = 1")
|
|
|
|
|
|
# Общие geo-поля, если есть
|
|
|
if isinstance(c.get("north_offset_deg"), (int, float)):
|
|
|
lines.append(f"north_offset_deg = {float(c['north_offset_deg']):.3f}")
|
|
|
|
|
|
# PTZ или «пуля/панорама»
|
|
|
is_ptz = bool(c.get("preset1") and c.get("preset2"))
|
|
|
if is_ptz:
|
|
|
lines.append(f"preset1 = \"{c.get('preset1', '1')}\"")
|
|
|
lines.append(f"preset2 = \"{c.get('preset2', '2')}\"")
|
|
|
sweep_sign = int(c.get("sweep_sign", c.get("pan_sign", 1)))
|
|
|
lines.append(f"sweep_sign = {sweep_sign}")
|
|
|
# Доп. гео-поля PTZ — если у вас они используются, можно оставить
|
|
|
for k in ("preset1_deg", "preset2_deg",
|
|
|
"preset1_tilt_deg", "preset2_tilt_deg",
|
|
|
"sector_min_deg", "sector_max_deg",
|
|
|
"preset1_deg_geo", "preset2_deg_geo",
|
|
|
"sector_min_deg_geo", "sector_max_deg_geo"):
|
|
|
v = c.get(k)
|
|
|
if isinstance(v, (int, float)):
|
|
|
lines.append(f"{k} = {float(v):.3f}")
|
|
|
else:
|
|
|
# Панорамы/пули — FOV
|
|
|
if isinstance(c.get("bullet_hfov_deg"), (int, float)):
|
|
|
lines.append(f"bullet_hfov_deg = {float(c['bullet_hfov_deg']):.3f}")
|
|
|
if isinstance(c.get("bullet_vfov_deg"), (int, float)):
|
|
|
lines.append(f"bullet_vfov_deg = {float(c['bullet_vfov_deg']):.3f}")
|
|
|
|
|
|
lines.append("") # пустая строка после блока
|
|
|
|
|
|
text = ("\n".join(lines).strip() + "\n") if lines else ""
|
|
|
tmp = path.with_suffix(path.suffix + ".tmp")
|
|
|
path.parent.mkdir(parents=True, exist_ok=True)
|
|
|
with open(tmp, "w", encoding="utf-8") as f:
|
|
|
f.write(text)
|
|
|
f.flush()
|
|
|
os.fsync(f.fileno())
|
|
|
os.replace(tmp, path)
|
|
|
|
|
|
|
|
|
def _maybe_persist_to_file(pano_id: int, north: float, hfov: float) -> None:
|
|
|
"""
|
|
|
Условно пишет cameras.toml, если изменилась оценка и сработал дебаунс.
|
|
|
После записи вызывает config.reload_cameras() (best effort).
|
|
|
"""
|
|
|
now = time.time()
|
|
|
|
|
|
# Дебаунс по времени
|
|
|
last_t = _last_saved_at.get(pano_id, 0.0)
|
|
|
if (now - last_t) < SAVE_DEBOUNCE_SEC:
|
|
|
return
|
|
|
|
|
|
# Проверка «существенности» изменений
|
|
|
prev = _last_saved_vals.get(pano_id)
|
|
|
if prev is not None:
|
|
|
p_north, p_hfov = prev
|
|
|
if (abs(north - p_north) < MIN_CHANGE_NORTH_DEG) and (abs(hfov - p_hfov) < MIN_CHANGE_HFOV_DEG):
|
|
|
return
|
|
|
|
|
|
# Готовим обновление в in-memory конфиге
|
|
|
cc = config.CAMERA_CONFIG.get(pano_id, {})
|
|
|
cc["north_offset_deg"] = float(north)
|
|
|
cc["bullet_hfov_deg"] = float(hfov)
|
|
|
|
|
|
# Пишем на диск
|
|
|
path = _cameras_toml_path()
|
|
|
try:
|
|
|
_rewrite_all_cameras_from_runtime(path)
|
|
|
_last_saved_at[pano_id] = now
|
|
|
_last_saved_vals[pano_id] = (float(north), float(hfov))
|
|
|
logger.info("[GEO] saved to %s: pano=%s north=%.1f hfov=%.1f",
|
|
|
path, pano_id, north, hfov)
|
|
|
except Exception as e:
|
|
|
logger.warning("[GEO] save cameras.toml failed: %s", e)
|
|
|
return
|
|
|
|
|
|
# Мягкий hot-reload, чтобы рантайм пересчитал производные структуры
|
|
|
try:
|
|
|
config.reload_cameras()
|
|
|
logger.info("[GEO] config reloaded after save")
|
|
|
except Exception as e:
|
|
|
logger.warning("[GEO] reload_cameras() failed: %s", e)
|
|
|
|
|
|
|
|
|
def feed_pano_sample(pano_id: int, cxn: float, now: float | None = None) -> None:
|
|
|
"""
|
|
|
Кормим наблюдение из постпроцесса панорамы.
|
|
|
Нужно, чтобы была «парная» PTZ (config.PAN_TO_PTZ) и у неё была телеметрия pan_deg.
|
|
|
"""
|
|
|
# есть ли связанная PTZ?
|
|
|
ptz_id = config.PAN_TO_PTZ.get(pano_id)
|
|
|
if ptz_id is None:
|
|
|
return
|
|
|
|
|
|
# актуальный азимут PTZ
|
|
|
st = state.ptz_states.get(ptz_id, {})
|
|
|
pan_deg = st.get("pan_deg")
|
|
|
if pan_deg is None:
|
|
|
return
|
|
|
|
|
|
t = time.time() if now is None else float(now)
|
|
|
|
|
|
dq = _samples.setdefault(pano_id, deque(maxlen=MAX_SAMPLES))
|
|
|
dq.append((float(cxn), float(pan_deg), t))
|
|
|
|
|
|
# Периодичность переобучения
|
|
|
last = _last_fit_at.get(pano_id, 0.0)
|
|
|
if (t - last) < REESTIMATE_EVERY:
|
|
|
return
|
|
|
|
|
|
if len(dq) < MIN_SAMPLES:
|
|
|
return
|
|
|
|
|
|
cx = np.array([s[0] for s in dq], dtype=np.float64)
|
|
|
pd = np.array([s[1] for s in dq], dtype=np.float64)
|
|
|
|
|
|
# нужен ощутимый охват по кадру
|
|
|
if (cx.max() - cx.min()) < MIN_SPREAD:
|
|
|
return
|
|
|
|
|
|
north, hfov, rms = _fit_linear(cx, pd)
|
|
|
_last_fit_at[pano_id] = t
|
|
|
|
|
|
# проверка качества
|
|
|
if rms <= RMS_OK and 30.0 <= hfov <= 160.0:
|
|
|
_estimate[pano_id] = (north, hfov, rms)
|
|
|
|
|
|
# применяем сразу в рантайм-конфиг (для текущей сессии)
|
|
|
cc = config.CAMERA_CONFIG.get(pano_id, {})
|
|
|
cc["north_offset_deg"] = float(north)
|
|
|
cc["bullet_hfov_deg"] = float(hfov)
|
|
|
|
|
|
logger.info(
|
|
|
"[GEO] pano %s AUTOCAL OK: north=%.1f° hfov=%.1f° rms=%.1f° (samples=%d)",
|
|
|
pano_id, north, hfov, rms, len(dq)
|
|
|
)
|
|
|
|
|
|
# Сохранить на диск (с дебаунсом и проверкой изменений)
|
|
|
_maybe_persist_to_file(pano_id, north, hfov)
|
|
|
else:
|
|
|
logger.info(
|
|
|
"[GEO] pano %s autocal tentative: north=%.1f°, hfov=%.1f°, rms=%.1f° (need more/better samples)",
|
|
|
pano_id, north, hfov, rms
|
|
|
)
|
|
|
|
|
|
|
|
|
def get_estimate(pano_id: int) -> Optional[Tuple[float, float, float]]:
|
|
|
"""Вернуть последнюю валидную оценку: (north_deg, hfov_deg, rms) или None."""
|
|
|
return _estimate.get(pano_id)
|