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)