commit 4340aa0790e2e98e676a050fe8820e8e1f5dddc1 Author: Ярослав Карташов Date: Tue Jun 30 14:36:17 2026 +0700 Initial project import diff --git a/.dockerignore b/.dockerignore new file mode 100644 index 0000000..918a453 --- /dev/null +++ b/.dockerignore @@ -0,0 +1,32 @@ +__pycache__/ +tests/__pycache__/ +.pytest_cache/ +.venv/ +venv/ + +cvat/ +dataset_prep/ +analysis_frames/ +__debug_frames/ + +*.mp4 +*.zip +*.csv +*.json + +out_infer*.mp4 +track_log_*.csv +track_summary_*.json +guidance_override*.csv +guidance_override*.json +smoke_*.csv +smoke_*.json +smoke_*.mp4 +segment_*.csv +segment_*.json +segment_*.mp4 +rollback_*.csv + +!best.pt +!docs/superpowers/specs/2026-06-29-docker-offline-gpu-proto-udp-design.md +!docs/superpowers/plans/2026-06-29-docker-offline-gpu-proto-udp-implementation.md diff --git a/Dockerfile b/Dockerfile new file mode 100644 index 0000000..e21892d --- /dev/null +++ b/Dockerfile @@ -0,0 +1,51 @@ +FROM nvidia/cuda:12.8.1-cudnn-runtime-ubuntu22.04 + +ARG DEBIAN_FRONTEND=noninteractive +ARG PYTORCH_INDEX_URL=https://download.pytorch.org/whl/cu128 + +ENV PYTHONUNBUFFERED=1 \ + PIP_NO_CACHE_DIR=1 \ + TZ=UTC + +RUN apt-get update && apt-get install -y --no-install-recommends \ + python3 \ + python3-pip \ + python3-venv \ + ffmpeg \ + libgl1 \ + libglib2.0-0 \ + libgomp1 \ + libsm6 \ + libxext6 \ + libxrender1 \ + v4l-utils \ + && rm -rf /var/lib/apt/lists/* + +WORKDIR /app + +COPY requirements-docker.txt /tmp/requirements-docker.txt + +RUN python3 -m pip install --upgrade pip setuptools wheel && \ + python3 -m pip install --index-url ${PYTORCH_INDEX_URL} torch torchvision && \ + python3 -m pip install -r /tmp/requirements-docker.txt + +COPY . /app + +RUN mkdir -p /data/out /data/guidance /data/autopilot /app/docker && \ + chmod +x /app/docker/entrypoint.sh + +ENV FPV_MODEL_PATH=/app/best.pt \ + FPV_SHOW_OUTPUT=0 \ + FPV_SAVE_INFER_VIDEO=1 \ + FPV_OUT_VIDEO_PATH=/data/out/out_infer.mp4 \ + FPV_GUIDANCE_EXPORT_ENABLE=1 \ + FPV_GUIDANCE_EXPORT_PATH=/data/guidance/guidance_state.json \ + FPV_AUTOPILOT_ENABLE=1 \ + FPV_AUTOPILOT_BACKEND=json \ + FPV_AUTOPILOT_JSON_PATH=/data/autopilot/autopilot_cmd.json \ + FPV_PROTO_UDP_ENABLE=0 \ + FPV_PROTO_UDP_HOST=127.0.0.1 \ + FPV_PROTO_UDP_PORT=5005 + +ENTRYPOINT ["/app/docker/entrypoint.sh"] +CMD ["python3", "main.py"] diff --git a/HOMOGRAPHY_INTEGRATION.py b/HOMOGRAPHY_INTEGRATION.py new file mode 100644 index 0000000..323b301 --- /dev/null +++ b/HOMOGRAPHY_INTEGRATION.py @@ -0,0 +1,143 @@ +# ============================================================ +# HOMOGRAPHY_INTEGRATION.py +# Как подключить homography-enabled motion saliency в main.py +# ============================================================ + +# ───────────────────────────────────────────────────────────── +# ИМПОРТЫ +# ───────────────────────────────────────────────────────────── +""" +from camera_motion import ( + estimate_ego_motion_adaptive, + estimate_global_homography_ex, + estimate_global_affine_ex, + homography_is_plausible, + affine_is_plausible, + warp_is_plausible, +) +from motion_saliency import MotionSaliency +""" + +# ───────────────────────────────────────────────────────────── +# ВАРИАНТ A (простой, рекомендуется начать с него): +# adaptive — homography с автоматическим fallback на affine +# ───────────────────────────────────────────────────────────── +""" + motion_sal = MotionSaliency() + prev_gray_for_ms = None + ms_kind_stats = {"homography": 0, "affine": 0, "none": 0} + + # В главном цикле: + gray_eff = cv2.cvtColor(frame_eff_bgr, cv2.COLOR_BGR2GRAY) + + M, kind, ego_inliers, ego_outliers = estimate_ego_motion_adaptive( + prev_gray_for_ms, gray_eff + ) + ms_kind_stats[kind] = ms_kind_stats.get(kind, 0) + 1 + + if not warp_is_plausible(M, kind): + M = None + kind = "none" + ego_outliers = np.zeros((0, 2), dtype=np.float32) + + # В терминальной фазе отключаем — слишком большой parallax + ms_active = True + if intercept_params is not None: + phase = intercept_params.get("phase", "") + if phase in ("terminal", "critical"): + ms_active = False + + if ms_active: + motion_sal.update(gray_eff, prev_gray_for_ms, M, kind=kind, + outlier_pts=ego_outliers) + else: + motion_sal.reset() + + prev_gray_for_ms = gray_eff +""" + +# ───────────────────────────────────────────────────────────── +# ВАРИАНТ B (dual residual, максимально устойчивый): +# считаем обе модели, берём min residual. Дороже по CPU но +# устойчивее на сценах с несколькими плоскостями (горизонт+земля). +# ───────────────────────────────────────────────────────────── +""" + # Считаем обе модели + H, h_in, h_out, h_meta = estimate_global_homography_ex( + prev_gray_for_ms, gray_eff + ) + A, a_in, a_out = estimate_global_affine_ex( + prev_gray_for_ms, gray_eff + ) + + # Фильтруем по качеству + H_use = H if homography_is_plausible(H, h_meta) else None + A_use = A if affine_is_plausible(A) else None + + # Объединяем outliers обеих моделей (точки, не объяснённые НИ одной) + # — пересечение дает самый чистый сигнал, но можно и объединение + if len(h_out) > 0 and len(a_out) > 0: + # Простое объединение обычно достаточно + ego_outliers = np.concatenate([h_out, a_out], axis=0) + elif len(h_out) > 0: + ego_outliers = h_out + elif len(a_out) > 0: + ego_outliers = a_out + else: + ego_outliers = np.zeros((0, 2), dtype=np.float32) + + motion_sal.update_dual( + gray_eff, prev_gray_for_ms, + H=H_use, A=A_use, + outlier_pts=ego_outliers, + ) + + prev_gray_for_ms = gray_eff +""" + +# ───────────────────────────────────────────────────────────── +# ТЮНИНГ ПО СЦЕНАМ +# ───────────────────────────────────────────────────────────── +# +# 1) Если в логах ms_kind_stats почти всегда "affine" — значит +# homography не проходит пороги качества. Варианты: +# - понизить HOMO_MIN_INLIERS до 35 +# - понизить HOMO_MIN_FILLED_CELLS до 3 +# - увеличить CAM_MOTION_MAX_CORNERS чтобы было больше точек +# +# 2) Если homography выбирается часто но daёт странные residual +# (вся земля горит) — это признак того, что она фитится в +# одну плоскость (небо), а земля как вторая плоскость даёт +# fake residual. Решение — переключиться на Variant B (dual), +# min-из-двух карт это исправляет. +# +# 3) Для сложных сцен с ЛЭП и лесом рекомендую сразу Variant B. +# Overhead ~2x по CPU для ego-motion, но motion_saliency +# всё равно дешёвый относительно YOLO, общий FPS просядет +# минимально. +# +# 4) Если летите очень низко над лесом (parallax огромный, никакая +# модель не помогает) — тогда motion saliency просто отключайте +# на такие периоды по высоте/скорости носителя, а полагайтесь +# на KLT+IMM которые у вас уже есть. +# +# 5) Если homography выдаёт вырожденные матрицы часто — уменьшите +# HOMO_MAX_CORNER_SHIFT до 80, это агрессивнее отсеивает плохие +# фиты (ценой иногда пропустить валидный резкий манёвр камеры). + +# ───────────────────────────────────────────────────────────── +# ДИАГНОСТИКА +# ───────────────────────────────────────────────────────────── +# В конце сессии напечатайте статистику: +""" + total = sum(ms_kind_stats.values()) + if total > 0: + print(f"Ego motion stats: " + f"homography={ms_kind_stats['homography']/total*100:.1f}% " + f"affine={ms_kind_stats['affine']/total*100:.1f}% " + f"none={ms_kind_stats['none']/total*100:.1f}%") +""" +# Здоровая статистика на вашем типе данных: +# homography: 50-80% (когда хватает текстуры) +# affine: 15-40% (чистое небо, слабая текстура) +# none: < 5% (если больше — проблема с LK/feature detection) diff --git a/INTEGRATION_GUIDE.py b/INTEGRATION_GUIDE.py new file mode 100644 index 0000000..6959487 --- /dev/null +++ b/INTEGRATION_GUIDE.py @@ -0,0 +1,299 @@ +# ============================================================ +# INTEGRATION_GUIDE.py +# Гайд по интеграции модулей перехвата в main.py +# +# НЕ ЗАПУСКАТЬ — это документация с фрагментами кода. +# Каждый блок показывает, куда именно вставить код в main.py. +# ============================================================ + +# ───────────────────────────────────────────────────────────── +# ШАГ 1: ИМПОРТЫ (добавить в начало main.py, после существующих) +# ───────────────────────────────────────────────────────────── + +""" +import config_intercept # новый конфиг +from config_intercept import * +from range_estimation import RangeEstimator +from proportional_navigation import ProportionalNavigator +from intercept_fsm import InterceptFSM, InterceptPhase +from autopilot_bridge import AutopilotBridge +from imm_filter import IMMFilter +""" + +# ───────────────────────────────────────────────────────────── +# ШАГ 2: ИНИЦИАЛИЗАЦИЯ (в main(), после создания guidance_ctrl) +# ───────────────────────────────────────────────────────────── + +""" + # --- Intercept modules --- + range_est = RangeEstimator() + print(range_est.status_line()) + + pn_nav = ProportionalNavigator() + print(pn_nav.status_line()) + + intercept_fsm = InterceptFSM() + print(intercept_fsm.status_line()) + + autopilot = AutopilotBridge() + autopilot.start() + print(autopilot.status_line()) + + imm = IMMFilter() + print(imm.status_line()) + + # Состояния для нового пайплайна + range_state = None + pn_state = None + intercept_params = None +""" + +# ───────────────────────────────────────────────────────────── +# ШАГ 3: KALMAN → IMM (опционально, если IMM_REPLACE_KALMAN=True) +# Заменяет строку: kf = Kalman8D() +# ───────────────────────────────────────────────────────────── + +""" + if IMM_REPLACE_KALMAN and IMM_ENABLE: + kf = imm # IMMFilter имеет тот же API что Kalman8D + else: + kf = Kalman8D() +""" + +# ───────────────────────────────────────────────────────────── +# ШАГ 4: RANGE ESTIMATION (внутри главного цикла while True, +# ПОСЛЕ того как locked_box_eff обновлён и guidance_state +# вычислен, т.е. перед отрисовкой) +# ───────────────────────────────────────────────────────────── + +""" + # --- Range estimation --- + if confirmed and locked_box_eff is not None: + range_state = range_est.update( + locked_box_eff, frame_ts, dt, confirmed=True + ) + elif pred_box_eff is not None: + range_state = range_est.update( + pred_box_eff, frame_ts, dt, confirmed=False + ) + else: + range_state = range_est.update(None, frame_ts, dt) + + # При потере цели — сброс + if miss_streak >= MAX_MISSES: + range_est.reset() + range_state = None +""" + +# ───────────────────────────────────────────────────────────── +# ШАГ 5: INTERCEPT FSM (после range estimation) +# ───────────────────────────────────────────────────────────── + +""" + # --- Intercept FSM --- + intercept_params = intercept_fsm.update( + confirmed=confirmed, + hit_streak=hit_streak, + miss_streak=miss_streak, + range_state=range_state, + guidance_state=guidance_state, + frame_id=frame_id, + ) + + # FSM может переопределить det_every + if intercept_params.get("det_every_override") is not None: + det_every = min(int(det_every), + int(intercept_params["det_every_override"])) + + # FSM может заставить fullscan + if intercept_params.get("force_fullscan"): + need_fullscan = True +""" + +# ───────────────────────────────────────────────────────────── +# ШАГ 6: PROPORTIONAL NAVIGATION (после FSM, перед autopilot) +# ───────────────────────────────────────────────────────────── + +""" + # --- Proportional Navigation --- + pn_state = None + if intercept_params.get("use_pn") and confirmed: + # Если есть 3D-оценка дальности + if (range_state is not None + and range_state.get("range_m") is not None + and range_state["confidence"] > 0.3): + + # Позиция цели в метрах (упрощённо: проекция из пикселей) + target_center = box_center(locked_box_eff if locked_box_eff + is not None else pred_box_eff) + my_center = np.array([ew * 0.5, eh * 0.5], dtype=np.float32) + + # Пиксели → метры (через range и FOV) + range_m = float(range_state["range_m"]) + px_to_m = range_m / max(1.0, range_est.focal_px) + + target_pos_m = (target_center - my_center) * px_to_m + my_pos_m = np.zeros(2, dtype=np.float32) + + # Скорости + vx_px = float(kf.x[4, 0]) if kf.initialized else 0.0 + vy_px = float(kf.x[5, 0]) if kf.initialized else 0.0 + target_vel_m = np.array([vx_px, vy_px], dtype=np.float32) * px_to_m + my_vel_m = np.zeros(2, dtype=np.float32) # неизвестна без IMU + + pn_state = pn_nav.update_3d( + my_pos_m, my_vel_m, + target_pos_m, target_vel_m, + closing_vel=range_state.get("closing_vel", 0.0), + timestamp=frame_ts, + ) + else: + # Fallback: screen-space PN + target_center = None + if locked_box_eff is not None: + target_center = box_center(locked_box_eff) + elif pred_box_eff is not None: + target_center = box_center(pred_box_eff) + + target_vel_px = None + if kf.initialized: + target_vel_px = np.array( + [float(kf.x[4, 0]), float(kf.x[5, 0])], + dtype=np.float32, + ) + + if target_center is not None: + pn_state = pn_nav.update_screen( + target_center, ew, eh, + target_vel_px, frame_ts, + ) + else: + pn_nav.reset() +""" + +# ───────────────────────────────────────────────────────────── +# ШАГ 7: AUTOPILOT BRIDGE (после PN) +# ───────────────────────────────────────────────────────────── + +""" + # --- Autopilot bridge --- + autopilot_cmd = autopilot.update( + frame_id=frame_id, + timestamp=frame_ts, + guidance_state=guidance_state, + intercept_params=intercept_params, + range_state=range_state, + pn_state=pn_state, + confirmed=confirmed, + miss_streak=miss_streak, + ) +""" + +# ───────────────────────────────────────────────────────────── +# ШАГ 8: ОТРИСОВКА (после существующей отрисовки, перед writer) +# ───────────────────────────────────────────────────────────── + +""" + # --- Intercept overlays --- + if range_state is not None: + range_est.draw_overlay(frame_orig, range_state, sx, sy) + + if pn_state is not None: + pn_nav.draw_overlay(frame_orig, pn_state, sx, sy) + + if intercept_params is not None: + intercept_fsm.draw_overlay(frame_orig, intercept_params) + + if IMM_ENABLE and IMM_REPLACE_KALMAN: + imm.draw_overlay(frame_orig) +""" + +# ───────────────────────────────────────────────────────────── +# ШАГ 9: ЛОГИРОВАНИЕ (расширить track_logger.log_frame) +# ───────────────────────────────────────────────────────────── + +""" + # Добавить к существующему вызову track_logger.log_frame(): + # (нужно расширить FIELDNAMES в decision_logger.py) + + # Или создать отдельный лог: + if intercept_params and range_state: + # Дополнительные метрики для анализа + pass # см. decision_logger.py → добавить поля +""" + +# ───────────────────────────────────────────────────────────── +# ШАГ 10: CLEANUP (после главного цикла, перед cap.release()) +# ───────────────────────────────────────────────────────────── + +""" + autopilot.stop() + # Остальной cleanup как прежде: + autogaze_worker.stop() + yolo_worker.stop() + ... +""" + +# ───────────────────────────────────────────────────────────── +# ШАГ 11: СБРОС ПРИ ПОТЕРЕ ЦЕЛИ +# В блоках где происходит полный reset (miss_streak >= MAX_MISSES +# и другие блоки сброса в main.py), добавить: +# ───────────────────────────────────────────────────────────── + +""" + range_est.reset() + pn_nav.reset() + intercept_fsm.reset() + range_state = None + pn_state = None + intercept_params = None +""" + +# ============================================================= +# ПОРЯДОК ВЫЗОВОВ В КАЖДОМ КАДРЕ (итоговый пайплайн): +# ============================================================= +# +# 1. Чтение кадра, get_effective_frame +# 2. Camera motion compensation +# 3. Kalman/IMM predict +# 4. KLT update +# 5. YOLO submit / get +# 6. ByteTrack update +# 7. Target selection (существующая логика) +# 8. Kalman/IMM update (chosen box) +# 9. Guidance update (screen guidance) ← существующий +# 10. Range estimation update ← НОВЫЙ +# 11. Intercept FSM update ← НОВЫЙ +# 12. Proportional Navigation update ← НОВЫЙ +# 13. Autopilot bridge update ← НОВЫЙ +# 14. Decision logger +# 15. Draw overlays +# 16. Video writer / display +# ============================================================= + +# ============================================================= +# КАЛИБРОВКА КАМЕРЫ +# ============================================================= +# +# Для точной оценки дальности нужно измерить focal_length_px. +# +# Метод 1 (рекомендуемый): +# 1. Поставить объект известного размера (например, 30см линейку) +# на известном расстоянии (например, 5 метров) +# 2. Сфотографировать, измерить размер в пикселях +# 3. focal_px = (pixel_size * distance) / real_size +# Пример: линейка 30см = 42px на расстоянии 5м: +# focal_px = 42 * 5.0 / 0.3 = 700 +# +# Метод 2 (из спецификации камеры): +# focal_px = focal_mm * image_width_px / sensor_width_mm +# Типичные FPV камеры: +# - Caddx Ratel 2: 2.1mm lens, 1/1.8" sensor → ~350px +# - RunCam Phoenix: 2.1mm lens, 1/2" sensor → ~315px +# - DJI O3 Air: 2.1mm lens → ~420px +# +# Метод 3 (автокалибровка, грубый): +# Если знаете размер цели (0.27м для FPV дрона) и примерно +# расстояние при первом захвате, можно подобрать focal_px +# итеративно. +# ============================================================= diff --git a/MOTION_SALIENCY_INTEGRATION.py b/MOTION_SALIENCY_INTEGRATION.py new file mode 100644 index 0000000..2f70f58 --- /dev/null +++ b/MOTION_SALIENCY_INTEGRATION.py @@ -0,0 +1,213 @@ +# ============================================================ +# MOTION_SALIENCY_INTEGRATION.py +# Где и как встроить motion_saliency в существующий пайплайн. +# НЕ ЗАПУСКАТЬ — это документация с фрагментами. +# ============================================================ + +# ───────────────────────────────────────────────────────────── +# ШАГ 1: ЗАМЕНИТЬ camera_motion.py на новую версию +# ───────────────────────────────────────────────────────────── +# Старая estimate_global_affine продолжает работать — новые +# места используют estimate_global_affine_ex, которая возвращает +# также inliers и outliers. Ничего в существующих вызовах ломать +# не нужно. + +# ───────────────────────────────────────────────────────────── +# ШАГ 2: ИМПОРТЫ в main.py +# ───────────────────────────────────────────────────────────── +""" +from camera_motion import estimate_global_affine_ex, affine_is_plausible +from motion_saliency import MotionSaliency +""" + +# ───────────────────────────────────────────────────────────── +# ШАГ 3: ИНИЦИАЛИЗАЦИЯ (рядом с другими воркерами) +# ───────────────────────────────────────────────────────────── +""" + motion_sal = MotionSaliency() + print(motion_sal.status_line()) + prev_gray_for_ms = None +""" + +# ───────────────────────────────────────────────────────────── +# ШАГ 4: В ГЛАВНОМ ЦИКЛЕ — сразу после получения effective frame, +# ДО вызова YOLO submit. +# ───────────────────────────────────────────────────────────── +""" + gray_eff = cv2.cvtColor(frame_eff_bgr, cv2.COLOR_BGR2GRAY) + + ego_A, ego_inliers, ego_outliers = estimate_global_affine_ex( + prev_gray_for_ms, gray_eff + ) + + if not affine_is_plausible(ego_A): + ego_A = None # fallback: без ego-compensation + ego_outliers = np.zeros((0, 2), dtype=np.float32) + + # Во время терминальной фазы сближения motion saliency + # становится шумной (большой parallax). Отключаем. + ms_active = True + if intercept_params is not None: + phase = intercept_params.get("phase", "") + if phase in ("terminal", "critical"): + ms_active = False + + if ms_active: + motion_sal.update(gray_eff, prev_gray_for_ms, ego_A, ego_outliers) + else: + motion_sal.reset() + + prev_gray_for_ms = gray_eff +""" + +# ───────────────────────────────────────────────────────────── +# ШАГ 5: RE-WEIGHTING YOLO ДЕТЕКТОВ +# Когда забираете результат из YOLOWorker.try_get(), прогоняете +# детекты через motion_sal.score_box() и корректируете confidence. +# ───────────────────────────────────────────────────────────── +""" +MOTION_BOOST_WEIGHT = 0.6 # сила буста (0..1) +MOTION_FLOOR = 0.15 # минимальный motion score, ниже которого + # confidence начинает УМЕНЬШАТЬСЯ + +def reweight_by_motion(dets, motion_sal): + if motion_sal.saliency is None or not dets: + return dets + out = [] + for d in dets: + box = d[:4] + conf = float(d[4]) + m = motion_sal.score_box(box) + # Буст для движущихся, штраф для статичных + factor = 1.0 + MOTION_BOOST_WEIGHT * (m - MOTION_FLOOR) + factor = max(0.25, min(1.75, factor)) + new_conf = float(min(1.0, conf * factor)) + new_d = d.copy() + new_d[4] = new_conf + out.append(new_d) + return out +""" +# Применение: +""" + yolo_result = yolo_worker.try_get() + if yolo_result is not None: + dets, ts, mode, infer_ms, used_roi = yolo_result + dets = reweight_by_motion(dets, motion_sal) + # дальше — как раньше (ByteTrack update и т.д.) +""" + +# ───────────────────────────────────────────────────────────── +# ШАГ 6: FALLBACK ROI ДЛЯ YOLO ИЗ MOTION SALIENCY +# Если AutoGaze stale / выключен — берём ROI из motion_sal.get_rois(). +# Вставляется в блок выбора roi_box_eff перед yolo_worker.submit(). +# ───────────────────────────────────────────────────────────── +""" + roi_box_eff = None + # 1. Сначала пробуем AutoGaze (как раньше) + ag_roi, ag_all, ag_info = autogaze_worker.get_latest( + frame_id, ew, eh, pred_ref_box=pred_box_eff + ) + if ag_roi is not None: + roi_box_eff = ag_roi + + # 2. Fallback: ROI из motion saliency + if roi_box_eff is None and motion_sal.saliency is not None: + ms_rois = motion_sal.get_rois(thr=0.35, min_area=25, max_rois=3) + if ms_rois: + # Если есть трек — берём ROI ближайший к предсказанию + if pred_box_eff is not None: + pc = box_center(pred_box_eff) + best = min( + ms_rois, + key=lambda r: float(np.linalg.norm(box_center(r) - pc)), + ) + roi_box_eff = best + else: + roi_box_eff = ms_rois[0] +""" + +# ───────────────────────────────────────────────────────────── +# ШАГ 7: MOTION CONSISTENCY В TARGET SELECTION +# Когда ByteTrack возвращает кандидатов в target, ранжируете их +# с учётом того, сколько кадров подряд их bbox попадает в motion map. +# Для этого каждый трек получает скользящее среднее motion score. +# ───────────────────────────────────────────────────────────── +""" +# В классе трека (или где у вас хранятся track state'ы): +# self.motion_history = deque(maxlen=10) +# self.motion_consistency = 0.0 + +# После обновления трека на кадре: +for trk in active_tracks: + if trk.box is not None: + m = motion_sal.score_box(trk.box) + trk.motion_history.append(m) + trk.motion_consistency = float(np.mean(trk.motion_history)) + +# В target selection — добавляем motion_consistency в score: +# track_score = base_score + 0.4 * trk.motion_consistency +# Таким образом статичные ложные треки (дерево похожее на дрон) +# получают motion_consistency ≈ 0 и проигрывают настоящему дрону. +""" + +# ───────────────────────────────────────────────────────────── +# ШАГ 8: ЛОГИРОВАНИЕ +# Добавить поля в decision_logger.py: +# ───────────────────────────────────────────────────────────── +""" +FIELDNAMES добавить: + "ms_enabled", + "ms_active_frac", + "ms_score_locked", # saliency внутри locked_box + "ms_score_pred", # saliency внутри pred_box + "ms_peak_locked", + +В log_frame kwargs: + ms_enabled=motion_sal.enabled, + ms_active_frac=motion_sal.last_active_frac, + ms_score_locked=motion_sal.score_box(locked_box_eff) if locked_box_eff is not None else 0.0, + ms_score_pred=motion_sal.score_box(pred_box_eff) if pred_box_eff is not None else 0.0, + ms_peak_locked=motion_sal.peak_box(locked_box_eff) if locked_box_eff is not None else 0.0, +""" + +# ───────────────────────────────────────────────────────────── +# ШАГ 9: ОТРИСОВКА (опционально, для отладки) +# ───────────────────────────────────────────────────────────── +""" + if DRAW_MOTION_SALIENCY: + motion_sal.draw_overlay(frame_orig, sx, sy) +""" + +# ───────────────────────────────────────────────────────────── +# ПОДСКАЗКИ ПО ТЮНИНГУ НА ВАШЕМ ДАТАСЕТЕ +# ───────────────────────────────────────────────────────────── +# +# 1) Если слишком много ложных residual на деревьях внизу кадра +# (parallax при полёте над лесом) — увеличить MS_MASK_BOTTOM_FRAC +# до 0.2-0.3 при высоких скоростях носителя. Альтернатива: +# переехать в camera_motion.py на cv2.findHomography вместо +# affine, это лучше описывает плоскость земли. +# +# 2) Для аналогового шумного видео повысить MS_BLUR_KSIZE до 7, +# MS_DIFF_THR до 22-25, MS_EMA_ALPHA до 0.65. Это замедлит +# реакцию, но сильно уменьшит ложные вспышки. +# +# 3) Для далёких мелких дронов (3-6 px) — diff map почти не +# сработает, основной вклад даст outlier density. Увеличить +# MS_OUTLIER_WEIGHT до 0.7, MS_DIFF_WEIGHT до 0.3. +# +# 4) Порог get_rois(thr=...) подбирается по гистограмме saliency +# на ваших клипах: 0.35 — среднее, для чистого неба можно 0.25, +# для сложных сцен — 0.45. +# +# 5) MOTION_BOOST_WEIGHT в reweight_by_motion — начните с 0.4, +# посмотрите по логам как меняется распределение confidence +# на true positives vs false positives, подкрутите. +# +# 6) Если в IMM хотите добавить "static" модель: +# - Q с околонулевой velocity noise (IMM_STATIC_Q_VEL = 0.05) +# - Трек, у которого static-модель имеет вероятность > 0.7 +# в течение 15+ кадров — помечаем как «stationary clutter» +# и либо снижаем его track_score, либо вообще убираем из +# кандидатов в target. Это добивает последние ложные цели +# на ЛЭП и столбах. diff --git a/README.md b/README.md new file mode 100644 index 0000000..95ac340 --- /dev/null +++ b/README.md @@ -0,0 +1,95 @@ +# FPV Tracker Optimized + +Русская документация для проекта сопровождения и удержания воздушной цели на базе `YOLO + ByteTrack + Kalman + KLT`. + +Проект умеет: + +- принимать видео с файла, камеры, RTSP и UDP-потока; +- обнаруживать и сопровождать цель; +- удерживать цель при кратковременной потере детекции; +- строить команды наведения в экранных координатах; +- экспортировать команды в `json`, `mavlink`, `msp` и испытательный `proto_udp`; +- работать локально или в Docker с GPU. + +## Быстрый старт + +### Локальный запуск + +1. Установите Python 3.12 и зависимости. +2. Проверьте, что модель `best.pt` лежит в корне проекта. +3. Настройте `SOURCE` и `MODEL_PATH` в `config.py`. +4. При необходимости настройте backend в `config_intercept.py`. +5. Запустите: + +```bash +python main.py +``` + +### Docker-запуск + +1. Установите Docker. +2. Для GPU установите NVIDIA Container Toolkit на хосте. +3. Соберите образ: + +```bash +docker compose build +``` + +4. Запустите: + +```bash +docker compose up +``` + +## Комплект документации + +- [Полный runbook](docs/RUNBOOK_RU.md) +- [Архитектура проекта](docs/ARCHITECTURE_RU.md) +- [Справочник конфигурации](docs/CONFIG_REFERENCE_RU.md) +- [Испытательный UDP-протокол](docs/PROTOCOL_UDP_RU.md) +- [Поиск и устранение проблем](docs/TROUBLESHOOTING_RU.md) + +## Что является точкой входа + +Основной runtime проекта запускается через: + +```bash +python main.py +``` + +В проекте нет полноценного CLI с аргументами командной строки. Конфигурация задается: + +- напрямую через `config.py`; +- напрямую через `config_intercept.py`; +- через env-переменные, если проект запускается в контейнере или через shell. + +## Основные файлы + +- `main.py` — основной цикл обработки видео. +- `config.py` — настройки источника, детектора, трекинга, ROI и вывода. +- `config_intercept.py` — настройки наведения, range/PN/FSM и backend'ов. +- `guidance.py` — экранный guidance и экспорт `guidance_state.json`. +- `autopilot_bridge.py` — отправка команд наружу. +- `runtime_env.py` — env-overrides для контейнера и shell-запуска. +- `bytetrack_min_aggressive.py` — локальная реализация ByteTrack. + +## Что генерируется во время работы + +В зависимости от настроек проект может создавать: + +- `out_infer_*.mp4` — видео с наложениями; +- `track_log_*.csv` — покадровый лог; +- `track_summary_*.json` — сводка по прогону; +- `guidance_state.json` — текущее состояние guidance; +- `autopilot_cmd.json` — команды автопилота при backend `json`. + +## Статус Docker-упаковки + +В проекте уже подготовлены: + +- `Dockerfile` +- `docker-compose.yml` +- `docker/entrypoint.sh` +- `requirements-docker.txt` + +После сборки образ рассчитан на запуск без интернета. diff --git a/README_docker.md b/README_docker.md new file mode 100644 index 0000000..d0d6ca6 --- /dev/null +++ b/README_docker.md @@ -0,0 +1,79 @@ +# Docker Runtime + +## Что собирается + +Один GPU-образ для `fpv_tracker_optimized` с уже встроенными: + +- `torch` + `torchvision` для CUDA 12.8 +- `ultralytics` +- `opencv` +- `pymavlink` +- `pyserial` +- моделью `best.pt` +- локальным `bytetrack_min_aggressive.py` + +После сборки контейнер может запускаться без интернета. + +## Сборка + +```bash +docker compose build +``` + +## Запуск + +```bash +docker compose up +``` + +## Основные env-переменные + +- `FPV_SOURCE` + Примеры: `"0"`, `"rtsp://192.168.1.10:8554/live"`, `"udp://@0.0.0.0:5600"`, `"/data/input/test.mp4"` +- `FPV_MODEL_PATH` +- `FPV_SHOW_OUTPUT` +- `FPV_SAVE_INFER_VIDEO` +- `FPV_OUT_VIDEO_PATH` +- `FPV_GUIDANCE_EXPORT_ENABLE` +- `FPV_GUIDANCE_EXPORT_PATH` +- `FPV_AUTOPILOT_ENABLE` +- `FPV_AUTOPILOT_BACKEND` + Значения: `json`, `mavlink`, `msp`, `proto_udp` + +## Испытательный UDP-протокол + +Включение: + +```yaml +FPV_AUTOPILOT_BACKEND: "proto_udp" +FPV_PROTO_UDP_ENABLE: "1" +FPV_PROTO_UDP_HOST: "192.168.1.50" +FPV_PROTO_UDP_PORT: "5005" +``` + +Пакет отправляется в формате little-endian: + +```text + 120: + fps = 25 + + print(f"Камера открыта: {width}x{height}, FPS={fps}") + + filename = datetime.now().strftime("record_%Y-%m-%d_%H-%M-%S.mp4") + + # Кодек для MP4 + fourcc = cv2.VideoWriter_fourcc(*"mp4v") + + writer = cv2.VideoWriter( + filename, + fourcc, + fps, + (width, height) + ) + + if not writer.isOpened(): + print("Ошибка: не удалось создать файл записи") + cap.release() + return + + print(f"Запись началась: {filename}") + print("Нажми Q для остановки") + + while True: + ret, frame = cap.read() + + if not ret: + print("Ошибка: кадр не получен") + break + + writer.write(frame) + + cv2.imshow("Camera recording", frame) + + if cv2.waitKey(1) & 0xFF == ord("q"): + print("Остановка записи") + break + + cap.release() + writer.release() + cv2.destroyAllWindows() + + print(f"Видео сохранено: {filename}") + + +if __name__ == "__main__": + main() diff --git a/__init__.py b/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/autogaze_runner.py b/autogaze_runner.py new file mode 100644 index 0000000..7fe987e --- /dev/null +++ b/autogaze_runner.py @@ -0,0 +1,342 @@ +import time +import threading +from collections import deque + +import cv2 +import numpy as np +import torch + +from config import * +from helpers import clip_box, box_center, box_area + + +class AutoGazeROIWorker: + def __init__(self): + self.enabled = bool(AUTOGAZE_ENABLE) + self.ready = False + self.last_error = "" + + self._autogaze_cls = None + self._processor_cls = None + if self.enabled: + try: + from autogaze.models.autogaze import AutoGaze, AutoGazeImageProcessor + self._autogaze_cls = AutoGaze + self._processor_cls = AutoGazeImageProcessor + except Exception as e: + self.last_error = f"import failed: {e}" + + self.device = torch.device(f"cuda:{int(DEVICE)}") if torch.cuda.is_available() else torch.device("cpu") + + self.clip_len = int(max(1, AUTOGAZE_CLIP_LEN)) + self.update_every = int(max(1, AUTOGAZE_UPDATE_EVERY)) + self.result_ttl = int(max(1, AUTOGAZE_RESULT_TTL)) + self.topk = int(max(1, AUTOGAZE_TOPK)) + self.min_active_cells = int(max(1, AUTOGAZE_MIN_ACTIVE_CELLS)) + self.margin_cells = int(max(0, AUTOGAZE_ROI_MARGIN_CELLS)) + self.min_side = float(max(8.0, AUTOGAZE_ROI_MIN_SIDE)) + self.gazing_ratio = float(min(1.0, max(0.01, AUTOGAZE_GAZING_RATIO))) + self.task_loss_requirement = float(min(1.0, max(0.0, AUTOGAZE_TASK_LOSS_REQUIREMENT))) + + self.model = None + self.processor = None + + self.frame_buf = deque(maxlen=self.clip_len) + self.req = deque(maxlen=1) + self.lock = threading.Lock() + self.running = False + self.thread = threading.Thread(target=self._loop, daemon=True) + self.latest = None + self.last_submit_frame = -10**9 + + def status_line(self): + if not self.enabled: + return "AutoGaze disabled by config" + if self.ready: + return f"AutoGaze ready: model={AUTOGAZE_MODEL_ID} device={self.device}" + if self.last_error: + return f"AutoGaze unavailable: {self.last_error}" + return "AutoGaze unavailable" + + def start(self): + if not self.enabled: + return + if self._autogaze_cls is None or self._processor_cls is None: + return + if not self._load_model(): + return + if self.running: + return + self.running = True + self.thread.start() + + def stop(self): + self.running = False + if self.thread.is_alive(): + self.thread.join(timeout=1.0) + + def submit(self, frame_eff_bgr, frame_id): + if (not self.running) or (not self.ready): + return + + frame_copy = np.ascontiguousarray(frame_eff_bgr.copy()) + fid = int(frame_id) + with self.lock: + self.frame_buf.append((fid, frame_copy)) + if len(self.frame_buf) < self.clip_len: + return + if (fid - self.last_submit_frame) < self.update_every: + return + items = list(self.frame_buf) + clip = [f for _, f in items] + end_fid = int(items[-1][0]) + self.req.append((clip, end_fid)) + self.last_submit_frame = fid + + def get_latest(self, current_frame_id, frame_w, frame_h, pred_ref_box=None): + info = { + "enabled": int(self.enabled), + "ready": int(self.ready), + "status": self.status_line(), + "stale": 1, + "age": -1, + "active_cells": 0, + "infer_ms": 0.0, + } + if not self.ready: + return None, [], info + + with self.lock: + latest = None if self.latest is None else dict(self.latest) + + if latest is None: + return None, [], info + + age = int(current_frame_id) - int(latest["frame_id"]) + info["age"] = age + info["active_cells"] = int(latest.get("active_cells", 0)) + info["infer_ms"] = float(latest.get("infer_ms", 0.0)) + if age > self.result_ttl: + return None, [], info + + rois = [] + for r in latest.get("rois", []): + rois.append(clip_box(np.array(r, dtype=np.float32), frame_w, frame_h)) + + roi = self._pick_roi(rois, pred_ref_box) + info["stale"] = 0 + return roi, rois, info + + def _load_model(self): + if self.ready and self.model is not None and self.processor is not None: + return True + + try: + self.processor = self._processor_cls.from_pretrained( + AUTOGAZE_MODEL_ID, + local_files_only=bool(AUTOGAZE_LOCAL_FILES_ONLY), + ) + self.model = self._autogaze_cls.from_pretrained( + AUTOGAZE_MODEL_ID, + use_flash_attn=bool(AUTOGAZE_USE_FLASH_ATTN), + local_files_only=bool(AUTOGAZE_LOCAL_FILES_ONLY), + ) + self.model.to(self.device) + self.model.eval() + self.ready = True + self.last_error = "" + return True + except Exception as e: + self.model = None + self.processor = None + self.ready = False + self.last_error = f"load failed: {e}" + return False + + def _loop(self): + while self.running: + item = None + with self.lock: + if self.req: + item = self.req.pop() + self.req.clear() + + if item is None: + time.sleep(0.001) + continue + + clip_frames, frame_id = item + infer_ms = 0.0 + rois = [] + active_cells = 0 + try: + t0 = time.perf_counter() + rois, active_cells = self._infer_rois(clip_frames) + infer_ms = (time.perf_counter() - t0) * 1000.0 + except Exception as e: + self.last_error = f"infer failed: {e}" + rois = [] + active_cells = 0 + infer_ms = 0.0 + + with self.lock: + self.latest = { + "frame_id": int(frame_id), + "rois": rois, + "active_cells": int(active_cells), + "infer_ms": float(infer_ms), + } + + def _infer_rois(self, clip_frames_bgr): + if (self.model is None) or (self.processor is None): + return [], 0 + if len(clip_frames_bgr) < self.clip_len: + return [], 0 + + h, w = clip_frames_bgr[-1].shape[:2] + clip_rgb = [cv2.cvtColor(f, cv2.COLOR_BGR2RGB) for f in clip_frames_bgr[-self.clip_len:]] + processed = self.processor([clip_rgb], return_tensors="pt") + pixel_values = processed.get("pixel_values", None) + if pixel_values is None: + return [], 0 + if isinstance(pixel_values, list): + pixel_values = torch.as_tensor(np.asarray(pixel_values)) + if pixel_values.ndim == 4: + pixel_values = pixel_values.unsqueeze(0) + + video = pixel_values.to(self.device) + if video.dtype != torch.float32 and self.device.type == "cpu": + video = video.float() + + with torch.inference_mode(): + outputs = self.model( + {"video": video}, + gazing_ratio=self.gazing_ratio, + task_loss_requirement=self.task_loss_requirement, + generate_only=True, + ) + + grid = self._extract_fine_grid(outputs) + if grid is None: + return [], 0 + + rois, active_cells = self._grid_to_rois(grid, w, h) + return rois, active_cells + + def _extract_fine_grid(self, outputs): + gazing_mask = outputs.get("gazing_mask", None) + if gazing_mask is None or len(gazing_mask) == 0: + return None + + fine = gazing_mask[-1] + if isinstance(fine, torch.Tensor): + arr = fine.detach().float().cpu().numpy() + else: + arr = np.asarray(fine) + + if arr.ndim == 2: + arr = arr[None, ...] + if arr.ndim != 3 or arr.shape[0] <= 0 or arr.shape[1] <= 0: + return None + + vec = arr[0, -1] + if vec.size <= 0: + return None + + side = int(round(np.sqrt(float(vec.size)))) + if side * side != int(vec.size): + return None + + grid = vec.reshape(side, side) + return (grid > 0.5).astype(np.uint8) + + def _grid_to_rois(self, grid, frame_w, frame_h): + if grid is None or grid.size == 0: + return [], 0 + + mask = (grid > 0).astype(np.uint8) + active_cells = int(mask.sum()) + if active_cells < self.min_active_cells: + return [], active_cells + + gh, gw = mask.shape + cell_w = float(frame_w) / float(max(1, gw)) + cell_h = float(frame_h) / float(max(1, gh)) + + candidates = [] + n_labels, labels, stats, _ = cv2.connectedComponentsWithStats(mask, connectivity=4) + for lbl in range(1, int(n_labels)): + x = int(stats[lbl, cv2.CC_STAT_LEFT]) + y = int(stats[lbl, cv2.CC_STAT_TOP]) + ww = int(stats[lbl, cv2.CC_STAT_WIDTH]) + hh = int(stats[lbl, cv2.CC_STAT_HEIGHT]) + area = int(stats[lbl, cv2.CC_STAT_AREA]) + if area < self.min_active_cells: + continue + + gx1 = max(0, x - self.margin_cells) + gy1 = max(0, y - self.margin_cells) + gx2 = min(gw, x + ww + self.margin_cells) + gy2 = min(gh, y + hh + self.margin_cells) + roi = np.array( + [gx1 * cell_w, gy1 * cell_h, gx2 * cell_w, gy2 * cell_h], + dtype=np.float32, + ) + roi = self._ensure_min_side(roi, frame_w, frame_h) + candidates.append((area, roi)) + + if not candidates: + ys, xs = np.where(mask > 0) + if xs.size <= 0: + return [], active_cells + gx1 = max(0, int(xs.min()) - self.margin_cells) + gy1 = max(0, int(ys.min()) - self.margin_cells) + gx2 = min(gw, int(xs.max()) + 1 + self.margin_cells) + gy2 = min(gh, int(ys.max()) + 1 + self.margin_cells) + roi = np.array( + [gx1 * cell_w, gy1 * cell_h, gx2 * cell_w, gy2 * cell_h], + dtype=np.float32, + ) + roi = self._ensure_min_side(roi, frame_w, frame_h) + candidates.append((active_cells, roi)) + + candidates.sort(key=lambda x: x[0], reverse=True) + rois = [np.array(r, dtype=np.float32) for _, r in candidates[: self.topk]] + return rois, active_cells + + def _ensure_min_side(self, roi, frame_w, frame_h): + roi = clip_box(roi, frame_w, frame_h) + rw = float(roi[2] - roi[0]) + rh = float(roi[3] - roi[1]) + if rw >= self.min_side and rh >= self.min_side: + return roi + + cx = 0.5 * (float(roi[0]) + float(roi[2])) + cy = 0.5 * (float(roi[1]) + float(roi[3])) + side = max(self.min_side, rw, rh) + return clip_box( + np.array([cx - 0.5 * side, cy - 0.5 * side, cx + 0.5 * side, cy + 0.5 * side], dtype=np.float32), + frame_w, + frame_h, + ) + + def _pick_roi(self, rois, pred_ref_box): + if not rois: + return None + if pred_ref_box is None: + return rois[0] + + pref = np.array(pred_ref_box, dtype=np.float32) + pc = box_center(pref) + + best = None + best_key = (1e9, 1e9) + for r in rois: + c = box_center(r) + dist = float(np.linalg.norm(c - pc)) + a = float(box_area(r)) + key = (dist, -a) + if key < best_key: + best_key = key + best = r + return best diff --git a/autopilot_bridge.py b/autopilot_bridge.py new file mode 100644 index 0000000..d9a6c73 --- /dev/null +++ b/autopilot_bridge.py @@ -0,0 +1,551 @@ +# ============================================================ +# autopilot_bridge.py +# Модуль 4: Мост к автопилоту (MAVLink / MSP / JSON) +# +# Конвертирует команды наведения в управляющие сигналы +# для полётного контроллера. Поддерживает: +# - JSON export (для симулятора / внешнего потребителя) +# - MAVLink (ArduPilot / PX4) +# - MSP (Betaflight / INAV) +# ============================================================ + +import json +import socket +import struct +import time +import threading +from pathlib import Path + +import numpy as np + +from config import * +from config_intercept import * +from helpers import clamp + + +def _rc_value(normalized, scale, center=1500, rc_min=1000, rc_max=2000): + """ + Конвертирует нормализованную команду [-1..+1] в RC PWM (мкс). + + Args: + normalized: float в диапазоне [-1, +1] + scale: масштаб (макс. отклонение от center) + center: центр (обычно 1500) + rc_min, rc_max: лимиты + + Returns: + int — PWM в микросекундах + """ + val = float(center) + float(normalized) * float(scale) + return int(clamp(val, float(rc_min), float(rc_max))) + + +def select_proto_udp_target_state(confirmed, miss_streak): + if not confirmed: + return 0 + if int(miss_streak) > 0: + return 3 + return 1 + + +def build_proto_udp_payload(cmd): + descriptor = int(clamp(getattr(cmd, "proto_descriptor", PROTO_UDP_DESCRIPTOR), 0, 255)) + target_state = int(clamp(getattr(cmd, "target_state", 0), 0, 255)) + + if target_state == 0: + object_id = 0 + off_y_px = 0 + off_x_px = 0 + off_y_pct = 0 + off_x_pct = 0 + bbox_area_pct = 0 + else: + frame_w = max(1.0, float(getattr(cmd, "frame_w", 0) or 0)) + frame_h = max(1.0, float(getattr(cmd, "frame_h", 0) or 0)) + aim_x = float(getattr(cmd, "aim_x", 0.0) or 0.0) + aim_y = float(getattr(cmd, "aim_y", 0.0) or 0.0) + bbox_w = max(0.0, float(getattr(cmd, "bbox_w", 0.0) or 0.0)) + bbox_h = max(0.0, float(getattr(cmd, "bbox_h", 0.0) or 0.0)) + + center_x = 0.5 * frame_w + center_y = 0.5 * frame_h + off_x_px = int(round(aim_x - center_x)) + off_y_px = int(round(center_y - aim_y)) + off_x_pct = int(round(clamp((off_x_px / max(1.0, center_x)) * 100.0, -100.0, 100.0))) + off_y_pct = int(round(clamp((off_y_px / max(1.0, center_y)) * 100.0, -100.0, 100.0))) + bbox_area_pct = int(round(clamp((bbox_w * bbox_h * 100.0) / max(1.0, frame_w * frame_h), 0.0, 100.0))) + object_id = int(clamp(getattr(cmd, "target_id", 0) or 0, 0, 255)) + + return struct.pack( + "= int(AUTOPILOT_FAILSAFE_MISS_FRAMES): + self._failsafe_active = True + cmd.failsafe = True + cmd.failsafe_action = str(AUTOPILOT_FAILSAFE_ACTION) + # В failsafe: центрируем стики, throttle на hover + cmd.roll = 1500 + cmd.pitch = 1500 + cmd.yaw = 1500 + cmd.throttle = 1500 + cmd.steer_x = 0.0 + cmd.steer_y = 0.0 + else: + self._failsafe_active = False + + # ─── Отправка ─────────────────────────────────────────── + now = time.perf_counter() + if (now - self._last_send_time) >= self._send_interval: + if self._backend is not None: + self._backend.send(cmd) + self._last_send_time = now + + self.last_command = cmd + return cmd + + +# ───────────────────────────────────────────────────────────── +# BACKENDS +# ───────────────────────────────────────────────────────────── + +class _JSONBackend: + """ + Экспорт команд в JSON-файл. + Расширенная версия текущего guidance_state.json. + """ + + def __init__(self): + self.path = Path(AUTOPILOT_JSON_PATH) + self.path.parent.mkdir(parents=True, exist_ok=True) + self.every = int(max(1, AUTOPILOT_JSON_EVERY)) + self._frame_count = 0 + + def start(self): + pass + + def stop(self): + pass + + def status_line(self): + return f"JSON export → {self.path}" + + def send(self, cmd): + self._frame_count += 1 + if self._frame_count % self.every != 0: + return + + payload = cmd.to_dict() + # Сериализация: inf → null для JSON + for k, v in payload.items(): + if isinstance(v, float) and (v == float('inf') or v == float('-inf')): + payload[k] = None + + tmp = self.path.with_suffix(self.path.suffix + ".tmp") + try: + with tmp.open("w", encoding="utf-8") as f: + json.dump(payload, f, ensure_ascii=True, indent=2) + tmp.replace(self.path) + except OSError: + pass + + +class _MAVLinkBackend: + """ + MAVLink backend для ArduPilot / PX4. + + Отправляет RC_CHANNELS_OVERRIDE или MANUAL_CONTROL. + Требует: pip install pymavlink + """ + + def __init__(self): + self._conn = None + self._connected = False + + def start(self): + try: + from pymavlink import mavutil + self._conn = mavutil.mavlink_connection( + str(MAVLINK_CONNECTION), + source_system=int(MAVLINK_SYSTEM_ID), + source_component=int(MAVLINK_COMPONENT_ID), + ) + self._connected = True + except Exception as e: + self._connected = False + print(f"[autopilot] MAVLink connection failed: {e}") + + def stop(self): + if self._conn is not None: + try: + self._conn.close() + except Exception: + pass + + def status_line(self): + if self._connected: + return f"MAVLink connected: {MAVLINK_CONNECTION}" + return "MAVLink not connected" + + def send(self, cmd): + if not self._connected or self._conn is None: + return + + try: + if cmd.failsafe: + # Failsafe: отправляем hover/RTL + if cmd.failsafe_action == "rtl": + self._conn.set_mode_rtl() + elif cmd.failsafe_action == "land": + self._conn.set_mode("LAND") + else: + # hover — отпускаем стики + self._send_rc_override(cmd) + else: + self._send_rc_override(cmd) + except Exception: + pass + + def _send_rc_override(self, cmd): + """Отправка RC_CHANNELS_OVERRIDE.""" + self._conn.mav.rc_channels_override_send( + self._conn.target_system, + self._conn.target_component, + cmd.roll, # chan1 — roll + cmd.pitch, # chan2 — pitch + cmd.throttle, # chan3 — throttle + cmd.yaw, # chan4 — yaw + 0, 0, 0, 0, # chan5-8 + ) + + +class _MSPBackend: + """ + MSP (MultiWii Serial Protocol) backend для Betaflight / INAV. + + Отправляет MSP_SET_RAW_RC (200) через serial. + Требует: pip install pyserial + """ + + def __init__(self): + self._ser = None + self._connected = False + + def start(self): + try: + import serial + self._ser = serial.Serial( + str(MSP_PORT), + int(MSP_BAUD), + timeout=0.05, + ) + self._connected = True + except Exception as e: + self._connected = False + print(f"[autopilot] MSP serial open failed: {e}") + + def stop(self): + if self._ser is not None: + try: + self._ser.close() + except Exception: + pass + + def status_line(self): + if self._connected: + return f"MSP connected: {MSP_PORT}@{MSP_BAUD}" + return "MSP not connected" + + def send(self, cmd): + if not self._connected or self._ser is None: + return + + try: + channels = [ + cmd.roll, # roll + cmd.pitch, # pitch + cmd.throttle, # throttle + cmd.yaw, # yaw + 1500, 1500, 1500, 1500, # aux 1-4 + ] + self._send_msp_set_raw_rc(channels) + except Exception: + pass + + def _send_msp_set_raw_rc(self, channels): + """ + Формирует и отправляет MSP пакет SET_RAW_RC (cmd=200). + + Формат MSP v1: + $M< [size] [cmd] [data...] [checksum] + """ + data = bytearray() + for ch in channels[:8]: + val = int(clamp(ch, 1000, 2000)) + data.append(val & 0xFF) + data.append((val >> 8) & 0xFF) + + size = len(data) + cmd_id = 200 # MSP_SET_RAW_RC + + # Checksum = XOR of size, cmd, data + checksum = size ^ cmd_id + for b in data: + checksum ^= b + + packet = bytearray([ + ord('$'), ord('M'), ord('<'), + size, + cmd_id, + ]) + packet.extend(data) + packet.append(checksum & 0xFF) + + self._ser.write(packet) + + +class _ProtoUDPBackend: + def __init__(self): + self._sock = None + self._enabled = bool(PROTO_UDP_ENABLE) + self._host = str(PROTO_UDP_HOST) + self._port = int(PROTO_UDP_PORT) + + def start(self): + if not self._enabled: + return + try: + self._sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) + except OSError as e: + self._sock = None + print(f"[autopilot] proto_udp socket failed: {e}") + + def stop(self): + if self._sock is not None: + try: + self._sock.close() + except OSError: + pass + self._sock = None + + def status_line(self): + if not self._enabled: + return "PROTO_UDP disabled" + return f"PROTO_UDP ready: {self._host}:{self._port}" + + def send(self, cmd): + if (not self._enabled) or (self._sock is None): + return + try: + payload = build_proto_udp_payload(cmd) + self._sock.sendto(payload, (self._host, self._port)) + except OSError: + pass diff --git a/best.pt b/best.pt new file mode 100644 index 0000000..8943dae Binary files /dev/null and b/best.pt differ diff --git a/bytetrack_min_aggressive.py b/bytetrack_min_aggressive.py new file mode 100644 index 0000000..2b230de --- /dev/null +++ b/bytetrack_min_aggressive.py @@ -0,0 +1,339 @@ +# bytetrack_min.py +import numpy as np + + +def tlbr_to_xyah(tlbr): + x1, y1, x2, y2 = tlbr + w = max(1.0, x2 - x1) + h = max(1.0, y2 - y1) + cx = x1 + w * 0.5 + cy = y1 + h * 0.5 + a = w / h + return np.array([cx, cy, a, h], dtype=np.float32) + + +def xyah_to_tlbr(xyah): + cx, cy, a, h = [float(v) for v in xyah] + h = max(2.0, h) + w = max(2.0, a * h) + x1 = cx - w * 0.5 + y1 = cy - h * 0.5 + x2 = cx + w * 0.5 + y2 = cy + h * 0.5 + return np.array([x1, y1, x2, y2], dtype=np.float32) + + +def iou_tlbr(a, b): + ax1, ay1, ax2, ay2 = a + bx1, by1, bx2, by2 = b + ix1, iy1 = max(ax1, bx1), max(ay1, by1) + ix2, iy2 = min(ax2, bx2), min(ay2, by2) + iw = max(0.0, ix2 - ix1) + ih = max(0.0, iy2 - iy1) + inter = iw * ih + if inter <= 0: + return 0.0 + area_a = max(1.0, (ax2 - ax1)) * max(1.0, (ay2 - ay1)) + area_b = max(1.0, (bx2 - bx1)) * max(1.0, (by2 - by1)) + union = area_a + area_b - inter + return float(inter / union) if union > 0 else 0.0 + + +def iou_matrix(tracks_tlbr, dets_tlbr): + if len(tracks_tlbr) == 0 or len(dets_tlbr) == 0: + return np.zeros((len(tracks_tlbr), len(dets_tlbr)), dtype=np.float32) + matrix = np.zeros((len(tracks_tlbr), len(dets_tlbr)), dtype=np.float32) + for i, track in enumerate(tracks_tlbr): + for j, det in enumerate(dets_tlbr): + matrix[i, j] = iou_tlbr(track, det) + return matrix + + +def hungarian(cost): + cost = cost.copy() + n, m = cost.shape + size = max(n, m) + + pad = np.zeros((size, size), dtype=np.float32) + pad[:n, :m] = cost + big = float(cost.max() + 1.0) if cost.size else 1.0 + if n < size: + pad[n:, :] = big + if m < size: + pad[:, m:] = big + cost = pad + + size = cost.shape[0] + u = np.zeros(size, dtype=np.float32) + v = np.zeros(size, dtype=np.float32) + p = np.zeros(size, dtype=np.int32) + way = np.zeros(size, dtype=np.int32) + + for i in range(1, size): + p[0] = i + j0 = 0 + minv = np.full(size, np.inf, dtype=np.float32) + used = np.zeros(size, dtype=bool) + way.fill(0) + + while True: + used[j0] = True + i0 = p[j0] + delta = np.inf + j1 = 0 + for j in range(1, size): + if not used[j]: + cur = cost[i0, j] - u[i0] - v[j] + if cur < minv[j]: + minv[j] = cur + way[j] = j0 + if minv[j] < delta: + delta = minv[j] + j1 = j + for j in range(size): + if used[j]: + u[p[j]] += delta + v[j] -= delta + else: + minv[j] -= delta + j0 = j1 + if p[j0] == 0: + break + + while True: + j1 = way[j0] + p[j0] = p[j1] + j0 = j1 + if j0 == 0: + break + + assignment = -np.ones(size, dtype=np.int32) + for j in range(1, size): + if p[j] != 0: + assignment[p[j]] = j + + row_to_col = assignment[:n] + matches = [] + for row, col in enumerate(row_to_col): + if 0 <= col < m: + matches.append((row, int(col))) + return matches + + +class KalmanXYAH: + def __init__(self): + self.ndim = 4 + self.dim_x = 8 + self._motion_mat = np.eye(self.dim_x, dtype=np.float32) + for i in range(self.ndim): + self._motion_mat[i, self.ndim + i] = 1.0 + self._update_mat = np.eye(self.ndim, self.dim_x, dtype=np.float32) + self.std_pos = np.array([1.5, 1.5, 1e-2, 2.0], dtype=np.float32) + self.std_vel = np.array([5.0, 5.0, 1e-2, 5.0], dtype=np.float32) + + def initiate(self, measurement_xyah): + mean = np.zeros((self.dim_x,), dtype=np.float32) + mean[:4] = measurement_xyah + cov = np.eye(self.dim_x, dtype=np.float32) + cov[:4, :4] *= 50.0 + cov[4:, 4:] *= 200.0 + return mean, cov + + def predict(self, mean, cov, dt=1.0): + motion_mat = self._motion_mat.copy() + for i in range(self.ndim): + motion_mat[i, self.ndim + i] = float(dt) + q = np.diag(np.concatenate([self.std_pos ** 2, self.std_vel ** 2]).astype(np.float32)) + mean = motion_mat @ mean + cov = motion_mat @ cov @ motion_mat.T + q + return mean, cov + + def update(self, mean, cov, measurement_xyah): + r = np.diag((self.std_pos ** 2).astype(np.float32)) + s = self._update_mat @ cov @ self._update_mat.T + r + k = cov @ self._update_mat.T @ np.linalg.inv(s) + y = measurement_xyah - (self._update_mat @ mean) + mean = mean + k @ y + cov = (np.eye(self.dim_x, dtype=np.float32) - k @ self._update_mat) @ cov + return mean, cov + + +class TrackState: + Tracked = 1 + Lost = 2 + Removed = 3 + + +class STrack: + _next_id = 1 + + def __init__(self, tlbr, score, kf: KalmanXYAH): + self.kf = kf + self.mean = None + self.cov = None + self.tlbr = np.array(tlbr, dtype=np.float32) + self.score = float(score) + self.track_id = STrack._next_id + STrack._next_id += 1 + self.state = TrackState.Tracked + self.is_activated = False + self.frame_id = 0 + self.start_frame = 0 + self.time_since_update = 0 + self.hits = 0 + + def activate(self, frame_id): + self.frame_id = frame_id + self.start_frame = frame_id + self.time_since_update = 0 + xyah = tlbr_to_xyah(self.tlbr) + self.mean, self.cov = self.kf.initiate(xyah) + self.is_activated = True + self.hits = 1 + self.state = TrackState.Tracked + + def predict(self, dt=1.0): + if self.mean is None: + return + self.mean, self.cov = self.kf.predict(self.mean, self.cov, dt=dt) + self.tlbr = xyah_to_tlbr(self.mean[:4]) + self.time_since_update += 1 + + def update(self, tlbr, score, frame_id): + self.frame_id = frame_id + self.time_since_update = 0 + self.score = float(score) + self.hits += 1 + xyah = tlbr_to_xyah(tlbr) + self.mean, self.cov = self.kf.update(self.mean, self.cov, xyah) + self.tlbr = xyah_to_tlbr(self.mean[:4]) + self.state = TrackState.Tracked + self.is_activated = True + + def mark_lost(self): + self.state = TrackState.Lost + + def mark_removed(self): + self.state = TrackState.Removed + + +class BYTETracker: + def __init__( + self, + track_high_thresh=0.35, + track_low_thresh=0.12, + new_track_thresh=0.35, + match_thresh=0.70, + track_buffer=50, + min_hits=2, + ): + self.high = float(track_high_thresh) + self.low = float(track_low_thresh) + self.new = float(new_track_thresh) + self.match_thresh = float(match_thresh) + self.track_buffer = int(track_buffer) + self.min_hits = int(min_hits) + self.kf = KalmanXYAH() + self.frame_id = 0 + self.tracked = [] + self.lost = [] + self.removed = [] + + def update(self, dets_tlbr_score, dt=1.0): + self.frame_id += 1 + frame_id = self.frame_id + + if dets_tlbr_score is None: + for track in self.tracked: + track.predict(dt=dt) + kept_lost = [] + for track in self.lost: + if (frame_id - track.frame_id) <= self.track_buffer: + kept_lost.append(track) + else: + track.mark_removed() + self.removed.append(track) + self.lost = kept_lost + out = [] + for track in self.tracked: + if track.hits >= self.min_hits: + out.append(track) + return out + + dets = dets_tlbr_score.astype(np.float32, copy=False) + scores = np.zeros((0,), dtype=np.float32) if len(dets) == 0 else dets[:, 4] + high_mask = scores >= self.high + low_mask = (scores >= self.low) & (scores < self.high) + dets_high = dets[high_mask] + dets_low = dets[low_mask] + + for track in self.tracked: + track.predict(dt=dt) + + matches_a, u_trk_a, u_det_a = self._associate(self.tracked, dets_high, iou_thresh=self.match_thresh) + for ti, di in matches_a: + track = self.tracked[ti] + det = dets_high[di] + track.update(det[:4], det[4], frame_id) + + remaining_tracks = [self.tracked[i] for i in u_trk_a] + matches_b, u_trk_b, _ = self._associate( + remaining_tracks, dets_low, iou_thresh=max(0.10, self.match_thresh - 0.15) + ) + for ti, di in matches_b: + track = remaining_tracks[ti] + det = dets_low[di] + track.update(det[:4], det[4], frame_id) + + unmatched_after_b = [remaining_tracks[i] for i in u_trk_b] + for track in unmatched_after_b: + track.mark_lost() + + new_tracked = [track for track in self.tracked if track.state == TrackState.Tracked] + newly_lost = [track for track in self.tracked if track.state == TrackState.Lost] + self.tracked = new_tracked + self.lost.extend(newly_lost) + + for di in u_det_a: + det = dets_high[di] + if float(det[4]) >= self.new: + new_track = STrack(det[:4], det[4], self.kf) + new_track.activate(frame_id) + self.tracked.append(new_track) + + kept_lost = [] + for track in self.lost: + if (frame_id - track.frame_id) <= self.track_buffer: + kept_lost.append(track) + else: + track.mark_removed() + self.removed.append(track) + self.lost = kept_lost + + out = [] + for track in self.tracked: + if track.hits >= self.min_hits: + out.append(track) + return out + + def _associate(self, tracks, dets, iou_thresh): + if len(tracks) == 0 or len(dets) == 0: + return [], list(range(len(tracks))), list(range(len(dets))) + + trk_boxes = [track.tlbr for track in tracks] + det_boxes = [det[:4] for det in dets] + ious = iou_matrix(trk_boxes, det_boxes) + cost = 1.0 - ious + matches = hungarian(cost) + + matched = [] + u_trk = set(range(len(tracks))) + u_det = set(range(len(dets))) + threshold = float(iou_thresh) + for ti, di in matches: + if ious[ti, di] >= threshold: + matched.append((ti, di)) + u_trk.discard(ti) + u_det.discard(di) + + return matched, sorted(list(u_trk)), sorted(list(u_det)) diff --git a/camera_motion.py b/camera_motion.py new file mode 100644 index 0000000..f561926 --- /dev/null +++ b/camera_motion.py @@ -0,0 +1,284 @@ +import cv2 +import numpy as np + +from config import * +from helpers import clamp + +# Camera motion compensation +# ========================= +# Поддерживает три режима оценки ego-motion: +# - affine (2x3, 4 DoF через estimateAffinePartial2D) +# - homography (3x3, 8 DoF через findHomography) +# - adaptive: пытаемся homography, при плохом качестве падаем на affine +# +# Основные функции: +# estimate_global_affine_ex — старая добрая affine + outliers +# estimate_global_homography_ex — homography + outliers + meta качества +# estimate_ego_motion_adaptive — автоматический выбор +# ========================= + +# Пороги качества homography +HOMO_MIN_INLIERS = 50 +HOMO_MIN_FILLED_CELLS = 4 # из 3x3 = 9 ячеек +HOMO_GRID_SIZE = 3 +HOMO_MAX_CORNER_SHIFT = 120.0 # px — защита от вырожденных H + + +def _rect_mask_exclude(gray, rects_norm): + h, w = gray.shape[:2] + mask = np.ones((h, w), dtype=np.uint8) * 255 + for (x1n, y1n, x2n, y2n) in rects_norm: + x1 = int(clamp(x1n * w, 0, w - 1)) + y1 = int(clamp(y1n * h, 0, h - 1)) + x2 = int(clamp(x2n * w, 0, w)) + y2 = int(clamp(y2n * h, 0, h)) + cv2.rectangle(mask, (x1, y1), (x2, y2), 0, -1) + return mask + + +def _lk_correspondences(prev_gray, gray): + """Общий блок: feature detection + LK tracking. (p0, p1) или (None, None).""" + if prev_gray is None or gray is None: + return None, None + + mask = _rect_mask_exclude(prev_gray, CAM_MOTION_EXCLUDE_NORM) if CAM_MOTION_EXCLUDE_NORM else None + pts = cv2.goodFeaturesToTrack( + prev_gray, + maxCorners=int(CAM_MOTION_MAX_CORNERS), + qualityLevel=float(CAM_MOTION_QL), + minDistance=int(CAM_MOTION_MIN_DIST), + mask=mask, + ) + if pts is None or len(pts) < 25: + return None, None + + nxt, st, err = cv2.calcOpticalFlowPyrLK( + prev_gray, gray, pts, None, winSize=(21, 21), maxLevel=3 + ) + if nxt is None or st is None: + return None, None + + st = st.reshape(-1).astype(bool) + p0 = pts[st].reshape(-1, 2).astype(np.float32) + p1 = nxt[st].reshape(-1, 2).astype(np.float32) + if len(p0) < 25: + return None, None + + return p0, p1 + + +def _classify_outliers(p0, p1, inlier_mask): + """ + Разбивает точки текущего кадра (p1) на inliers/outliers. + Отфильтровывает outliers с слишком малым/большим потоком (LK шум). + """ + inlier_mask = inlier_mask.reshape(-1).astype(bool) + inliers_curr = p1[inlier_mask] + outliers_curr = p1[~inlier_mask] + + if len(outliers_curr) == 0: + return inliers_curr, outliers_curr + + flow = p1[~inlier_mask] - p0[~inlier_mask] + mag = np.linalg.norm(flow, axis=1) + + if len(inliers_curr) > 0: + ego_flow = p1[inlier_mask] - p0[inlier_mask] + ego_med = float(np.linalg.norm(np.median(ego_flow, axis=0))) + else: + ego_med = 0.0 + + valid = (mag > max(1.5, ego_med * 1.8)) & (mag < 80.0) + outliers_curr = outliers_curr[valid] + return inliers_curr, outliers_curr + + +# ───────────────────────────────────────────────────────────── +# AFFINE +# ───────────────────────────────────────────────────────────── + +def estimate_global_affine_ex(prev_gray, gray): + empty = (None, np.zeros((0, 2), dtype=np.float32), np.zeros((0, 2), dtype=np.float32)) + + p0, p1 = _lk_correspondences(prev_gray, gray) + if p0 is None: + return empty + + A, inlier_mask = cv2.estimateAffinePartial2D( + p0, p1, + method=cv2.RANSAC, + ransacReprojThreshold=float(CAM_MOTION_RANSAC_THR), + ) + if A is None or inlier_mask is None: + return (A, np.zeros((0, 2), dtype=np.float32), p1) + + inliers_curr, outliers_curr = _classify_outliers(p0, p1, inlier_mask) + return A, inliers_curr, outliers_curr + + +def estimate_global_affine(prev_gray, gray): + """Backward-compatible wrapper.""" + A, _, _ = estimate_global_affine_ex(prev_gray, gray) + return A + + +# ───────────────────────────────────────────────────────────── +# HOMOGRAPHY +# ───────────────────────────────────────────────────────────── + +def estimate_global_homography_ex(prev_gray, gray): + """ + Homography 3x3. Возвращает (H, inliers_curr, outliers_curr, meta). + meta: n_inliers, filled_cells, spatial_ok, corner_shift_ok + """ + empty_meta = { + "n_inliers": 0, + "filled_cells": 0, + "spatial_ok": False, + "corner_shift_ok": False, + } + empty = ( + None, + np.zeros((0, 2), dtype=np.float32), + np.zeros((0, 2), dtype=np.float32), + empty_meta, + ) + + p0, p1 = _lk_correspondences(prev_gray, gray) + if p0 is None: + return empty + + H, inlier_mask = cv2.findHomography( + p0, p1, + method=cv2.RANSAC, + ransacReprojThreshold=float(CAM_MOTION_RANSAC_THR), + maxIters=2000, + confidence=0.995, + ) + if H is None or inlier_mask is None: + return empty + + inlier_mask_flat = inlier_mask.reshape(-1).astype(bool) + n_inliers = int(inlier_mask_flat.sum()) + + # Распределение inliers по 3x3 сетке + h, w = gray.shape[:2] + gs = int(HOMO_GRID_SIZE) + cells = np.zeros((gs, gs), dtype=np.uint8) + if n_inliers > 0: + inl = p1[inlier_mask_flat] + gx = np.clip((inl[:, 0] / w * gs).astype(int), 0, gs - 1) + gy = np.clip((inl[:, 1] / h * gs).astype(int), 0, gs - 1) + for x, y in zip(gx, gy): + cells[y, x] = 1 + filled_cells = int(cells.sum()) + spatial_ok = filled_cells >= int(HOMO_MIN_FILLED_CELLS) + + # Проверка вырождения: сдвиг углов кадра + corners = np.array( + [[0, 0], [w - 1, 0], [w - 1, h - 1], [0, h - 1]], dtype=np.float32 + ).reshape(-1, 1, 2) + try: + warped_corners = cv2.perspectiveTransform(corners, H).reshape(-1, 2) + shifts = np.linalg.norm(warped_corners - corners.reshape(-1, 2), axis=1) + corner_shift_ok = bool(np.max(shifts) < float(HOMO_MAX_CORNER_SHIFT)) + except cv2.error: + corner_shift_ok = False + + meta = { + "n_inliers": n_inliers, + "filled_cells": filled_cells, + "spatial_ok": spatial_ok, + "corner_shift_ok": corner_shift_ok, + } + + inliers_curr, outliers_curr = _classify_outliers(p0, p1, inlier_mask_flat) + return H, inliers_curr, outliers_curr, meta + + +def homography_is_plausible(H, meta): + """Проходит ли homography пороги качества.""" + if H is None: + return False + if meta.get("n_inliers", 0) < int(HOMO_MIN_INLIERS): + return False + if not meta.get("spatial_ok", False): + return False + if not meta.get("corner_shift_ok", False): + return False + try: + det = float(np.linalg.det(H[:2, :2])) + except Exception: + return False + if not (0.3 < abs(det) < 3.0): + return False + return True + + +# ───────────────────────────────────────────────────────────── +# ADAPTIVE SELECTION +# ───────────────────────────────────────────────────────────── + +def estimate_ego_motion_adaptive(prev_gray, gray): + """ + Сначала homography, при плохом качестве — affine. + + Returns: + M — 3x3 (homography) / 2x3 (affine) / None + kind — "homography" | "affine" | "none" + inliers — Nx2 точки статичного мира + outliers — Nx2 точки независимо движущихся объектов + """ + H, h_in, h_out, meta = estimate_global_homography_ex(prev_gray, gray) + if homography_is_plausible(H, meta): + return H, "homography", h_in, h_out + + A, a_in, a_out = estimate_global_affine_ex(prev_gray, gray) + if A is not None and affine_is_plausible(A): + return A, "affine", a_in, a_out + + empty = np.zeros((0, 2), dtype=np.float32) + return None, "none", empty, empty + + +# ───────────────────────────────────────────────────────────── +# Utilities / backward compat +# ───────────────────────────────────────────────────────────── + +def apply_affine_to_point(A, x, y): + return ( + A[0, 0] * x + A[0, 1] * y + A[0, 2], + A[1, 0] * x + A[1, 1] * y + A[1, 2], + ) + + +def affine_is_plausible(A): + if A is None: + return False + tx = float(A[0, 2]) + ty = float(A[1, 2]) + shift = float(np.hypot(tx, ty)) + if shift > float(CAM_MOTION_MAX_SHIFT_PX): + return False + + a00 = float(A[0, 0]) + a10 = float(A[1, 0]) + col0_norm = float(np.hypot(a00, a10)) + if col0_norm < 1e-6: + return False + if not (float(CAM_MOTION_SCALE_MIN) <= col0_norm <= float(CAM_MOTION_SCALE_MAX)): + return False + + ang = float(np.degrees(np.arctan2(a10, a00))) + if abs(ang) > float(CAM_MOTION_MAX_ROT_DEG): + return False + + return True + + +def warp_is_plausible(M, kind): + if kind == "homography": + return M is not None + if kind == "affine": + return affine_is_plausible(M) + return False diff --git a/cbam_register.py b/cbam_register.py new file mode 100644 index 0000000..05f4820 --- /dev/null +++ b/cbam_register.py @@ -0,0 +1,81 @@ +import torch +import torch.nn as nn +import ultralytics.nn.modules as um +import ultralytics.nn.tasks as ut +import sys + + +class ChannelAttentionDyn(nn.Module): + def __init__(self, reduction=16): + super().__init__() + self.reduction = reduction + self.avg_pool = nn.AdaptiveAvgPool2d(1) + self.max_pool = nn.AdaptiveMaxPool2d(1) + self.mlp = None + self.sigmoid = nn.Sigmoid() + + def _build(self, c, device, dtype): + hidden = max(c // self.reduction, 1) + self.mlp = nn.Sequential( + nn.Conv2d(c, hidden, 1, bias=False), + nn.ReLU(inplace=True), + nn.Conv2d(hidden, c, 1, bias=False), + ).to(device=device, dtype=dtype) + + def forward(self, x): + if self.mlp is None: + self._build(x.shape[1], x.device, x.dtype) + else: + p = next(self.mlp.parameters()) + if p.device != x.device or p.dtype != x.dtype: + self.mlp = self.mlp.to(device=x.device, dtype=x.dtype) + + a = self.mlp(self.avg_pool(x)) + m = self.mlp(self.max_pool(x)) + return x * self.sigmoid(a + m) + + +class SpatialAttention(nn.Module): + def __init__(self, kernel_size=7): + super().__init__() + padding = kernel_size // 2 + self.conv = nn.Conv2d(2, 1, kernel_size, padding=padding, bias=False) + self.sigmoid = nn.Sigmoid() + + def forward(self, x): + if self.conv.weight.device != x.device or self.conv.weight.dtype != x.dtype: + self.conv = self.conv.to(device=x.device, dtype=x.dtype) + + avg = torch.mean(x, dim=1, keepdim=True) + mx, _ = torch.max(x, dim=1, keepdim=True) + w = self.sigmoid(self.conv(torch.cat([avg, mx], dim=1))) + return x * w + + +class CBAM(nn.Module): + def __init__(self, reduction=16, sa_kernel=7): + super().__init__() + self.ca = ChannelAttentionDyn(reduction=reduction) + self.sa = SpatialAttention(kernel_size=sa_kernel) + + def forward(self, x): + return self.sa(self.ca(x)) + + +# Register so torch/ultralytics can deserialize CBAM checkpoints. +um.CBAM = CBAM +ut.CBAM = CBAM +um.ChannelAttentionDyn = ChannelAttentionDyn +ut.ChannelAttentionDyn = ChannelAttentionDyn +um.SpatialAttention = SpatialAttention +ut.SpatialAttention = SpatialAttention +globals()["ChannelAttentionDyn"] = ChannelAttentionDyn +globals()["SpatialAttention"] = SpatialAttention +globals()["CBAM"] = CBAM + +# Compatibility for checkpoints serialized with __main__.CBAM +main_mod = sys.modules.get("__main__") +if main_mod is not None: + setattr(main_mod, "ChannelAttentionDyn", ChannelAttentionDyn) + setattr(main_mod, "SpatialAttention", SpatialAttention) + setattr(main_mod, "CBAM", CBAM) diff --git a/config.py b/config.py new file mode 100644 index 0000000..598a4b9 --- /dev/null +++ b/config.py @@ -0,0 +1,821 @@ +import cv2 + +# Configuration and tunables (extracted from original script) +# ========================= + +# ВАЖНО: укажите путь к новой YOLO11s-P2-CBAM модели (best.pt, imgsz=1280) +MODEL_PATH = r"D:\PycharmProjects\YOLOTrain\BYTETRACK\TEST\fpv_tracker_optimized\best.pt" +DEVICE = 0 +USE_HALF = True + +# Универсальный источник: +# Видео: +# SOURCE = r"C:\Users\Legion\Desktop\dataset01.26\24-01_2.mp4" +# Камера / capture card: +#SOURCE = 0 +# SOURCE = 1 +# RTSP: +# SOURCE = "rtsp://192.168.1.10:8554/live" +# UDP: +# SOURCE = "udp://@0.0.0.0:5600" +SOURCE = r"C:\Users\Legion\Desktop\Видео для МАИ ФУЛЛ\Группа 1\23-01_10один.mp4" + +VIDEO_REALTIME = True + +# Windows: +CAP_BACKEND = cv2.CAP_DSHOW # или cv2.CAP_MSMF +# Linux обычно: +# CAP_BACKEND = cv2.CAP_V4L2 + +SHOW_OUTPUT = True +WINDOW_NAME = "FPV LOCK PAL + ByteTrack" +TARGET_OUT_FPS = 0 # <=0: file -> source fps, camera/stream -> no forced pacing + +# Frame timebase / dt stabilization +TIMESTAMP_USE_SOURCE_CLOCK = True +INPUT_FPS_FALLBACK = 25.0 +DT_MIN_SEC = 1.0 / 120.0 +DT_MAX_SEC = 0.20 +DT_RESET_GAP_SEC = 0.40 + +# Effective resolution policy (for analog PAL) +EFFECTIVE_W = 720 +EFFECTIVE_H = 576 +FORCE_EFFECTIVE_PAL = True + +# YOLO inference settings +CONF = 0.25 +IMG_SIZE_ROI = 640 +IMG_SIZE_FULL = 1280 +MAX_DET = 60 +TARGET_CLASS_ID = 0 + +# Filters (small drones far away) +MIN_BOX_AREA = 8 +MAX_BOX_AREA = 0 +ASPECT_RANGE = (0.15, 6.0) + +# FSM +CONFIRM_HITS = 2 +MAX_MISSES = 20 +RECOVER_FULLSCAN_EVERY = 30 +RECOVER_FORCED_DET_EVERY = 2 + +# ROI sizing (in effective pixels) +BASE_RADIUS = 150 +MAX_RADIUS = 520 +SPEED_RADIUS_FACTOR = 18.0 +FORWARD_BIAS = 1.8 +SIDE_BIAS = 1.2 + +# KLT optical flow tracker (effective) +KLT_MAX_CORNERS = 140 +KLT_QUALITY = 0.01 +KLT_MIN_DIST = 6 +KLT_WIN = (21, 21) +KLT_MAX_LEVEL = 3 +KLT_OUTLIER_THRESH = 20.0 + +# KLT validity gates (anti-drift) +KLT_OK_Q = 0.22 +KLT_OK_PTS = 14 +KLT_REINIT_MIN_POINTS = 18 +KLT_REFRESH_WITH_YOLO = True +KLT_REFRESH_EVERY = 6 +KLT_REFRESH_MIN_SCORE = 0.18 +KLT_REFRESH_MIN_IOU = 0.25 +KLT_REFRESH_MIN_QUALITY = 0.35 + +# YOLO safety +YOLO_NO_DET_LIMIT = 20 + +# Robust single-target logic +GAP_MAX_SECONDS = 1.2 +ACQUIRE_CONFIRM_SCORE = 6 +ACQUIRE_HIT_BONUS = 2 +ACQUIRE_MISS_PENALTY = 1 +ACQUIRE_MAX_MISS = 90 +LOCK_MAX_MISS_FRAMES = 120 +FULLSCAN_WHEN_MISS_GE = 16 + +# Template matching fallback +TM_ENABLE = True +TM_TEMPLATE_SIZE = 40 +TM_SEARCH_SCALE_BASE = 2.5 +TM_SEARCH_SCALE_PER_MISS = 0.20 +TM_MIN_SCORE = 0.55 + +# Kalman safety limits +KALMAN_CLAMP_V_PX_S = 180.0 +KALMAN_RESET_IF_OUT_OF_FRAME = True +YOLO_FRESH_SEC = 0.50 +YOLO_FORCE_DET_WHEN_WEAK = True +KALMAN_Q_DT_BOOST_ENABLE = True +KALMAN_Q_DT_BOOST_START_SEC = 0.08 +KALMAN_Q_DT_BOOST_SLOPE = 16.0 +KALMAN_Q_DT_BOOST_MAX = 4.0 + +# Appearance gate +USE_HSV_GATE = False +HSV_HIST_BINS = (16, 16) +HSV_GATE_MIN_SIM = 0.06 +HSV_UPDATE_EVERY = 8 + +# Async YOLO worker +YOLO_QUEUE_MAX = 1 + +# Debug / draw +DEBUG = True +DRAW_ALL_BOXES = True +DRAW_LOCK_BOX = True +DRAW_KALMAN = True +DRAW_KLT_POINTS = False +DRAW_BT_TRACKS = True +BT_DRAW_ONLY_CONFIRMED = False + +# Output recording +SAVE_INFER_VIDEO = True +OUT_VIDEO_PATH = "out_infer.mp4" +TRACK_LOG_ENABLE = True +TRACK_LOG_PATH = "track_log.csv" +TRACK_SUMMARY_PATH = "track_summary.json" +TRACK_LOG_FLUSH_EVERY = 30 + +# Screen-space lock/guidance layer for simulator integration +GUIDANCE_ENABLE = True +DRAW_GUIDANCE = True +GUIDANCE_EXPORT_ENABLE = True +GUIDANCE_EXPORT_PATH = "guidance_state.json" +GUIDANCE_EXPORT_EVERY = 1 +GUIDANCE_LEAD_SEC = 0.12 +GUIDANCE_MAX_LEAD_PX = 90.0 +GUIDANCE_BOX_BIAS_Y = 0.0 +GUIDANCE_DEADZONE_NORM = 0.04 +GUIDANCE_EXPO = 1.35 +GUIDANCE_GAIN_X = 1.0 +GUIDANCE_GAIN_Y = 1.0 +GUIDANCE_CMD_SMOOTH = 0.70 +GUIDANCE_POINT_SMOOTH = 0.55 +GUIDANCE_CONF_MIN = 0.35 +GUIDANCE_ON_TARGET_NORM = 0.12 +GUIDANCE_LOOK_MAX_STEP_PX = 28.0 +GUIDANCE_OVERRIDE_ENABLE = True +GUIDANCE_OVERRIDE_MISS_GE = 2 +GUIDANCE_OVERRIDE_TTL = 2 + +# Trajectory drawing +DRAW_TRAJ = False +TRAIL_SECONDS = 1.2 +TRAIL_ALPHA = 0.25 +TRAIL_DRAW_EVERY_N = 2 +TRAIL_MIN_STEP_PX = 3 +CLEAR_TRAJ_ON_RECOVER = True +DRAW_TRAJ_ONLY_WHEN_LOCKED = True + +DRAW_KALMAN_WHEN_MISS_LE = 8 + +# Camera motion compensation +USE_CAM_MOTION_COMP = True +CAM_MOTION_MAX_CORNERS = 500 +CAM_MOTION_QL = 0.01 +CAM_MOTION_MIN_DIST = 12 +CAM_MOTION_RANSAC_THR = 3.0 +CAM_MOTION_MAX_SHIFT_PX = 55.0 +CAM_MOTION_SCALE_MIN = 0.90 +CAM_MOTION_SCALE_MAX = 1.10 +CAM_MOTION_MAX_ROT_DEG = 12.0 +CAM_MOTION_EXCLUDE_NORM = [ + (0.00, 0.00, 0.25, 0.85), + (0.75, 0.00, 1.00, 0.85), + (0.00, 0.00, 1.00, 0.12), + (0.30, 0.86, 0.70, 1.00), +] + +# ByteTrack settings +BT_HIGH = 0.25 +BT_LOW = 0.03 +BT_NEW = 0.12 +BT_MATCH_IOU = 0.15 +BT_BUFFER = 140 +BT_MIN_HITS = 1 +BT_MAX_PREDICT_AGE = 6 + +YOLO_CONF_EFFECTIVE = min(CONF, BT_LOW) +KALMAN_HOLD_MAX = 10 + +# Single-target lock policy +TARGET_SWITCH_MISS_FRAMES = 1 +TARGET_PICK_MIN_SCORE = 0.00 +TARGET_REACQ_DIST_FACTOR = 7.5 +TURN_MAX_ASPECT_RATIO = 6.0 +TURN_MAX_AREA_RATIO = 9.0 +TARGET_SWITCH_CONFIRM_HITS = 1 +TARGET_STICKY_SCORE_BONUS = 0.45 +TARGET_SWITCH_IOU_FLOOR = 0.0 +STALE_LOCK_BREAK_ENABLE = False +STALE_LOCK_BREAK_DIST_DIAG = 3.5 +STALE_LOCK_BREAK_MIN_MISS = 2 +STALE_LOCK_BREAK_KLT_QUALITY = 0.35 +STALE_LOCK_BREAK_KLT_IOU = 0.25 +FAST_HANDOFF_ENABLE = False +FAST_HANDOFF_CONFIRM_HITS = 2 +FAST_HANDOFF_GUIDANCE_CMD_DAMP = 0.0 +CONFIRMED_NO_ID_NEAR_FACTOR = 2.6 +CONFIRMED_NO_ID_NEAR_MIN = 26.0 +APPROACH_MODE_ENABLE = True +ADAPTIVE_CHASE_ENABLE = True +ADAPTIVE_STAGE_MID_AREA = 260.0 +APPROACH_CLOSE_AREA = 850.0 +ADAPTIVE_STAGE_AREA_HYST = 120.0 +APPROACH_MID_FORCE_DET_EVERY = 2 +APPROACH_FORCE_DET_AREA = 850.0 +APPROACH_FORCE_DET_EVERY = 1 +APPROACH_MID_KF_ROI_SCALE = 1.15 +APPROACH_KF_ROI_SCALE = 1.35 +APPROACH_MID_MAX_AREA_RATIO = 14.0 +APPROACH_MAX_AREA_RATIO = 24.0 +APPROACH_MID_NEAR_DIST_DIAG = 1.35 +APPROACH_NEAR_DIST_DIAG = 1.6 +APPROACH_HSV_RELAX_GROW_RATIO = 2.5 +APPROACH_MID_HSV_MIN_SCALE = 0.65 +APPROACH_HSV_MIN_SCALE = 0.35 +APPROACH_MID_SCORE_WEIGHT = 0.08 +APPROACH_MID_SCORE_CLIP = 0.16 +APPROACH_SCORE_WEIGHT = 0.18 +APPROACH_SCORE_CLIP = 0.35 +APPROACH_MID_SWITCH_EXTRA_MISS = 1 +APPROACH_MID_SWITCH_EXTRA_HITS = 1 +APPROACH_CLOSE_SWITCH_EXTRA_MISS = 2 +APPROACH_CLOSE_SWITCH_EXTRA_HITS = 2 +CLOSE_FORCE_FULLSCAN = True +PART_MERGE_ENABLE = True +PART_MERGE_MIN_PARTS = 2 +PART_MERGE_MIN_REF_DIAG = 28.0 +PART_MERGE_REF_DIST_DIAG = 2.2 +PART_MERGE_CLUSTER_DIST_DIAG = 1.4 +PART_MERGE_IOU_FLOOR = 0.02 +PART_MERGE_MIN_UNION_GAIN = 1.35 +PART_MERGE_MAX_UNION_RATIO = 40.0 +PART_MERGE_MAX_ASPECT = 8.0 +PART_MERGE_SCORE_BONUS = 0.03 +SWITCH_FAST_MANEUVER_ENABLE = True +SWITCH_FAST_MANEUVER_SPEED = 60.0 +SWITCH_FAST_MANEUVER_EXTRA_MISS = 2 +SWITCH_FAST_MANEUVER_EXTRA_HITS = 2 + +# Switch-only trajectory gate (candidate must be kinematically plausible +# относительно направления цели) +SWITCH_TRAJ_GATE_ENABLE = True +SWITCH_TRAJ_MISS_GE = 1 +SWITCH_TRAJ_MIN_SPEED = 1.2 +SWITCH_TRAJ_COS_MIN = 0.10 +SWITCH_TRAJ_PERP_DIAG = 0.35 +SWITCH_TRAJ_PERP_MIN = 10.0 +SWITCH_TRAJ_REQUIRE_RESIDUAL_BYPASS = True + +# Switch-only anti-static gate (reject candidates that do not move over time) +SWITCH_STATIC_GUARD_ENABLE = True +SWITCH_TRACK_HIST_WINDOW = 8 +SWITCH_STATIC_MIN_DISP = 4.0 +SWITCH_STATIC_MIN_DIAG = 0.10 +SWITCH_STATIC_REQUIRE_RESIDUAL_BYPASS = True + +# Weak-reacquire guard (suppress one-frame turn false positives) +WEAK_REACQ_GUARD_ENABLE = True +WEAK_REACQ_MISS_GE = 6 +WEAK_REACQ_MIN_HITS = 2 +WEAK_REACQ_MIN_SCORE = 0.05 +WEAK_REACQ_MAX_DIST_FACTOR = 4.0 +WEAK_REACQ_MAX_DIST_MIN = 24.0 +WEAK_REACQ_ADOPT_HITS = 3 + +# Separate score floors for actual target adoption. +# ByteTrack still receives low-score candidates, but lock/switch decisions +# require stronger track evidence. +TRACK_SCORE_MIN_ACQUIRE = 0.08 +TRACK_SCORE_MIN_REACQUIRE = 0.07 +TRACK_SCORE_MIN_SWITCH = 0.10 + +# Provisional reacquire memory (keep priority near last true micro-detection) +PROVISIONAL_HOLD_ENABLE = True +PROVISIONAL_HOLD_MAX = 8 +PROVISIONAL_HIT_DECAY = 1 +RECOVER_NEAR_DIST_FACTOR = 3.2 +RECOVER_NEAR_DIST_MIN = 26.0 + +# Analog FPV hardening +ANALOG_FPV_MODE = True +APPLY_YOLO_PREPROC = True +PRE_CLAHE_CLIP = 2.2 +PRE_CLAHE_TILE = (8, 8) +PRE_BLUR_K = 3 +PRE_UNSHARP = 0.12 + +# OSD false positives rejection +REJECT_OSD_ZONES = True +REJECT_OSD_SMALL_AREA_MAX = 2600.0 +REJECT_OSD_ZONES_NORM = [ + (0.00, 0.00, 0.20, 0.80), + (0.80, 0.00, 1.00, 0.80), + (0.00, 0.00, 1.00, 0.08), + (0.35, 0.90, 0.65, 1.00), +] +# Additional lock-switch guard: do not switch to candidates inside OSD zones +# when they are far from predicted target position. +OSD_SWITCH_BLOCK_ENABLE = True +OSD_SWITCH_BLOCK_DIST_DIAG = 1.25 + +# Turn-safe recovery +TURN_SAFE_ENABLE = True +TURN_SAFE_UNC_LIMIT = 2800.0 +TURN_SAFE_MISS_BEFORE_RESET = 12 +TURN_SAFE_FORCE_FULLSCAN = True +TURN_SAFE_MIN_SCORE = 0.08 +TURN_SAFE_REQUIRE_TRACK_ID = True + +# Safety against drifting "CONFIRMED" state without a valid track anchor +CONFIRMED_NO_ID_MAX_MISS = 3 + +# Motion-zone recover assist (for tiny distant targets) +MOTION_ZONE_ENABLE = True +MOTION_ZONE_GRID = (8, 6) +MOTION_DIFF_THR = 16 +MOTION_BLUR_K = 3 +MOTION_MORPH_K = 3 +MOTION_ZONE_MIN_PIXELS = 10 +MOTION_ZONE_MIN_RATIO = 0.0012 +MOTION_MAX_ACTIVE_ZONE_RATIO = 0.55 +MOTION_ROI_MARGIN = 28 +MOTION_ROI_MIN_SIZE = 80 +DRAW_MOTION_ROI = False +MOTION_TURN_THR_GAIN = 2.0 +MOTION_TURN_THR_MAX_BONUS = 10 +MOTION_ZONE_HEAT_DECAY = 0.88 +MOTION_ZONE_HEAT_BOOST = 1.0 +MOTION_ZONE_HEAT_CLIP = 4.0 +MOTION_ZONE_HEAT_MIN = 0.65 +MOTION_ZONE_HOT_FAVOR_BONUS = 0.35 +MOTION_ZONE_ENFORCE_HOT = True +MOTION_ZONE_ENFORCE_MISS_GE = 2 +# Use motion ROI also in CONFIRMED state when target starts slipping. +MOTION_CONF_ENABLE = True +MOTION_CONF_MISS_GE = 2 +MOTION_CONF_KLT_INVALID_ONLY = True +# Extra anti-jump guard for switching while using motion/wave ROI in CONFIRMED state. +MOTION_CONF_SWITCH_MISS_FRAMES = 3 +MOTION_CONF_SWITCH_MIN_HITS = 2 +MOTION_CONF_SWITCH_MIN_SCORE = 0.08 +MOTION_CONF_SWITCH_IOU_FLOOR = 0.05 +MOTION_CONF_SWITCH_DIST_DIAG = 0.90 +MOTION_CONF_SWITCH_CONFIRM_HITS = 4 +MOTION_CONF_SWITCH_REQUIRE_RESIDUAL = True +# While confirmed+miss, force periodic FULL scans so detector can reacquire globally. +CONF_MISS_FORCE_FULLSCAN_EVERY = 8 +EGO_RESIDUAL_GATE_ENABLE = True +EGO_RESIDUAL_MISS_GE = 2 +EGO_RESIDUAL_MIN_PIXELS = 2 +EGO_RESIDUAL_MIN_RATIO = 0.006 + +# Wavelet assist (tiny distant target recover) +WAVELET_ASSIST_ENABLE = True +WAVELET_ACTIVE_MISS_GE = 2 +WAVELET_ONLY_UNCONFIRMED = True +WAVELET_ONLY_RECOVER_PHASE = True +WAVELET_BOX_MARGIN = 8 +WAVELET_MIN_SIDE = 20 +WAVELET_GATE_TINY_AREA_MAX = 260.0 +WAVELET_GATE_MISS_GE = 2 +WAVELET_GATE_MIN_ENERGY = 0.14 +WAVELET_ENERGY_BASE = 0.22 +WAVELET_SCORE_WEIGHT = 0.28 +WAVELET_SCORE_CLIP = 0.35 +# Temporal M/N stabilization for wavelet (per track id) +WAVELET_MN_ENABLE = True +WAVELET_MN_WINDOW = 4 +WAVELET_MN_MIN_HITS = 1 +WAVELET_MN_MISS_GE = 2 +# Wavelet-driven micro ROI for recover +WAVELET_ROI_ENABLE = True +WAVELET_ROI_MISS_GE = 2 +WAVELET_ROI_CONF_MISS_GE = 6 +WAVELET_ROI_TTL = 4 +WAVELET_ROI_EVERY_N = 3 +WAVELET_ROI_MARGIN = 20 +WAVELET_ROI_MIN_SIDE = 84 +WAVELET_ROI_PRED_SCALE = 3.0 +WAVELET_ROI_MIN_PEAK_RATIO = 1.5 + +# AutoGaze assist (optional ROI prior for recover) +AUTOGAZE_ENABLE = False +AUTOGAZE_MODEL_ID = "nvidia/AutoGaze" # can be HF id or local folder +AUTOGAZE_USE_FLASH_ATTN = False # keep False unless flash-attn is installed and stable +AUTOGAZE_LOCAL_FILES_ONLY = True # avoids HF network retries; set False to download/update model +AUTOGAZE_CLIP_LEN = 16 +AUTOGAZE_UPDATE_EVERY = 3 +AUTOGAZE_GAZING_RATIO = 0.75 +AUTOGAZE_TASK_LOSS_REQUIREMENT = 0.70 +AUTOGAZE_RESULT_TTL = 12 +AUTOGAZE_MISS_GE = 2 +AUTOGAZE_CONF_MISS_GE = 2 +AUTOGAZE_TOPK = 2 +AUTOGAZE_MIN_ACTIVE_CELLS = 1 +AUTOGAZE_ROI_MARGIN_CELLS = 1 +AUTOGAZE_ROI_MIN_SIDE = 90 +DRAW_AUTOGAZE_ROI = False +MODULE_TIME_BUDGET_MS = 30.0 +AUTOGAZE_COOLDOWN_FRAMES = 12 +WAVELET_COOLDOWN_FRAMES = 8 + +# Recover anti-noise gates for tiny distant target +RECOVER_FILTER_ENABLE = True +RECOVER_MIN_SCORE = 0.03 +RECOVER_TINY_AREA_MAX = 180.0 +RECOVER_TINY_MIN_SCORE = 0.045 +RECOVER_TINY_MIN_MOTION_PIXELS = 1 +RECOVER_TINY_MIN_MOTION_RATIO = 0.003 +RECOVER_TARGET_MIN_HITS = 1 + +# Keep scanning around last brief detection to stabilize reacquire +FLASH_ROI_ENABLE = True +FLASH_ROI_TTL = 7 +FLASH_ROI_MARGIN = 72 +FLASH_ROI_MIN_SIDE = 90 +FLASH_ROI_MIN_SCORE = 0.04 + +# Anti-stall recover (avoid getting stuck in wrong ROI-KF when detections disappear) +RECOVER_ROI_KF_DISABLE_MISS_GE = 6 +RECOVER_FORCE_MOTION_FIRST_MISS_GE = 8 +RECOVER_FORCE_FULLSCAN_MISS_GE = 18 + +# Pre-acquire for tiny distant target (M/N logic before stable track-id appears) +PREACQ_ENABLE = True +PREACQ_WINDOW = 7 +PREACQ_MIN_HITS = 3 +PREACQ_MIN_SCORE = 0.035 +PREACQ_NEAR_FACTOR = 3.4 +PREACQ_NEAR_MIN = 20.0 +PREACQ_ACQ_BONUS = 2 +PREACQ_INIT_KF_ON_MN = True + +# Anti-false profile: "soft" (max recall), "medium", "hard", "hard_far" +ANTI_FP_PROFILE = "hard_far" +ACTIVE_ANTI_FP_PROFILE = "custom" + + +def apply_anti_fp_profile(): + global CONFIRM_HITS, TARGET_SWITCH_CONFIRM_HITS + global BT_NEW, BT_MATCH_IOU, BT_LOW, YOLO_CONF_EFFECTIVE + global TRACK_SCORE_MIN_ACQUIRE, TRACK_SCORE_MIN_REACQUIRE, TRACK_SCORE_MIN_SWITCH + global RECOVER_MIN_SCORE, RECOVER_TINY_MIN_SCORE, RECOVER_TINY_MIN_MOTION_PIXELS + global RECOVER_TINY_MIN_MOTION_RATIO, RECOVER_TARGET_MIN_HITS + global WEAK_REACQ_MISS_GE, WEAK_REACQ_MIN_HITS, WEAK_REACQ_MIN_SCORE, WEAK_REACQ_ADOPT_HITS + global RECOVER_NEAR_DIST_FACTOR, PROVISIONAL_HOLD_MAX, PROVISIONAL_HIT_DECAY + global MOTION_DIFF_THR, MOTION_ZONE_MIN_RATIO, MOTION_MAX_ACTIVE_ZONE_RATIO + global MOTION_ZONE_HEAT_MIN, MOTION_ZONE_HOT_FAVOR_BONUS, MOTION_ZONE_ENFORCE_MISS_GE + global EGO_RESIDUAL_MISS_GE, EGO_RESIDUAL_MIN_PIXELS, EGO_RESIDUAL_MIN_RATIO + global ACTIVE_ANTI_FP_PROFILE + + p = str(ANTI_FP_PROFILE).strip().lower() + if p not in ("soft", "medium", "hard", "hard_far"): + p = "medium" + ACTIVE_ANTI_FP_PROFILE = p + + if p == "soft": + CONFIRM_HITS = 2 + TARGET_SWITCH_CONFIRM_HITS = 1 + BT_LOW = 0.03 + BT_NEW = 0.12 + BT_MATCH_IOU = 0.15 + RECOVER_MIN_SCORE = 0.03 + RECOVER_TINY_MIN_SCORE = 0.045 + RECOVER_TINY_MIN_MOTION_PIXELS = 1 + RECOVER_TINY_MIN_MOTION_RATIO = 0.003 + RECOVER_TARGET_MIN_HITS = 1 + TRACK_SCORE_MIN_ACQUIRE = 0.06 + TRACK_SCORE_MIN_REACQUIRE = 0.05 + TRACK_SCORE_MIN_SWITCH = 0.08 + WEAK_REACQ_MISS_GE = 8 + WEAK_REACQ_MIN_HITS = 1 + WEAK_REACQ_MIN_SCORE = 0.045 + WEAK_REACQ_ADOPT_HITS = 2 + RECOVER_NEAR_DIST_FACTOR = 3.8 + PROVISIONAL_HOLD_MAX = 10 + PROVISIONAL_HIT_DECAY = 1 + MOTION_DIFF_THR = 16 + MOTION_ZONE_MIN_RATIO = 0.0012 + MOTION_MAX_ACTIVE_ZONE_RATIO = 0.60 + MOTION_ZONE_HEAT_MIN = 0.55 + MOTION_ZONE_HOT_FAVOR_BONUS = 0.25 + MOTION_ZONE_ENFORCE_MISS_GE = 3 + EGO_RESIDUAL_MISS_GE = 4 + EGO_RESIDUAL_MIN_PIXELS = 1 + EGO_RESIDUAL_MIN_RATIO = 0.002 + + elif p == "medium": + CONFIRM_HITS = 3 + TARGET_SWITCH_CONFIRM_HITS = 2 + BT_LOW = 0.03 + BT_NEW = 0.14 + BT_MATCH_IOU = 0.17 + RECOVER_MIN_SCORE = 0.04 + RECOVER_TINY_MIN_SCORE = 0.060 + RECOVER_TINY_MIN_MOTION_PIXELS = 2 + RECOVER_TINY_MIN_MOTION_RATIO = 0.008 + RECOVER_TARGET_MIN_HITS = 2 + TRACK_SCORE_MIN_ACQUIRE = 0.08 + TRACK_SCORE_MIN_REACQUIRE = 0.07 + TRACK_SCORE_MIN_SWITCH = 0.10 + WEAK_REACQ_MISS_GE = 5 + WEAK_REACQ_MIN_HITS = 2 + WEAK_REACQ_MIN_SCORE = 0.060 + WEAK_REACQ_ADOPT_HITS = 3 + RECOVER_NEAR_DIST_FACTOR = 2.9 + PROVISIONAL_HOLD_MAX = 7 + PROVISIONAL_HIT_DECAY = 1 + MOTION_DIFF_THR = 17 + MOTION_ZONE_MIN_RATIO = 0.0016 + MOTION_MAX_ACTIVE_ZONE_RATIO = 0.50 + MOTION_ZONE_HEAT_MIN = 0.70 + MOTION_ZONE_HOT_FAVOR_BONUS = 0.35 + MOTION_ZONE_ENFORCE_MISS_GE = 2 + EGO_RESIDUAL_MISS_GE = 2 + EGO_RESIDUAL_MIN_PIXELS = 2 + EGO_RESIDUAL_MIN_RATIO = 0.006 + + else: # hard / hard_far + CONFIRM_HITS = 3 + TARGET_SWITCH_CONFIRM_HITS = 3 + BT_LOW = 0.035 + BT_NEW = 0.11 + BT_MATCH_IOU = 0.17 + RECOVER_MIN_SCORE = 0.05 + RECOVER_TINY_MIN_SCORE = 0.075 + RECOVER_TINY_MIN_MOTION_PIXELS = 3 + RECOVER_TINY_MIN_MOTION_RATIO = 0.015 + RECOVER_TARGET_MIN_HITS = 3 + TRACK_SCORE_MIN_ACQUIRE = 0.10 + TRACK_SCORE_MIN_REACQUIRE = 0.09 + TRACK_SCORE_MIN_SWITCH = 0.12 + WEAK_REACQ_MISS_GE = 3 + WEAK_REACQ_MIN_HITS = 3 + WEAK_REACQ_MIN_SCORE = 0.080 + WEAK_REACQ_ADOPT_HITS = 3 + RECOVER_NEAR_DIST_FACTOR = 2.4 + PROVISIONAL_HOLD_MAX = 5 + PROVISIONAL_HIT_DECAY = 2 + MOTION_DIFF_THR = 19 + MOTION_ZONE_MIN_RATIO = 0.0022 + MOTION_MAX_ACTIVE_ZONE_RATIO = 0.42 + MOTION_ZONE_HEAT_MIN = 0.85 + MOTION_ZONE_HOT_FAVOR_BONUS = 0.45 + MOTION_ZONE_ENFORCE_MISS_GE = 1 + EGO_RESIDUAL_MISS_GE = 1 + EGO_RESIDUAL_MIN_PIXELS = 3 + EGO_RESIDUAL_MIN_RATIO = 0.010 + + YOLO_CONF_EFFECTIVE = min(CONF, BT_LOW) + + +apply_anti_fp_profile() + +# ===== Chase-drone preset overrides (applied after anti-fp profile) ===== +# Ужесточаем переключение на альтернативный track, чтобы меньше липнуть на землю. +CONFIRM_HITS = 4 +ACQUIRE_CONFIRM_SCORE = 8 +PREACQ_MIN_HITS = 4 +PREACQ_MIN_SCORE = 0.05 +TARGET_SWITCH_MISS_FRAMES = 3 +TARGET_SWITCH_CONFIRM_HITS = 4 +TARGET_SWITCH_IOU_FLOOR = 0.10 +TARGET_REACQ_DIST_FACTOR = 5.5 +TRACK_SCORE_MIN_ACQUIRE = 0.10 +TRACK_SCORE_MIN_REACQUIRE = 0.09 +TRACK_SCORE_MIN_SWITCH = 0.14 + +# Switch gates preset for chase-scene +SWITCH_TRAJ_GATE_ENABLE = True +SWITCH_TRAJ_MISS_GE = 1 +SWITCH_TRAJ_MIN_SPEED = 8.0 +SWITCH_TRAJ_COS_MIN = 0.18 +SWITCH_TRAJ_PERP_DIAG = 0.28 +SWITCH_TRAJ_PERP_MIN = 9.0 +SWITCH_TRAJ_REQUIRE_RESIDUAL_BYPASS = True + +SWITCH_STATIC_GUARD_ENABLE = True +SWITCH_TRACK_HIST_WINDOW = 8 +SWITCH_STATIC_MIN_DISP = 5.0 +SWITCH_STATIC_MIN_DIAG = 0.12 +SWITCH_STATIC_REQUIRE_RESIDUAL_BYPASS = True + +WEAK_REACQ_MISS_GE = 4 +WEAK_REACQ_MIN_HITS = 3 +WEAK_REACQ_MIN_SCORE = 0.09 +WEAK_REACQ_ADOPT_HITS = 5 + +MOTION_CONF_SWITCH_MIN_SCORE = 0.12 +MOTION_CONF_SWITCH_CONFIRM_HITS = 6 +MOTION_CONF_SWITCH_DIST_DIAG = 0.70 +MOTION_CONF_SWITCH_IOU_FLOOR = 0.10 + +# Режем псевдодвижение земли. +EGO_RESIDUAL_MIN_PIXELS = 3 +EGO_RESIDUAL_MIN_RATIO = 0.010 + +RECOVER_MIN_SCORE = 0.05 +RECOVER_TINY_MIN_SCORE = 0.08 +RECOVER_TARGET_MIN_HITS = 2 +TURN_SAFE_MIN_SCORE = 0.12 + +# Appearance gate для подавления статичных ложняков. +USE_HSV_GATE = True +HSV_GATE_MIN_SIM = 0.10 +HSV_UPDATE_EVERY = 4 + +# Wavelet чаще цепляет текстуру земли на chase-сценах. +WAVELET_ROI_ENABLE = False + +# AutoGaze подключаем только на более явном срыве. +AUTOGAZE_MISS_GE = 4 +AUTOGAZE_CONF_MISS_GE = 3 +AUTOGAZE_TOPK = 1 +AUTOGAZE_UPDATE_EVERY = 4 +AUTOGAZE_RESULT_TTL = 8 + +# ===== Soft handoff patch for tiny FPV chase targets ===== +# Lets YOLO refresh Kalman/KLT even when ByteTrack changes track_id. +SOFT_YOLO_HANDOFF_ENABLE = True +SOFT_YOLO_MIN_MISS = 1 +SOFT_YOLO_MIN_ABSENT = 1 +SOFT_YOLO_MIN_SCORE = 0.055 +SOFT_YOLO_DIST_DIAG = 8.0 +SOFT_YOLO_DIST_MIN = 48.0 +SOFT_YOLO_IOU_FLOOR = 0.0 +SOFT_YOLO_MAX_AREA_RATIO = 18.0 +SOFT_YOLO_MAX_ASPECT_RATIO = 8.0 +SOFT_YOLO_TRACK_IOU = 0.02 +SOFT_YOLO_TRACK_DIST_DIAG = 2.2 +SOFT_YOLO_TRACK_DIST_MIN = 32.0 +SOFT_YOLO_REFRESH_KLT = True + +# Keep a valid KLT anchor alive instead of letting prediction hold run to miss=10. +KLT_VALID_HOLD_ENABLE = True +KLT_VALID_HOLD_MAX_MISS = 3 + +# Make handoff less rigid. The old chase preset required several frames plus +# approach/fast extras, so swHit often stayed at 1 and the lock was lost. +TARGET_SWITCH_MISS_FRAMES = 1 +TARGET_SWITCH_CONFIRM_HITS = 1 +TARGET_SWITCH_IOU_FLOOR = 0.0 +TRACK_SCORE_MIN_SWITCH = 0.07 +WEAK_REACQ_MISS_GE = 2 +WEAK_REACQ_MIN_HITS = 1 +WEAK_REACQ_MIN_SCORE = 0.05 +WEAK_REACQ_ADOPT_HITS = 1 +MOTION_CONF_SWITCH_MIN_HITS = 1 +MOTION_CONF_SWITCH_MIN_SCORE = 0.06 +MOTION_CONF_SWITCH_CONFIRM_HITS = 1 +MOTION_CONF_SWITCH_DIST_DIAG = 1.25 +MOTION_CONF_SWITCH_IOU_FLOOR = 0.0 +MOTION_CONF_SWITCH_REQUIRE_RESIDUAL = False +APPROACH_MID_SWITCH_EXTRA_MISS = 0 +APPROACH_MID_SWITCH_EXTRA_HITS = 0 +APPROACH_CLOSE_SWITCH_EXTRA_MISS = 0 +APPROACH_CLOSE_SWITCH_EXTRA_HITS = 0 +SWITCH_FAST_MANEUVER_EXTRA_MISS = 0 +SWITCH_FAST_MANEUVER_EXTRA_HITS = 0 +SWITCH_TRAJ_GATE_ENABLE = False +STALE_LOCK_BREAK_ENABLE = True +STALE_LOCK_BREAK_DIST_DIAG = 0.8 +STALE_LOCK_BREAK_MIN_MISS = 1 +FAST_HANDOFF_ENABLE = True +FAST_HANDOFF_CONFIRM_HITS = 1 +FAST_HANDOFF_GUIDANCE_CMD_DAMP = 0.35 +USE_HSV_GATE = False +CONFIRMED_NO_ID_MAX_MISS = 8 +KALMAN_HOLD_MAX = 6 + + + +# ===== Anti-flicker handoff patch v2 ===== +# Сохраняем мягкий handoff, но не даем ByteTrack/YOLO утащить Kalman +# на одиночный ложняк, пока KLT уверенно держит физическую цель. +BT_KLT_ANCHOR_GUARD_ENABLE = True +BT_KLT_ANCHOR_MIN_Q = 0.55 +BT_KLT_ANCHOR_DIST_DIAG = 1.65 +BT_KLT_ANCHOR_DIST_MIN = 34.0 +BT_KLT_ANCHOR_IOU_FLOOR = 0.015 +BT_KLT_ANCHOR_MAX_AREA_RATIO = 9.0 +BT_KLT_ANCHOR_MAX_ASPECT_RATIO = 7.0 +BT_KLT_ANCHOR_HOLD_ON_REJECT = True +BT_KLT_ANCHOR_DEBUG_EVERY = 30 + +# Не считаем постоянную смену ByteTrack id физическим переключением цели, +# если KLT якорь сильный и bbox принят рядом с ним. +BT_ID_STABILIZE_WHEN_KLT_VALID = True + +# Soft YOLO handoff: широкий радиус оставляем только для реального восстановления. +# Когда KLT валидный, handoff обязан быть рядом с KLT-якорем. +SOFT_YOLO_KLT_MIN_Q = 0.55 +SOFT_YOLO_KLT_DIST_DIAG = 1.85 +SOFT_YOLO_KLT_DIST_MIN = 34.0 +SOFT_YOLO_KLT_IOU_FLOOR = 0.015 +SOFT_YOLO_KLT_MAX_AREA_RATIO = 9.0 +SOFT_YOLO_LOST_DIST_DIAG = 7.0 +SOFT_YOLO_LOST_DIST_MIN = 48.0 + +# Предыдущий hotfix был слишком быстрый: иногда принимал ложняк на 1 кадр. +TARGET_SWITCH_CONFIRM_HITS = 2 +WEAK_REACQ_ADOPT_HITS = 2 +FAST_HANDOFF_CONFIRM_HITS = 2 +STALE_LOCK_BREAK_DIST_DIAG = 1.6 +STALE_LOCK_BREAK_MIN_MISS = 2 +GUIDANCE_OVERRIDE_MISS_GE = 3 + +# ===== Safe YOLO re-anchor patch v3 ===== +# Fixes stale KLT/ByteTrack hold: if KLT is confident but stuck on the old +# position, a repeated YOLO candidate can re-anchor Kalman/KLT after M/N hits. +YOLO_REANCHOR_ENABLE = True +YOLO_REANCHOR_MISS_GE = 2 +YOLO_REANCHOR_ABSENT_GE = 1 +YOLO_REANCHOR_MIN_SCORE = 0.065 +YOLO_REANCHOR_HITS = 2 +YOLO_REANCHOR_TRACK_HITS = 2 +YOLO_REANCHOR_WINDOW = 3 +YOLO_REANCHOR_REPEAT_DIST_MIN = 46.0 +YOLO_REANCHOR_REPEAT_DIST_DIAG = 2.8 +YOLO_REANCHOR_PRED_DIST_MIN = 72.0 +YOLO_REANCHOR_PRED_DIST_DIAG = 7.0 +YOLO_REANCHOR_NEAR_PRED_ONE_SHOT_SCORE = 0.18 +YOLO_REANCHOR_MAX_AREA_RATIO = 20.0 +YOLO_REANCHOR_MAX_ASPECT_RATIO = 8.0 +YOLO_REANCHOR_OSD_REJECT = True +YOLO_REANCHOR_OSD_MIN_SCORE = 0.12 +YOLO_REANCHOR_RESET_KLT = True + +# Let the re-anchor path do the recovery work before guidance is reset. +STALE_LOCK_BREAK_MIN_MISS = 3 +FAST_HANDOFF_CONFIRM_HITS = 2 +# Debug: draw only fresh ByteTrack boxes; predicted old BT boxes caused visual "sticking" confusion. +DRAW_BT_ONLY_FRESH = True + + +# ===== Multi-hypothesis trajectory prediction patch v4 ===== +# Builds several short-term hypotheses when YOLO/ByteTrack lose the target: +# constant velocity, acceleration, damped velocity, left/right turn, up/down. +# These hypotheses expand the ROI and help YOLO re-anchor without accepting one-frame false positives. +TRAJ_PREDICT_ENABLE = True +TRAJ_PREDICT_MISS_GE = 1 +TRAJ_HISTORY_LEN = 18 +TRAJ_HISTORY_LOOKBACK = 8 +TRAJ_HISTORY_KEEP_MISS_LE = 2 +TRAJ_VEL_HIST_WEIGHT = 0.55 +TRAJ_MAX_ACCEL_PX_S2 = 1200.0 +TRAJ_HORIZON_MISS_OFFSET = 1 +TRAJ_HORIZON_MIN_SEC = 0.08 +TRAJ_HORIZON_MAX_SEC = 0.55 +TRAJ_BOX_GROW_PER_MISS = 0.08 +TRAJ_BOX_GROW_MAX = 1.75 +TRAJ_MAX_HYPOTHESES = 7 +TRAJ_DUP_CENTER_DIST = 8.0 +TRAJ_USE_ACCEL = True +TRAJ_MIN_SPEED_FOR_MANEUVER = 8.0 +TRAJ_LATERAL_ACCEL_MIN = 90.0 +TRAJ_LATERAL_ACCEL_MAX = 620.0 +TRAJ_LATERAL_ACCEL_SPEED_GAIN = 5.5 +TRAJ_VERTICAL_ACCEL_MIN = 60.0 +TRAJ_VERTICAL_ACCEL_MAX = 420.0 +TRAJ_VERTICAL_ACCEL_SPEED_GAIN = 3.5 + +# Use prediction hypotheses for detector search and re-anchor scoring. +TRAJ_ROI_ENABLE = True +TRAJ_ROI_MISS_GE = 1 +TRAJ_ROI_MARGIN = 70 +TRAJ_ROI_MIN_SIDE = 170 +TRAJ_REANCHOR_ENABLE = True +TRAJ_REANCHOR_MIN_SCORE = 0.055 +TRAJ_REANCHOR_DIST_MIN = 64.0 +TRAJ_REANCHOR_DIST_DIAG = 4.2 +TRAJ_USE_PRIMARY_FOR_HOLD = True +TRAJ_HOLD_MISS_GE = 1 +DRAW_TRAJ_PREDICTIONS = True + +# Trajectory evidence may adopt faster than a generic far re-anchor, but still requires +# repeat confirmation unless the score is strong and close to a predicted hypothesis. +YOLO_REANCHOR_NEAR_PRED_ONE_SHOT_SCORE = 0.16 +YOLO_REANCHOR_HITS = 2 +YOLO_REANCHOR_TRACK_HITS = 1 + +# ===== Close-stage ROI priority patch v5 ===== +# In close stage do not run full-screen YOLO on every detector tick. +# Use ROI-KF/ROI-TRAJ while lock is healthy; run FULL-CLOSE only as a periodic +# health-check or when the anchor is weak/missing. This reduces false positives +# entering ByteTrack while the real target is already locked. +CLOSE_PERIODIC_FULLSCAN_ENABLE = True +CLOSE_PERIODIC_FULLSCAN_EVERY = 12 +CLOSE_FULLSCAN_MISS_GE = 2 +CLOSE_FULLSCAN_WHEN_KLT_INVALID = True +CLOSE_FULLSCAN_TARGET_ABSENT_GE = 2 + +from runtime_env import apply_config_env_overrides + +apply_config_env_overrides(globals()) diff --git a/config_intercept.py b/config_intercept.py new file mode 100644 index 0000000..67fab53 --- /dev/null +++ b/config_intercept.py @@ -0,0 +1,179 @@ +# ============================================================ +# config_intercept.py +# Параметры для модулей перехвата. +# Импортируется в main.py ПОСЛЕ config.py +# ============================================================ + +# ───────────────────────────────────────────────────────────── +# 1. RANGE ESTIMATION (range_estimation.py) +# ───────────────────────────────────────────────────────────── +RANGE_ENABLE = True + +# Калибровка камеры (нужно измерить для вашего объектива) +# focal_length_px = focal_length_mm * image_width_px / sensor_width_mm +# Для типичной FPV камеры 2.1mm, 1/3" сенсор (4.8mm), 720px ширина: +# focal_length_px ≈ 2.1 * 720 / 4.8 ≈ 315 +CAMERA_FOCAL_LENGTH_PX = 315.0 + +# Известный размер цели (метры). Типичный FPV-дрон ~0.25-0.30м по диагонали +TARGET_KNOWN_SIZE_M = 0.27 + +# Минимальный размер bbox (пиксели) для оценки дальности +RANGE_MIN_APPARENT_SIZE_PX = 4.0 + +# Максимальная правдоподобная дальность (метры) +RANGE_MAX_M = 500.0 +RANGE_MIN_M = 0.3 + +# Сглаживание оценки дальности (EMA alpha, 0=нет сглаживания, 0.9=сильное) +RANGE_SMOOTH_ALPHA = 0.65 + +# Tau (time-to-contact) +TAU_SMOOTH_ALPHA = 0.50 +TAU_MIN_AREA_RATE = 0.5 # минимальная скорость роста площади (px²/s) +TAU_WARN_SEC = 3.0 # предупреждение: < N секунд до контакта +TAU_CRITICAL_SEC = 1.0 # критическая фаза + +# ───────────────────────────────────────────────────────────── +# 2. PROPORTIONAL NAVIGATION (proportional_navigation.py) +# ───────────────────────────────────────────────────────────── +PN_ENABLE = True + +# Навигационная константа (N). Обычно 3-5. +# N=3 — базовый PN, N=4-5 — более агрессивный перехват +PN_NAV_GAIN = 4.0 + +# Использовать Augmented PN (учитывает ускорение цели) +PN_AUGMENTED = True +PN_AUG_GAIN = 0.5 # вес компенсации ускорения цели + +# Максимальная команда ускорения (нормализованная, 0..1) +PN_MAX_ACCEL_CMD = 1.0 + +# Если range недоступен, fallback к screen-space PN +PN_SCREEN_FALLBACK = True + +# Сглаживание LOS rate +PN_LOS_RATE_SMOOTH = 0.40 + +# Минимальная closing velocity для активации PN (м/с) +PN_MIN_CLOSING_VEL = 0.5 + +# Lead bias (секунды предсказания вперед для упреждения) +PN_LEAD_TIME_SEC = 0.15 + +# ───────────────────────────────────────────────────────────── +# 3. INTERCEPT FSM (intercept_fsm.py) +# ───────────────────────────────────────────────────────────── +INTERCEPT_FSM_ENABLE = True + +# Пороги перехода между фазами (по дальности, метры) +FSM_ACQUIRE_RANGE_M = 200.0 # SEARCH → ACQUIRE: цель обнаружена +FSM_TRACK_CONFIRM_HITS = 5 # ACQUIRE → TRACK: стабильный трек +FSM_INTERCEPT_RANGE_M = 80.0 # TRACK → INTERCEPT: начинаем атаку +FSM_TERMINAL_TAU_SEC = 1.5 # INTERCEPT → TERMINAL: осталось < N сек +FSM_TERMINAL_RANGE_M = 8.0 # или расстояние < N метров + +# Таймауты (кадры) +FSM_ACQUIRE_TIMEOUT = 150 # не подтвердили → SEARCH +FSM_LOST_TIMEOUT = 60 # потеря в TRACK/INTERCEPT → LOST +FSM_LOST_RECOVER_TIMEOUT = 90 # LOST → SEARCH (полный сброс) + +# Throttle / скорость сближения +FSM_INTERCEPT_THROTTLE_BIAS = 0.85 # базовый газ в фазе INTERCEPT +FSM_TERMINAL_THROTTLE = 1.0 # максимальный газ в TERMINAL +FSM_TRACK_THROTTLE_BIAS = 0.60 # удержание дистанции в TRACK + +# Terminal phase — переход на инерциальную навигацию +FSM_TERMINAL_USE_KLT_ONLY = True # в TERMINAL отключить YOLO, только KLT +FSM_TERMINAL_LOCK_COMMANDS = False # заморозить команды в последний момент +FSM_TERMINAL_LOCK_TAU_SEC = 0.3 # за сколько секунд заморозить + +# ───────────────────────────────────────────────────────────── +# 4. AUTOPILOT BRIDGE (autopilot_bridge.py) +# ───────────────────────────────────────────────────────────── +AUTOPILOT_ENABLE = True +AUTOPILOT_BACKEND = "json" # "json" | "mavlink" | "msp" | "proto_udp" + +# JSON export (расширенный вариант текущего guidance_state.json) +AUTOPILOT_JSON_PATH = "autopilot_cmd.json" +AUTOPILOT_JSON_EVERY = 1 # каждый N-й кадр + +# MAVLink +MAVLINK_CONNECTION = "udpout:127.0.0.1:14550" +MAVLINK_SYSTEM_ID = 1 +MAVLINK_COMPONENT_ID = 191 # MAV_COMP_ID_ONBOARD_COMPUTER +MAVLINK_CMD_RATE_HZ = 30.0 + +# MSP (Betaflight / INAV) +MSP_PORT = "/dev/ttyUSB0" # или "COM3" на Windows +MSP_BAUD = 115200 +MSP_CMD_RATE_HZ = 50.0 + +# Испытательный UDP-протокол "нейросеть -> блок" +PROTO_UDP_ENABLE = False +PROTO_UDP_HOST = "127.0.0.1" +PROTO_UDP_PORT = 5005 +PROTO_UDP_DESCRIPTOR = 1 + +# Маппинг команд наведения → управления дроном +# Коэффициенты конвертации steer → RC channels (микросекунды) +AUTOPILOT_ROLL_SCALE = 500.0 # steer_x * scale + 1500 +AUTOPILOT_PITCH_SCALE = 500.0 # steer_y * scale + 1500 +AUTOPILOT_YAW_SCALE = 300.0 +AUTOPILOT_THROTTLE_CENTER = 1500 +AUTOPILOT_RC_MIN = 1000 +AUTOPILOT_RC_MAX = 2000 + +# Экстренный стоп (kill switch при потере цели) +AUTOPILOT_FAILSAFE_ENABLE = True +AUTOPILOT_FAILSAFE_MISS_FRAMES = 30 # потеря > N кадров → hover/RTL +AUTOPILOT_FAILSAFE_ACTION = "hover" # "hover" | "rtl" | "land" + +# ───────────────────────────────────────────────────────────── +# 5. IMM FILTER (imm_filter.py) +# ───────────────────────────────────────────────────────────── +IMM_ENABLE = True + +# Процессный шум для каждой модели +IMM_CV_Q_POS = 2.0 # Constant Velocity — позиция +IMM_CV_Q_VEL = 4.0 # Constant Velocity — скорость +IMM_CA_Q_POS = 1.5 # Constant Acceleration — позиция +IMM_CA_Q_VEL = 3.0 # Constant Acceleration — скорость +IMM_CA_Q_ACC = 12.0 # Constant Acceleration — ускорение +IMM_CT_Q_POS = 2.0 # Coordinated Turn — позиция +IMM_CT_Q_VEL = 5.0 # Coordinated Turn — скорость +IMM_CT_Q_OMEGA = 1.0 # Coordinated Turn — угловая скорость + +# Начальные вероятности моделей [CV, CA, CT] +IMM_INIT_PROBS = [0.6, 0.25, 0.15] + +# Матрица переходов между моделями (Markov) +# [из CV→CV, из CV→CA, из CV→CT] +# [из CA→CV, из CA→CA, из CA→CT] +# [из CT→CV, из CT→CA, из CT→CT] +IMM_TRANSITION_MATRIX = [ + [0.90, 0.07, 0.03], + [0.10, 0.82, 0.08], + [0.08, 0.07, 0.85], +] + +# Measurement noise +IMM_R_POS = 16.0 +IMM_R_SIZE = 25.0 + +# Заменить стандартный Kalman8D на IMM (если True, main.py использует IMM) +IMM_REPLACE_KALMAN = False # включить после тестирования + +# ───────────────────────────────────────────────────────────── +# 6. DRAW / DEBUG для новых модулей +# ───────────────────────────────────────────────────────────── +DRAW_RANGE_INFO = True +DRAW_INTERCEPT_FSM = True +DRAW_PN_VECTOR = True +DRAW_IMM_PROBS = True +DRAW_INTERCEPT_POINT = True + +from runtime_env import apply_intercept_env_overrides + +apply_intercept_env_overrides(globals()) diff --git a/dataset_prep/package_for_cvat.py b/dataset_prep/package_for_cvat.py new file mode 100644 index 0000000..722ccdb --- /dev/null +++ b/dataset_prep/package_for_cvat.py @@ -0,0 +1,135 @@ +import argparse +import csv +import shutil +import zipfile +from pathlib import Path + + +def parse_args(): + parser = argparse.ArgumentParser( + description="Package extracted frame folders into per-video ZIP archives for CVAT." + ) + parser.add_argument("--frames-root", required=True, help="Root with extracted frames, e.g. D:\\MAI_CVAT_frames_s15\\frames_all") + parser.add_argument("--output-root", required=True, help="Where ZIP archives and manifests will be written") + parser.add_argument("--group-filter", action="append", default=[], help="Optional substring filter for group names") + parser.add_argument("--max-files-per-zip", type=int, default=0, help="Optional limit; skip items above this count when > 0") + parser.add_argument("--overwrite", action="store_true") + return parser.parse_args() + + +def main(): + args = parse_args() + frames_root = Path(args.frames_root).expanduser().resolve() + output_root = Path(args.output_root).expanduser().resolve() + + if not frames_root.exists(): + raise SystemExit(f"Frames root not found: {frames_root}") + + filters = [s.strip().lower() for s in args.group_filter if s.strip()] + zip_root = output_root / "zips" + zip_root.mkdir(parents=True, exist_ok=True) + + rows = [] + video_dirs = list(find_video_dirs(frames_root)) + for video_dir in video_dirs: + rel_parts = video_dir.relative_to(frames_root).parts + if len(rel_parts) < 5: + continue + + split, role, group_name, video_name, interval_name = rel_parts[:5] + if filters and not any(token in group_name.lower() for token in filters): + continue + + files = sorted( + [ + p for p in video_dir.iterdir() + if p.is_file() and p.suffix.lower() in {".jpg", ".jpeg", ".png"} + ] + ) + file_count = len(files) + if file_count == 0: + continue + if args.max_files_per_zip > 0 and file_count > args.max_files_per_zip: + continue + + safe_group = sanitize(group_name) + safe_video = sanitize(video_name) + safe_interval = sanitize(interval_name) + zip_name = f"{safe_group}__{safe_video}__{safe_interval}.zip" + zip_path = zip_root / zip_name + + if zip_path.exists(): + if args.overwrite: + zip_path.unlink() + else: + rows.append(build_row(split, role, group_name, video_name, interval_name, file_count, zip_path, video_dir)) + continue + + with zipfile.ZipFile(zip_path, "w", compression=zipfile.ZIP_STORED) as zf: + for file_path in files: + zf.write(file_path, arcname=file_path.name) + + rows.append(build_row(split, role, group_name, video_name, interval_name, file_count, zip_path, video_dir)) + + manifest_path = output_root / "cvat_task_packages.csv" + write_csv( + manifest_path, + rows, + [ + "split", + "role", + "group_name", + "video_name", + "interval_name", + "frame_count", + "zip_path", + "source_dir", + ], + ) + print(f"Wrote {len(rows)} package rows to {manifest_path}") + print(f"ZIP archives are in {zip_root}") + + +def find_video_dirs(frames_root: Path): + for split_dir in sorted(p for p in frames_root.iterdir() if p.is_dir()): + for role_dir in sorted(p for p in split_dir.iterdir() if p.is_dir()): + for group_dir in sorted(p for p in role_dir.iterdir() if p.is_dir()): + for video_dir in sorted(p for p in group_dir.iterdir() if p.is_dir()): + for interval_dir in sorted(p for p in video_dir.iterdir() if p.is_dir()): + yield interval_dir + + +def sanitize(value: str): + out = [] + for ch in value: + if ch in '<>:"/\\|?*': + out.append("_") + else: + out.append(ch) + return "".join(out).strip().rstrip(".") + + +def build_row(split, role, group_name, video_name, interval_name, file_count, zip_path: Path, source_dir: Path): + return { + "split": split, + "role": role, + "group_name": group_name, + "video_name": video_name, + "interval_name": interval_name, + "frame_count": file_count, + "zip_path": str(zip_path), + "source_dir": str(source_dir), + } + + +def write_csv(path: Path, rows, fieldnames): + path.parent.mkdir(parents=True, exist_ok=True) + with path.open("w", newline="", encoding="utf-8-sig") as f: + writer = csv.DictWriter(f, fieldnames=fieldnames) + writer.writeheader() + for row in rows: + writer.writerow(row) + + +if __name__ == "__main__": + main() diff --git a/dataset_prep/prepare_cvat_dataset.py b/dataset_prep/prepare_cvat_dataset.py new file mode 100644 index 0000000..fc7a9fa --- /dev/null +++ b/dataset_prep/prepare_cvat_dataset.py @@ -0,0 +1,735 @@ +import argparse +import csv +import json +import math +import shutil +import subprocess +from dataclasses import dataclass +from pathlib import Path + + +VIDEO_EXTS = {".mp4", ".avi", ".mov", ".mkv", ".mts", ".m2ts"} + + +@dataclass +class VideoMeta: + abs_path: Path + rel_path: Path + group_name: str + file_name: str + size_bytes: int + width: int + height: int + fps: float + duration_sec: float + nb_frames: int + osd_present: bool + + +def parse_args(): + parser = argparse.ArgumentParser( + description="Prepare grouped frame datasets from many videos for CVAT and YOLO." + ) + subparsers = parser.add_subparsers(dest="command", required=True) + + catalog = subparsers.add_parser("catalog", help="Scan videos and write catalog CSV files.") + catalog.add_argument("--input-root", required=True, help="Root folder with grouped videos.") + catalog.add_argument("--output-root", required=True, help="Workspace folder for CSV outputs.") + + preview = subparsers.add_parser( + "preview", + help="Extract low-rate preview frames for interval selection.", + ) + preview.add_argument("--input-root", required=True) + preview.add_argument("--output-root", required=True) + preview.add_argument("--every-sec", type=float, default=2.0) + preview.add_argument("--image-ext", choices=("jpg", "png"), default="jpg") + preview.add_argument("--jpg-quality", type=int, default=95) + preview.add_argument("--overwrite", action="store_true") + + extract = subparsers.add_parser( + "extract", + help="Extract frames for selected intervals from intervals CSV.", + ) + extract.add_argument("--input-root", required=True) + extract.add_argument("--intervals-csv", required=True) + extract.add_argument("--output-root", required=True) + extract.add_argument("--overwrite", action="store_true") + + extract_all = subparsers.add_parser( + "extract-all", + help="Extract frames from every video while preserving the input group structure.", + ) + extract_all.add_argument("--input-root", required=True) + extract_all.add_argument("--output-root", required=True) + extract_all.add_argument("--frame-step", type=int, default=2) + extract_all.add_argument("--image-ext", choices=("jpg", "png"), default="png") + extract_all.add_argument("--jpg-quality", type=int, default=95) + extract_all.add_argument("--split", default="train") + extract_all.add_argument("--role", default="bulk_raw") + extract_all.add_argument( + "--group-filter", + action="append", + default=[], + help="Optional substring filter for group names. Can be repeated.", + ) + extract_all.add_argument("--overwrite", action="store_true") + + extract_flat_all = subparsers.add_parser( + "extract-flat-all", + help="Extract frames from every video into one flat output folder.", + ) + extract_flat_all.add_argument("--input-root", required=True) + extract_flat_all.add_argument( + "--output-root", + required=True, + help="Flat folder where all images will be written.", + ) + extract_flat_all.add_argument("--frame-step", type=int, default=1) + extract_flat_all.add_argument("--image-ext", choices=("jpg", "png"), default="jpg") + extract_flat_all.add_argument("--jpg-quality", type=int, default=95) + extract_flat_all.add_argument( + "--group-filter", + action="append", + default=[], + help="Optional substring filter for group names. Can be repeated.", + ) + extract_flat_all.add_argument("--overwrite", action="store_true") + + return parser.parse_args() + + +def main(): + args = parse_args() + input_root = Path(args.input_root).expanduser().resolve() + output_root = Path(args.output_root).expanduser().resolve() + + if not input_root.exists(): + raise SystemExit(f"Input root not found: {input_root}") + + ensure_tool("ffprobe") + ensure_tool("ffmpeg") + + if args.command == "catalog": + videos = scan_videos(input_root) + write_catalog_outputs(videos, output_root) + print(f"Wrote catalog for {len(videos)} videos into {output_root}") + return + + if args.command == "preview": + videos = scan_videos(input_root) + preview_root = output_root / "previews" + preview_root.mkdir(parents=True, exist_ok=True) + manifest_path = output_root / "preview_manifest.csv" + rows = [] + for meta in videos: + out_dir = preview_root / sanitize_path_component(meta.group_name) / sanitize_path_component(meta.rel_path.stem) + if args.overwrite and out_dir.exists(): + shutil.rmtree(out_dir) + out_dir.mkdir(parents=True, exist_ok=True) + + pattern = out_dir / f"preview_%06d.{args.image_ext}" + cmd = [ + "ffmpeg", + "-hide_banner", + "-loglevel", + "warning", + "-y", + "-i", + str(meta.abs_path), + "-vf", + f"fps=1/{float(args.every_sec)}", + ] + if args.image_ext == "jpg": + cmd.extend(["-q:v", str(jpeg_qscale(args.jpg_quality))]) + cmd.append(str(pattern)) + run_cmd(cmd) + + files = sorted(out_dir.glob(f"*.{args.image_ext}")) + for idx, file_path in enumerate(files): + rows.append( + { + "group_name": meta.group_name, + "video_relpath": str(meta.rel_path).replace("\\", "/"), + "preview_file": str(file_path.relative_to(output_root)).replace("\\", "/"), + "preview_index": idx + 1, + "approx_time_sec": round(idx * float(args.every_sec), 3), + } + ) + + write_csv( + manifest_path, + rows, + ["group_name", "video_relpath", "preview_file", "preview_index", "approx_time_sec"], + ) + print(f"Wrote previews into {preview_root}") + print(f"Wrote preview manifest: {manifest_path}") + return + + if args.command == "extract": + video_map = {str(v.rel_path).replace("\\", "/"): v for v in scan_videos(input_root)} + intervals_csv = Path(args.intervals_csv).expanduser().resolve() + if not intervals_csv.exists(): + raise SystemExit(f"Intervals CSV not found: {intervals_csv}") + + rows = load_intervals(intervals_csv) + manifest_rows = [] + extracted_root = output_root / "frames" + extracted_root.mkdir(parents=True, exist_ok=True) + + for row in rows: + if not is_enabled(row.get("enabled", "")): + continue + + rel_key = normalize_relpath(row["video_relpath"]) + if rel_key not in video_map: + raise SystemExit(f"Video not found in catalog: {rel_key}") + + meta = video_map[rel_key] + split = row.get("split", "train").strip() or "train" + role = row.get("role", "positive").strip() or "positive" + frame_step = max(1, int(float(row.get("frame_step", "1") or "1"))) + start_sec = clamp_float(row.get("start_sec", "0"), 0.0, meta.duration_sec) + end_sec_raw = row.get("end_sec", "") + end_sec = meta.duration_sec if end_sec_raw == "" else clamp_float(end_sec_raw, 0.0, meta.duration_sec) + if end_sec <= start_sec: + raise SystemExit(f"Invalid interval for {rel_key}: end_sec <= start_sec") + + image_ext = (row.get("image_ext", "png").strip().lower() or "png") + if image_ext not in {"png", "jpg"}: + raise SystemExit(f"Unsupported image_ext '{image_ext}' in intervals CSV") + jpg_quality = max(70, min(100, int(float(row.get("jpg_quality", "95") or "95")))) + + extract_rows = extract_range_to_dir( + meta=meta, + output_root=output_root, + extracted_root=extracted_root, + split=split, + role=role, + start_sec=start_sec, + end_sec=end_sec, + frame_step=frame_step, + image_ext=image_ext, + jpg_quality=jpg_quality, + overwrite=args.overwrite, + interval_name=format_interval_name(start_sec, end_sec, frame_step), + ) + manifest_rows.extend(extract_rows) + + manifest_path = output_root / "extracted_frames_manifest.csv" + write_csv( + manifest_path, + manifest_rows, + ["split", "role", "group_name", "video_relpath", "output_file", "frame_idx", "time_sec", "frame_step", "image_ext"], + ) + print(f"Wrote extracted frames into {extracted_root}") + print(f"Wrote extraction manifest: {manifest_path}") + return + + if args.command == "extract-all": + videos = scan_videos(input_root) + if args.group_filter: + filters = [v.strip().lower() for v in args.group_filter if v.strip()] + videos = [meta for meta in videos if any(token in meta.group_name.lower() for token in filters)] + + frame_step = max(1, int(args.frame_step)) + image_ext = args.image_ext + jpg_quality = max(70, min(100, int(args.jpg_quality))) + split = args.split.strip() or "train" + role = args.role.strip() or "bulk_raw" + + extracted_root = output_root / "frames_all" + extracted_root.mkdir(parents=True, exist_ok=True) + manifest_rows = [] + + for meta in videos: + extract_rows = extract_range_to_dir( + meta=meta, + output_root=output_root, + extracted_root=extracted_root, + split=split, + role=role, + start_sec=0.0, + end_sec=meta.duration_sec, + frame_step=frame_step, + image_ext=image_ext, + jpg_quality=jpg_quality, + overwrite=args.overwrite, + interval_name=f"all_step{frame_step}", + ) + manifest_rows.extend(extract_rows) + + manifest_path = output_root / "extract_all_manifest.csv" + write_csv( + manifest_path, + manifest_rows, + ["split", "role", "group_name", "video_relpath", "output_file", "frame_idx", "time_sec", "frame_step", "image_ext"], + ) + print(f"Wrote bulk extracted frames into {extracted_root}") + print(f"Wrote bulk extraction manifest: {manifest_path}") + return + + if args.command == "extract-flat-all": + videos = scan_videos(input_root) + if args.group_filter: + filters = [v.strip().lower() for v in args.group_filter if v.strip()] + videos = [meta for meta in videos if any(token in meta.group_name.lower() for token in filters)] + + frame_step = max(1, int(args.frame_step)) + image_ext = args.image_ext + jpg_quality = max(70, min(100, int(args.jpg_quality))) + flat_root = output_root + + if args.overwrite and flat_root.exists(): + shutil.rmtree(flat_root) + flat_root.mkdir(parents=True, exist_ok=True) + + manifest_rows = [] + for meta in videos: + manifest_rows.extend( + extract_range_flat( + meta=meta, + flat_root=flat_root, + frame_step=frame_step, + image_ext=image_ext, + jpg_quality=jpg_quality, + ) + ) + + manifest_path = flat_root / "_flat_manifest.csv" + write_csv( + manifest_path, + manifest_rows, + ["group_name", "video_relpath", "output_file", "frame_idx", "time_sec", "frame_step", "image_ext"], + ) + print(f"Wrote flat extracted frames into {flat_root}") + print(f"Wrote flat extraction manifest: {manifest_path}") + return + + +def ensure_tool(name: str): + if shutil.which(name) is None: + raise SystemExit(f"Required tool not found in PATH: {name}") + + +def scan_videos(input_root: Path): + videos = [] + for path in sorted(input_root.rglob("*")): + if not path.is_file() or path.suffix.lower() not in VIDEO_EXTS: + continue + rel_path = path.relative_to(input_root) + group_name = rel_path.parts[0] if len(rel_path.parts) > 1 else "UNGROUPED" + info = probe_video(path) + videos.append( + VideoMeta( + abs_path=path, + rel_path=rel_path, + group_name=group_name, + file_name=path.name, + size_bytes=path.stat().st_size, + width=info["width"], + height=info["height"], + fps=info["fps"], + duration_sec=info["duration_sec"], + nb_frames=info["nb_frames"], + osd_present=("без osd" not in group_name.lower()), + ) + ) + return videos + + +def probe_video(path: Path): + cmd = [ + "ffprobe", + "-v", + "error", + "-select_streams", + "v:0", + "-show_entries", + "stream=width,height,r_frame_rate,avg_frame_rate,nb_frames,duration", + "-of", + "json", + str(path), + ] + data = json.loads(run_cmd(cmd)) + streams = data.get("streams", []) + if not streams: + raise SystemExit(f"No video stream found: {path}") + stream = streams[0] + fps_str = stream.get("avg_frame_rate") or stream.get("r_frame_rate") or "0/1" + fps = parse_fraction(fps_str) + duration = float(stream.get("duration") or 0.0) + nb_frames_raw = stream.get("nb_frames") + nb_frames = int(nb_frames_raw) if str(nb_frames_raw).isdigit() else int(round(duration * fps)) + return { + "width": int(stream.get("width") or 0), + "height": int(stream.get("height") or 0), + "fps": fps, + "duration_sec": duration, + "nb_frames": nb_frames, + } + + +def extract_range_to_dir( + meta: VideoMeta, + output_root: Path, + extracted_root: Path, + split: str, + role: str, + start_sec: float, + end_sec: float, + frame_step: int, + image_ext: str, + jpg_quality: int, + overwrite: bool, + interval_name: str, +): + target_dir = ( + extracted_root + / sanitize_path_component(split) + / sanitize_path_component(role) + / sanitize_path_component(meta.group_name) + / sanitize_path_component(meta.rel_path.stem) + / sanitize_path_component(interval_name) + ) + + if overwrite and target_dir.exists(): + shutil.rmtree(target_dir) + + existing_files = sorted(target_dir.glob(f"*.{image_ext}")) if target_dir.exists() else [] + if existing_files and (not overwrite): + return build_manifest_rows( + output_root=output_root, + meta=meta, + split=split, + role=role, + files=existing_files, + frame_step=frame_step, + ) + + target_dir.mkdir(parents=True, exist_ok=True) + + tmp_dir = target_dir / "_tmp" + if tmp_dir.exists(): + shutil.rmtree(tmp_dir) + tmp_dir.mkdir(parents=True, exist_ok=True) + + tmp_pattern = tmp_dir / f"tmp_%06d.{image_ext}" + cmd = [ + "ffmpeg", + "-hide_banner", + "-loglevel", + "warning", + "-y", + "-ss", + f"{start_sec:.3f}", + "-to", + f"{end_sec:.3f}", + "-i", + str(meta.abs_path), + "-vf", + f"select=not(mod(n\\,{frame_step}))", + "-vsync", + "vfr", + ] + if image_ext == "jpg": + cmd.extend(["-q:v", str(jpeg_qscale(jpg_quality))]) + cmd.append(str(tmp_pattern)) + run_cmd(cmd) + + files = sorted(tmp_dir.glob(f"*.{image_ext}")) + start_frame_idx = max(0, int(round(start_sec * meta.fps))) + final_files = [] + for idx, tmp_file in enumerate(files): + frame_idx = start_frame_idx + idx * frame_step + time_sec = frame_idx / max(1e-6, meta.fps) + final_name = ( + f"{sanitize_path_component(meta.rel_path.stem)}__" + f"f{frame_idx:06d}__t{time_sec:010.3f}.{image_ext}" + ) + final_path = target_dir / final_name + tmp_file.replace(final_path) + final_files.append(final_path) + + shutil.rmtree(tmp_dir) + return build_manifest_rows( + output_root=output_root, + meta=meta, + split=split, + role=role, + files=final_files, + frame_step=frame_step, + ) + + +def build_manifest_rows(output_root: Path, meta: VideoMeta, split: str, role: str, files, frame_step: int): + rows = [] + for file_path in files: + frame_idx, time_sec = parse_frame_info_from_name(file_path.stem, meta.fps) + rows.append( + { + "split": split, + "role": role, + "group_name": meta.group_name, + "video_relpath": normalize_relpath(meta.rel_path), + "output_file": normalize_relpath(file_path.relative_to(output_root)), + "frame_idx": frame_idx, + "time_sec": round(time_sec, 3), + "frame_step": frame_step, + "image_ext": file_path.suffix.lstrip(".").lower(), + } + ) + return rows + + +def parse_frame_info_from_name(stem: str, fps: float): + frame_idx = 0 + time_sec = 0.0 + for part in stem.split("__"): + if part.startswith("f") and part[1:].isdigit(): + frame_idx = int(part[1:]) + elif part.startswith("t"): + try: + time_sec = float(part[1:]) + except ValueError: + time_sec = frame_idx / max(1e-6, fps) + if time_sec <= 0.0 and frame_idx > 0: + time_sec = frame_idx / max(1e-6, fps) + return frame_idx, time_sec + + +def extract_range_flat( + meta: VideoMeta, + flat_root: Path, + frame_step: int, + image_ext: str, + jpg_quality: int, +): + tmp_dir = flat_root / f"_tmp_{sanitize_path_component(meta.rel_path.stem)}" + if tmp_dir.exists(): + shutil.rmtree(tmp_dir) + tmp_dir.mkdir(parents=True, exist_ok=True) + + tmp_pattern = tmp_dir / f"tmp_%06d.{image_ext}" + cmd = [ + "ffmpeg", + "-hide_banner", + "-loglevel", + "warning", + "-y", + "-i", + str(meta.abs_path), + "-vf", + f"select=not(mod(n\\,{frame_step}))", + "-vsync", + "vfr", + ] + if image_ext == "jpg": + cmd.extend(["-q:v", str(jpeg_qscale(jpg_quality))]) + cmd.append(str(tmp_pattern)) + run_cmd(cmd) + + rows = [] + files = sorted(tmp_dir.glob(f"*.{image_ext}")) + safe_group = sanitize_path_component(meta.group_name) + safe_video = sanitize_path_component(meta.rel_path.stem) + for idx, tmp_file in enumerate(files): + frame_idx = idx * frame_step + time_sec = frame_idx / max(1e-6, meta.fps) + final_name = f"{safe_group}__{safe_video}__f{frame_idx:06d}__t{time_sec:010.3f}.{image_ext}" + final_path = flat_root / final_name + tmp_file.replace(final_path) + rows.append( + { + "group_name": meta.group_name, + "video_relpath": normalize_relpath(meta.rel_path), + "output_file": final_path.name, + "frame_idx": frame_idx, + "time_sec": round(time_sec, 3), + "frame_step": frame_step, + "image_ext": image_ext, + } + ) + + shutil.rmtree(tmp_dir) + return rows + + +def parse_fraction(value: str): + num, den = value.split("/") + den_v = float(den) + if abs(den_v) < 1e-9: + return 0.0 + return float(num) / den_v + + +def write_catalog_outputs(videos, output_root: Path): + output_root.mkdir(parents=True, exist_ok=True) + + catalog_rows = [] + summary_map = {} + template_rows = [] + + for meta in videos: + rel_path_str = normalize_relpath(meta.rel_path) + catalog_rows.append( + { + "group_name": meta.group_name, + "osd_present": int(meta.osd_present), + "video_relpath": rel_path_str, + "file_name": meta.file_name, + "size_bytes": meta.size_bytes, + "size_gb": round(meta.size_bytes / (1024 ** 3), 4), + "duration_sec": round(meta.duration_sec, 3), + "duration_min": round(meta.duration_sec / 60.0, 2), + "fps": round(meta.fps, 6), + "width": meta.width, + "height": meta.height, + "nb_frames": meta.nb_frames, + } + ) + + key = meta.group_name + info = summary_map.setdefault(key, {"videos": 0, "bytes": 0, "duration": 0.0}) + info["videos"] += 1 + info["bytes"] += meta.size_bytes + info["duration"] += meta.duration_sec + + template_rows.append( + { + "enabled": "no", + "split": "train", + "role": "positive", + "video_relpath": rel_path_str, + "group_name": meta.group_name, + "start_sec": "", + "end_sec": "", + "frame_step": "2", + "image_ext": "png", + "jpg_quality": "95", + "notes": "Duplicate row for more intervals or use role=hard_negative for sparse negatives.", + } + ) + + summary_rows = [] + for group_name in sorted(summary_map): + info = summary_map[group_name] + summary_rows.append( + { + "group_name": group_name, + "video_count": info["videos"], + "total_size_gb": round(info["bytes"] / (1024 ** 3), 3), + "total_duration_min": round(info["duration"] / 60.0, 2), + "osd_present": int("без osd" not in group_name.lower()), + } + ) + + write_csv( + output_root / "video_catalog.csv", + catalog_rows, + [ + "group_name", + "osd_present", + "video_relpath", + "file_name", + "size_bytes", + "size_gb", + "duration_sec", + "duration_min", + "fps", + "width", + "height", + "nb_frames", + ], + ) + write_csv( + output_root / "group_summary.csv", + summary_rows, + ["group_name", "video_count", "total_size_gb", "total_duration_min", "osd_present"], + ) + write_csv( + output_root / "intervals_template.csv", + template_rows, + [ + "enabled", + "split", + "role", + "video_relpath", + "group_name", + "start_sec", + "end_sec", + "frame_step", + "image_ext", + "jpg_quality", + "notes", + ], + ) + + +def write_csv(path: Path, rows, fieldnames): + path.parent.mkdir(parents=True, exist_ok=True) + with path.open("w", newline="", encoding="utf-8-sig") as f: + writer = csv.DictWriter(f, fieldnames=fieldnames) + writer.writeheader() + for row in rows: + writer.writerow(row) + + +def load_intervals(path: Path): + with path.open("r", newline="", encoding="utf-8-sig") as f: + return list(csv.DictReader(f)) + + +def is_enabled(value: str): + return str(value).strip().lower() in {"1", "true", "yes", "y", "on"} + + +def clamp_float(value, min_value: float, max_value: float): + value_f = float(value) + return max(min_value, min(max_value, value_f)) + + +def format_interval_name(start_sec: float, end_sec: float, frame_step: int): + return f"s{int(math.floor(start_sec)):06d}_e{int(math.ceil(end_sec)):06d}_step{frame_step}" + + +def sanitize_path_component(value: str): + keep = [] + for ch in value: + if ch in '<>:"/\\|?*': + keep.append("_") + else: + keep.append(ch) + return "".join(keep).strip().rstrip(".") + + +def normalize_relpath(path): + return str(path).replace("\\", "/") + + +def jpeg_qscale(quality_pct: int): + quality_pct = max(70, min(100, int(quality_pct))) + if quality_pct >= 98: + return 1 + if quality_pct >= 92: + return 2 + if quality_pct >= 88: + return 3 + if quality_pct >= 82: + return 4 + return 5 + + +def run_cmd(cmd): + result = subprocess.run(cmd, capture_output=True, text=True) + if result.returncode != 0: + raise SystemExit( + f"Command failed ({result.returncode}): {' '.join(cmd)}\n" + f"stdout:\n{result.stdout}\n" + f"stderr:\n{result.stderr}" + ) + return result.stdout + + +if __name__ == "__main__": + main() diff --git a/dataset_prep/split_flat_dataset.py b/dataset_prep/split_flat_dataset.py new file mode 100644 index 0000000..a2a1505 --- /dev/null +++ b/dataset_prep/split_flat_dataset.py @@ -0,0 +1,159 @@ +import argparse +import csv +import os +import shutil +from pathlib import Path + + +def parse_args() -> argparse.Namespace: + parser = argparse.ArgumentParser( + description="Split a flat image folder by source-video prefixes from CSV." + ) + parser.add_argument( + "--flat-root", + type=Path, + required=True, + help="Flat folder with extracted images.", + ) + parser.add_argument( + "--split-csv", + type=Path, + required=True, + help="CSV with recommended_split and flat_prefix columns.", + ) + parser.add_argument( + "--output-root", + type=Path, + required=True, + help="Output folder that will contain train/valid/test.", + ) + parser.add_argument( + "--image-ext", + default="jpg", + help="Image extension in the flat folder, default: jpg.", + ) + parser.add_argument( + "--mode", + choices=("hardlink", "copy"), + default="hardlink", + help="Write mode for split files. hardlink avoids doubling disk usage.", + ) + parser.add_argument( + "--overwrite", + action="store_true", + help="Remove existing output_root before writing.", + ) + return parser.parse_args() + + +def load_split_rows(csv_path: Path) -> list[dict]: + with csv_path.open("r", encoding="utf-8-sig", newline="") as f: + rows = list(csv.DictReader(f)) + required = {"recommended_split", "flat_prefix"} + if not rows: + raise ValueError(f"No rows found in {csv_path}") + missing = required - set(rows[0].keys()) + if missing: + raise ValueError(f"Missing required CSV columns: {sorted(missing)}") + return rows + + +def ensure_clean_dir(path: Path, overwrite: bool) -> None: + if path.exists(): + if not overwrite: + raise FileExistsError( + f"Output path already exists: {path}. Use --overwrite to replace it." + ) + shutil.rmtree(path) + path.mkdir(parents=True, exist_ok=True) + + +def link_or_copy(src: Path, dst: Path, mode: str) -> str: + if mode == "hardlink": + try: + os.link(src, dst) + return "hardlink" + except OSError: + shutil.copy2(src, dst) + return "copy_fallback" + shutil.copy2(src, dst) + return "copy" + + +def main() -> None: + args = parse_args() + rows = load_split_rows(args.split_csv) + ext = args.image_ext.lower().lstrip(".") + + ensure_clean_dir(args.output_root, args.overwrite) + manifest_path = args.output_root / "_split_manifest.csv" + + split_name_map = { + "train": "train", + "val": "valid", + "valid": "valid", + "test": "test", + } + + total_written = 0 + total_missing = 0 + + with manifest_path.open("w", encoding="utf-8-sig", newline="") as mf: + writer = csv.DictWriter( + mf, + fieldnames=[ + "recommended_split", + "output_split", + "group_name", + "file_name", + "flat_prefix", + "written_count", + "missing", + "write_mode", + ], + ) + writer.writeheader() + + for row in rows: + split_raw = row["recommended_split"].strip().lower() + output_split = split_name_map.get(split_raw) + if output_split is None: + raise ValueError(f"Unsupported split value: {row['recommended_split']}") + + prefix = row["flat_prefix"].strip() + pattern = f"{prefix}__f*.{ext}" + matches = sorted(args.flat_root.glob(pattern)) + + out_dir = args.output_root / output_split + out_dir.mkdir(parents=True, exist_ok=True) + + write_mode = "" + for src in matches: + dst = out_dir / src.name + write_mode = link_or_copy(src, dst, args.mode) + + missing = 0 if matches else 1 + total_missing += missing + total_written += len(matches) + + writer.writerow( + { + "recommended_split": row["recommended_split"], + "output_split": output_split, + "group_name": row.get("group_name", ""), + "file_name": row.get("file_name", ""), + "flat_prefix": prefix, + "written_count": len(matches), + "missing": missing, + "write_mode": write_mode, + } + ) + + print(f"output_root={args.output_root}") + print(f"manifest={manifest_path}") + print(f"total_written={total_written}") + print(f"missing_prefixes={total_missing}") + + +if __name__ == "__main__": + main() diff --git a/decision_logger.py b/decision_logger.py new file mode 100644 index 0000000..c02818d --- /dev/null +++ b/decision_logger.py @@ -0,0 +1,347 @@ +import csv +import json +import time +from collections import Counter +from pathlib import Path + + +def _build_unique_path(base_path, default_suffix): + base = Path(base_path) + if base.suffix == "": + base = base.with_suffix(default_suffix) + + parent = base.parent if str(base.parent) not in ("", ".") else Path.cwd() + parent.mkdir(parents=True, exist_ok=True) + + stamp = time.strftime("%Y%m%d_%H%M%S") + candidate = parent / f"{base.stem}_{stamp}{base.suffix}" + attempt = 1 + while candidate.exists(): + candidate = parent / f"{base.stem}_{stamp}_{attempt:02d}{base.suffix}" + attempt += 1 + return candidate + + +def _box_to_metrics(box): + if box is None: + return "", "", "", "" + + x1, y1, x2, y2 = [float(v) for v in box] + cx = 0.5 * (x1 + x2) + cy = 0.5 * (y1 + y2) + w = max(0.0, x2 - x1) + h = max(0.0, y2 - y1) + return round(cx, 3), round(cy, 3), round(w, 3), round(h, 3) + + +class TrackingDecisionLogger: + FIELDNAMES = [ + "frame_id", + "timestamp_sec", + "dt_sec", + "source_kind", + "status", + "confirmed", + "target_id", + "target_track_score", + "locked", + "locked_cx", + "locked_cy", + "locked_w", + "locked_h", + "pred_cx", + "pred_cy", + "pred_w", + "pred_h", + "hit_streak", + "miss_streak", + "acquire_score", + "preacq_hits", + "switch_candidate_hits", + "best_score", + "have_yolo", + "yolo_mode", + "yolo_raw_count", + "det_count", + "merged_part_count", + "infer_ms", + "stale_lock_active", + "stale_lock_reason", + "fast_handoff_active", + "guidance_reset_event", + "used_prediction_hold", + "klt_valid", + "klt_quality", + "klt_points", + "speed", + "adaptive_chase_stage", + "approach_active", + "close_force_fullscan", + "fast_maneuver_guard", + "motion_active_zones", + "wavelet_active", + "wavelet_energy", + "wavelet_bonus", + "wavelet_hits", + "autogaze_ready", + "autogaze_stale", + "autogaze_age", + "autogaze_cells", + "autogaze_ms", + "guidance_active", + "guidance_status", + "guidance_confidence", + "guidance_error_x", + "guidance_error_y", + "guidance_cmd_x", + "guidance_cmd_y", + "guidance_on_target", + "events", + ] + + def __init__(self, enabled, csv_path, summary_path, flush_every=30): + self.enabled = bool(enabled) + self.flush_every = max(1, int(flush_every)) + self.csv_path = None + self.summary_path = None + self._csv_file = None + self._writer = None + self._rows_since_flush = 0 + + self.prev_confirmed = None + self.prev_locked = None + self.prev_target_id = None + + self.frames = 0 + self.confirmed_frames = 0 + self.locked_frames = 0 + self.prediction_hold_frames = 0 + self.stale_lock_frames = 0 + self.fast_handoff_frames = 0 + self.autogaze_used_frames = 0 + self.wavelet_active_frames = 0 + self.guidance_active_frames = 0 + self.guidance_on_target_frames = 0 + self.guidance_reset_events = 0 + self.guidance_confidence_sum = 0.0 + self.max_hit_streak = 0 + self.max_miss_streak = 0 + self.klt_quality_confirmed_sum = 0.0 + self.klt_quality_confirmed_count = 0 + self.infer_ms_sum = 0.0 + self.yolo_mode_counts = Counter() + self.event_counts = Counter() + + if not self.enabled: + return + + self.csv_path = _build_unique_path(csv_path, ".csv") + self.summary_path = _build_unique_path(summary_path, ".json") + self._csv_file = self.csv_path.open("w", newline="", encoding="utf-8") + self._writer = csv.DictWriter(self._csv_file, fieldnames=self.FIELDNAMES) + self._writer.writeheader() + + def status_line(self): + if not self.enabled: + return "Tracking decision log disabled" + return f"Tracking decision log: csv={self.csv_path} summary={self.summary_path}" + + def log_frame(self, **kwargs): + if not self.enabled or self._writer is None: + return + + confirmed = bool(kwargs.get("confirmed", False)) + locked = bool(kwargs.get("locked", False)) + target_id = kwargs.get("target_id", None) + + events = [] + if self.prev_confirmed is not None: + if (not self.prev_confirmed) and confirmed: + events.append("confirm_enter") + elif self.prev_confirmed and (not confirmed): + events.append("confirm_exit") + + if (not self.prev_locked) and locked: + events.append("lock_acquired") + elif self.prev_locked and (not locked): + events.append("lock_lost") + + if ( + confirmed + and (self.prev_target_id is not None) + and (target_id is not None) + and (self.prev_target_id != target_id) + ): + events.append("target_switch") + guidance_reset_event = str(kwargs.get("guidance_reset_event", "")).strip() + if guidance_reset_event == "stale_lock_break": + events.append("stale_lock_break") + if bool(kwargs.get("fast_handoff_active", False)) and ("target_switch" in events): + events.append("fast_handoff_switch") + if guidance_reset_event: + events.append("guidance_reset") + + locked_cx, locked_cy, locked_w, locked_h = _box_to_metrics(kwargs.get("locked_box")) + pred_cx, pred_cy, pred_w, pred_h = _box_to_metrics(kwargs.get("pred_box")) + + row = { + "frame_id": int(kwargs.get("frame_id", -1)), + "timestamp_sec": round(float(kwargs.get("timestamp_sec", 0.0)), 6), + "dt_sec": round(float(kwargs.get("dt_sec", 0.0)), 6), + "source_kind": kwargs.get("source_kind", ""), + "status": kwargs.get("status", ""), + "confirmed": int(confirmed), + "target_id": "" if target_id is None else int(target_id), + "target_track_score": self._maybe_float(kwargs.get("target_track_score")), + "locked": int(locked), + "locked_cx": locked_cx, + "locked_cy": locked_cy, + "locked_w": locked_w, + "locked_h": locked_h, + "pred_cx": pred_cx, + "pred_cy": pred_cy, + "pred_w": pred_w, + "pred_h": pred_h, + "hit_streak": int(kwargs.get("hit_streak", 0)), + "miss_streak": int(kwargs.get("miss_streak", 0)), + "acquire_score": int(kwargs.get("acquire_score", 0)), + "preacq_hits": int(kwargs.get("preacq_hits", 0)), + "switch_candidate_hits": int(kwargs.get("switch_candidate_hits", 0)), + "best_score": self._maybe_float(kwargs.get("best_score")), + "have_yolo": int(bool(kwargs.get("have_yolo", False))), + "yolo_mode": kwargs.get("yolo_mode", ""), + "yolo_raw_count": int(kwargs.get("yolo_raw_count", 0)), + "det_count": int(kwargs.get("det_count", 0)), + "merged_part_count": int(kwargs.get("merged_part_count", 0)), + "infer_ms": round(float(kwargs.get("infer_ms", 0.0)), 3), + "stale_lock_active": int(bool(kwargs.get("stale_lock_active", False))), + "stale_lock_reason": kwargs.get("stale_lock_reason", ""), + "fast_handoff_active": int(bool(kwargs.get("fast_handoff_active", False))), + "guidance_reset_event": guidance_reset_event, + "used_prediction_hold": int(bool(kwargs.get("used_prediction_hold", False))), + "klt_valid": int(bool(kwargs.get("klt_valid", False))), + "klt_quality": round(float(kwargs.get("klt_quality", 0.0)), 4), + "klt_points": int(kwargs.get("klt_points", 0)), + "speed": round(float(kwargs.get("speed", 0.0)), 3), + "adaptive_chase_stage": kwargs.get("adaptive_chase_stage", ""), + "approach_active": int(bool(kwargs.get("approach_active", False))), + "close_force_fullscan": int(bool(kwargs.get("close_force_fullscan", False))), + "fast_maneuver_guard": int(bool(kwargs.get("fast_maneuver_guard", False))), + "motion_active_zones": int(kwargs.get("motion_active_zones", 0)), + "wavelet_active": int(bool(kwargs.get("wavelet_active", False))), + "wavelet_energy": round(float(kwargs.get("wavelet_energy", 0.0)), 4), + "wavelet_bonus": round(float(kwargs.get("wavelet_bonus", 0.0)), 4), + "wavelet_hits": int(kwargs.get("wavelet_hits", 0)), + "autogaze_ready": int(bool(kwargs.get("autogaze_ready", False))), + "autogaze_stale": int(bool(kwargs.get("autogaze_stale", True))), + "autogaze_age": int(kwargs.get("autogaze_age", -1)), + "autogaze_cells": int(kwargs.get("autogaze_cells", 0)), + "autogaze_ms": round(float(kwargs.get("autogaze_ms", 0.0)), 3), + "guidance_active": int(bool(kwargs.get("guidance_active", False))), + "guidance_status": kwargs.get("guidance_status", ""), + "guidance_confidence": round(float(kwargs.get("guidance_confidence", 0.0)), 4), + "guidance_error_x": round(float(kwargs.get("guidance_error_x", 0.0)), 4), + "guidance_error_y": round(float(kwargs.get("guidance_error_y", 0.0)), 4), + "guidance_cmd_x": round(float(kwargs.get("guidance_cmd_x", 0.0)), 4), + "guidance_cmd_y": round(float(kwargs.get("guidance_cmd_y", 0.0)), 4), + "guidance_on_target": int(bool(kwargs.get("guidance_on_target", False))), + "events": "|".join(events), + } + + self._writer.writerow(row) + self._rows_since_flush += 1 + if self._rows_since_flush >= self.flush_every: + self._csv_file.flush() + self._rows_since_flush = 0 + + self.frames += 1 + self.confirmed_frames += int(confirmed) + self.locked_frames += int(locked) + self.prediction_hold_frames += int(bool(kwargs.get("used_prediction_hold", False))) + self.stale_lock_frames += int(bool(kwargs.get("stale_lock_active", False))) + self.fast_handoff_frames += int(bool(kwargs.get("fast_handoff_active", False))) + self.autogaze_used_frames += int(bool(kwargs.get("autogaze_ready", False)) and (not bool(kwargs.get("autogaze_stale", True)))) + self.wavelet_active_frames += int(bool(kwargs.get("wavelet_active", False))) + self.guidance_active_frames += int(bool(kwargs.get("guidance_active", False))) + self.guidance_on_target_frames += int(bool(kwargs.get("guidance_on_target", False))) + self.guidance_reset_events += int(bool(guidance_reset_event)) + self.guidance_confidence_sum += float(kwargs.get("guidance_confidence", 0.0)) + self.max_hit_streak = max(self.max_hit_streak, int(kwargs.get("hit_streak", 0))) + self.max_miss_streak = max(self.max_miss_streak, int(kwargs.get("miss_streak", 0))) + self.infer_ms_sum += float(kwargs.get("infer_ms", 0.0)) + + if confirmed: + self.klt_quality_confirmed_sum += float(kwargs.get("klt_quality", 0.0)) + self.klt_quality_confirmed_count += 1 + + yolo_mode = str(kwargs.get("yolo_mode", "")).strip() + if yolo_mode: + self.yolo_mode_counts[yolo_mode] += 1 + for event in events: + self.event_counts[event] += 1 + + self.prev_confirmed = confirmed + self.prev_locked = locked + self.prev_target_id = target_id + + def close(self): + if not self.enabled: + return None + + if self._csv_file is not None: + self._csv_file.flush() + self._csv_file.close() + self._csv_file = None + + avg_klt_quality_confirmed = 0.0 + if self.klt_quality_confirmed_count > 0: + avg_klt_quality_confirmed = self.klt_quality_confirmed_sum / float(self.klt_quality_confirmed_count) + + avg_infer_ms = 0.0 + if self.frames > 0: + avg_infer_ms = self.infer_ms_sum / float(self.frames) + + avg_guidance_confidence = 0.0 + if self.frames > 0: + avg_guidance_confidence = self.guidance_confidence_sum / float(self.frames) + + summary = { + "frames": self.frames, + "confirmed_frames": self.confirmed_frames, + "recover_frames": max(0, self.frames - self.confirmed_frames), + "locked_frames": self.locked_frames, + "prediction_hold_frames": self.prediction_hold_frames, + "stale_lock_frames": self.stale_lock_frames, + "fast_handoff_frames": self.fast_handoff_frames, + "autogaze_used_frames": self.autogaze_used_frames, + "wavelet_active_frames": self.wavelet_active_frames, + "guidance_active_frames": self.guidance_active_frames, + "guidance_on_target_frames": self.guidance_on_target_frames, + "guidance_reset_events": self.guidance_reset_events, + "confirm_entries": int(self.event_counts.get("confirm_enter", 0)), + "confirm_exits": int(self.event_counts.get("confirm_exit", 0)), + "lock_acquired_events": int(self.event_counts.get("lock_acquired", 0)), + "lock_lost_events": int(self.event_counts.get("lock_lost", 0)), + "target_switches": int(self.event_counts.get("target_switch", 0)), + "max_hit_streak": self.max_hit_streak, + "max_miss_streak": self.max_miss_streak, + "avg_klt_quality_confirmed": round(avg_klt_quality_confirmed, 6), + "avg_guidance_confidence": round(avg_guidance_confidence, 6), + "avg_infer_ms": round(avg_infer_ms, 6), + "yolo_mode_counts": dict(self.yolo_mode_counts), + "event_counts": dict(self.event_counts), + "csv_path": str(self.csv_path), + "summary_path": str(self.summary_path), + } + + if self.summary_path is not None: + with self.summary_path.open("w", encoding="utf-8") as f: + json.dump(summary, f, ensure_ascii=True, indent=2) + + return summary + + @staticmethod + def _maybe_float(value): + if value is None: + return "" + return round(float(value), 6) diff --git a/docker-compose.yml b/docker-compose.yml new file mode 100644 index 0000000..46d28e1 --- /dev/null +++ b/docker-compose.yml @@ -0,0 +1,34 @@ +services: + fpv-tracker: + build: + context: . + dockerfile: Dockerfile + args: + PYTORCH_INDEX_URL: https://download.pytorch.org/whl/cu128 + image: fpv-tracker:cu128-offline + gpus: all + restart: unless-stopped + stdin_open: true + tty: true + environment: + FPV_MODEL_PATH: /app/best.pt + FPV_SOURCE: "0" + FPV_SHOW_OUTPUT: "0" + FPV_SAVE_INFER_VIDEO: "1" + FPV_OUT_VIDEO_PATH: /data/out/out_infer.mp4 + FPV_GUIDANCE_EXPORT_ENABLE: "1" + FPV_GUIDANCE_EXPORT_PATH: /data/guidance/guidance_state.json + FPV_AUTOPILOT_ENABLE: "1" + FPV_AUTOPILOT_BACKEND: "json" + FPV_AUTOPILOT_JSON_PATH: /data/autopilot/autopilot_cmd.json + FPV_PROTO_UDP_ENABLE: "0" + FPV_PROTO_UDP_HOST: "192.168.1.10" + FPV_PROTO_UDP_PORT: "5005" + volumes: + - ./runtime-data:/data + ports: + - "5600:5600/udp" + + # Для Linux-камеры можно раскомментировать: + # devices: + # - /dev/video0:/dev/video0 diff --git a/docker/entrypoint.sh b/docker/entrypoint.sh new file mode 100644 index 0000000..8525d6a --- /dev/null +++ b/docker/entrypoint.sh @@ -0,0 +1,20 @@ +#!/usr/bin/env bash +set -euo pipefail + +mkdir -p /data/out /data/guidance /data/autopilot + +if [[ -n "${FPV_OUT_VIDEO_PATH:-}" ]]; then + mkdir -p "$(dirname "${FPV_OUT_VIDEO_PATH}")" +fi + +if [[ -n "${FPV_GUIDANCE_EXPORT_PATH:-}" ]]; then + mkdir -p "$(dirname "${FPV_GUIDANCE_EXPORT_PATH}")" +fi + +if [[ -n "${FPV_AUTOPILOT_JSON_PATH:-}" ]]; then + mkdir -p "$(dirname "${FPV_AUTOPILOT_JSON_PATH}")" +fi + +echo "[entrypoint] source=${FPV_SOURCE:-} backend=${FPV_AUTOPILOT_BACKEND:-json} proto_udp=${FPV_PROTO_UDP_ENABLE:-0}" + +exec "$@" diff --git a/docs/ARCHITECTURE_RU.md b/docs/ARCHITECTURE_RU.md new file mode 100644 index 0000000..aa90b7b --- /dev/null +++ b/docs/ARCHITECTURE_RU.md @@ -0,0 +1,219 @@ +# Архитектура проекта + +## 1. Общая схема + +Проект состоит из нескольких слоев: + +1. прием кадров; +2. детекция; +3. трекинг и удержание; +4. оценка движения и восстановление; +5. guidance; +6. выдача команд наружу; +7. логирование и отладка. + +## 2. Основная точка входа + +### `main.py` + +Это главный runtime-файл. Он: + +- загружает модель YOLO; +- открывает источник через `open_source`; +- запускает YOLO worker; +- создает `AutoGazeROIWorker` и пытается его запустить, но по умолчанию `AUTOGAZE_ENABLE = False`, поэтому модуль обычно не активен; +- инициализирует Kalman, KLT и ByteTrack; +- ведет основной цикл обработки кадров; +- строит итоговый lock и guidance; +- пишет выходное видео и логи. + +## 3. Конфигурация + +### `config.py` + +Содержит: + +- путь к модели; +- источник видео; +- настройки YOLO; +- настройки KLT; +- настройки ByteTrack; +- настройки guidance; +- настройки ROI и recovery; +- параметры записи видео и логов. + +### `config_intercept.py` + +Содержит: + +- range estimation; +- proportional navigation; +- intercept FSM; +- настройки backend'ов управления; +- испытательный UDP-протокол; +- IMM/дополнительные модули. + +### `runtime_env.py` + +Накладывает env-overrides поверх значений из `config.py` и `config_intercept.py`. + +Это особенно важно для Docker, потому что позволяет не редактировать код под каждый запуск. + +## 4. Источник видео + +### `helpers.py` + +Ключевые функции: + +- `open_source` — открывает `camera`, `stream` или `file`; +- `get_frame_timestamp_seconds` — вычисляет временную метку кадра; +- `sanitize_dt` — стабилизирует `dt`; +- геометрические и ROI-функции. + +## 5. Детекция + +### `yolo_worker.py` + +Асинхронный worker для запуска YOLO. + +Работает в двух режимах: + +- по ROI; +- по полному кадру. + +Возвращает: + +- список детекций; +- время инференса; +- признак, использовался ли ROI. + +## 6. Трекинг и удержание + +### `bytetrack_min_aggressive.py` + +Локальная реализация ByteTrack, добавленная в корень проекта для самодостаточного контейнерного runtime. + +### `trackers.py` + +Базовые Kalman-трекеры. + +### `trackers_safe.py` + +Более безопасные ограничения для Kalman. + +### `trackers_hybrid.py` + +Гибридный KLT-трекер. + +## 7. Восстановление и anti-stall логика + +### `target_handoff.py` + +Логика: + +- stale lock; +- fast handoff; +- override guidance; +- временное удержание re-anchor-кандидата. + +### `template_matching.py` + +Fallback через шаблонное сопоставление. + +### `camera_motion.py` + +Оценка глобального движения камеры и компенсация. + +### `motion_saliency.py` + +Помогает переоценивать детекции по движению. + +### `stationary_killer.py` + +Отбрасывает залипшие и статичные треки. + +### `autogaze_runner.py` + +Опциональный ROI prior для recover. По умолчанию `AUTOGAZE_ENABLE = False`. + +## 8. Guidance + +### `guidance.py` + +Задачи: + +- построение точки прицеливания; +- сглаживание команд; +- удержание при miss; +- экспорт `guidance_state.json`; +- отрисовка crosshair, точки и желтой линии. + +Именно этот слой строит: + +- `aim_x`, `aim_y`; +- `steer_x`, `steer_y`; +- `confidence`; +- `status` (`SEARCH`, `LOCK`, `HOLD`, `REACQ`). + +## 9. Наведение и внешние backend'ы + +### `autopilot_bridge.py` + +Преобразует guidance-state в внешние команды. + +Поддерживает: + +- `json` +- `mavlink` +- `msp` +- `proto_udp` + +### `range_estimation.py` + +Оценка дальности по видимому размеру цели. + +### `proportional_navigation.py` + +PN-логика. + +### `intercept_fsm.py` + +FSM фаз: + +- `SEARCH` +- `ACQUIRE` +- `TRACK` +- `INTERCEPT` +- `TERMINAL` +- `LOST` + +### `imu_fusion.py` + +Дополнительный модуль IMU/MAVLink/MSP. Отдельный, не является обязательной частью базового запуска. + +## 10. Логирование + +### `decision_logger.py` + +Записывает: + +- покадровые решения; +- причины переключений; +- stale-lock и guidance-reset события. + +Выход: + +- `track_log_*.csv` +- `track_summary_*.json` + +## 11. Что не относится к основному runtime + +### `cvat/` + +Это отдельный большой каталог для CVAT и не нужен для повседневного запуска трекера. + +### `dataset_prep/` + +Скрипты подготовки датасета. + +Они не нужны для обычного запуска `main.py`. diff --git a/docs/CONFIG_REFERENCE_RU.md b/docs/CONFIG_REFERENCE_RU.md new file mode 100644 index 0000000..9408143 --- /dev/null +++ b/docs/CONFIG_REFERENCE_RU.md @@ -0,0 +1,233 @@ +# Справочник конфигурации + +## 1. Общий принцип + +У проекта два основных файла конфигурации: + +- `config.py` +- `config_intercept.py` + +Плюс слой env-overrides: + +- `runtime_env.py` + +Если проект запускается локально, обычно проще править `config.py` и `config_intercept.py`. + +Если проект запускается в Docker, лучше использовать env-переменные. + +## 2. Главные параметры `config.py` + +### Источник и модель + +- `MODEL_PATH` — путь к `best.pt` +- `DEVICE` — индекс CUDA-устройства +- `USE_HALF` — half precision на GPU +- `SOURCE` — видеофайл, индекс камеры, RTSP или UDP +- `VIDEO_REALTIME` — pacing по времени источника +- `CAP_BACKEND` — backend OpenCV для камер + +### Вывод + +- `SHOW_OUTPUT` — показывать окно OpenCV +- `WINDOW_NAME` — имя окна +- `SAVE_INFER_VIDEO` — писать выходное видео +- `OUT_VIDEO_PATH` — базовое имя выходного видео + +### Детектор YOLO + +- `CONF` +- `IMG_SIZE_ROI` +- `IMG_SIZE_FULL` +- `MAX_DET` +- `TARGET_CLASS_ID` + +### KLT + +- `KLT_MAX_CORNERS` +- `KLT_QUALITY` +- `KLT_MIN_DIST` +- `KLT_WIN` +- `KLT_MAX_LEVEL` +- `KLT_OUTLIER_THRESH` +- `KLT_OK_Q` +- `KLT_OK_PTS` +- `KLT_REFRESH_WITH_YOLO` + +### ByteTrack / Kalman + +- `BT_HIGH` +- `BT_LOW` +- `BT_NEW` +- `BT_MATCH_IOU` +- `BT_BUFFER` +- `BT_MIN_HITS` +- `BT_MAX_PREDICT_AGE` +- `KALMAN_HOLD_MAX` + +### Guidance + +- `GUIDANCE_ENABLE` +- `DRAW_GUIDANCE` +- `GUIDANCE_EXPORT_ENABLE` +- `GUIDANCE_EXPORT_PATH` +- `GUIDANCE_EXPORT_EVERY` +- `GUIDANCE_CMD_SMOOTH` +- `GUIDANCE_POINT_SMOOTH` +- `GUIDANCE_CONF_MIN` +- `GUIDANCE_OVERRIDE_ENABLE` +- `GUIDANCE_OVERRIDE_MISS_GE` +- `GUIDANCE_OVERRIDE_TTL` + +## 3. Главные параметры `config_intercept.py` + +### Range estimation + +- `RANGE_ENABLE` +- `CAMERA_FOCAL_LENGTH_PX` +- `TARGET_KNOWN_SIZE_M` +- `RANGE_MIN_APPARENT_SIZE_PX` +- `RANGE_MAX_M` +- `RANGE_MIN_M` + +### Proportional navigation + +- `PN_ENABLE` +- `PN_NAV_GAIN` +- `PN_AUGMENTED` +- `PN_AUG_GAIN` +- `PN_MAX_ACCEL_CMD` + +### Intercept FSM + +- `INTERCEPT_FSM_ENABLE` +- `FSM_ACQUIRE_RANGE_M` +- `FSM_TRACK_CONFIRM_HITS` +- `FSM_INTERCEPT_RANGE_M` +- `FSM_TERMINAL_TAU_SEC` +- `FSM_TERMINAL_RANGE_M` +- `FSM_LOST_TIMEOUT` + +### Внешние backend'ы + +- `AUTOPILOT_ENABLE` +- `AUTOPILOT_BACKEND` +- `AUTOPILOT_JSON_PATH` +- `AUTOPILOT_JSON_EVERY` +- `MAVLINK_CONNECTION` +- `MSP_PORT` +- `MSP_BAUD` + +### Испытательный UDP-протокол + +- `PROTO_UDP_ENABLE` +- `PROTO_UDP_HOST` +- `PROTO_UDP_PORT` +- `PROTO_UDP_DESCRIPTOR` + +### IMM / расширенные модули + +- `IMM_ENABLE` +- `IMM_REPLACE_KALMAN` + +## 4. Env-переменные, которые реально поддерживаются + +Ниже перечислен точный набор env-overrides из `runtime_env.py`. + +### Переопределения `config.py` + +- `FPV_MODEL_PATH` +- `FPV_SOURCE` +- `FPV_VIDEO_REALTIME` +- `FPV_SHOW_OUTPUT` +- `FPV_SAVE_INFER_VIDEO` +- `FPV_OUT_VIDEO_PATH` +- `FPV_GUIDANCE_EXPORT_ENABLE` +- `FPV_GUIDANCE_EXPORT_PATH` + +### Переопределения `config_intercept.py` + +- `FPV_AUTOPILOT_ENABLE` +- `FPV_AUTOPILOT_BACKEND` +- `FPV_AUTOPILOT_JSON_PATH` +- `FPV_MAVLINK_CONNECTION` +- `FPV_MSP_PORT` +- `FPV_PROTO_UDP_ENABLE` +- `FPV_PROTO_UDP_HOST` +- `FPV_PROTO_UDP_PORT` +- `FPV_PROTO_UDP_DESCRIPTOR` + +## 5. Как интерпретируется `FPV_SOURCE` + +### Камера + +```text +FPV_SOURCE=0 +``` + +Если строка состоит только из цифр, она будет преобразована в индекс камеры. + +### RTSP / HTTP / UDP / RTMP + +```text +FPV_SOURCE=rtsp://192.168.1.10:8554/live +FPV_SOURCE=udp://@0.0.0.0:5600 +``` + +Такие источники открываются как поток. + +### Видео-файл + +```text +FPV_SOURCE=/data/input/test.mp4 +``` + +## 6. Рекомендуемые пресеты запуска + +### Отладка на видеофайле + +- `SOURCE` = путь к файлу +- `AUTOPILOT_BACKEND = "json"` +- `SHOW_OUTPUT = True` +- `SAVE_INFER_VIDEO = True` + +### Реальный live-поток + +- `SOURCE` = камера/RTSP/UDP +- `AUTOPILOT_BACKEND = "json"` на первом этапе +- `SHOW_OUTPUT` по ситуации + +### Испытания протокола + +- `AUTOPILOT_BACKEND = "proto_udp"` +- `PROTO_UDP_ENABLE = True` +- `PROTO_UDP_HOST` и `PROTO_UDP_PORT` под вашу сеть + +## 7. Что лучше менять через env, а что через файлы + +Через env лучше менять: + +- источник; +- backend; +- пути к выходным файлам; +- включение UDP-протокола; +- сетевые адреса и порты. + +Через `config.py` и `config_intercept.py` лучше менять: + +- глубокий тюнинг трекера; +- пороги KLT/ByteTrack/Kalman; +- ROI- и recover-параметры; +- параметры PN/FSM; +- экспериментальные флаги. + +## 8. Что не переопределяется через env + +На текущий момент через env не вынесены все сотни тюнинговых параметров из `config.py`. + +Это сделано специально, чтобы: + +- оставить Docker-конфигурацию компактной; +- не раздувать runtime API; +- не превращать проект в огромный набор shell-параметров. + +Если вам нужно контейнерно менять дополнительный параметр, его можно добавить в `runtime_env.py`. diff --git a/docs/PROTOCOL_UDP_RU.md b/docs/PROTOCOL_UDP_RU.md new file mode 100644 index 0000000..1b4fe25 --- /dev/null +++ b/docs/PROTOCOL_UDP_RU.md @@ -0,0 +1,232 @@ +# Испытательный UDP-протокол + +## 1. Назначение + +Этот backend нужен для испытаний, когда проект должен отправлять состояние цели в блок наведения по UDP в локальной сети. + +В проекте этот режим включается через: + +```text +AUTOPILOT_BACKEND = "proto_udp" +PROTO_UDP_ENABLE = True +``` + +или через env: + +```text +FPV_AUTOPILOT_BACKEND=proto_udp +FPV_PROTO_UDP_ENABLE=1 +``` + +## 2. Куда это встроено + +Реализация находится в: + +- `autopilot_bridge.py` + +Новый backend называется: + +- `proto_udp` + +## 3. Транспорт + +- протокол: UDP +- адрес: `PROTO_UDP_HOST` +- порт: `PROTO_UDP_PORT` + +По умолчанию: + +```python +PROTO_UDP_HOST = "127.0.0.1" +PROTO_UDP_PORT = 5005 +``` + +## 4. Формат пакета + +Формат упаковки: + +```text + если цели нет, все значения остаются нулевыми + +## 7. Как вычисляются значения + +### Смещение в пикселях + +Берется из guidance-точки: + +- `aim_x` +- `aim_y` + +Относительно центра кадра: + +- `offset_x_px = aim_x - center_x` +- `offset_y_px = center_y - aim_y` + +То есть знак сделан так, чтобы: + +- вправо было положительным; +- вверх было положительным. + +### Смещение в процентах + +Нормируется к половине размера кадра: + +- по X к `frame_w / 2` +- по Y к `frame_h / 2` + +И затем ограничивается до `-100..100`. + +### Площадь bbox + +Используются: + +- `box_w` +- `box_h` +- `frame_w` +- `frame_h` + +Формула: + +```text +bbox_area_pct = 100 * (box_w * box_h) / (frame_w * frame_h) +``` + +## 8. Включение в локальном запуске + +### PowerShell + +```powershell +$env:FPV_AUTOPILOT_BACKEND = "proto_udp" +$env:FPV_PROTO_UDP_ENABLE = "1" +$env:FPV_PROTO_UDP_HOST = "192.168.1.50" +$env:FPV_PROTO_UDP_PORT = "5005" +python main.py +``` + +### Bash + +```bash +export FPV_AUTOPILOT_BACKEND=proto_udp +export FPV_PROTO_UDP_ENABLE=1 +export FPV_PROTO_UDP_HOST=192.168.1.50 +export FPV_PROTO_UDP_PORT=5005 +python3 main.py +``` + +## 9. Включение в Docker + +В `docker-compose.yml`: + +```yaml +FPV_AUTOPILOT_BACKEND: "proto_udp" +FPV_PROTO_UDP_ENABLE: "1" +FPV_PROTO_UDP_HOST: "192.168.1.50" +FPV_PROTO_UDP_PORT: "5005" +``` + +## 10. Пример приема пакета на Python + +```python +import socket +import struct + +sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) +sock.bind(("0.0.0.0", 5005)) + +while True: + data, addr = sock.recvfrom(1024) + descriptor, object_id, target_state, off_y_px, off_x_px, off_y_pct, off_x_pct, bbox_area_pct = struct.unpack( + " **For agentic workers:** REQUIRED SUB-SKILL: Use superpowers:subagent-driven-development (recommended) or superpowers:executing-plans to implement this plan task-by-task. Steps use checkbox (`- [ ]`) syntax for tracking. + +**Goal:** Make the tracker abandon stale lock anchors quickly during sharp turns, switch to valid new YOLO/ByteTrack candidates in `1-2` frames when safe, and reset guidance smoothing so the blue point and yellow line immediately follow the new target region. + +**Architecture:** Extract the stale-lock and fast-handoff decision logic into a small pure-Python helper module so it can be unit-tested without running the full video pipeline. Then integrate that logic into `main.py`, add event-driven guidance reset hooks in `guidance.py`, and extend `decision_logger.py` so before/after behavior can be measured from logs. The plan assumes the current workspace is not a git repository; commit commands are included for use once the code is executed inside a repo-backed workspace. + +**Tech Stack:** Python 3.12, pytest, NumPy, OpenCV-based tracking pipeline, existing local CSV/JSON diagnostics. + +--- + +## File Structure + +- Create: `target_handoff.py` + - Responsibility: pure decision helpers for stale-lock detection, fast-handoff threshold selection, and lightweight switch event representation. +- Create: `tests/test_target_handoff.py` + - Responsibility: unit tests for stale-lock break and fast-handoff policy. +- Modify: `guidance.py` + - Responsibility: add explicit reset hook for smoothing state on stale-break and target switch. +- Create: `tests/test_guidance_reset.py` + - Responsibility: unit tests that prove aim/command smoothing is reset correctly. +- Modify: `main.py` + - Responsibility: compute stale-lock inputs, invoke handoff helpers, suppress stale prediction hold, propagate reset events to guidance, and emit diagnostics. +- Modify: `decision_logger.py` + - Responsibility: add CSV fields and summary accounting for stale-lock/handoff/guidance-reset events. +- Create: `tests/test_decision_logger_events.py` + - Responsibility: verify new CSV fields and event serialization. +- Modify: `config.py` + - Responsibility: add narrowly scoped tuning knobs for stale-lock break and fast handoff. + +## Task 1: Add Pure Handoff Decision Module + +**Files:** +- Create: `target_handoff.py` +- Test: `tests/test_target_handoff.py` + +- [ ] **Step 1: Write the failing tests** + +```python +from pathlib import Path +import sys + + +PROJECT_ROOT = Path(__file__).resolve().parents[1] +if str(PROJECT_ROOT) not in sys.path: + sys.path.insert(0, str(PROJECT_ROOT)) + +from target_handoff import ( + HandoffDecision, + compute_fast_handoff_hits, + evaluate_stale_lock, +) + + +def test_evaluate_stale_lock_requires_fresh_candidate_and_weak_anchor(): + decision = evaluate_stale_lock( + confirmed=True, + have_fresh_candidate=True, + target_has_fresh_update=False, + candidate_dist_px=420.0, + pred_diag_px=70.0, + miss_streak=8, + klt_valid=False, + klt_quality=0.18, + klt_iou=0.12, + candidate_motion_ok=True, + target_motion_ok=False, + stale_break_dist_diag=3.5, + stale_break_min_miss=2, + stale_break_klt_quality=0.35, + stale_break_klt_iou=0.25, + ) + + assert decision.stale_lock_active is True + assert "target_not_fresh" in decision.reasons + assert "weak_klt_quality" in decision.reasons + + +def test_evaluate_stale_lock_rejects_close_candidate_when_anchor_is_healthy(): + decision = evaluate_stale_lock( + confirmed=True, + have_fresh_candidate=True, + target_has_fresh_update=True, + candidate_dist_px=90.0, + pred_diag_px=70.0, + miss_streak=0, + klt_valid=True, + klt_quality=0.92, + klt_iou=0.71, + candidate_motion_ok=True, + target_motion_ok=True, + stale_break_dist_diag=3.5, + stale_break_min_miss=2, + stale_break_klt_quality=0.35, + stale_break_klt_iou=0.25, + ) + + assert decision.stale_lock_active is False + assert decision.reasons == [] + + +def test_compute_fast_handoff_hits_reduces_confirmation_under_stale_lock(): + hits = compute_fast_handoff_hits( + stale_lock_active=True, + motion_switch_mode=False, + approach_extra_hits=2, + fast_maneuver_extra_hits=2, + default_switch_hits=4, + fast_handoff_hits=2, + motion_switch_hits=6, + ) + + assert hits == 2 + + +def test_handoff_decision_repr_is_stable_for_logging(): + decision = HandoffDecision( + stale_lock_active=True, + allow_fast_handoff=True, + required_switch_hits=2, + disable_prediction_hold=True, + reasons=["target_not_fresh", "weak_klt_quality"], + ) + + assert decision.reason_text() == "target_not_fresh|weak_klt_quality" +``` + +- [ ] **Step 2: Run test to verify it fails** + +Run: `pytest tests/test_target_handoff.py -v` + +Expected: FAIL with `ModuleNotFoundError: No module named 'target_handoff'` + +- [ ] **Step 3: Write minimal implementation** + +```python +from dataclasses import dataclass + + +@dataclass(frozen=True) +class HandoffDecision: + stale_lock_active: bool + allow_fast_handoff: bool + required_switch_hits: int + disable_prediction_hold: bool + reasons: list[str] + + def reason_text(self) -> str: + return "|".join(self.reasons) + + +def evaluate_stale_lock( + *, + confirmed, + have_fresh_candidate, + target_has_fresh_update, + candidate_dist_px, + pred_diag_px, + miss_streak, + klt_valid, + klt_quality, + klt_iou, + candidate_motion_ok, + target_motion_ok, + stale_break_dist_diag, + stale_break_min_miss, + stale_break_klt_quality, + stale_break_klt_iou, +): + reasons = [] + if not confirmed or not have_fresh_candidate: + return HandoffDecision(False, False, 0, False, []) + + far_enough = candidate_dist_px >= max(40.0, stale_break_dist_diag * max(pred_diag_px, 1.0)) + weak_anchor = ( + (not klt_valid) + or (klt_quality < stale_break_klt_quality) + or (klt_iou < stale_break_klt_iou) + or (miss_streak >= stale_break_min_miss) + or (candidate_motion_ok and not target_motion_ok) + or (not target_has_fresh_update) + ) + + if not target_has_fresh_update: + reasons.append("target_not_fresh") + if not klt_valid: + reasons.append("klt_invalid") + if klt_quality < stale_break_klt_quality: + reasons.append("weak_klt_quality") + if klt_iou < stale_break_klt_iou: + reasons.append("weak_klt_iou") + if miss_streak >= stale_break_min_miss: + reasons.append("miss_streak") + if candidate_motion_ok and not target_motion_ok: + reasons.append("motion_conflict") + + stale = bool(far_enough and weak_anchor) + return HandoffDecision( + stale_lock_active=stale, + allow_fast_handoff=stale, + required_switch_hits=0, + disable_prediction_hold=stale, + reasons=reasons if stale else [], + ) + + +def compute_fast_handoff_hits( + *, + stale_lock_active, + motion_switch_mode, + approach_extra_hits, + fast_maneuver_extra_hits, + default_switch_hits, + fast_handoff_hits, + motion_switch_hits, +): + if stale_lock_active: + return int(fast_handoff_hits) + + hits = int(default_switch_hits) + int(approach_extra_hits) + int(fast_maneuver_extra_hits) + if motion_switch_mode: + hits = max(hits, int(motion_switch_hits)) + return int(hits) +``` + +- [ ] **Step 4: Run test to verify it passes** + +Run: `pytest tests/test_target_handoff.py -v` + +Expected: `4 passed` + +- [ ] **Step 5: Commit** + +```bash +git add target_handoff.py tests/test_target_handoff.py +git commit -m "feat: add stale-lock and fast-handoff decision helpers" +``` + +## Task 2: Add Guidance Reset Hook + +**Files:** +- Modify: `guidance.py` +- Create: `tests/test_guidance_reset.py` + +- [ ] **Step 1: Write the failing tests** + +```python +from pathlib import Path +import sys + +import numpy as np + + +PROJECT_ROOT = Path(__file__).resolve().parents[1] +if str(PROJECT_ROOT) not in sys.path: + sys.path.insert(0, str(PROJECT_ROOT)) + +from guidance import ScreenGuidanceController + + +def test_guidance_reset_reinitializes_smoothed_point(): + ctrl = ScreenGuidanceController() + ctrl.smoothed_point = np.array([100.0, 200.0], dtype=np.float32) + ctrl.smoothed_cmd = np.array([0.7, -0.4], dtype=np.float32) + + ctrl.reset_for_target_switch(np.array([500.0, 300.0], dtype=np.float32)) + + assert np.allclose(ctrl.smoothed_point, np.array([500.0, 300.0], dtype=np.float32)) + assert np.allclose(ctrl.smoothed_cmd, np.zeros(2, dtype=np.float32)) + + +def test_guidance_reset_can_damp_instead_of_zero_when_requested(): + ctrl = ScreenGuidanceController() + ctrl.smoothed_cmd = np.array([0.8, -0.6], dtype=np.float32) + + ctrl.reset_for_target_switch(np.array([10.0, 20.0], dtype=np.float32), cmd_damp=0.25) + + assert np.allclose(ctrl.smoothed_cmd, np.array([0.2, -0.15], dtype=np.float32)) +``` + +- [ ] **Step 2: Run test to verify it fails** + +Run: `pytest tests/test_guidance_reset.py -v` + +Expected: FAIL with `AttributeError: 'ScreenGuidanceController' object has no attribute 'reset_for_target_switch'` + +- [ ] **Step 3: Write minimal implementation** + +```python + def reset_for_target_switch(self, aim_point, cmd_damp=0.0): + aim = np.array(aim_point, dtype=np.float32) + self.smoothed_point = aim.copy() + damp = float(clamp(cmd_damp, 0.0, 1.0)) + self.smoothed_cmd = (self.smoothed_cmd * damp).astype(np.float32) +``` + +Add it inside `ScreenGuidanceController` near `status_line()` / `idle_state()` in `guidance.py`. + +- [ ] **Step 4: Run test to verify it passes** + +Run: `pytest tests/test_guidance_reset.py -v` + +Expected: `2 passed` + +- [ ] **Step 5: Commit** + +```bash +git add guidance.py tests/test_guidance_reset.py +git commit -m "feat: add guidance reset hook for target handoff" +``` + +## Task 3: Integrate Stale Lock And Fast Handoff Into Main Loop + +**Files:** +- Modify: `config.py` +- Modify: `main.py` +- Modify: `decision_logger.py` +- Create: `tests/test_decision_logger_events.py` + +- [ ] **Step 1: Write the failing logger test** + +```python +from pathlib import Path +import sys + + +PROJECT_ROOT = Path(__file__).resolve().parents[1] +if str(PROJECT_ROOT) not in sys.path: + sys.path.insert(0, str(PROJECT_ROOT)) + +from decision_logger import FIELDNAMES + + +def test_decision_logger_contains_handoff_fields(): + assert "stale_lock_active" in FIELDNAMES + assert "stale_lock_reason" in FIELDNAMES + assert "fast_handoff_active" in FIELDNAMES + assert "guidance_reset_event" in FIELDNAMES +``` + +- [ ] **Step 2: Run test to verify it fails** + +Run: `pytest tests/test_decision_logger_events.py -v` + +Expected: FAIL because the new field names are missing from `FIELDNAMES` + +- [ ] **Step 3: Add config defaults** + +Add these exact settings near the existing single-target lock policy section in `config.py`: + +```python +STALE_LOCK_BREAK_ENABLE = True +STALE_LOCK_BREAK_DIST_DIAG = 3.5 +STALE_LOCK_BREAK_MIN_MISS = 2 +STALE_LOCK_BREAK_KLT_QUALITY = 0.35 +STALE_LOCK_BREAK_KLT_IOU = 0.25 +FAST_HANDOFF_ENABLE = True +FAST_HANDOFF_CONFIRM_HITS = 2 +FAST_HANDOFF_GUIDANCE_CMD_DAMP = 0.0 +``` + +- [ ] **Step 4: Wire stale-lock and fast-handoff in `main.py`** + +Add imports near the top of `main.py`: + +```python +from target_handoff import compute_fast_handoff_hits, evaluate_stale_lock +``` + +Add per-frame state defaults near the other per-frame flags: + +```python + stale_lock_active = False + stale_lock_reason = "" + fast_handoff_active = False + guidance_reset_event = "" +``` + +After `best` candidate selection is known and before switch confirmation is finalized, add stale-lock evaluation using current values: + +```python + handoff_decision = evaluate_stale_lock( + confirmed=confirmed, + have_fresh_candidate=(best is not None and best.time_since_update == 0), + target_has_fresh_update=(target_track is not None and target_track.time_since_update == 0), + candidate_dist_px=float(dist) if pred_ref is not None else 0.0, + pred_diag_px=float(pred_diag), + miss_streak=int(miss_streak), + klt_valid=bool(klt_valid), + klt_quality=float(klt.quality), + klt_iou=float(iou(klt.box, locked_box_eff)) if (klt.box is not None and locked_box_eff is not None) else 0.0, + candidate_motion_ok=bool(residual_ok), + target_motion_ok=bool(target_track is not None and target_track.time_since_update == 0), + stale_break_dist_diag=float(STALE_LOCK_BREAK_DIST_DIAG), + stale_break_min_miss=int(STALE_LOCK_BREAK_MIN_MISS), + stale_break_klt_quality=float(STALE_LOCK_BREAK_KLT_QUALITY), + stale_break_klt_iou=float(STALE_LOCK_BREAK_KLT_IOU), + ) + stale_lock_active = bool(STALE_LOCK_BREAK_ENABLE and handoff_decision.stale_lock_active) + stale_lock_reason = handoff_decision.reason_text() +``` + +When computing switch confirmation hits, replace the current manual calculation with: + +```python + req_switch_hits = compute_fast_handoff_hits( + stale_lock_active=bool(FAST_HANDOFF_ENABLE and stale_lock_active), + motion_switch_mode=bool(motion_switch_mode), + approach_extra_hits=int(approach_switch_extra_hits), + fast_maneuver_extra_hits=int(max(0, SWITCH_FAST_MANEUVER_EXTRA_HITS)) if fast_maneuver_guard else 0, + default_switch_hits=int(TARGET_SWITCH_CONFIRM_HITS), + fast_handoff_hits=int(FAST_HANDOFF_CONFIRM_HITS), + motion_switch_hits=int(MOTION_CONF_SWITCH_CONFIRM_HITS), + ) + fast_handoff_active = bool(FAST_HANDOFF_ENABLE and stale_lock_active and req_switch_hits == int(FAST_HANDOFF_CONFIRM_HITS)) +``` + +Disable prediction hold while stale-lock is active by modifying the existing hold branch: + +```python + if confirmed and pred_box_eff is not None and (not stale_lock_active) and miss_streak < KALMAN_HOLD_MAX: +``` + +When a fast handoff or target switch is accepted, reset guidance: + +```python + old_target_id = target_id +``` + +before replacing `target_id`, then after `locked_box_eff = chosen`: + +```python + if confirmed and target_track is not None and old_target_id is not None and target_track.track_id != old_target_id: + guidance_ctrl.reset_for_target_switch( + box_center(locked_box_eff), + cmd_damp=float(FAST_HANDOFF_GUIDANCE_CMD_DAMP), + ) + guidance_reset_event = "target_switch" + elif stale_lock_active and locked_box_eff is not None: + guidance_ctrl.reset_for_target_switch( + box_center(locked_box_eff), + cmd_damp=float(FAST_HANDOFF_GUIDANCE_CMD_DAMP), + ) + guidance_reset_event = "stale_lock_break" +``` + +- [ ] **Step 5: Add logger fields** + +Extend `decision_logger.py` field names and row payload with: + +```python + "stale_lock_active", + "stale_lock_reason", + "fast_handoff_active", + "guidance_reset_event", +``` + +and map them in `log_frame()`: + +```python + "stale_lock_active": int(bool(kwargs.get("stale_lock_active", False))), + "stale_lock_reason": kwargs.get("stale_lock_reason", ""), + "fast_handoff_active": int(bool(kwargs.get("fast_handoff_active", False))), + "guidance_reset_event": kwargs.get("guidance_reset_event", ""), +``` + +Also append event names if present: + +```python + if kwargs.get("stale_lock_active", False): + events.append("stale_lock_break") + if kwargs.get("fast_handoff_active", False): + events.append("fast_handoff_switch") + if kwargs.get("guidance_reset_event", ""): + events.append("guidance_reset") +``` + +- [ ] **Step 6: Run focused tests** + +Run: + +```bash +pytest tests/test_target_handoff.py tests/test_guidance_reset.py tests/test_decision_logger_events.py tests/test_track_score_gates.py -v +``` + +Expected: all tests PASS + +- [ ] **Step 7: Run a smoke video pass** + +Run: + +```bash +py -3 main.py +``` + +Expected: +- no import/runtime errors +- new CSV fields appear in `track_log_*.csv` +- problem segment near `231.5s - 232.3s` shows earlier stale-lock break and faster visual realignment + +- [ ] **Step 8: Commit** + +```bash +git add config.py main.py guidance.py decision_logger.py tests/test_decision_logger_events.py +git commit -m "feat: add stale-lock break and fast handoff integration" +``` + +## Self-Review + +### Spec coverage + +- Stale-lock detection: Task 1 + Task 3 +- Fast handoff switch policy: Task 1 + Task 3 +- Prediction hold restrictions: Task 3 +- Guidance reset on switch: Task 2 + Task 3 +- Logging and diagnostics: Task 3 +- Verification on known failure segment: Task 3 smoke pass + +No spec gaps remain for the first-pass implementation. + +### Placeholder scan + +- No `TBD`, `TODO`, or deferred placeholders included. +- Every code-changing step contains explicit code snippets. +- Every verification step contains an exact command and expected result. + +### Type consistency + +- `HandoffDecision` property names are consistent across tests and integration steps. +- `reset_for_target_switch()` is referenced consistently in tests and integration. +- New logger field names match the values referenced in `main.py`. + +## Execution Handoff + +Plan complete and saved to `docs/superpowers/plans/2026-06-24-fast-handoff-stale-lock-implementation.md`. Two execution options: + +**1. Subagent-Driven (recommended)** - I dispatch a fresh subagent per task, review between tasks, fast iteration + +**2. Inline Execution** - Execute tasks in this session using executing-plans, batch execution with checkpoints + +**Which approach?** diff --git a/docs/superpowers/plans/2026-06-29-docker-offline-gpu-proto-udp-implementation.md b/docs/superpowers/plans/2026-06-29-docker-offline-gpu-proto-udp-implementation.md new file mode 100644 index 0000000..25dbd71 --- /dev/null +++ b/docs/superpowers/plans/2026-06-29-docker-offline-gpu-proto-udp-implementation.md @@ -0,0 +1,77 @@ +# Docker Offline GPU + Proto UDP Implementation Plan + +> **For agentic workers:** REQUIRED SUB-SKILL: Use superpowers:subagent-driven-development (recommended) or superpowers:executing-plans to implement this plan task-by-task. Steps use checkbox (`- [ ]`) syntax for tracking. + +**Goal:** Добавить офлайн GPU Docker-упаковку для live/video запуска и новый опциональный UDP backend испытательного протокола. + +**Architecture:** Конфигурация контейнера будет управляться env-переменными поверх текущих дефолтов в `config.py` и `config_intercept.py`. Новый backend `proto_udp` будет встроен в существующий `AutopilotBridge`, а Docker-артефакты обеспечат офлайн runtime с уже установленными зависимостями и моделью. + +**Tech Stack:** Python 3.12, PyTorch CUDA, Ultralytics YOLO, OpenCV, Docker, docker-compose, UDP sockets. + +--- + +### Task 1: Add tests for protocol backend and env overrides + +**Files:** +- Create: `tests/test_autopilot_proto_udp.py` +- Create: `tests/test_runtime_env.py` + +- [ ] **Step 1: Write failing tests for UDP packet packing and backend selection** +- [ ] **Step 2: Run targeted pytest commands and confirm failure** +- [ ] **Step 3: Add failing tests for env override parsing** +- [ ] **Step 4: Run targeted pytest commands and confirm failure** + +### Task 2: Implement runtime env overrides + +**Files:** +- Create: `runtime_env.py` +- Modify: `config.py` +- Modify: `config_intercept.py` + +- [ ] **Step 1: Add minimal env parsing helpers and runtime override API** +- [ ] **Step 2: Apply overrides to source/model/output/backend-related settings** +- [ ] **Step 3: Re-run env override tests until green** + +### Task 3: Implement protocol UDP backend + +**Files:** +- Modify: `autopilot_bridge.py` +- Modify: `config_intercept.py` + +- [ ] **Step 1: Add protocol config defaults** +- [ ] **Step 2: Add packet builder and `proto_udp` backend** +- [ ] **Step 3: Wire backend selection into `AutopilotBridge`** +- [ ] **Step 4: Re-run protocol tests until green** + +### Task 4: Make runtime self-contained for container use + +**Files:** +- Create: `bytetrack_min_aggressive.py` +- Modify: `main.py` + +- [ ] **Step 1: Vendor the local ByteTrack implementation into the project root** +- [ ] **Step 2: Remove dependency on parent-directory import for container runtime** +- [ ] **Step 3: Re-run affected tests and compile checks** + +### Task 5: Add Docker packaging + +**Files:** +- Create: `Dockerfile` +- Create: `.dockerignore` +- Create: `requirements-docker.txt` +- Create: `docker-compose.yml` +- Create: `docker/entrypoint.sh` +- Create: `README_docker.md` + +- [ ] **Step 1: Define offline-friendly GPU image build** +- [ ] **Step 2: Add compose service with GPU/runtime env setup** +- [ ] **Step 3: Document build and launch modes for camera/rtsp/udp/video** + +### Task 6: Verify end-to-end artifacts + +**Files:** +- Modify: `docs/superpowers/plans/2026-06-29-docker-offline-gpu-proto-udp-implementation.md` + +- [ ] **Step 1: Run targeted pytest for new and touched tests** +- [ ] **Step 2: Run `python -m compileall` for changed Python files** +- [ ] **Step 3: Report any remaining runtime limits that require host-side NVIDIA/Docker setup** diff --git a/docs/superpowers/specs/2026-06-24-fast-handoff-stale-lock-design.md b/docs/superpowers/specs/2026-06-24-fast-handoff-stale-lock-design.md new file mode 100644 index 0000000..3984b85 --- /dev/null +++ b/docs/superpowers/specs/2026-06-24-fast-handoff-stale-lock-design.md @@ -0,0 +1,218 @@ +# Fast Handoff And Stale Lock Design + +## Goal + +Reduce the delay between a valid new YOLO detection and the moment when: +- `target_id` switches to the new target track, +- `locked_box_eff` stops following stale KLT/Kalman state, +- the blue point and yellow guidance line stop pointing at the old scene region. + +The fix must improve turning behavior without materially increasing false switches to terrain, OSD, or random high-contrast background features. + +## Current Problem + +The current pipeline has three separate lag sources that compound during sharp turns: + +1. `main.py` keeps the old target anchor alive through prediction hold and KLT, even when YOLO has already found a better candidate elsewhere. +2. The target switch policy requires multiple confirming frames before accepting the new candidate, especially in chase/close regimes. +3. `guidance.py` smooths both aim point and command state, so even after a switch the yellow line continues to drift toward the new target instead of snapping quickly. + +Observed failure mode: +- YOLO draws fresh boxes around the real target in a new image region. +- `target_id`, `locked_box_eff`, and `pred_box_eff` still describe the stale region. +- KLT may still look "valid" because it is tracking texture on the wrong patch. +- Guidance uses the stale lock, so the blue point and yellow line remain pointed away from the true target. + +## Recommended Approach + +Implement a three-part fix: + +1. Add stale-lock detection in `main.py`. +2. Add a fast-handoff switch path for valid replacement tracks. +3. Reset guidance smoothing state on stale-break and on confirmed target switch. + +This is intentionally an incremental fix that preserves the existing architecture: +- YOLO +- ByteTrack +- SafeKalman +- Hybrid KLT tracker +- guidance overlay/export + +## Design + +### 1. Stale-Lock Detection + +Add a new state evaluation step in `main.py` after fresh YOLO tracks are available and before final target selection/hold behavior is applied. + +The stale-lock detector should mark the current lock as stale when all of the following are true: + +- The system is in `confirmed` state. +- There is at least one fresh YOLO candidate track. +- The current `target_id` either has no fresh update or is significantly less plausible than another candidate. +- The best candidate is far from the current `pred_ref` / `locked_box_eff`. +- The current anchor is weak by at least one anchor-quality signal. + +Anchor-quality signals: +- `klt_valid` is false. +- `klt_quality` is below a configurable stale threshold. +- `klt.box` exists but has poor IoU with `locked_box_eff`. +- `miss_streak` is already elevated. +- A fresh candidate is motion-consistent while the current target is not. + +Distance signal: +- Compute distance from candidate center to `pred_ref` center. +- Compare against a dedicated stale-break distance threshold based on target diagonal, not just the existing reacquisition threshold. + +Important behavior: +- Marking the old lock as stale does not immediately force a blind jump to a new candidate. +- It only disables continued trust in the stale anchor for guidance and prediction-hold purposes. + +### 2. Fast Handoff Switch Policy + +Keep the current conservative switch logic as the default path. + +Add a second switch path named fast handoff, enabled only when the stale-lock detector fires. + +Fast handoff should accept a replacement candidate after only `1-2` confirming frames if the candidate passes strict gating: + +- fresh ByteTrack update (`time_since_update == 0`) +- score above switch floor +- residual/motion gate passes when available +- not inside blocked OSD zones unless near the expected path +- distance is large enough to prove it is not the stale lock, but not absurdly far +- optional trajectory agreement if reliable target velocity exists + +Fast handoff should bypass or reduce: +- `TARGET_SWITCH_CONFIRM_HITS` +- `APPROACH_*_SWITCH_EXTRA_HITS` +- `SWITCH_FAST_MANEUVER_EXTRA_HITS` + +Fast handoff should also reduce or bypass old-lock waiting: +- `TARGET_SWITCH_MISS_FRAMES` +- `APPROACH_*_SWITCH_EXTRA_MISS` +- `SWITCH_FAST_MANEUVER_EXTRA_MISS` + +If stale-lock is active and no candidate survives gates: +- do not keep driving guidance from old stale lock indefinitely, +- prefer degrading to neutral/low-confidence hold rather than continuing to steer at the wrong patch. + +### 3. Prediction Hold Restrictions + +The current `pred_box_eff`/Kalman hold is useful when YOLO drops out briefly, but harmful once a stale-lock condition is detected. + +Modify hold behavior so that: +- normal prediction hold remains available during benign short misses, +- stale-lock condition disables or sharply shortens prediction hold, +- KLT-only continuation is not allowed to dominate when fresh YOLO is contradicting it. + +This change should affect the branch that currently sets: +- `locked_box_eff = pred_box_eff` +- increments `miss_streak` +- keeps `confirmed = True` + +Expected result: +- the system stops visually "dragging" the old lock across the frame for up to `KALMAN_HOLD_MAX` frames while the real target is already detected somewhere else. + +### 4. Guidance Reset On Switch + +Update `guidance.py` so that guidance smoothing state is reset when: +- a stale-lock break occurs, or +- `target_id` changes to a new confirmed target. + +Required behavior: +- `smoothed_point` should be reinitialized to the new aim point. +- `smoothed_cmd` should be zeroed or strongly damped. +- command continuity should favor fast visual alignment over preserving momentum from the old target. + +This reset must be explicit and event-driven. + +Without it, even a correct target switch still leaves the yellow line lagging for multiple frames because: +- aim point is EMA-smoothed, +- command is EMA-smoothed. + +### 5. Logging And Diagnostics + +Extend diagnostics so the fix can be evaluated from logs rather than by eye only. + +Add fields/events such as: +- `stale_lock_active` +- `stale_lock_reason` +- `fast_handoff_candidate_id` +- `fast_handoff_active` +- `guidance_reset_event` + +Add event tags such as: +- `stale_lock_break` +- `fast_handoff_switch` +- `guidance_reset` + +This allows measuring: +- frames between first valid new candidate and switch +- frames where YOLO sees the new target while lock still follows the stale region +- false switch counts before/after the fix + +## Implementation Boundaries + +Files expected to change: +- `main.py` +- `guidance.py` +- `decision_logger.py` +- possibly `config.py` for new thresholds + +Files not in scope for the first pass: +- `range_estimation.py` +- `proportional_navigation.py` +- `autopilot_bridge.py` +- major ByteTrack internals rewrite + +## Suggested New Config Controls + +Add narrowly scoped tuning parameters, for example: + +- stale-lock enable flag +- stale-lock distance threshold by target diagonal +- stale-lock KLT quality floor +- stale-lock max miss before break +- fast handoff enable flag +- fast handoff confirm hits +- fast handoff max switch distance +- guidance reset damping factor + +The first implementation should keep defaults conservative but clearly more responsive than the current chase preset. + +## Success Criteria + +The fix is successful if: + +- During sharp turns, once YOLO consistently detects the target in a new region, the stale lock is abandoned within `1-3` frames. +- `target_id` switches to the correct candidate substantially earlier than today. +- The blue point stops following the stale region almost immediately after stale-break. +- The yellow line reorients quickly after switch instead of easing over many frames. +- False switches to terrain/OSD do not materially increase. + +## Verification Plan + +Use the most recent problem video and compare before/after on the known failure segment around `231.5s - 232.3s`. + +Measure: +- first frame where new YOLO candidate is valid +- frame where stale lock is broken +- frame where `target_switch` occurs +- frame where guidance visually realigns + +Also review aggregate log metrics: +- target switch count +- lock lost count +- time spent in stale-lock-active state +- rate of false handoffs + +## Risks + +- If stale-lock conditions are too loose, the tracker may become jumpy. +- If fast handoff ignores too many gates, terrain switches may increase. +- If guidance reset is too aggressive, command output may become visually jerky. + +The design therefore uses: +- explicit stale-lock conditions, +- gated fast handoff, +- targeted guidance reset only on real switch/break events. diff --git a/docs/superpowers/specs/2026-06-29-docker-offline-gpu-proto-udp-design.md b/docs/superpowers/specs/2026-06-29-docker-offline-gpu-proto-udp-design.md new file mode 100644 index 0000000..1f397c8 --- /dev/null +++ b/docs/superpowers/specs/2026-06-29-docker-offline-gpu-proto-udp-design.md @@ -0,0 +1,70 @@ +# Docker Offline GPU + Proto UDP Design + +## Goal + +Упаковать `fpv_tracker_optimized` в один офлайн GPU-контейнер, который может работать с live/video-источниками и переключаемыми backend'ами управления, включая новый испытательный UDP-протокол. + +## Scope + +- Один Docker-образ для runtime проекта. +- Запуск без интернета после сборки. +- Поддержка `camera`, `rtsp`, `udp`, `video` через переменные окружения. +- Поддержка backend'ов `json`, `mavlink`, `msp`, `proto_udp`. +- Возможность включать и выключать испытательный протокол без смены образа. + +## Architecture + +### Runtime packaging + +- Базовый образ: CUDA runtime + Python. +- Все Python-зависимости ставятся на этапе сборки. +- В образ копируются код проекта, модель `best.pt` и локальная реализация `BYTETracker`. +- Запуск идет через entrypoint-скрипт, который настраивает runtime через env. + +### Configuration model + +- Существующие значения в `config.py` и `config_intercept.py` остаются как дефолты. +- Для контейнера добавляется слой env-overrides, чтобы не редактировать код под каждый запуск. +- Переключаемые параметры: + - источник видео; + - путь к модели; + - режим отображения/записи; + - backend автопилота; + - параметры испытательного UDP-протокола. + +### Protocol backend + +- Новый backend в `autopilot_bridge.py`: `proto_udp`. +- Он сериализует состояние цели в фиксированный бинарный UDP-пакет и отправляет на заданный `host:port`. +- Если цели нет, поля пакета передаются нулевыми согласно протоколу. + +## Protocol mapping + +Сообщение `нейросеть -> блок`: + +- `descriptor`: `1`, `uint8` +- `object_id`: `1..255`, `uint8` +- `target_state`: `0..3`, `uint8` +- `offset_y_px`: `int16` +- `offset_x_px`: `int16` +- `offset_y_pct`: `int8` +- `offset_x_pct`: `int8` +- `bbox_area_pct`: `uint8` + +Порядок полей фиксированный. Для упаковки используется little-endian бинарный формат. + +## Docker artifacts + +- `Dockerfile` +- `.dockerignore` +- `requirements-docker.txt` +- `docker-compose.yml` +- `docker/entrypoint.sh` +- `README_docker.md` + +## Verification + +- Unit-тесты для упаковки UDP-пакета и выбора backend'а. +- Unit-тесты для env-overrides. +- `python -m compileall` по измененным Python-файлам. +- Проверка синтаксиса compose/config файлов локально без сетевых шагов runtime. diff --git a/guidance.py b/guidance.py new file mode 100644 index 0000000..fcd28a9 --- /dev/null +++ b/guidance.py @@ -0,0 +1,316 @@ +import json +import time +from pathlib import Path + +import cv2 +import numpy as np + +from config import * +from helpers import box_center, clamp, clip_box + + +def _curve_command(value, deadzone, expo, gain): + v = float(np.clip(value, -1.0, 1.0)) + av = abs(v) + if av <= deadzone: + return 0.0 + t = (av - deadzone) / max(1e-6, 1.0 - deadzone) + out = min(1.0, float(gain) * (t ** float(expo))) + return out if v >= 0.0 else -out + + +class ScreenGuidanceController: + def __init__(self): + self.enabled = bool(GUIDANCE_ENABLE) + self.export_enable = bool(self.enabled and GUIDANCE_EXPORT_ENABLE) + self.export_every = int(max(1, GUIDANCE_EXPORT_EVERY)) + self.export_path = Path(GUIDANCE_EXPORT_PATH) + if self.export_enable: + self.export_path.parent.mkdir(parents=True, exist_ok=True) + + self.cmd_keep = float(clamp(GUIDANCE_CMD_SMOOTH, 0.0, 0.97)) + self.point_keep = float(clamp(GUIDANCE_POINT_SMOOTH, 0.0, 0.97)) + self.last_export_frame = -10**9 + self.export_retry_at = 0.0 + self.last_export_warn_ts = 0.0 + self.smoothed_cmd = np.zeros(2, dtype=np.float32) + self.smoothed_point = None + + def status_line(self): + if not self.enabled: + return "Guidance disabled" + if self.export_enable: + return f"Guidance ready: export={self.export_path}" + return "Guidance ready" + + def reset_for_target_switch(self, aim_point, cmd_damp=0.0): + aim = np.array(aim_point, dtype=np.float32) + self.smoothed_point = aim.copy() + damp = float(clamp(cmd_damp, 0.0, 1.0)) + self.smoothed_cmd = (self.smoothed_cmd * damp).astype(np.float32) + + def idle_state(self): + return { + "active": False, + "status": "SEARCH", + "frame_id": -1, + "target_id": None, + "frame_w": None, + "frame_h": None, + "aim_x": None, + "aim_y": None, + "box_w": None, + "box_h": None, + "error_x": 0.0, + "error_y": 0.0, + "cmd_x": 0.0, + "cmd_y": 0.0, + "steer_x": 0.0, + "steer_y": 0.0, + "look_dx": 0.0, + "look_dy": 0.0, + "confidence": 0.0, + "on_target": False, + } + + def update( + self, + frame_id, + frame_w, + frame_h, + confirmed, + locked_box, + pred_box, + override_box, + target_id, + target_track_score, + klt_valid, + klt_quality, + miss_streak, + vx, + vy, + ): + state = self.idle_state() + state["frame_id"] = int(frame_id) + state["target_id"] = None if target_id is None else int(target_id) + state["frame_w"] = int(frame_w) + state["frame_h"] = int(frame_h) + + if not self.enabled: + return state + + guide_box = None + if confirmed and override_box is not None: + guide_box = np.array(override_box, dtype=np.float32) + elif confirmed and locked_box is not None: + guide_box = np.array(locked_box, dtype=np.float32) + elif confirmed and pred_box is not None: + guide_box = np.array(pred_box, dtype=np.float32) + + if guide_box is None: + self.smoothed_cmd *= 0.60 + state["cmd_x"] = float(self.smoothed_cmd[0]) + state["cmd_y"] = float(self.smoothed_cmd[1]) + state["steer_x"] = float(self.smoothed_cmd[0]) + state["steer_y"] = float(self.smoothed_cmd[1]) + state["look_dx"] = float(self.smoothed_cmd[0] * float(GUIDANCE_LOOK_MAX_STEP_PX)) + state["look_dy"] = float(self.smoothed_cmd[1] * float(GUIDANCE_LOOK_MAX_STEP_PX)) + self._maybe_export(state) + return state + + guide_box = clip_box(guide_box, frame_w, frame_h) + center = box_center(guide_box).astype(np.float32) + box_w = float(max(0.0, guide_box[2] - guide_box[0])) + box_h = float(max(0.0, guide_box[3] - guide_box[1])) + + lead = np.array([float(vx), float(vy)], dtype=np.float32) * float(GUIDANCE_LEAD_SEC) + lead_norm = float(np.linalg.norm(lead)) + if lead_norm > float(GUIDANCE_MAX_LEAD_PX): + lead *= float(GUIDANCE_MAX_LEAD_PX) / max(1e-6, lead_norm) + + aim = center + lead + aim[1] += float(GUIDANCE_BOX_BIAS_Y) * max(2.0, float(guide_box[3] - guide_box[1])) + aim[0] = float(clamp(aim[0], 0.0, float(frame_w - 1))) + aim[1] = float(clamp(aim[1], 0.0, float(frame_h - 1))) + + if self.smoothed_point is None: + self.smoothed_point = aim.copy() + else: + self.smoothed_point = ( + (self.point_keep * self.smoothed_point) + + ((1.0 - self.point_keep) * aim) + ).astype(np.float32) + + screen_center = np.array([0.5 * float(frame_w), 0.5 * float(frame_h)], dtype=np.float32) + error_px = self.smoothed_point - screen_center + error_norm = np.array( + [ + float(error_px[0] / max(1.0, 0.5 * float(frame_w))), + float(error_px[1] / max(1.0, 0.5 * float(frame_h))), + ], + dtype=np.float32, + ) + error_norm = np.clip(error_norm, -1.0, 1.0) + + raw_cmd = np.array( + [ + _curve_command(error_norm[0], GUIDANCE_DEADZONE_NORM, GUIDANCE_EXPO, GUIDANCE_GAIN_X), + _curve_command(error_norm[1], GUIDANCE_DEADZONE_NORM, GUIDANCE_EXPO, GUIDANCE_GAIN_Y), + ], + dtype=np.float32, + ) + self.smoothed_cmd = ( + (self.cmd_keep * self.smoothed_cmd) + + ((1.0 - self.cmd_keep) * raw_cmd) + ).astype(np.float32) + + confidence = 0.0 + confidence += 0.40 if confirmed else 0.0 + confidence += 0.15 if locked_box is not None else 0.05 + + # KLT теперь главный источник доверия (klt_quality ≈ 0.93) + if klt_valid: + confidence += 0.30 * float(clamp(klt_quality, 0.0, 1.0)) + else: + confidence += 0.05 * float(clamp(klt_quality, 0.0, 1.0)) + + # ByteTrack score — второстепенный + if target_track_score is not None: + confidence += 0.10 * float(clamp(target_track_score, 0.0, 1.0)) + + # Мягче штраф за miss (YOLO слабый, это временно) + confidence -= 0.025 * float(clamp(miss_streak, 0, 10)) + + confidence = float(clamp(confidence, 0.0, 1.0)) + + err_mag = float(np.linalg.norm(error_norm)) + on_target = bool( + confirmed + and (confidence >= float(GUIDANCE_CONF_MIN)) + and (err_mag <= float(GUIDANCE_ON_TARGET_NORM)) + ) + + status = "SEARCH" + if confirmed and override_box is not None and miss_streak > 0: + status = "REACQ" + elif confirmed and miss_streak > 0: + status = "HOLD" + elif confirmed: + status = "LOCK" + + state.update( + { + "active": bool(confirmed), + "status": status, + "aim_x": float(self.smoothed_point[0]), + "aim_y": float(self.smoothed_point[1]), + "box_w": box_w, + "box_h": box_h, + "error_x": float(error_norm[0]), + "error_y": float(error_norm[1]), + "cmd_x": float(self.smoothed_cmd[0]), + "cmd_y": float(self.smoothed_cmd[1]), + "steer_x": float(self.smoothed_cmd[0]), + "steer_y": float(self.smoothed_cmd[1]), + "look_dx": float(self.smoothed_cmd[0] * float(GUIDANCE_LOOK_MAX_STEP_PX)), + "look_dy": float(self.smoothed_cmd[1] * float(GUIDANCE_LOOK_MAX_STEP_PX)), + "confidence": confidence, + "on_target": on_target, + } + ) + self._maybe_export(state) + return state + + def draw_overlay(self, frame_bgr, state, sx, sy): + if (not self.enabled) or (not DRAW_GUIDANCE): + return + + frame_h, frame_w = frame_bgr.shape[:2] + cx = int(0.5 * frame_w) + cy = int(0.5 * frame_h) + + color = (0, 220, 255) if state.get("active", False) else (120, 120, 120) + cv2.drawMarker(frame_bgr, (cx, cy), color, markerType=cv2.MARKER_CROSS, markerSize=18, thickness=1) + + aim_x = state.get("aim_x", None) + aim_y = state.get("aim_y", None) + if aim_x is not None and aim_y is not None: + ax = int(float(aim_x) / max(1e-6, float(sx))) + ay = int(float(aim_y) / max(1e-6, float(sy))) + cv2.circle(frame_bgr, (ax, ay), 8, color, 2) + cv2.line(frame_bgr, (cx, cy), (ax, ay), color, 1, cv2.LINE_AA) + cv2.putText( + frame_bgr, + f"G {state['status']} steer=({state['steer_x']:+.2f},{state['steer_y']:+.2f}) conf={state['confidence']:.2f}", + (20, 165), + cv2.FONT_HERSHEY_SIMPLEX, + 0.60, + color, + 2, + ) + + def _maybe_export(self, state): + frame_id = int(state["frame_id"]) + now = time.monotonic() + if (not self.export_enable) or ((frame_id - self.last_export_frame) < self.export_every) or (now < self.export_retry_at): + return + + payload = { + "frame_id": frame_id, + "active": bool(state["active"]), + "status": str(state["status"]), + "target_id": state["target_id"], + "frame_w": state["frame_w"], + "frame_h": state["frame_h"], + "aim_x": state["aim_x"], + "aim_y": state["aim_y"], + "box_w": state["box_w"], + "box_h": state["box_h"], + "error_x": float(state["error_x"]), + "error_y": float(state["error_y"]), + "cmd_x": float(state["cmd_x"]), + "cmd_y": float(state["cmd_y"]), + "steer_x": float(state["steer_x"]), + "steer_y": float(state["steer_y"]), + "look_dx": float(state["look_dx"]), + "look_dy": float(state["look_dy"]), + "confidence": float(state["confidence"]), + "on_target": bool(state["on_target"]), + } + tmp_path = self.export_path.with_suffix(self.export_path.suffix + ".tmp") + try: + with tmp_path.open("w", encoding="utf-8") as f: + json.dump(payload, f, ensure_ascii=True, indent=2) + self._replace_export_file(tmp_path) + except PermissionError as exc: + self.export_retry_at = time.monotonic() + 0.25 + self._warn_export_error(frame_id, exc) + return + except OSError as exc: + self.export_retry_at = time.monotonic() + 0.50 + self._warn_export_error(frame_id, exc) + return + + self.export_retry_at = 0.0 + self.last_export_frame = frame_id + + def _replace_export_file(self, tmp_path): + last_err = None + for delay_sec in (0.0, 0.01, 0.03): + if delay_sec > 0.0: + time.sleep(delay_sec) + try: + tmp_path.replace(self.export_path) + return + except PermissionError as exc: + last_err = exc + + if last_err is not None: + raise last_err + + def _warn_export_error(self, frame_id, exc): + now = time.monotonic() + if (now - self.last_export_warn_ts) < 2.0: + return + print(f"[guidance] export skipped at frame {frame_id} for {self.export_path}: {exc}") + self.last_export_warn_ts = now diff --git a/helpers.py b/helpers.py new file mode 100644 index 0000000..7d41ba4 --- /dev/null +++ b/helpers.py @@ -0,0 +1,644 @@ +import cv2 +import numpy as np + +from config import * + +# Helpers and utility functions +# ========================= +# HELPERS +# ========================= + +def is_box_outside(box, w, h, margin=20): + x1, y1, x2, y2 = [float(v) for v in box] + return (x2 < -margin) or (y2 < -margin) or (x1 > (w + margin)) or (y1 > (h + margin)) + + +def pick_best_det(dets_eff, ew, eh): + if not dets_eff: + return None + best = None + best_s = -1.0 + for d in dets_eff: + b = clip_box(d[:4], ew, eh) + s = float(d[4]) + a = box_area(b) + score = s + 0.00015 * a + if score > best_s: + best_s = score + best = b + return best + + +def pick_best_det_with_score(dets_eff, ew, eh): + if not dets_eff: + return None, 0.0 + best = None + best_s = -1.0 + best_conf = 0.0 + for d in dets_eff: + b = clip_box(d[:4], ew, eh) + conf = float(d[4]) + a = box_area(b) + score = conf + 0.00015 * a + if score > best_s: + best_s = score + best = b + best_conf = conf + return best, best_conf + + +def clamp(x, a, b): + return max(a, min(b, x)) + + +def box_center(box): + x1, y1, x2, y2 = box + return np.array([(x1 + x2) * 0.5, (y1 + y2) * 0.5], dtype=np.float32) + + +def box_wh(box): + x1, y1, x2, y2 = box + return np.array([max(1.0, x2 - x1), max(1.0, y2 - y1)], dtype=np.float32) + + +def box_area(box): + w, h = box_wh(box) + return float(w * h) + + +def iou(a, b): + ax1, ay1, ax2, ay2 = a + bx1, by1, bx2, by2 = b + ix1, iy1 = max(ax1, bx1), max(ay1, by1) + ix2, iy2 = min(ax2, bx2), min(ay2, by2) + inter = max(0.0, ix2 - ix1) * max(0.0, iy2 - iy1) + if inter <= 0: + return 0.0 + union = box_area(a) + box_area(b) - inter + return 0.0 if union <= 0 else float(inter / union) + + +def clip_box(box, w, h): + x1, y1, x2, y2 = box + x1 = clamp(float(x1), 0.0, float(w - 2)) + y1 = clamp(float(y1), 0.0, float(h - 2)) + x2 = clamp(float(x2), 1.0, float(w - 1)) + y2 = clamp(float(y2), 1.0, float(h - 1)) + if x2 <= x1 + 1: + x2 = x1 + 2 + if y2 <= y1 + 1: + y2 = y1 + 2 + return np.array([x1, y1, x2, y2], dtype=np.float32) + + +def crop_roi(frame, roi_box): + x1, y1, x2, y2 = map(int, roi_box) + return frame[y1:y2, x1:x2], x1, y1 + + +def compute_hsv_hist(frame, box): + x1, y1, x2, y2 = map(int, box) + patch = frame[y1:y2, x1:x2] + if patch.size == 0: + return None + hsv = cv2.cvtColor(patch, cv2.COLOR_BGR2HSV) + hist = cv2.calcHist([hsv], [0, 1], None, list(HSV_HIST_BINS), [0, 180, 0, 256]) + cv2.normalize(hist, hist, alpha=0, beta=1, norm_type=cv2.NORM_MINMAX) + return hist + + +def hsv_sim(h1, h2): + if h1 is None or h2 is None: + return 0.0 + d = cv2.compareHist(h1, h2, cv2.HISTCMP_BHATTACHARYYA) + return float(1.0 - d) + + +def safe_ratio(a, b): + a = float(max(1e-3, a)) + b = float(max(1e-3, b)) + return max(a / b, b / a) + + +def box_ar(box): + w, h = box_wh(box) + return float(w / max(1e-3, h)) + + +def blend_hist(ref_hist, new_hist, alpha=0.85): + if ref_hist is None: + return new_hist + if new_hist is None: + return ref_hist + out = (alpha * ref_hist) + ((1.0 - alpha) * new_hist) + out = out.astype(np.float32, copy=False) + cv2.normalize(out, out, alpha=0, beta=1, norm_type=cv2.NORM_MINMAX) + return out + + +def preprocess_for_yolo(frame_bgr): + if (not ANALOG_FPV_MODE) or (not APPLY_YOLO_PREPROC): + return frame_bgr + + out = frame_bgr + if PRE_BLUR_K >= 3: + k = int(PRE_BLUR_K) + if (k % 2) == 0: + k += 1 + out = cv2.GaussianBlur(out, (k, k), 0) + + ycrcb = cv2.cvtColor(out, cv2.COLOR_BGR2YCrCb) + y, cr, cb = cv2.split(ycrcb) + clahe = cv2.createCLAHE(clipLimit=float(PRE_CLAHE_CLIP), tileGridSize=PRE_CLAHE_TILE) + y = clahe.apply(y) + out = cv2.cvtColor(cv2.merge((y, cr, cb)), cv2.COLOR_YCrCb2BGR) + + if PRE_UNSHARP > 1e-6: + blur = cv2.GaussianBlur(out, (0, 0), 1.1) + out = cv2.addWeighted(out, 1.0 + float(PRE_UNSHARP), blur, -float(PRE_UNSHARP), 0) + + return out + + +def in_norm_rect(x, y, w, h, rect_norm): + rx1, ry1, rx2, ry2 = rect_norm + return (rx1 * w) <= x <= (rx2 * w) and (ry1 * h) <= y <= (ry2 * h) + + +def in_osd_zone(center_x, center_y, frame_w, frame_h): + for z in REJECT_OSD_ZONES_NORM: + if in_norm_rect(center_x, center_y, frame_w, frame_h, z): + return True + return False + + +def filter_yolo_boxes_with_scores(result, frame_w, frame_h, offset_x=0, offset_y=0, min_conf=0.12): + dets_out = [] + if result.boxes is None or len(result.boxes) == 0: + return dets_out + + xyxy = result.boxes.xyxy.detach().cpu().numpy() + confs = result.boxes.conf.detach().cpu().numpy() + clss = result.boxes.cls.detach().cpu().numpy().astype(int) + + for b, c, cls_id in zip(xyxy, confs, clss): + score = float(c) + if score < float(min_conf): + continue + if TARGET_CLASS_ID is not None and cls_id != TARGET_CLASS_ID: + continue + + x1, y1, x2, y2 = map(float, b) + ww = max(0.0, x2 - x1) + hh = max(0.0, y2 - y1) + if ww <= 1.0 or hh <= 1.0: + continue + + a = ww * hh + if a < MIN_BOX_AREA: + continue + if MAX_BOX_AREA > 0 and a > MAX_BOX_AREA: + continue + + ar = ww / hh + if not (ASPECT_RANGE[0] <= ar <= ASPECT_RANGE[1]): + continue + + gx1, gy1, gx2, gy2 = x1 + offset_x, y1 + offset_y, x2 + offset_x, y2 + offset_y + if REJECT_OSD_ZONES: + cx = 0.5 * (gx1 + gx2) + cy = 0.5 * (gy1 + gy2) + if (a <= REJECT_OSD_SMALL_AREA_MAX) and in_osd_zone(cx, cy, frame_w, frame_h): + continue + + dets_out.append(np.array([gx1, gy1, gx2, gy2, score], dtype=np.float32)) + + return dets_out + + +def merge_close_part_dets(dets_eff, ref_box, frame_w, frame_h): + if (not PART_MERGE_ENABLE) or ref_box is None or len(dets_eff) < int(max(2, PART_MERGE_MIN_PARTS)): + return dets_eff, 0 + + ref = clip_box(ref_box, frame_w, frame_h) + ref_center = box_center(ref) + ref_diag = max(float(np.linalg.norm(box_wh(ref))), float(PART_MERGE_MIN_REF_DIAG)) + ref_area = box_area(ref) + + boxes = [clip_box(d[:4], frame_w, frame_h) for d in dets_eff] + scores = [float(d[4]) for d in dets_eff] + + near_ids = [] + for idx, b in enumerate(boxes): + c = box_center(b) + dist = float(np.linalg.norm(c - ref_center)) + if (dist <= float(PART_MERGE_REF_DIST_DIAG) * ref_diag) or (iou(b, ref) >= float(PART_MERGE_IOU_FLOOR)): + near_ids.append(idx) + + if len(near_ids) < int(max(2, PART_MERGE_MIN_PARTS)): + return dets_eff, 0 + + seed = max(near_ids, key=lambda ii: scores[ii]) + cluster = {int(seed)} + + changed = True + while changed: + changed = False + cluster_boxes = [boxes[ii] for ii in cluster] + ux1 = min(float(b[0]) for b in cluster_boxes) + uy1 = min(float(b[1]) for b in cluster_boxes) + ux2 = max(float(b[2]) for b in cluster_boxes) + uy2 = max(float(b[3]) for b in cluster_boxes) + union_box = clip_box([ux1, uy1, ux2, uy2], frame_w, frame_h) + union_center = box_center(union_box) + union_diag = max(float(np.linalg.norm(box_wh(union_box))), ref_diag) + + for idx in near_ids: + if idx in cluster: + continue + b = boxes[idx] + c = box_center(b) + center_close = float(np.linalg.norm(c - union_center)) <= (float(PART_MERGE_CLUSTER_DIST_DIAG) * union_diag) + overlap = iou(b, union_box) >= float(PART_MERGE_IOU_FLOOR) + if center_close or overlap: + cluster.add(int(idx)) + changed = True + + if len(cluster) < int(max(2, PART_MERGE_MIN_PARTS)): + return dets_eff, 0 + + cluster_boxes = [boxes[ii] for ii in cluster] + ux1 = min(float(b[0]) for b in cluster_boxes) + uy1 = min(float(b[1]) for b in cluster_boxes) + ux2 = max(float(b[2]) for b in cluster_boxes) + uy2 = max(float(b[3]) for b in cluster_boxes) + union_box = clip_box([ux1, uy1, ux2, uy2], frame_w, frame_h) + union_area = box_area(union_box) + max_part_area = max(box_area(b) for b in cluster_boxes) + union_ar = box_ar(union_box) + + if union_area < (float(PART_MERGE_MIN_UNION_GAIN) * max_part_area): + return dets_eff, 0 + if union_area > (float(PART_MERGE_MAX_UNION_RATIO) * max(1.0, ref_area)): + return dets_eff, 0 + if union_ar > float(PART_MERGE_MAX_ASPECT): + return dets_eff, 0 + + merged_score = min(0.99, max(scores[ii] for ii in cluster) + float(PART_MERGE_SCORE_BONUS)) + merged_det = np.array([union_box[0], union_box[1], union_box[2], union_box[3], merged_score], dtype=np.float32) + + out = [dets_eff[idx] for idx in range(len(dets_eff)) if idx not in cluster] + out.append(merged_det) + return out, int(len(cluster)) + + +def _odd_ksize(k): + k = int(max(1, k)) + if (k % 2) == 0: + k += 1 + return k + + +def build_motion_mask(prev_gray, gray_now, affine=None): + if prev_gray is None or gray_now is None: + return None + + h, w = gray_now.shape[:2] + ref = prev_gray + if affine is not None: + ref = cv2.warpAffine( + prev_gray, + affine, + (w, h), + flags=cv2.INTER_LINEAR, + borderMode=cv2.BORDER_REPLICATE + ) + + cur = gray_now + if MOTION_BLUR_K >= 3: + bk = _odd_ksize(MOTION_BLUR_K) + ref = cv2.GaussianBlur(ref, (bk, bk), 0) + cur = cv2.GaussianBlur(cur, (bk, bk), 0) + + diff = cv2.absdiff(cur, ref) + _, mask = cv2.threshold(diff, int(MOTION_DIFF_THR), 255, cv2.THRESH_BINARY) + + mk = int(max(1, MOTION_MORPH_K)) + if mk > 1: + kernel = np.ones((mk, mk), dtype=np.uint8) + mask = cv2.morphologyEx(mask, cv2.MORPH_OPEN, kernel) + mask = cv2.dilate(mask, kernel, iterations=1) + + return mask + + +def motion_zones_to_roi(mask, frame_w, frame_h): + if mask is None or mask.size == 0: + return None, 0 + + gx = int(max(1, MOTION_ZONE_GRID[0])) + gy = int(max(1, MOTION_ZONE_GRID[1])) + zone_w = float(frame_w) / float(gx) + zone_h = float(frame_h) / float(gy) + + active = [] + for iy in range(gy): + y1 = int(round(iy * zone_h)) + y2 = int(round((iy + 1) * zone_h)) + y2 = max(y2, y1 + 1) + for ix in range(gx): + x1 = int(round(ix * zone_w)) + x2 = int(round((ix + 1) * zone_w)) + x2 = max(x2, x1 + 1) + patch = mask[y1:y2, x1:x2] + if patch.size == 0: + continue + px = int(cv2.countNonZero(patch)) + area = max(1, (x2 - x1) * (y2 - y1)) + min_need = max(int(MOTION_ZONE_MIN_PIXELS), int(area * float(MOTION_ZONE_MIN_RATIO))) + if px >= min_need: + active.append((x1, y1, x2, y2)) + + active_count = len(active) + if active_count == 0: + return None, 0 + + max_active = int(round(float(gx * gy) * float(MOTION_MAX_ACTIVE_ZONE_RATIO))) + max_active = max(1, max_active) + if active_count > max_active: + return None, active_count + + ux1 = min(z[0] for z in active) - int(MOTION_ROI_MARGIN) + uy1 = min(z[1] for z in active) - int(MOTION_ROI_MARGIN) + ux2 = max(z[2] for z in active) + int(MOTION_ROI_MARGIN) + uy2 = max(z[3] for z in active) + int(MOTION_ROI_MARGIN) + + roi = clip_box([ux1, uy1, ux2, uy2], frame_w, frame_h) + + rw = float(roi[2] - roi[0]) + rh = float(roi[3] - roi[1]) + min_side = float(max(2, MOTION_ROI_MIN_SIZE)) + if rw < min_side or rh < min_side: + cx, cy = box_center(roi) + rw = max(rw, min_side) + rh = max(rh, min_side) + roi = clip_box([cx - rw * 0.5, cy - rh * 0.5, cx + rw * 0.5, cy + rh * 0.5], frame_w, frame_h) + + return roi, active_count + + +def box_motion_stats(mask, box): + if mask is None or mask.size == 0: + return 0, 0.0 + h, w = mask.shape[:2] + x1, y1, x2, y2 = map(int, clip_box(box, w, h)) + patch = mask[y1:y2, x1:x2] + if patch.size == 0: + return 0, 0.0 + px = int(cv2.countNonZero(patch)) + ratio = float(px) / float(max(1, patch.shape[0] * patch.shape[1])) + return px, ratio + + +def wavelet_energy_haar(gray, box, margin=8, min_side=20): + if gray is None: + return 0.0 + h, w = gray.shape[:2] + b = clip_box(box, w, h) + cx, cy = box_center(b) + bw, bh = box_wh(b) + side = max(float(min_side), max(float(bw), float(bh)) + 2.0 * float(margin)) + x1 = int(max(0, round(cx - 0.5 * side))) + y1 = int(max(0, round(cy - 0.5 * side))) + x2 = int(min(w, round(cx + 0.5 * side))) + y2 = int(min(h, round(cy + 0.5 * side))) + if x2 - x1 < 4 or y2 - y1 < 4: + return 0.0 + + p = gray[y1:y2, x1:x2] + if p.size == 0: + return 0.0 + + # 1-level Haar on 2x2 quads, then normalized high-frequency energy. + if (p.shape[0] % 2) == 1: + p = p[:-1, :] + if (p.shape[1] % 2) == 1: + p = p[:, :-1] + if p.shape[0] < 4 or p.shape[1] < 4: + return 0.0 + + f = p.astype(np.float32) / 255.0 + a = f[0::2, 0::2] + b0 = f[0::2, 1::2] + c0 = f[1::2, 0::2] + d0 = f[1::2, 1::2] + + lh = a - b0 + c0 - d0 + hl = a + b0 - c0 - d0 + hh = a - b0 - c0 + d0 + ll = 0.25 * (a + b0 + c0 + d0) + + hf = (np.mean(np.abs(lh)) + np.mean(np.abs(hl)) + np.mean(np.abs(hh))) / 3.0 + lf = float(np.mean(np.abs(ll)) + 1e-3) + return float(hf / lf) + + +def wavelet_hot_roi( + gray, + frame_w, + frame_h, + motion_mask=None, + pred_ref=None, + margin=20, + min_side=84, + pred_scale=3.0, + min_peak_ratio=1.8, +): + if gray is None: + return None, 0.0 + + g = gray + if (g.shape[0] % 2) == 1: + g = g[:-1, :] + if (g.shape[1] % 2) == 1: + g = g[:, :-1] + if g.shape[0] < 8 or g.shape[1] < 8: + return None, 0.0 + + f = g.astype(np.float32) / 255.0 + a = f[0::2, 0::2] + b0 = f[0::2, 1::2] + c0 = f[1::2, 0::2] + d0 = f[1::2, 1::2] + + lh = a - b0 + c0 - d0 + hl = a + b0 - c0 - d0 + hh = a - b0 - c0 + d0 + e = (np.abs(lh) + np.abs(hl) + np.abs(hh)) / 3.0 + e = cv2.GaussianBlur(e, (0, 0), 1.0) + + if motion_mask is not None and motion_mask.size > 0: + m = cv2.resize( + (motion_mask.astype(np.float32) / 255.0), + (e.shape[1], e.shape[0]), + interpolation=cv2.INTER_AREA, + ) + e *= (0.25 + 0.75 * m) + + if pred_ref is not None: + pc = box_center(pred_ref) * 0.5 # energy map is downsampled by 2 + ys, xs = np.indices(e.shape, dtype=np.float32) + sig = max(8.0, 0.22 * float(max(e.shape[0], e.shape[1]))) + prior = np.exp(-((xs - pc[0]) ** 2 + (ys - pc[1]) ** 2) / (2.0 * sig * sig)) + e *= (0.70 + 0.30 * prior) + + peak = float(np.max(e)) + mean = float(np.mean(e) + 1e-6) + peak_ratio = peak / mean + if peak_ratio < float(min_peak_ratio): + return None, peak_ratio + + iy, ix = np.unravel_index(np.argmax(e), e.shape) + cx = (float(ix) + 0.5) * 2.0 + cy = (float(iy) + 0.5) * 2.0 + + base_side = max(float(min_side), float(2.0 * margin + 24.0)) + if pred_ref is not None: + pdiag = float(np.linalg.norm(box_wh(pred_ref))) + side = max(base_side, float(pred_scale) * pdiag) + else: + side = base_side + + roi = clip_box([cx - 0.5 * side, cy - 0.5 * side, cx + 0.5 * side, cy + 0.5 * side], frame_w, frame_h) + return roi, peak_ratio + + +def recover_det_is_valid(det, motion_mask): + score = float(det[4]) + if score < float(RECOVER_MIN_SCORE): + return False + + b = det[:4] + a = box_area(b) + if a <= float(RECOVER_TINY_AREA_MAX): + if score < float(RECOVER_TINY_MIN_SCORE): + return False + px, ratio = box_motion_stats(motion_mask, b) + if px < int(RECOVER_TINY_MIN_MOTION_PIXELS) and ratio < float(RECOVER_TINY_MIN_MOTION_RATIO): + return False + + return True + + +def make_focus_roi_from_box(box, frame_w, frame_h, margin=72, min_side=90): + b = clip_box(box, frame_w, frame_h) + cx, cy = box_center(b) + bw, bh = box_wh(b) + rw = max(float(min_side), float(bw) + 2.0 * float(margin)) + rh = max(float(min_side), float(bh) + 2.0 * float(margin)) + return clip_box([cx - 0.5 * rw, cy - 0.5 * rh, cx + 0.5 * rw, cy + 0.5 * rh], frame_w, frame_h) + + +def pick_preacq_det(dets_eff, pred_ref, frame_w, frame_h): + if not dets_eff or pred_ref is None: + return None + pc = box_center(pred_ref) + pdiag = float(np.linalg.norm(box_wh(pred_ref))) + near_lim = max(float(PREACQ_NEAR_MIN), float(PREACQ_NEAR_FACTOR) * pdiag) + + best = None + best_s = -1e9 + for d in dets_eff: + score = float(d[4]) + if score < float(PREACQ_MIN_SCORE): + continue + b = clip_box(d[:4], frame_w, frame_h) + c = box_center(b) + dist = float(np.linalg.norm(c - pc)) + if dist > near_lim: + continue + s = score - (0.0025 * dist) + if s > best_s: + best_s = s + best = np.array([b[0], b[1], b[2], b[3], score], dtype=np.float32) + return best + + +def track_residual_motion_ok(track, motion_mask, frame_w, frame_h): + if motion_mask is None: + return False + b = clip_box(track.tlbr, frame_w, frame_h) + px, ratio = box_motion_stats(motion_mask, b) + return (px >= int(EGO_RESIDUAL_MIN_PIXELS)) or (ratio >= float(EGO_RESIDUAL_MIN_RATIO)) + + +def get_effective_frame(orig_bgr): + oh, ow = orig_bgr.shape[:2] + if not FORCE_EFFECTIVE_PAL: + return orig_bgr, 1.0, 1.0 + eff = cv2.resize(orig_bgr, (EFFECTIVE_W, EFFECTIVE_H), interpolation=cv2.INTER_AREA) + sx = EFFECTIVE_W / float(ow) + sy = EFFECTIVE_H / float(oh) + return eff, sx, sy + + +def unscale_box(box_eff, sx, sy): + x1, y1, x2, y2 = box_eff + return np.array([x1 / sx, y1 / sy, x2 / sx, y2 / sy], dtype=np.float32) + + +def is_int_source(src): + if isinstance(src, int): + return True + if isinstance(src, str) and src.isdigit(): + return True + return False + + +def is_stream_source(src): + if not isinstance(src, str): + return False + s = src.lower() + return ( + s.startswith("rtsp://") + or s.startswith("udp://") + or s.startswith("rtmp://") + or s.startswith("http://") + or s.startswith("https://") + ) + + +def open_source(source, backend=cv2.CAP_DSHOW): + if is_int_source(source): + cap = cv2.VideoCapture(int(source), backend) + return cap, "camera" + + if isinstance(source, str): + if is_stream_source(source): + cap = cv2.VideoCapture(source) + return cap, "stream" + cap = cv2.VideoCapture(source) + return cap, "file" + + raise ValueError(f"Unsupported SOURCE type: {type(source)}") + + +def get_frame_timestamp_seconds(cap, source_kind, frame_id, input_fps, loop_ts): + if TIMESTAMP_USE_SOURCE_CLOCK: + pos_ms = float(cap.get(cv2.CAP_PROP_POS_MSEC)) + if np.isfinite(pos_ms) and pos_ms > 0.0: + return float(pos_ms * 1e-3), "source" + if source_kind == "file" and input_fps > 1.0: + return float(frame_id) / float(input_fps), "fps" + return float(loop_ts), "wall" + + +def sanitize_dt(frame_ts, prev_frame_ts, nominal_dt): + dt = float(nominal_dt) + if prev_frame_ts is not None: + raw_dt = float(frame_ts - prev_frame_ts) + if np.isfinite(raw_dt) and (1e-6 < raw_dt <= float(DT_RESET_GAP_SEC)): + dt = raw_dt + return float(clamp(dt, float(DT_MIN_SEC), float(DT_MAX_SEC))) + + diff --git a/imm_filter.py b/imm_filter.py new file mode 100644 index 0000000..5ab44b2 --- /dev/null +++ b/imm_filter.py @@ -0,0 +1,565 @@ +# ============================================================ +# imm_filter.py +# Модуль 5: IMM-фильтр (Interacting Multiple Model) +# +# Три модели движения цели: +# 1) CV — Constant Velocity (прямолинейное) +# 2) CA — Constant Acceleration (разгон / торможение) +# 3) CT — Coordinated Turn (вираж) +# +# IMM автоматически определяет, какую модель использовать, +# через вероятности Маркова. Это критично для маневрирующей цели, +# т.к. обычный Kalman (CV) запаздывает при манёврах. +# ============================================================ + +import numpy as np +from config import * +from config_intercept import * +from helpers import clamp, box_center, box_wh, clip_box + + +class _KalmanCV: + """ + Constant Velocity модель. + Состояние: [cx, cy, w, h, vx, vy, vw, vh] (8D) + """ + + def __init__(self, q_pos, q_vel): + self.dim_x = 8 + self.dim_z = 4 + self.x = np.zeros((8, 1), dtype=np.float64) + self.P = np.eye(8, dtype=np.float64) * 50.0 + + self.H = np.zeros((4, 8), dtype=np.float64) + for i in range(4): + self.H[i, i] = 1.0 + + self.R = np.diag([IMM_R_POS, IMM_R_POS, IMM_R_SIZE, IMM_R_SIZE]).astype(np.float64) + self.q_pos = float(q_pos) + self.q_vel = float(q_vel) + + def F(self, dt): + F = np.eye(8, dtype=np.float64) + F[0, 4] = dt + F[1, 5] = dt + F[2, 6] = dt + F[3, 7] = dt + return F + + def Q(self, dt): + q = np.zeros((8, 8), dtype=np.float64) + dt2 = dt * dt + q[0, 0] = self.q_pos * dt2 + q[1, 1] = self.q_pos * dt2 + q[2, 2] = self.q_pos * dt2 + q[3, 3] = self.q_pos * dt2 + q[4, 4] = self.q_vel * dt + q[5, 5] = self.q_vel * dt + q[6, 6] = self.q_vel * dt + q[7, 7] = self.q_vel * dt + return q + + def predict(self, dt): + F = self.F(dt) + self.x = F @ self.x + self.P = F @ self.P @ F.T + self.Q(dt) + + def update(self, z): + z = np.array(z, dtype=np.float64).reshape(4, 1) + y = z - self.H @ self.x + S = self.H @ self.P @ self.H.T + self.R + K = self.P @ self.H.T @ np.linalg.inv(S) + self.x = self.x + K @ y + I = np.eye(8, dtype=np.float64) + self.P = (I - K @ self.H) @ self.P + return y, S + + def likelihood(self, z): + """Правдоподобие измерения (для IMM mixing).""" + z = np.array(z, dtype=np.float64).reshape(4, 1) + y = z - self.H @ self.x + S = self.H @ self.P @ self.H.T + self.R + try: + S_inv = np.linalg.inv(S) + det_S = max(np.linalg.det(S), 1e-300) + n = self.dim_z + exp_val = float(-0.5 * (y.T @ S_inv @ y).item()) + exp_val = max(exp_val, -500.0) # предотвращение underflow + norm = (2.0 * np.pi) ** (-n / 2.0) * det_S ** (-0.5) + return max(norm * np.exp(exp_val), 1e-300) + except np.linalg.LinAlgError: + return 1e-300 + + +class _KalmanCA: + """ + Constant Acceleration модель. + Состояние: [cx, cy, w, h, vx, vy, ax, ay, vw, vh] (10D) + Измерение: [cx, cy, w, h] + """ + + def __init__(self, q_pos, q_vel, q_acc): + self.dim_x = 10 + self.dim_z = 4 + self.x = np.zeros((10, 1), dtype=np.float64) + self.P = np.eye(10, dtype=np.float64) * 50.0 + + self.H = np.zeros((4, 10), dtype=np.float64) + self.H[0, 0] = 1.0 # cx + self.H[1, 1] = 1.0 # cy + self.H[2, 2] = 1.0 # w + self.H[3, 3] = 1.0 # h + + self.R = np.diag([IMM_R_POS, IMM_R_POS, IMM_R_SIZE, IMM_R_SIZE]).astype(np.float64) + self.q_pos = float(q_pos) + self.q_vel = float(q_vel) + self.q_acc = float(q_acc) + + def F(self, dt): + F = np.eye(10, dtype=np.float64) + dt2 = 0.5 * dt * dt + F[0, 4] = dt # cx += vx*dt + F[0, 6] = dt2 # cx += 0.5*ax*dt² + F[1, 5] = dt # cy += vy*dt + F[1, 7] = dt2 # cy += 0.5*ay*dt² + F[4, 6] = dt # vx += ax*dt + F[5, 7] = dt # vy += ay*dt + F[2, 8] = dt # w += vw*dt + F[3, 9] = dt # h += vh*dt + return F + + def Q(self, dt): + q = np.zeros((10, 10), dtype=np.float64) + dt2 = dt * dt + q[0, 0] = self.q_pos * dt2 + q[1, 1] = self.q_pos * dt2 + q[2, 2] = self.q_pos * dt2 + q[3, 3] = self.q_pos * dt2 + q[4, 4] = self.q_vel * dt + q[5, 5] = self.q_vel * dt + q[6, 6] = self.q_acc * dt + q[7, 7] = self.q_acc * dt + q[8, 8] = self.q_vel * dt + q[9, 9] = self.q_vel * dt + return q + + def predict(self, dt): + F = self.F(dt) + self.x = F @ self.x + self.P = F @ self.P @ F.T + self.Q(dt) + + def update(self, z): + z = np.array(z, dtype=np.float64).reshape(4, 1) + y = z - self.H @ self.x + S = self.H @ self.P @ self.H.T + self.R + K = self.P @ self.H.T @ np.linalg.inv(S) + self.x = self.x + K @ y + I = np.eye(10, dtype=np.float64) + self.P = (I - K @ self.H) @ self.P + return y, S + + def likelihood(self, z): + z = np.array(z, dtype=np.float64).reshape(4, 1) + y = z - self.H @ self.x + S = self.H @ self.P @ self.H.T + self.R + try: + S_inv = np.linalg.inv(S) + det_S = max(np.linalg.det(S), 1e-300) + n = self.dim_z + exp_val = float(-0.5 * (y.T @ S_inv @ y).item()) + exp_val = max(exp_val, -500.0) + norm = (2.0 * np.pi) ** (-n / 2.0) * det_S ** (-0.5) + return max(norm * np.exp(exp_val), 1e-300) + except np.linalg.LinAlgError: + return 1e-300 + + +class _KalmanCT: + """ + Coordinated Turn модель. + Состояние: [cx, cy, w, h, vx, vy, omega, vw, vh] (9D) + omega = угловая скорость виража (рад/с) + """ + + def __init__(self, q_pos, q_vel, q_omega): + self.dim_x = 9 + self.dim_z = 4 + self.x = np.zeros((9, 1), dtype=np.float64) + self.P = np.eye(9, dtype=np.float64) * 50.0 + + self.H = np.zeros((4, 9), dtype=np.float64) + self.H[0, 0] = 1.0 + self.H[1, 1] = 1.0 + self.H[2, 2] = 1.0 + self.H[3, 3] = 1.0 + + self.R = np.diag([IMM_R_POS, IMM_R_POS, IMM_R_SIZE, IMM_R_SIZE]).astype(np.float64) + self.q_pos = float(q_pos) + self.q_vel = float(q_vel) + self.q_omega = float(q_omega) + + def F(self, dt): + omega = float(self.x[6, 0]) + F = np.eye(9, dtype=np.float64) + + if abs(omega) < 1e-4: + # При нулевой omega — линейная модель + F[0, 4] = dt + F[1, 5] = dt + else: + # Нелинейная CT-модель + so = np.sin(omega * dt) + co = np.cos(omega * dt) + F[0, 4] = so / omega + F[0, 5] = -(1.0 - co) / omega + F[1, 4] = (1.0 - co) / omega + F[1, 5] = so / omega + F[4, 4] = co + F[4, 5] = -so + F[5, 4] = so + F[5, 5] = co + + F[2, 7] = dt + F[3, 8] = dt + return F + + def Q(self, dt): + q = np.zeros((9, 9), dtype=np.float64) + dt2 = dt * dt + q[0, 0] = self.q_pos * dt2 + q[1, 1] = self.q_pos * dt2 + q[2, 2] = self.q_pos * dt2 + q[3, 3] = self.q_pos * dt2 + q[4, 4] = self.q_vel * dt + q[5, 5] = self.q_vel * dt + q[6, 6] = self.q_omega * dt + q[7, 7] = self.q_vel * dt + q[8, 8] = self.q_vel * dt + return q + + def predict(self, dt): + F = self.F(dt) + self.x = F @ self.x + self.P = F @ self.P @ F.T + self.Q(dt) + + def update(self, z): + z = np.array(z, dtype=np.float64).reshape(4, 1) + y = z - self.H @ self.x + S = self.H @ self.P @ self.H.T + self.R + K = self.P @ self.H.T @ np.linalg.inv(S) + self.x = self.x + K @ y + I = np.eye(9, dtype=np.float64) + self.P = (I - K @ self.H) @ self.P + return y, S + + def likelihood(self, z): + z = np.array(z, dtype=np.float64).reshape(4, 1) + y = z - self.H @ self.x + S = self.H @ self.P @ self.H.T + self.R + try: + S_inv = np.linalg.inv(S) + det_S = max(np.linalg.det(S), 1e-300) + n = self.dim_z + exp_val = float(-0.5 * (y.T @ S_inv @ y).item()) + exp_val = max(exp_val, -500.0) + norm = (2.0 * np.pi) ** (-n / 2.0) * det_S ** (-0.5) + return max(norm * np.exp(exp_val), 1e-300) + except np.linalg.LinAlgError: + return 1e-300 + + +# ───────────────────────────────────────────────────────────── +# IMM FILTER +# ───────────────────────────────────────────────────────────── + +class IMMFilter: + """ + Interacting Multiple Model фильтр. + + Управляет тремя фильтрами Калмана с разными моделями движения + и переключается между ними через байесовское взвешивание. + + Интерфейс совместим с Kalman8D из trackers.py: + - init_from_box(box) + - predict(dt) + - update(z) где z = [cx, cy, w, h] + - to_box() → [x1, y1, x2, y2] + - uncertainty() → float + """ + + def __init__(self): + self.enabled = bool(IMM_ENABLE) + + # Три модели + self.models = [ + _KalmanCV(IMM_CV_Q_POS, IMM_CV_Q_VEL), + _KalmanCA(IMM_CA_Q_POS, IMM_CA_Q_VEL, IMM_CA_Q_ACC), + _KalmanCT(IMM_CT_Q_POS, IMM_CT_Q_VEL, IMM_CT_Q_OMEGA), + ] + self.n_models = len(self.models) + self.model_names = ["CV", "CA", "CT"] + + # Вероятности моделей + self.mu = np.array(IMM_INIT_PROBS, dtype=np.float64) + self.mu /= self.mu.sum() + + # Матрица переходов Маркова + self.TPM = np.array(IMM_TRANSITION_MATRIX, dtype=np.float64) + # Нормализация строк + for i in range(self.n_models): + self.TPM[i] /= max(self.TPM[i].sum(), 1e-12) + + self.initialized = False + + # Кэш для API-совместимости с Kalman8D + self._merged_x = np.zeros(8, dtype=np.float64) # [cx, cy, w, h, vx, vy, vw, vh] + self._merged_P = np.eye(8, dtype=np.float64) * 50.0 + + def init_from_box(self, box): + """Инициализация из bounding box.""" + cx, cy = box_center(box) + w, h = box_wh(box) + + for m in self.models: + m.x[:] = 0 + m.x[0, 0] = float(cx) + m.x[1, 0] = float(cy) + m.x[2, 0] = float(w) + m.x[3, 0] = float(h) + m.P = np.eye(m.dim_x, dtype=np.float64) * 50.0 + + self.mu = np.array(IMM_INIT_PROBS, dtype=np.float64) + self.mu /= self.mu.sum() + self.initialized = True + self._update_merged() + + def predict(self, dt, q_scale=1.0): + """ + IMM Predict: interaction → predict каждой модели. + + Returns: + np.array [cx, cy, w, h] — merged prediction + """ + if not self.initialized: + return np.zeros(4, dtype=np.float32) + + dt = float(max(1e-3, dt)) + + # ─── 1. Interaction (mixing) ───────────────────────────── + # Вычисляем mixing probabilities + c_bar = self.TPM.T @ self.mu # predicted model probs + c_bar = np.maximum(c_bar, 1e-12) + + mixing_probs = np.zeros((self.n_models, self.n_models), dtype=np.float64) + for j in range(self.n_models): + for i in range(self.n_models): + mixing_probs[i, j] = self.TPM[i, j] * self.mu[i] / c_bar[j] + + # Mixed states for each model + for j in range(self.n_models): + mj = self.models[j] + dim = mj.dim_x + + # Mixed state + x_mixed = np.zeros((dim, 1), dtype=np.float64) + for i in range(self.n_models): + mi = self.models[i] + # Проецируем состояние mi на размерность mj + xi_proj = self._project_state(mi.x, mi.dim_x, dim) + x_mixed += mixing_probs[i, j] * xi_proj + + # Mixed covariance + P_mixed = np.zeros((dim, dim), dtype=np.float64) + for i in range(self.n_models): + mi = self.models[i] + xi_proj = self._project_state(mi.x, mi.dim_x, dim) + Pi_proj = self._project_cov(mi.P, mi.dim_x, dim) + diff = xi_proj - x_mixed + P_mixed += mixing_probs[i, j] * (Pi_proj + diff @ diff.T) + + mj.x = x_mixed + mj.P = P_mixed + + # ─── 2. Predict each model ────────────────────────────── + for m in self.models: + m.predict(dt) + + self._update_merged() + return self._merged_x[:4].astype(np.float32) + + def update(self, z): + """ + IMM Update: update каждой модели → пересчёт вероятностей. + + Args: + z: [cx, cy, w, h] + """ + if not self.initialized: + return + + z = np.array(z, dtype=np.float64).reshape(4) + + # ─── 1. Likelihood каждой модели ───────────────────────── + likelihoods = np.array( + [m.likelihood(z) for m in self.models], + dtype=np.float64 + ) + + # ─── 2. Update каждой модели ───────────────────────────── + for m in self.models: + m.update(z) + + # ─── 3. Обновление вероятностей моделей ────────────────── + c_bar = self.TPM.T @ self.mu + c_bar = np.maximum(c_bar, 1e-12) + + self.mu = c_bar * likelihoods + total = self.mu.sum() + if total > 1e-300: + self.mu /= total + else: + self.mu = np.array(IMM_INIT_PROBS, dtype=np.float64) + self.mu /= self.mu.sum() + + self._update_merged() + + def to_box(self): + """Возвращает merged bounding box [x1, y1, x2, y2].""" + cx = float(self._merged_x[0]) + cy = float(self._merged_x[1]) + w = float(max(2.0, self._merged_x[2])) + h = float(max(2.0, self._merged_x[3])) + return np.array([cx - w * 0.5, cy - h * 0.5, + cx + w * 0.5, cy + h * 0.5], dtype=np.float32) + + def uncertainty(self): + """Суммарная неопределённость (для совместимости с Kalman8D).""" + return float( + self._merged_P[0, 0] + self._merged_P[1, 1] + + self._merged_P[2, 2] + self._merged_P[3, 3] + ) + + def get_velocity(self): + """Возвращает (vx, vy) — merged скорость в пикс/сек.""" + return float(self._merged_x[4]), float(self._merged_x[5]) + + def get_acceleration(self): + """Возвращает (ax, ay) — оценка ускорения из CA-модели.""" + ca = self.models[1] # CA модель + if ca.dim_x >= 8: + return float(ca.x[6, 0]), float(ca.x[7, 0]) + return 0.0, 0.0 + + def get_turn_rate(self): + """Возвращает omega — угловая скорость из CT-модели.""" + ct = self.models[2] # CT модель + if ct.dim_x >= 7: + return float(ct.x[6, 0]) + return 0.0 + + def get_model_probs(self): + """Возвращает вероятности моделей [p_CV, p_CA, p_CT].""" + return self.mu.copy() + + def get_dominant_model(self): + """Возвращает название наиболее вероятной модели.""" + idx = int(np.argmax(self.mu)) + return self.model_names[idx] + + @property + def x(self): + """Совместимость с Kalman8D.x — возвращает 8x1 вектор.""" + return self._merged_x.reshape(8, 1).astype(np.float32) + + # ─── Private ───────────────────────────────────────────────── + + def _project_state(self, x, from_dim, to_dim): + """Проецирует состояние между моделями разной размерности.""" + out = np.zeros((to_dim, 1), dtype=np.float64) + # Первые 4 компонента (cx, cy, w, h) всегда совпадают + n_copy = min(4, from_dim, to_dim) + out[:n_copy] = x[:n_copy] + + # Скорости vx, vy (индексы 4,5 в CV/CT, 4,5 в CA) + if from_dim >= 6 and to_dim >= 6: + out[4] = x[4] + out[5] = x[5] + + # Скорости размера: зависит от модели + # CV: vw=x[6], vh=x[7] + # CA: vw=x[8], vh=x[9] + # CT: vw=x[7], vh=x[8] + # Для простоты: копируем что можем + return out + + def _project_cov(self, P, from_dim, to_dim): + """Проецирует ковариацию между моделями.""" + out = np.eye(to_dim, dtype=np.float64) * 50.0 + n = min(from_dim, to_dim) + out[:n, :n] = P[:n, :n] + return out + + def _update_merged(self): + """Обновляет merged state как взвешенную сумму моделей.""" + self._merged_x[:] = 0 + self._merged_P[:] = 0 + + for i, m in enumerate(self.models): + xi = self._project_state(m.x, m.dim_x, 8).flatten() + self._merged_x += self.mu[i] * xi + + for i, m in enumerate(self.models): + xi = self._project_state(m.x, m.dim_x, 8).flatten() + Pi = self._project_cov(m.P, m.dim_x, 8) + diff = (xi - self._merged_x).reshape(8, 1) + self._merged_P += self.mu[i] * (Pi + diff @ diff.T) + + # ─── Status / Draw ─────────────────────────────────────────── + + def status_line(self): + if not self.enabled: + return "IMM filter disabled" + return f"IMM filter ready: models={self.model_names}" + + def draw_overlay(self, frame_bgr): + """Рисует вероятности моделей.""" + if not self.enabled or not self.initialized or not DRAW_IMM_PROBS: + return + + import cv2 + + h, w = frame_bgr.shape[:2] + probs = self.get_model_probs() + dominant = self.get_dominant_model() + + # Барграф вероятностей + bar_x = w - 150 + bar_y = 100 + bar_w = 120 + bar_h = 16 + + colors = [ + (200, 200, 200), # CV — серый + (0, 165, 255), # CA — оранжевый + (0, 0, 255), # CT — красный + ] + + for i, (name, prob) in enumerate(zip(self.model_names, probs)): + y = bar_y + i * (bar_h + 6) + # Фон + cv2.rectangle(frame_bgr, (bar_x, y), (bar_x + bar_w, y + bar_h), + (60, 60, 60), -1) + # Заполнение + fill_w = int(bar_w * prob) + cv2.rectangle(frame_bgr, (bar_x, y), (bar_x + fill_w, y + bar_h), + colors[i], -1) + # Текст + cv2.putText(frame_bgr, f"{name} {prob:.0%}", + (bar_x + 4, y + bar_h - 3), + cv2.FONT_HERSHEY_SIMPLEX, 0.40, + (255, 255, 255), 1) + + cv2.putText(frame_bgr, f"IMM: {dominant}", + (bar_x, bar_y - 8), + cv2.FONT_HERSHEY_SIMPLEX, 0.50, + (0, 255, 255), 1) diff --git a/imu_fusion.py b/imu_fusion.py new file mode 100644 index 0000000..439333d --- /dev/null +++ b/imu_fusion.py @@ -0,0 +1,726 @@ +# ============================================================ +# imu_fusion.py +# Sensor Fusion: IMU + Camera + Latency Compensation +# +# Решает три критических проблемы реального перехвата: +# +# 1) ЛАТЕНТНОСТЬ: между захватом кадра и моментом когда команда +# дойдёт до моторов проходит 50-120ms. При Vc=20 м/с это +# 1-2.4 метра промаха. Этот модуль экстраполирует состояние +# цели ВПЕРЁД на величину латентности. +# +# 2) СОБСТВЕННОЕ ДВИЖЕНИЕ: без IMU невозможно отличить движение +# цели от движения камеры. Optical flow affine — грубое +# приближение, ломается при тряске и быстрых манёврах. +# IMU даёт точную ориентацию и скорость перехватчика. +# +# 3) ПРЕДСКАЗАНИЕ ТОЧКИ ВСТРЕЧИ: с IMU мы знаем свою скорость, +# а с latency compensation — где цель БУДЕТ, а не где она +# БЫЛА. Это позволяет PN работать корректно. +# +# Поддерживает: +# - MAVLink ATTITUDE + LOCAL_POSITION_NED (ArduPilot/PX4) +# - MSP_ATTITUDE + MSP_RAW_IMU (Betaflight/INAV) +# - Fallback без IMU (только латентность, по Kalman-предсказанию) +# ============================================================ + +import time +import math +import threading +from collections import deque + +import numpy as np + +from config import * +from config_intercept import * +from helpers import clamp, box_center, box_wh, clip_box + + +# ───────────────────────────────────────────────────────────── +# Config (добавить в config_intercept.py) +# ───────────────────────────────────────────────────────────── + +# Латентность пайплайна (секунды). +# Измеряется как: время захвата кадра → момент исполнения команды мотором. +# Типичные значения: +# Camera capture + USB: 15-25ms +# YOLO inference (async): 8-15ms (но результат приходит на 1-2 кадра позже) +# ByteTrack + Kalman: <1ms +# Guidance compute: <1ms +# Serial/MAVLink send: 1-5ms +# FC processing: 5-15ms +# ESC + motor response: 10-20ms +# ИТОГО: ~50-80ms для ROI, ~80-120ms для fullscan +LATENCY_FIXED_MS = 65.0 # фиксированная оценка полной латентности +LATENCY_ADAPTIVE = True # адаптивная оценка (подстраивается) +LATENCY_EXTRA_TERMINAL_MS = 0.0 # доп. латентность в TERMINAL (0 = не добавлять) +LATENCY_MAX_MS = 200.0 # верхний предел (защита от багов) +LATENCY_MIN_MS = 20.0 # нижний предел + +# IMU +IMU_ENABLE = True +IMU_SOURCE = "mavlink" # "mavlink" | "msp" | "none" +IMU_MAVLINK_CONNECTION = "udpin:0.0.0.0:14550" # получаем телеметрию +IMU_MSP_PORT = "/dev/ttyUSB0" +IMU_MSP_BAUD = 115200 +IMU_POLL_HZ = 100.0 # частота опроса IMU + +# Сглаживание IMU +IMU_SMOOTH_ALPHA = 0.30 # EMA для скоростей +IMU_GRAVITY = 9.81 + +# Camera-IMU alignment (поворот IMU относительно камеры) +# Для типичного FPV: камера наклонена вперёд на 25-35° +CAMERA_TILT_DEG = 30.0 # наклон камеры вперёд (градусы) + +# Latency measurement (для адаптивной оценки) +LATENCY_MEASURE_ENABLE = True +LATENCY_MEASURE_WINDOW = 30 # окно усреднения + +# Debug +DRAW_LATENCY_COMP = True +DRAW_IMU_STATE = True + + +class IMUState: + """Текущее состояние IMU перехватчика.""" + __slots__ = [ + "timestamp", + "roll", "pitch", "yaw", # радианы + "roll_rate", "pitch_rate", "yaw_rate", # рад/с + "vx", "vy", "vz", # м/с, NED frame + "ax", "ay", "az", # м/с², body frame + "lat", "lon", "alt", # GPS (если есть) + "valid", + ] + + def __init__(self): + self.timestamp = 0.0 + self.roll = 0.0 + self.pitch = 0.0 + self.yaw = 0.0 + self.roll_rate = 0.0 + self.pitch_rate = 0.0 + self.yaw_rate = 0.0 + self.vx = 0.0 + self.vy = 0.0 + self.vz = 0.0 + self.ax = 0.0 + self.ay = 0.0 + self.az = 0.0 + self.lat = 0.0 + self.lon = 0.0 + self.alt = 0.0 + self.valid = False + + +class LatencyCompensator: + """ + Компенсация латентности пайплайна. + + Принцип: вместо того чтобы наводиться на позицию цели "сейчас" + (которая на самом деле была latency_ms миллисекунд назад), + экстраполируем состояние цели вперёд на величину латентности. + + Использует: + - Kalman/IMM-предсказание для экстраполяции цели + - IMU для вычитания собственного движения + - Адаптивную оценку латентности по корреляции предсказание/факт + """ + + def __init__(self): + self.latency_sec = float(LATENCY_FIXED_MS) / 1000.0 + self.adaptive = bool(LATENCY_ADAPTIVE) + + # Адаптивная оценка латентности + self._pred_hist = deque(maxlen=int(LATENCY_MEASURE_WINDOW)) + self._measure_enable = bool(LATENCY_MEASURE_ENABLE) + + # Статистика + self.compensated_shift_px = 0.0 + self.ego_shift_px = 0.0 + + def get_latency_sec(self, phase="TRACK"): + """Возвращает текущую оценку латентности.""" + lat = self.latency_sec + if phase == "TERMINAL": + lat += float(LATENCY_EXTRA_TERMINAL_MS) / 1000.0 + return float(clamp(lat, + float(LATENCY_MIN_MS) / 1000.0, + float(LATENCY_MAX_MS) / 1000.0)) + + def compensate(self, kf_state, imu_state, frame_w, frame_h, phase="TRACK"): + """ + Компенсирует латентность — предсказывает где цель БУДЕТ + в момент исполнения команды. + + Args: + kf_state: dict с полями из Kalman/IMM: + cx, cy — текущий центр цели (пиксели) + vx, vy — скорость цели (пиксели/сек) + w, h — размер bbox + ax, ay — ускорение (если IMM CA) + imu_state: IMUState или None + frame_w, frame_h: размеры кадра + phase: текущая фаза FSM + + Returns: + dict: + comp_cx, comp_cy — компенсированный центр цели + comp_box — компенсированный bbox [x1,y1,x2,y2] + ego_dx, ego_dy — смещение из-за собственного движения + pred_dx, pred_dy — смещение из-за движения цели + latency_sec — использованная латентность + shift_px — полное смещение (пиксели) + """ + lat = self.get_latency_sec(phase) + + cx = float(kf_state.get("cx", frame_w * 0.5)) + cy = float(kf_state.get("cy", frame_h * 0.5)) + vx = float(kf_state.get("vx", 0.0)) + vy = float(kf_state.get("vy", 0.0)) + ax = float(kf_state.get("ax", 0.0)) + ay = float(kf_state.get("ay", 0.0)) + bw = float(kf_state.get("w", 20.0)) + bh = float(kf_state.get("h", 20.0)) + + # ─── 1. Предсказание движения цели ────────────────────── + # Линейная экстраполяция + ускорение (если есть) + pred_dx = vx * lat + 0.5 * ax * lat * lat + pred_dy = vy * lat + 0.5 * ay * lat * lat + + # ─── 2. Вычитание собственного движения ───────────────── + ego_dx = 0.0 + ego_dy = 0.0 + + if imu_state is not None and imu_state.valid: + ego_dx, ego_dy = self._compute_ego_shift( + imu_state, lat, frame_w, frame_h + ) + + # ─── 3. Компенсированная позиция ──────────────────────── + comp_cx = cx + pred_dx - ego_dx + comp_cy = cy + pred_dy - ego_dy + + # Clamp к границам кадра + comp_cx = float(clamp(comp_cx, 0.0, float(frame_w - 1))) + comp_cy = float(clamp(comp_cy, 0.0, float(frame_h - 1))) + + # Компенсированный bbox + comp_box = np.array([ + comp_cx - bw * 0.5, + comp_cy - bh * 0.5, + comp_cx + bw * 0.5, + comp_cy + bh * 0.5, + ], dtype=np.float32) + comp_box = clip_box(comp_box, frame_w, frame_h) + + shift = float(np.hypot(pred_dx - ego_dx, pred_dy - ego_dy)) + self.compensated_shift_px = shift + self.ego_shift_px = float(np.hypot(ego_dx, ego_dy)) + + return { + "comp_cx": float(comp_cx), + "comp_cy": float(comp_cy), + "comp_box": comp_box, + "ego_dx": float(ego_dx), + "ego_dy": float(ego_dy), + "pred_dx": float(pred_dx), + "pred_dy": float(pred_dy), + "latency_sec": float(lat), + "shift_px": float(shift), + } + + def feed_measurement(self, predicted_center, actual_center, dt): + """ + Для адаптивной оценки латентности. + Вызывается когда YOLO даёт новую детекцию — сравниваем + где мы предсказывали цель vs где она реально оказалась. + + Если prediction overshoots — латентность завышена. + Если undershoots — занижена. + """ + if not self._measure_enable or not self.adaptive: + return + + if predicted_center is None or actual_center is None: + return + + pred = np.array(predicted_center, dtype=np.float32) + actual = np.array(actual_center, dtype=np.float32) + error = float(np.linalg.norm(pred - actual)) + + self._pred_hist.append({ + "error": error, + "dt": float(dt), + }) + + # Пока простая эвристика: если средняя ошибка растёт, + # уменьшаем латентность (overshooting) + if len(self._pred_hist) >= 10: + errors = [p["error"] for p in self._pred_hist] + recent = np.mean(errors[-5:]) + older = np.mean(errors[:5]) + if recent > older * 1.3: + self.latency_sec *= 0.95 + elif recent < older * 0.7: + self.latency_sec *= 1.05 + self.latency_sec = float(clamp( + self.latency_sec, + float(LATENCY_MIN_MS) / 1000.0, + float(LATENCY_MAX_MS) / 1000.0, + )) + + def _compute_ego_shift(self, imu, lat, frame_w, frame_h): + """ + Вычисляет смещение изображения из-за собственного вращения/движения + дрона-перехватчика за время lat секунд. + + Используем угловые скорости IMU → пиксельное смещение через + фокусное расстояние камеры. + """ + focal = float(CAMERA_FOCAL_LENGTH_PX) + if focal < 10.0: + return 0.0, 0.0 + + tilt_rad = float(CAMERA_TILT_DEG) * (math.pi / 180.0) + + # Угловые скорости (body frame → camera frame) + # Камера наклонена вперёд, поэтому rotation mapping: + # camera_pan ≈ yaw_rate * cos(tilt) + pitch_rate * sin(tilt) + # camera_tilt ≈ pitch_rate * cos(tilt) - yaw_rate * sin(tilt) + cos_t = math.cos(tilt_rad) + sin_t = math.sin(tilt_rad) + + # Угловая скорость камеры + cam_pan_rate = imu.yaw_rate * cos_t + imu.pitch_rate * sin_t + cam_tilt_rate = imu.pitch_rate * cos_t - imu.yaw_rate * sin_t + cam_roll_rate = imu.roll_rate + + # Angular velocity → pixel shift + # Для pinhole camera: dx_px ≈ focal * d_angle + ego_dx = focal * cam_pan_rate * lat # горизонтальное смещение + ego_dy = focal * cam_tilt_rate * lat # вертикальное смещение + + # Roll создаёт вращение вокруг центра — для малых углов: + # dx_roll ≈ -(y - cy) * roll_rate * lat + # dy_roll ≈ (x - cx) * roll_rate * lat + # Это применяется к конкретной точке, здесь пропускаем + # (применится в compensate() если нужно) + + return float(ego_dx), float(ego_dy) + + +class IMUReader: + """ + Асинхронное чтение IMU с полётного контроллера. + Работает в отдельном потоке. + """ + + def __init__(self): + self.enabled = bool(IMU_ENABLE) and (IMU_SOURCE != "none") + self.source = str(IMU_SOURCE).lower() + self.state = IMUState() + self._lock = threading.Lock() + self._running = False + self._thread = None + self._conn = None + + # Сглаживание + self._alpha = float(IMU_SMOOTH_ALPHA) + self._smooth_vx = 0.0 + self._smooth_vy = 0.0 + self._smooth_vz = 0.0 + + def start(self): + if not self.enabled: + return + self._running = True + self._thread = threading.Thread(target=self._loop, daemon=True) + self._thread.start() + + def stop(self): + self._running = False + if self._thread is not None: + self._thread.join(timeout=1.0) + if self._conn is not None: + try: + self._conn.close() + except Exception: + pass + + def get_state(self): + """Потокобезопасное чтение последнего состояния IMU.""" + with self._lock: + s = IMUState() + for attr in IMUState.__slots__: + setattr(s, attr, getattr(self.state, attr)) + return s + + def status_line(self): + if not self.enabled: + return "IMU disabled" + if self.state.valid: + return (f"IMU ready: {self.source} " + f"rpy=({math.degrees(self.state.roll):.1f}, " + f"{math.degrees(self.state.pitch):.1f}, " + f"{math.degrees(self.state.yaw):.1f})") + return f"IMU not ready: {self.source}" + + def _loop(self): + """Главный цикл чтения IMU.""" + if self.source == "mavlink": + self._loop_mavlink() + elif self.source == "msp": + self._loop_msp() + + def _loop_mavlink(self): + """Чтение через MAVLink.""" + try: + from pymavlink import mavutil + self._conn = mavutil.mavlink_connection( + str(IMU_MAVLINK_CONNECTION), + baud=115200, + ) + except Exception as e: + print(f"[imu] MAVLink connection failed: {e}") + return + + poll_interval = 1.0 / max(1.0, float(IMU_POLL_HZ)) + + while self._running: + try: + # Читаем ATTITUDE + msg = self._conn.recv_match( + type=["ATTITUDE", "LOCAL_POSITION_NED", + "SCALED_IMU2", "HIGHRES_IMU"], + blocking=True, + timeout=poll_interval, + ) + if msg is None: + continue + + with self._lock: + msg_type = msg.get_type() + + if msg_type == "ATTITUDE": + self.state.roll = float(msg.roll) + self.state.pitch = float(msg.pitch) + self.state.yaw = float(msg.yaw) + self.state.roll_rate = float(msg.rollspeed) + self.state.pitch_rate = float(msg.pitchspeed) + self.state.yaw_rate = float(msg.yawspeed) + self.state.timestamp = time.perf_counter() + self.state.valid = True + + elif msg_type == "LOCAL_POSITION_NED": + raw_vx = float(msg.vx) + raw_vy = float(msg.vy) + raw_vz = float(msg.vz) + a = self._alpha + self._smooth_vx = a * self._smooth_vx + (1 - a) * raw_vx + self._smooth_vy = a * self._smooth_vy + (1 - a) * raw_vy + self._smooth_vz = a * self._smooth_vz + (1 - a) * raw_vz + self.state.vx = self._smooth_vx + self.state.vy = self._smooth_vy + self.state.vz = self._smooth_vz + + elif msg_type in ("SCALED_IMU2", "HIGHRES_IMU"): + if hasattr(msg, "xacc"): + self.state.ax = float(msg.xacc) / 1000.0 * IMU_GRAVITY + self.state.ay = float(msg.yacc) / 1000.0 * IMU_GRAVITY + self.state.az = float(msg.zacc) / 1000.0 * IMU_GRAVITY + + except Exception: + time.sleep(poll_interval) + + def _loop_msp(self): + """Чтение через MSP (Betaflight/INAV).""" + try: + import serial + self._conn = serial.Serial( + str(IMU_MSP_PORT), + int(IMU_MSP_BAUD), + timeout=0.02, + ) + except Exception as e: + print(f"[imu] MSP serial open failed: {e}") + return + + poll_interval = 1.0 / max(1.0, float(IMU_POLL_HZ)) + + while self._running: + try: + # Запрашиваем MSP_ATTITUDE (108) + attitude = self._msp_request(108, 6) + if attitude is not None and len(attitude) >= 6: + roll_deci = int.from_bytes(attitude[0:2], 'little', signed=True) + pitch_deci = int.from_bytes(attitude[2:4], 'little', signed=True) + yaw_deg = int.from_bytes(attitude[4:6], 'little', signed=False) + + with self._lock: + self.state.roll = float(roll_deci) / 10.0 * (math.pi / 180.0) + self.state.pitch = float(pitch_deci) / 10.0 * (math.pi / 180.0) + self.state.yaw = float(yaw_deg) * (math.pi / 180.0) + self.state.timestamp = time.perf_counter() + self.state.valid = True + + # Запрашиваем MSP_RAW_IMU (102) для угловых скоростей + raw_imu = self._msp_request(102, 18) + if raw_imu is not None and len(raw_imu) >= 18: + gx = int.from_bytes(raw_imu[6:8], 'little', signed=True) + gy = int.from_bytes(raw_imu[8:10], 'little', signed=True) + gz = int.from_bytes(raw_imu[10:12], 'little', signed=True) + # Betaflight gyro: raw → °/s зависит от настройки + # Типично: raw / 16.4 для ±2000°/s + scale = math.pi / (180.0 * 16.4) + with self._lock: + self.state.roll_rate = float(gx) * scale + self.state.pitch_rate = float(gy) * scale + self.state.yaw_rate = float(gz) * scale + + time.sleep(poll_interval) + + except Exception: + time.sleep(poll_interval) + + def _msp_request(self, cmd, expected_len): + """ + Отправляет MSP запрос и читает ответ. + Формат MSP v1: $M< [0] [cmd] [checksum] + Ответ: $M> [size] [cmd] [data...] [checksum] + """ + if self._conn is None: + return None + + # Запрос (пустой payload) + checksum = 0 ^ cmd + packet = bytearray([ + ord('$'), ord('M'), ord('<'), + 0, # size = 0 + cmd, + checksum & 0xFF, + ]) + self._conn.write(packet) + self._conn.flush() + + # Чтение ответа + header = self._conn.read(5) # $M> size cmd + if len(header) < 5: + return None + if header[0:3] != b'$M>': + return None + + size = header[3] + cmd_resp = header[4] + if size < expected_len: + # Читаем что есть + checksum + data = self._conn.read(size + 1) + return data[:size] if len(data) >= size else None + + data = self._conn.read(size + 1) # data + checksum + if len(data) < size: + return None + return data[:size] + + +class SensorFusion: + """ + Высокоуровневый модуль: объединяет IMU + Latency Compensation. + + Вызывается один раз в каждом кадре main loop. + Принимает текущее состояние трекера и выдаёт + компенсированную позицию/bbox для наведения. + """ + + def __init__(self): + self.imu_reader = IMUReader() + self.latency_comp = LatencyCompensator() + self.enabled = True + + def start(self): + self.imu_reader.start() + + def stop(self): + self.imu_reader.stop() + + def status_line(self): + imu_status = self.imu_reader.status_line() + lat = self.latency_comp.latency_sec * 1000.0 + return f"Sensor fusion: latency={lat:.0f}ms | {imu_status}" + + def update(self, kf, locked_box, pred_box, frame_w, frame_h, + phase="TRACK", confirmed=False): + """ + Полный пайплайн компенсации. + + Args: + kf: Kalman8D или IMMFilter (с .x, .initialized) + locked_box: текущий locked bbox или None + pred_box: Kalman-предсказанный bbox или None + frame_w, frame_h: размеры effective frame + phase: фаза FSM + confirmed: цель подтверждена + + Returns: + dict: + comp_center — компенсированный центр [cx, cy] + comp_box — компенсированный bbox [x1, y1, x2, y2] + raw_center — исходный центр (без компенсации) + imu_valid — IMU доступен + latency_ms — использованная латентность + shift_px — смещение компенсации + ego_speed — скорость дрона (м/с) + ego_yaw_rate — скорость рыскания (°/с) + """ + result = { + "comp_center": None, + "comp_box": None, + "raw_center": None, + "imu_valid": False, + "latency_ms": 0.0, + "shift_px": 0.0, + "ego_speed": 0.0, + "ego_yaw_rate": 0.0, + } + + # Определяем исходную позицию цели + ref_box = locked_box if locked_box is not None else pred_box + if ref_box is None or not kf.initialized: + return result + + center = box_center(ref_box) + w, h = box_wh(ref_box) + result["raw_center"] = center.copy() + + # Состояние из Kalman/IMM + kf_state = { + "cx": float(center[0]), + "cy": float(center[1]), + "vx": float(kf.x[4, 0]), + "vy": float(kf.x[5, 0]), + "w": float(w), + "h": float(h), + "ax": 0.0, + "ay": 0.0, + } + + # Если IMM — берём ускорение из CA-модели + if hasattr(kf, 'get_acceleration'): + ax, ay = kf.get_acceleration() + kf_state["ax"] = ax + kf_state["ay"] = ay + + # Читаем IMU + imu_state = self.imu_reader.get_state() if self.imu_reader.enabled else None + result["imu_valid"] = (imu_state is not None and imu_state.valid) + + if imu_state is not None and imu_state.valid: + speed = float(np.hypot(imu_state.vx, imu_state.vy)) + result["ego_speed"] = speed + result["ego_yaw_rate"] = float( + math.degrees(imu_state.yaw_rate) + ) + + # Компенсация латентности + comp = self.latency_comp.compensate( + kf_state, imu_state, frame_w, frame_h, phase + ) + + result["comp_center"] = np.array( + [comp["comp_cx"], comp["comp_cy"]], dtype=np.float32 + ) + result["comp_box"] = comp["comp_box"] + result["latency_ms"] = comp["latency_sec"] * 1000.0 + result["shift_px"] = comp["shift_px"] + + return result + + def feed_detection(self, predicted_center, actual_center, dt): + """Для адаптивной калибровки латентности.""" + self.latency_comp.feed_measurement(predicted_center, actual_center, dt) + + def draw_overlay(self, frame_bgr, result, sx, sy): + """Визуализация компенсации.""" + if not DRAW_LATENCY_COMP: + return + + import cv2 + + raw = result.get("raw_center") + comp_center = result.get("comp_center") + shift = result.get("shift_px", 0.0) + lat = result.get("latency_ms", 0.0) + + if raw is not None and comp_center is not None: + # Линия от raw к compensated + rx = int(float(raw[0]) / max(1e-6, sx)) + ry = int(float(raw[1]) / max(1e-6, sy)) + cx = int(float(comp_center[0]) / max(1e-6, sx)) + cy = int(float(comp_center[1]) / max(1e-6, sy)) + + color = (255, 128, 0) # оранжевый + cv2.arrowedLine(frame_bgr, (rx, ry), (cx, cy), + color, 2, cv2.LINE_AA) + cv2.circle(frame_bgr, (cx, cy), 5, color, -1) + + # Текст + y0 = 320 + imu_str = "IMU:OK" if result.get("imu_valid") else "IMU:---" + ego = result.get("ego_speed", 0.0) + yaw_r = result.get("ego_yaw_rate", 0.0) + + txt = f"LAT: {lat:.0f}ms shift={shift:.1f}px {imu_str}" + cv2.putText(frame_bgr, txt, (20, y0), + cv2.FONT_HERSHEY_SIMPLEX, 0.55, (255, 128, 0), 2) + + if DRAW_IMU_STATE and result.get("imu_valid"): + txt2 = f"EGO: v={ego:.1f}m/s yaw_r={yaw_r:.1f}d/s" + cv2.putText(frame_bgr, txt2, (20, y0 + 25), + cv2.FONT_HERSHEY_SIMPLEX, 0.55, (255, 128, 0), 2) + + +# ============================================================= +# ИНТЕГРАЦИЯ В MAIN.PY +# ============================================================= +# +# 1. Импорт: +# from imu_fusion import SensorFusion +# +# 2. Инициализация (после guidance_ctrl): +# sensor_fusion = SensorFusion() +# sensor_fusion.start() +# print(sensor_fusion.status_line()) +# +# 3. В главном цикле, ПОСЛЕ guidance_state и ПЕРЕД autopilot: +# +# fusion_result = sensor_fusion.update( +# kf=kf, +# locked_box=locked_box_eff, +# pred_box=pred_box_eff, +# frame_w=ew, +# frame_h=eh, +# phase=intercept_params.get("phase", "TRACK") if intercept_params else "TRACK", +# confirmed=confirmed, +# ) +# +# # Используем компенсированную позицию для PN и autopilot: +# if fusion_result["comp_center"] is not None: +# # Передаём в PN вместо raw center +# target_center_for_pn = fusion_result["comp_center"] +# target_box_for_guidance = fusion_result["comp_box"] +# +# 4. При получении новой YOLO-детекции — адаптивная калибровка: +# if have_yolo and chosen_valid: +# sensor_fusion.feed_detection( +# predicted_center=box_center(pred_box_eff) if pred_box_eff is not None else None, +# actual_center=box_center(chosen), +# dt=dt, +# ) +# +# 5. Отрисовка: +# sensor_fusion.draw_overlay(frame_orig, fusion_result, sx, sy) +# +# 6. Cleanup: +# sensor_fusion.stop() +# ============================================================= diff --git a/intercept_fsm.py b/intercept_fsm.py new file mode 100644 index 0000000..5ba3964 --- /dev/null +++ b/intercept_fsm.py @@ -0,0 +1,387 @@ +# ============================================================ +# intercept_fsm.py +# Модуль 3: Finite State Machine фаз перехвата +# +# Состояния: +# SEARCH — нет цели, полное сканирование +# ACQUIRE — цель обнаружена, набираем подтверждений +# TRACK — стабильный трек, удержание / сближение +# INTERCEPT — активная атака, PN-наведение +# TERMINAL — финальная фаза (<1.5 сек до контакта) +# LOST — потеря цели в ходе перехвата, попытка реакквизиции +# +# FSM управляет: +# - какой закон наведения использовать +# - throttle bias +# - разрешение на переключение трека +# - параметры детектора (ROI / fullscan) +# ============================================================ + +import time +import numpy as np + +from config import * +from config_intercept import * +from helpers import clamp + +import cv2 + + +class InterceptPhase: + """Enum-like для фаз.""" + SEARCH = "SEARCH" + ACQUIRE = "ACQUIRE" + TRACK = "TRACK" + INTERCEPT = "INTERCEPT" + TERMINAL = "TERMINAL" + LOST = "LOST" + + +class InterceptFSM: + """ + Конечный автомат фаз перехвата. + + Определяет текущую фазу на основе: + - состояния трекера (confirmed, miss_streak, hit_streak) + - оценки дальности (range_m) + - tau (time-to-contact) + - уверенности (confidence) + + Каждая фаза определяет набор параметров для guidance, throttle и трекера. + """ + + def __init__(self): + self.enabled = bool(INTERCEPT_FSM_ENABLE) + self.phase = InterceptPhase.SEARCH + self.prev_phase = InterceptPhase.SEARCH + self.phase_start_time = time.perf_counter() + self.phase_frame_count = 0 + + # Счётчики для переходов + self._acquire_frames = 0 + self._lost_frames = 0 + self._terminal_locked = False + + # Замороженные команды для TERMINAL lock + self._frozen_cmd = None + + # Лог переходов + self.transitions = [] + + def reset(self): + self._transition_to(InterceptPhase.SEARCH) + self._acquire_frames = 0 + self._lost_frames = 0 + self._terminal_locked = False + self._frozen_cmd = None + self.transitions.clear() + + def update(self, confirmed, hit_streak, miss_streak, + range_state, guidance_state, frame_id): + """ + Обновить FSM. Вызывается каждый кадр. + + Args: + confirmed: bool — цель подтверждена трекером + hit_streak: int — кол-во последовательных hit'ов + miss_streak: int — кол-во последовательных miss'ов + range_state: dict из RangeEstimator.update() + guidance_state: dict из guidance.py или PN + frame_id: int + + Returns: + dict — параметры текущей фазы для всех модулей + """ + if not self.enabled: + return self._default_params() + + range_m = range_state.get("range_m") if range_state else None + tau = range_state.get("tau", float('inf')) if range_state else float('inf') + closing_vel = range_state.get("closing_vel", 0.0) if range_state else 0.0 + range_conf = range_state.get("confidence", 0.0) if range_state else 0.0 + range_phase = range_state.get("phase", "far") if range_state else "far" + + self.phase_frame_count += 1 + + # ─── Переходы ─────────────────────────────────────────── + + if self.phase == InterceptPhase.SEARCH: + if confirmed: + self._transition_to(InterceptPhase.ACQUIRE) + + elif self.phase == InterceptPhase.ACQUIRE: + if not confirmed and miss_streak > 3: + self._transition_to(InterceptPhase.SEARCH) + elif hit_streak >= int(FSM_TRACK_CONFIRM_HITS): + self._transition_to(InterceptPhase.TRACK) + elif self.phase_frame_count > int(FSM_ACQUIRE_TIMEOUT): + self._transition_to(InterceptPhase.SEARCH) + + elif self.phase == InterceptPhase.TRACK: + if not confirmed and miss_streak > int(FSM_LOST_TIMEOUT): + self._transition_to(InterceptPhase.LOST) + elif self._should_intercept(range_m, tau, closing_vel, range_conf): + self._transition_to(InterceptPhase.INTERCEPT) + + elif self.phase == InterceptPhase.INTERCEPT: + if not confirmed and miss_streak > int(FSM_LOST_TIMEOUT) // 2: + self._transition_to(InterceptPhase.LOST) + elif self._should_terminal(range_m, tau, range_conf): + self._transition_to(InterceptPhase.TERMINAL) + elif self._should_back_to_track(range_m, closing_vel): + self._transition_to(InterceptPhase.TRACK) + + elif self.phase == InterceptPhase.TERMINAL: + # В TERMINAL мы уже не откатываемся — либо контакт, + # либо пролёт → LOST + if not confirmed and miss_streak > 10: + # Вероятно пролетели мимо или столкнулись + self._transition_to(InterceptPhase.SEARCH) + elif tau > float(TAU_WARN_SEC) * 2.0 and miss_streak > 3: + # Tau вырос обратно — пролёт + self._transition_to(InterceptPhase.LOST) + + elif self.phase == InterceptPhase.LOST: + if confirmed and hit_streak >= 2: + # Реакквизиция — возвращаемся в TRACK или INTERCEPT + if self._should_intercept(range_m, tau, closing_vel, range_conf): + self._transition_to(InterceptPhase.INTERCEPT) + else: + self._transition_to(InterceptPhase.TRACK) + elif self.phase_frame_count > int(FSM_LOST_RECOVER_TIMEOUT): + self._transition_to(InterceptPhase.SEARCH) + + # ─── Параметры текущей фазы ───────────────────────────── + return self._build_params(range_m, tau, closing_vel, + guidance_state, confirmed) + + # ─── Условия переходов ─────────────────────────────────────── + + def _should_intercept(self, range_m, tau, closing_vel, conf): + """TRACK → INTERCEPT""" + if range_m is not None and range_m <= float(FSM_INTERCEPT_RANGE_M): + return True + if closing_vel > float(PN_MIN_CLOSING_VEL) and tau < float(TAU_WARN_SEC) * 2.0: + return True + return False + + def _should_terminal(self, range_m, tau, conf): + """INTERCEPT → TERMINAL""" + if tau <= float(FSM_TERMINAL_TAU_SEC) and tau > 0: + return True + if range_m is not None and range_m <= float(FSM_TERMINAL_RANGE_M): + return True + return False + + def _should_back_to_track(self, range_m, closing_vel): + """INTERCEPT → TRACK (цель удаляется)""" + if closing_vel < -2.0 and self.phase_frame_count > 30: + return True + if range_m is not None and range_m > float(FSM_INTERCEPT_RANGE_M) * 1.5: + return True + return False + + # ─── Параметры фаз ────────────────────────────────────────── + + def _build_params(self, range_m, tau, closing_vel, + guidance_state, confirmed): + """Собирает параметры для всех модулей на основе текущей фазы.""" + + params = { + "phase": self.phase, + "prev_phase": self.prev_phase, + "phase_frame_count": self.phase_frame_count, + "throttle_bias": 0.5, + "use_pn": False, + "use_screen_guidance": True, + "allow_target_switch": True, + "force_fullscan": False, + "det_every_override": None, + "yolo_enabled": True, + "klt_only": False, + "lock_commands": False, + "frozen_cmd": None, + "range_m": range_m, + "tau": tau, + "closing_vel": closing_vel, + } + + if self.phase == InterceptPhase.SEARCH: + params.update({ + "throttle_bias": 0.4, + "use_pn": False, + "use_screen_guidance": False, + "allow_target_switch": True, + "force_fullscan": True, + }) + + elif self.phase == InterceptPhase.ACQUIRE: + params.update({ + "throttle_bias": 0.45, + "use_pn": False, + "use_screen_guidance": True, + "allow_target_switch": True, + "det_every_override": 2, + }) + + elif self.phase == InterceptPhase.TRACK: + params.update({ + "throttle_bias": float(FSM_TRACK_THROTTLE_BIAS), + "use_pn": False, + "use_screen_guidance": True, + "allow_target_switch": False, + }) + + elif self.phase == InterceptPhase.INTERCEPT: + params.update({ + "throttle_bias": float(FSM_INTERCEPT_THROTTLE_BIAS), + "use_pn": True, + "use_screen_guidance": False, + "allow_target_switch": False, + "det_every_override": 1, # максимальная частота детекции + }) + + elif self.phase == InterceptPhase.TERMINAL: + should_lock = ( + bool(FSM_TERMINAL_LOCK_COMMANDS) + and tau <= float(FSM_TERMINAL_LOCK_TAU_SEC) + ) + + if should_lock and not self._terminal_locked: + # Замораживаем команды + self._terminal_locked = True + if guidance_state is not None: + self._frozen_cmd = { + "steer_x": guidance_state.get("steer_x", 0.0), + "steer_y": guidance_state.get("steer_y", 0.0), + "cmd_x": guidance_state.get("cmd_x", 0.0), + "cmd_y": guidance_state.get("cmd_y", 0.0), + } + + params.update({ + "throttle_bias": float(FSM_TERMINAL_THROTTLE), + "use_pn": not should_lock, + "use_screen_guidance": False, + "allow_target_switch": False, + "yolo_enabled": not bool(FSM_TERMINAL_USE_KLT_ONLY), + "klt_only": bool(FSM_TERMINAL_USE_KLT_ONLY), + "lock_commands": should_lock, + "frozen_cmd": self._frozen_cmd if should_lock else None, + "det_every_override": 1, + }) + + elif self.phase == InterceptPhase.LOST: + params.update({ + "throttle_bias": 0.5, + "use_pn": False, + "use_screen_guidance": False, + "allow_target_switch": True, + "force_fullscan": True, + }) + + return params + + def _default_params(self): + """Параметры по умолчанию когда FSM отключена.""" + return { + "phase": "DISABLED", + "prev_phase": "DISABLED", + "phase_frame_count": 0, + "throttle_bias": 0.5, + "use_pn": False, + "use_screen_guidance": True, + "allow_target_switch": True, + "force_fullscan": False, + "det_every_override": None, + "yolo_enabled": True, + "klt_only": False, + "lock_commands": False, + "frozen_cmd": None, + "range_m": None, + "tau": float('inf'), + "closing_vel": 0.0, + } + + def _transition_to(self, new_phase): + """Выполнить переход в новую фазу.""" + if new_phase == self.phase: + return + + old_phase = self.phase + self.prev_phase = old_phase + self.phase = new_phase + self.phase_start_time = time.perf_counter() + self.phase_frame_count = 0 + + # Сброс фазо-специфичных счётчиков + if new_phase == InterceptPhase.TERMINAL: + self._terminal_locked = False + self._frozen_cmd = None + elif new_phase == InterceptPhase.SEARCH: + self._acquire_frames = 0 + self._lost_frames = 0 + self._terminal_locked = False + self._frozen_cmd = None + + self.transitions.append({ + "time": time.perf_counter(), + "from": old_phase, + "to": new_phase, + }) + + # Ограничиваем лог + if len(self.transitions) > 200: + self.transitions = self.transitions[-100:] + + # ─── Status / Draw ─────────────────────────────────────────── + + def status_line(self): + if not self.enabled: + return "Intercept FSM disabled" + return f"Intercept FSM ready: phase={self.phase}" + + def draw_overlay(self, frame_bgr, params): + """Рисует текущую фазу перехвата.""" + if not self.enabled or not DRAW_INTERCEPT_FSM: + return + + phase = params.get("phase", "?") + tau = params.get("tau", float('inf')) + range_m = params.get("range_m") + throttle = params.get("throttle_bias", 0.5) + + # Цвета по фазе + phase_colors = { + InterceptPhase.SEARCH: (150, 150, 150), + InterceptPhase.ACQUIRE: (0, 200, 255), + InterceptPhase.TRACK: (255, 200, 0), + InterceptPhase.INTERCEPT: (0, 128, 255), + InterceptPhase.TERMINAL: (0, 0, 255), + InterceptPhase.LOST: (128, 0, 128), + } + color = phase_colors.get(phase, (180, 180, 180)) + + # Фон для фазы (полоска сверху) + h, w = frame_bgr.shape[:2] + if phase == InterceptPhase.TERMINAL: + # Мигающая красная полоска + if (self.phase_frame_count // 4) % 2 == 0: + cv2.rectangle(frame_bgr, (0, 0), (w, 6), (0, 0, 255), -1) + + # Текст фазы + range_str = f"{range_m:.1f}m" if range_m is not None else "---" + tau_str = f"{tau:.2f}s" if tau < 100 else "---" + + txt = (f"FSM: {phase} R={range_str} " + f"tau={tau_str} thr={throttle:.0%}") + cv2.putText(frame_bgr, txt, (20, 290), + cv2.FONT_HERSHEY_SIMPLEX, 0.60, color, 2) + + # Индикатор use_pn + if params.get("use_pn"): + cv2.putText(frame_bgr, "PN", (w - 60, 40), + cv2.FONT_HERSHEY_SIMPLEX, 0.8, (0, 255, 128), 2) + + if params.get("lock_commands"): + cv2.putText(frame_bgr, "CMD LOCK", (w - 160, 70), + cv2.FONT_HERSHEY_SIMPLEX, 0.7, (0, 0, 255), 2) diff --git a/main.py b/main.py new file mode 100644 index 0000000..7bb552f --- /dev/null +++ b/main.py @@ -0,0 +1,2553 @@ +import sys +from pathlib import Path +import cv2 +import numpy as np +import time +from collections import deque +import torch +from ultralytics import YOLO + +import cbam_register # noqa: F401 +# Keep compatibility with checkpoints that reference __main__.CBAM. +ChannelAttentionDyn = cbam_register.ChannelAttentionDyn +SpatialAttention = cbam_register.SpatialAttention +CBAM = cbam_register.CBAM +from config import * +from helpers import * +from trackers import Kalman8D +from trackers_hybrid import HybridTracker +from trackers_safe import SafeKalman8D +from yolo_worker import YOLOWorker +from autogaze_runner import AutoGazeROIWorker +from camera_motion import ( + estimate_global_affine, + estimate_global_affine_ex, + apply_affine_to_point, + affine_is_plausible, +) +from motion_saliency import MotionSaliency +from stationary_killer import StationaryKiller +from decision_logger import TrackingDecisionLogger +from guidance import ScreenGuidanceController +from template_matching import tm_update_template, tm_search +from track_score_policy import track_passes_score_gate +from target_handoff import ( + compute_fast_handoff_hits, + evaluate_stale_lock, + pick_guidance_override_box, + should_override_guidance, + update_guidance_override_latch, +) + +# Allow importing ByteTrack implementation from parent TEST directory. +PARENT_TEST = Path(__file__).resolve().parent.parent +if str(PARENT_TEST) not in sys.path: + sys.path.insert(0, str(PARENT_TEST)) + +try: + from bytetrack_min_aggressive import BYTETracker +except Exception: + from bytetrack_min_aggressive import BYTETracker + + +def build_unique_out_video_path(base_path: str) -> str: + base = Path(base_path) + suffix = base.suffix or ".mp4" + stem = base.stem if base.suffix else base.name + parent = base.parent if str(base.parent) not in ("", ".") else Path.cwd() + parent.mkdir(parents=True, exist_ok=True) + + stamp = time.strftime("%Y%m%d_%H%M%S") + candidate = parent / f"{stem}_{stamp}{suffix}" + attempt = 1 + while candidate.exists(): + candidate = parent / f"{stem}_{stamp}_{attempt:02d}{suffix}" + attempt += 1 + return str(candidate) + + +# ─── Motion saliency re-weighting ──────────────────────────── +# Параметры тюнинга (подбираются по логам): +MS_BOOST_WEIGHT = 0.6 # сила буста для движущихся детектов +MS_FLOOR = 0.12 # ниже этого ms_score — confidence штрафуется +MS_FACTOR_MIN = 0.15 # clamp factor снизу (до -85% confidence для статики) +MS_FACTOR_MAX = 1.80 # clamp factor сверху + + +def reweight_dets_by_motion(dets, motion_sal): + """ + Корректирует confidence детектов в зависимости от motion + saliency внутри bbox каждого детекта. + + Движущиеся цели получают буст, статичные (на горизонте, + ЛЭП, деревьях) — штраф. Защищает от wrong target lock на + статических объектах сцены. + """ + if motion_sal is None or motion_sal.saliency is None or not dets: + return dets + + out = [] + for d in dets: + box = d[:4] + conf = float(d[4]) + ms_score = motion_sal.score_box(box) + delta = ms_score - float(MS_FLOOR) + factor = 1.0 + float(MS_BOOST_WEIGHT) * delta + factor = max(float(MS_FACTOR_MIN), min(float(MS_FACTOR_MAX), factor)) + new_conf = float(max(0.0, min(1.0, conf * factor))) + new_d = d.copy() if isinstance(d, np.ndarray) else np.array(d, dtype=np.float32) + new_d[4] = new_conf + out.append(new_d) + return out + + + + + +def pick_soft_yolo_handoff_det( + dets, + ref_box, + ew, + eh, + *, + min_score, + dist_diag, + dist_min, + iou_floor, + max_area_ratio, + max_aspect_ratio, +): + """Pick a YOLO detection that is spatially consistent with the current lock. + + This is intentionally independent from ByteTrack id. It lets Kalman/KLT + accept a fresh detector measurement when ByteTrack keeps creating new ids + for a tiny fast target. + """ + if ref_box is None or not dets: + return None, 0.0 + + ref = clip_box(ref_box, ew, eh) + rc = box_center(ref) + rw, rh = box_wh(ref) + rdiag = max(1.0, float(np.hypot(float(rw), float(rh)))) + rarea = max(1.0, float(box_area(ref))) + max_dist = max(float(dist_min), float(dist_diag) * rdiag) + + best_box = None + best_conf = 0.0 + best_score = -1e9 + for det in dets: + arr = np.asarray(det, dtype=np.float32).reshape(-1) + if arr.size < 5: + continue + conf = float(arr[4]) + if conf < float(min_score): + continue + b = clip_box(arr[:4], ew, eh) + if box_area(b) <= 1.0: + continue + + cc = box_center(b) + dist = float(np.linalg.norm(cc - rc)) + ov = float(iou(b, ref)) + if dist > max_dist and ov < float(iou_floor): + continue + + carea = max(1.0, float(box_area(b))) + area_ratio = max(carea / rarea, rarea / carea) + if float(max_area_ratio) > 0.0 and area_ratio > float(max_area_ratio): + continue + + ar_ratio = safe_ratio(box_ar(b), box_ar(ref)) + if float(max_aspect_ratio) > 0.0 and ar_ratio > float(max_aspect_ratio): + continue + + # Prefer overlap/near-center, but keep confidence meaningful. + score = (2.5 * ov) + (0.6 * conf) + (1.0 / (1.0 + dist)) - (0.015 * dist / rdiag) + if score > best_score: + best_score = score + best_box = b + best_conf = conf + + return best_box, best_conf + + +def match_fresh_track_to_box(fresh_tracks, box, ew, eh, *, min_iou, max_center_dist): + if box is None or not fresh_tracks: + return None + bc = box_center(box) + best_track = None + best_score = -1e9 + for t in fresh_tracks: + tb = clip_box(t.tlbr, ew, eh) + ov = float(iou(tb, box)) + dist = float(np.linalg.norm(box_center(tb) - bc)) + if ov < float(min_iou) and dist > float(max_center_dist): + continue + score = (3.0 * ov) + (0.5 * float(t.score)) + (1.0 / (1.0 + dist)) + if score > best_score: + best_score = score + best_track = t + return best_track + + +def candidate_box_is_similar(candidate_box, prev_box, ew, eh, *, dist_min, dist_diag): + if candidate_box is None or prev_box is None: + return False + cand = clip_box(candidate_box, ew, eh) + prev = clip_box(prev_box, ew, eh) + pc = box_center(prev) + cc = box_center(cand) + pw, ph = box_wh(prev) + cw, ch = box_wh(cand) + diag = max(1.0, float(np.hypot(max(float(pw), float(cw)), max(float(ph), float(ch))))) + limit = max(float(dist_min), float(dist_diag) * diag) + return float(np.linalg.norm(cc - pc)) <= limit + + + +def make_box_from_center_wh(cx, cy, bw, bh, ew, eh): + bw = max(2.0, float(bw)) + bh = max(2.0, float(bh)) + return clip_box([float(cx) - 0.5 * bw, float(cy) - 0.5 * bh, + float(cx) + 0.5 * bw, float(cy) + 0.5 * bh], ew, eh) + + +def make_union_roi_from_boxes(boxes, ew, eh, *, margin, min_side): + valid = [] + for b in boxes or []: + if b is None: + continue + bb = clip_box(b, ew, eh) + if box_area(bb) > 1.0: + valid.append(bb) + if not valid: + return None + arr = np.asarray(valid, dtype=np.float32) + x1 = float(np.min(arr[:, 0])) - float(margin) + y1 = float(np.min(arr[:, 1])) - float(margin) + x2 = float(np.max(arr[:, 2])) + float(margin) + y2 = float(np.max(arr[:, 3])) + float(margin) + side = max(float(min_side), x2 - x1, y2 - y1) + cx = 0.5 * (x1 + x2) + cy = 0.5 * (y1 + y2) + return clip_box([cx - 0.5 * side, cy - 0.5 * side, cx + 0.5 * side, cy + 0.5 * side], ew, eh) + + +def _trajectory_velocity_from_history(obs_hist, fallback_vx, fallback_vy): + if obs_hist is None or len(obs_hist) < 2: + return float(fallback_vx), float(fallback_vy), 0.0, 0.0 + recent = list(obs_hist)[-min(len(obs_hist), int(max(2, TRAJ_HISTORY_LOOKBACK))):] + p0 = np.asarray(recent[0]["center"], dtype=np.float32) + p1 = np.asarray(recent[-1]["center"], dtype=np.float32) + t0 = float(recent[0]["ts"]) + t1 = float(recent[-1]["ts"]) + dt_hist = max(1e-3, t1 - t0) + hv = (p1 - p0) / dt_hist + vx = float(TRAJ_VEL_HIST_WEIGHT) * float(hv[0]) + (1.0 - float(TRAJ_VEL_HIST_WEIGHT)) * float(fallback_vx) + vy = float(TRAJ_VEL_HIST_WEIGHT) * float(hv[1]) + (1.0 - float(TRAJ_VEL_HIST_WEIGHT)) * float(fallback_vy) + ax = 0.0 + ay = 0.0 + if len(recent) >= 4: + mid = len(recent) // 2 + pa = np.asarray(recent[0]["center"], dtype=np.float32) + pb = np.asarray(recent[mid]["center"], dtype=np.float32) + pc = np.asarray(recent[-1]["center"], dtype=np.float32) + ta = float(recent[0]["ts"]) + tb = float(recent[mid]["ts"]) + tc = float(recent[-1]["ts"]) + v1 = (pb - pa) / max(1e-3, tb - ta) + v2 = (pc - pb) / max(1e-3, tc - tb) + acc = (v2 - v1) / max(1e-3, tc - ta) + ax = float(np.clip(acc[0], -float(TRAJ_MAX_ACCEL_PX_S2), float(TRAJ_MAX_ACCEL_PX_S2))) + ay = float(np.clip(acc[1], -float(TRAJ_MAX_ACCEL_PX_S2), float(TRAJ_MAX_ACCEL_PX_S2))) + return vx, vy, ax, ay + + +def build_maneuver_hypotheses(obs_hist, ref_box, kf, miss_streak, dt, ew, eh): + """Build short-horizon trajectory hypotheses for detector loss.""" + if ref_box is None or kf is None or (not getattr(kf, "initialized", False)): + return [] + ref = clip_box(ref_box, ew, eh) + if box_area(ref) <= 1.0: + return [] + cx, cy = box_center(ref) + bw, bh = box_wh(ref) + vx_kf = float(kf.x[4, 0]) if getattr(kf, "x", None) is not None else 0.0 + vy_kf = float(kf.x[5, 0]) if getattr(kf, "x", None) is not None else 0.0 + vx, vy, ax, ay = _trajectory_velocity_from_history(obs_hist, vx_kf, vy_kf) + speed = float(np.hypot(vx, vy)) + horizon = float(dt) * float(max(1, int(miss_streak) + int(TRAJ_HORIZON_MISS_OFFSET))) + horizon = float(np.clip(horizon, float(TRAJ_HORIZON_MIN_SEC), float(TRAJ_HORIZON_MAX_SEC))) + grow = min(1.0 + float(TRAJ_BOX_GROW_PER_MISS) * float(max(0, int(miss_streak))), float(TRAJ_BOX_GROW_MAX)) + bw2 = float(bw) * grow + bh2 = float(bh) * grow + + hypotheses = [] + def add(label, px, py, weight): + b = make_box_from_center_wh(px, py, bw2, bh2, ew, eh) + hypotheses.append({"label": str(label), "box": b, "center": box_center(b), "weight": float(weight)}) + + add("cv", cx + vx * horizon, cy + vy * horizon, 1.00) + if TRAJ_USE_ACCEL: + add("ca", cx + vx * horizon + 0.5 * ax * horizon * horizon, + cy + vy * horizon + 0.5 * ay * horizon * horizon, 0.95) + add("damp", cx + 0.55 * vx * horizon, cy + 0.55 * vy * horizon, 0.72) + + if speed >= float(TRAJ_MIN_SPEED_FOR_MANEUVER): + ux = vx / max(speed, 1e-6) + uy = vy / max(speed, 1e-6) + px1, py1 = -uy, ux + px2, py2 = uy, -ux + lat = min(float(TRAJ_LATERAL_ACCEL_MAX), max(float(TRAJ_LATERAL_ACCEL_MIN), speed * float(TRAJ_LATERAL_ACCEL_SPEED_GAIN))) + shift = 0.5 * lat * horizon * horizon + add("turnL", cx + vx * horizon + px1 * shift, cy + vy * horizon + py1 * shift, 0.82) + add("turnR", cx + vx * horizon + px2 * shift, cy + vy * horizon + py2 * shift, 0.82) + vert = min(float(TRAJ_VERTICAL_ACCEL_MAX), max(float(TRAJ_VERTICAL_ACCEL_MIN), speed * float(TRAJ_VERTICAL_ACCEL_SPEED_GAIN))) + vshift = 0.5 * vert * horizon * horizon + add("up", cx + vx * horizon, cy + vy * horizon - vshift, 0.65) + add("down", cx + vx * horizon, cy + vy * horizon + vshift, 0.65) + + filtered = [] + for h in hypotheses: + hc = np.asarray(h["center"], dtype=np.float32) + if any(float(np.linalg.norm(hc - np.asarray(old["center"], dtype=np.float32))) < float(TRAJ_DUP_CENTER_DIST) for old in filtered): + continue + filtered.append(h) + if len(filtered) >= int(TRAJ_MAX_HYPOTHESES): + break + return filtered + + +def pick_det_near_trajectory_hypotheses(dets, hypotheses, ew, eh, *, min_score, dist_min, dist_diag): + if not dets or not hypotheses: + return None, 0.0, "-", -1e9 + best_box = None + best_conf = 0.0 + best_label = "-" + best_score = -1e9 + for det in dets: + arr = np.asarray(det, dtype=np.float32).reshape(-1) + if arr.size < 5: + continue + conf = float(arr[4]) + if conf < float(min_score): + continue + b = clip_box(arr[:4], ew, eh) + if box_area(b) <= 1.0: + continue + bc = box_center(b) + for h in hypotheses: + hb = clip_box(h["box"], ew, eh) + hc = np.asarray(h["center"], dtype=np.float32) + hw, hh = box_wh(hb) + hdiag = max(1.0, float(np.hypot(float(hw), float(hh)))) + lim = max(float(dist_min), float(dist_diag) * hdiag) + dist = float(np.linalg.norm(bc - hc)) + ov = float(iou(b, hb)) + if dist > lim and ov < 0.01: + continue + score = (float(h.get("weight", 1.0)) * 0.35) + conf + 2.0 * ov + (1.0 / (1.0 + dist)) - 0.01 * (dist / hdiag) + if score > best_score: + best_score = score + best_box = b + best_conf = conf + best_label = str(h.get("label", "traj")) + return best_box, best_conf, best_label, best_score + +def pick_yolo_reanchor_candidate( + dets, + *, + ew, + eh, + pred_ref, + prev_candidate_box, + fresh_tracks, + min_score, + repeat_dist_min, + repeat_dist_diag, + pred_dist_min, + pred_dist_diag, + max_area_ratio, + max_aspect_ratio, + osd_reject, + osd_min_score, + trajectory_hypotheses=None, + traj_dist_min=0.0, + traj_dist_diag=0.0, +): + """Pick a detector candidate for stale-KLT re-anchoring. + + This path is intentionally separate from the normal KLT-anchor guard. + It is only allowed to adopt a far box after a short M/N persistence check, + so a single false positive cannot pull Kalman away from a real lock. + """ + if not dets: + return None, 0.0, None, False, False + + pred = clip_box(pred_ref, ew, eh) if pred_ref is not None else None + pred_c = box_center(pred) if pred is not None else None + pred_diag = 40.0 + pred_area = 1.0 + if pred is not None: + pw, ph = box_wh(pred) + pred_diag = max(1.0, float(np.hypot(float(pw), float(ph)))) + pred_area = max(1.0, float(box_area(pred))) + + best_box = None + best_conf = 0.0 + best_track = None + best_same = False + best_near_pred = False + best_score = -1e9 + + for det in dets: + arr = np.asarray(det, dtype=np.float32).reshape(-1) + if arr.size < 5: + continue + conf = float(arr[4]) + if conf < float(min_score): + continue + b = clip_box(arr[:4], ew, eh) + if box_area(b) <= 1.0: + continue + + c = box_center(b) + if osd_reject and in_osd_zone(float(c[0]), float(c[1]), ew, eh) and conf < float(osd_min_score): + continue + + if pred is not None: + carea = max(1.0, float(box_area(b))) + area_ratio = max(carea / pred_area, pred_area / carea) + if float(max_area_ratio) > 0.0 and area_ratio > float(max_area_ratio): + continue + ar_ratio = safe_ratio(box_ar(b), box_ar(pred)) + if float(max_aspect_ratio) > 0.0 and ar_ratio > float(max_aspect_ratio): + continue + + near_pred = False + near_traj = False + traj_bonus = 0.0 + dist_pred = 0.0 + if pred_c is not None: + dist_pred = float(np.linalg.norm(c - pred_c)) + pred_limit = max(float(pred_dist_min), float(pred_dist_diag) * pred_diag) + near_pred = bool(dist_pred <= pred_limit) + + if trajectory_hypotheses: + best_traj_score = -1e9 + for h in trajectory_hypotheses: + hb = clip_box(h["box"], ew, eh) + hc = np.asarray(h["center"], dtype=np.float32) + hw, hh = box_wh(hb) + hdiag = max(1.0, float(np.hypot(float(hw), float(hh)))) + lim = max(float(traj_dist_min), float(traj_dist_diag) * hdiag) + dtraj = float(np.linalg.norm(c - hc)) + ovtraj = float(iou(b, hb)) + if dtraj <= lim or ovtraj >= 0.01: + near_traj = True + best_traj_score = max(best_traj_score, (2.0 * ovtraj) + (1.0 / (1.0 + dtraj)) + 0.25 * float(h.get("weight", 1.0))) + if near_traj: + traj_bonus = max(0.0, best_traj_score) + + same_prev = candidate_box_is_similar( + b, + prev_candidate_box, + ew, + eh, + dist_min=float(repeat_dist_min), + dist_diag=float(repeat_dist_diag), + ) + + fresh_track = match_fresh_track_to_box( + fresh_tracks, + b, + ew, + eh, + min_iou=float(SOFT_YOLO_TRACK_IOU), + max_center_dist=max(float(SOFT_YOLO_TRACK_DIST_MIN), float(SOFT_YOLO_TRACK_DIST_DIAG) * pred_diag), + ) + + # For the first far candidate we still keep memory, but we do not adopt + # it until it repeats. Give repeated/fresh/near-pred boxes priority. + score = 1.0 * conf + if same_prev: + score += 0.80 + if fresh_track is not None: + score += 0.35 + 0.25 * float(fresh_track.score) + if near_pred: + score += 0.25 + (1.0 / (1.0 + dist_pred)) + if near_traj: + score += 0.45 + traj_bonus + if pred_c is not None and (not near_traj): + score -= 0.0025 * dist_pred + + if score > best_score: + best_score = score + best_box = b + best_conf = conf + best_track = fresh_track + best_same = bool(same_prev) + best_near_pred = bool(near_pred or near_traj) + + return best_box, best_conf, best_track, best_same, best_near_pred + + + +def klt_anchor_accepts_box( + candidate_box, + anchor_box, + ew, + eh, + *, + dist_diag, + dist_min, + iou_floor, + max_area_ratio, + max_aspect_ratio, +): + """Reject one-frame ByteTrack/YOLO jumps when KLT still has a strong anchor.""" + if candidate_box is None or anchor_box is None: + return True, "no_anchor" + + cand = clip_box(candidate_box, ew, eh) + anch = clip_box(anchor_box, ew, eh) + if box_area(cand) <= 1.0 or box_area(anch) <= 1.0: + return False, "empty_box" + + ac = box_center(anch) + cc = box_center(cand) + aw, ah = box_wh(anch) + adiag = max(1.0, float(np.hypot(float(aw), float(ah)))) + max_dist = max(float(dist_min), float(dist_diag) * adiag) + dist = float(np.linalg.norm(cc - ac)) + ov = float(iou(cand, anch)) + + if dist > max_dist and ov < float(iou_floor): + return False, f"far_from_klt:dist={dist:.1f}>lim={max_dist:.1f},iou={ov:.3f}" + + aarea = max(1.0, float(box_area(anch))) + carea = max(1.0, float(box_area(cand))) + area_ratio = max(carea / aarea, aarea / carea) + if float(max_area_ratio) > 0.0 and area_ratio > float(max_area_ratio): + return False, f"area_jump:{area_ratio:.1f}" + + ar_ratio = safe_ratio(box_ar(cand), box_ar(anch)) + if float(max_aspect_ratio) > 0.0 and ar_ratio > float(max_aspect_ratio): + return False, f"aspect_jump:{ar_ratio:.1f}" + + return True, "ok" + +def main(): + print("Loading YOLO...") + model = YOLO(MODEL_PATH) + + if torch.cuda.is_available(): + model.to(f"cuda:{DEVICE}") + + if torch.cuda.is_available(): + # Warmup на обоих размерах — критично для dynamic CBAM + # чтобы MLP построились на обоих scales до реального инференса. + dummy_roi = np.zeros((IMG_SIZE_ROI, IMG_SIZE_ROI, 3), dtype=np.uint8) + dummy_full = np.zeros((IMG_SIZE_FULL, IMG_SIZE_FULL, 3), dtype=np.uint8) + with torch.inference_mode(): + for _ in range(3): + _ = model( + dummy_roi, + conf=YOLO_CONF_EFFECTIVE, + imgsz=IMG_SIZE_ROI, + verbose=False, + max_det=MAX_DET, + device=DEVICE, + half=USE_HALF + ) + for _ in range(2): + _ = model( + dummy_full, + conf=YOLO_CONF_EFFECTIVE, + imgsz=IMG_SIZE_FULL, + verbose=False, + max_det=MAX_DET, + device=DEVICE, + half=USE_HALF + ) + print(f"Model warmed up at {IMG_SIZE_ROI} and {IMG_SIZE_FULL}") + + cap, source_kind = open_source(SOURCE, CAP_BACKEND) + if not cap.isOpened(): + print(f"Capture open failed: {SOURCE}") + return + + try: + cap.set(cv2.CAP_PROP_BUFFERSIZE, 1) + except Exception: + pass + + print(f"Opened source: {SOURCE} ({source_kind})") + + input_fps = float(cap.get(cv2.CAP_PROP_FPS)) + if (not np.isfinite(input_fps)) or (input_fps <= 1.0): + input_fps = 0.0 + + if TARGET_OUT_FPS > 0: + target_out_fps = float(TARGET_OUT_FPS) + elif source_kind == "file" and input_fps > 0.0: + target_out_fps = input_fps + else: + target_out_fps = 0.0 + + if VIDEO_REALTIME and target_out_fps > 0.0: + print(f"Pacing output at ~{target_out_fps:.2f} FPS") + else: + print("Pacing disabled (show as fast as processing allows)") + + if SHOW_OUTPUT: + cv2.namedWindow(WINDOW_NAME, cv2.WINDOW_NORMAL) + + writer = None + if SAVE_INFER_VIDEO: + ow = int(cap.get(cv2.CAP_PROP_FRAME_WIDTH)) + oh = int(cap.get(cv2.CAP_PROP_FRAME_HEIGHT)) + fpsw = float(cap.get(cv2.CAP_PROP_FPS)) + if (not np.isfinite(fpsw)) or fpsw <= 1.0: + fpsw = target_out_fps if target_out_fps > 0 else 30.0 + fourcc = cv2.VideoWriter_fourcc(*"mp4v") + out_video_path = build_unique_out_video_path(OUT_VIDEO_PATH) + writer = cv2.VideoWriter(out_video_path, fourcc, fpsw, (ow, oh)) + if not writer.isOpened(): + print(f"WARN: video writer open failed: {out_video_path}") + writer = None + else: + print(f"Saving inference video to: {out_video_path}") + + yolo_worker = YOLOWorker(model) + yolo_worker.start() + autogaze_worker = AutoGazeROIWorker() + autogaze_worker.start() + print(autogaze_worker.status_line()) + track_logger = TrackingDecisionLogger( + TRACK_LOG_ENABLE, + TRACK_LOG_PATH, + TRACK_SUMMARY_PATH, + flush_every=TRACK_LOG_FLUSH_EVERY, + ) + print(track_logger.status_line()) + guidance_ctrl = ScreenGuidanceController() + print(guidance_ctrl.status_line()) + + kf = SafeKalman8D() + klt = HybridTracker() + + bt = BYTETracker( + track_high_thresh=BT_HIGH, + track_low_thresh=BT_LOW, + new_track_thresh=BT_NEW, + match_thresh=BT_MATCH_IOU, + track_buffer=BT_BUFFER, + min_hits=BT_MIN_HITS, + ) + target_id = None + target_absent_frames = 0 + switch_candidate_id = None + switch_candidate_hits = 0 + + locked_box_eff = None + confirmed = False + hit_streak = 0 + miss_streak = 0 + acquire_score = 0 + acquire_miss = 0 + ref_hist = None + + nominal_dt = 1.0 / float(input_fps if input_fps > 1.0 else INPUT_FPS_FALLBACK) + prev_frame_ts = None + frame_id = 0 + last_det_frame = -999 + last_fullscan_frame = -999 + last_klt_init_frame = -999 + + yolo_no_det = 0 + last_yolo_ok_ts = -1e9 + + perf_hist = deque(maxlen=120) + yolo_hist = deque(maxlen=120) + + traj = deque(maxlen=max(10, int(TRAIL_SECONDS * (input_fps if input_fps > 0 else 60.0)))) + traj_frame_i = 0 + guidance_override_memory_box_eff = None + guidance_override_memory_ttl = 0 + + prev_gray_global = None + template_gray = None + + # --- Motion saliency --- + motion_sal = MotionSaliency() + print(motion_sal.status_line()) + ms_outliers = np.zeros((0, 2), dtype=np.float32) + + # --- Stationary clutter killer --- + killer = StationaryKiller() + print(killer.status_line()) + motion_roi_eff = None + motion_active_zones = 0 + motion_mask = None + flash_roi_eff = None + flash_ttl = 0 + wavelet_roi_eff = None + wavelet_roi_ttl = 0 + wavelet_roi_peak = 0.0 + wavelet_track_hist = {} + wavelet_track_last_seen = {} + track_center_hist = {} + track_center_last_seen = {} + preacq_hist = deque(maxlen=max(1, int(PREACQ_WINDOW))) + preacq_hits = 0 + autogaze_cooldown = 0 + wavelet_cooldown = 0 + adaptive_chase_stage = "far" + + # M/N memory for safe YOLO re-anchor when KLT/Kalman are stale. + yolo_reanchor_memory_box_eff = None + yolo_reanchor_memory_hits = 0 + yolo_reanchor_memory_last_frame = -999 + yolo_reanchor_last_reason = "" + + # Short history of accepted target centers for multi-hypothesis prediction. + trajectory_obs_hist = deque(maxlen=max(6, int(TRAJ_HISTORY_LEN))) + + print("Start") + + while True: + ret, frame_orig = cap.read() + loop_ts = time.perf_counter() + if not ret: + break + + frame_eff, sx, sy = get_effective_frame(frame_orig) + eh, ew = frame_eff.shape[:2] + frame_ts, dt_source = get_frame_timestamp_seconds(cap, source_kind, frame_id, input_fps, loop_ts) + dt = sanitize_dt(frame_ts, prev_frame_ts, nominal_dt) + prev_frame_ts = frame_ts + + if autogaze_cooldown > 0: + autogaze_cooldown -= 1 + if wavelet_cooldown > 0: + wavelet_cooldown -= 1 + + if autogaze_cooldown <= 0: + autogaze_worker.submit(frame_eff, frame_id) + autogaze_roi_eff = None + autogaze_rois_eff = [] + autogaze_info = None + gray_now = cv2.cvtColor(frame_eff, cv2.COLOR_BGR2GRAY) + target_track = None + pred_box_eff = None + best_score = None + have_yolo = False + yolo_mode = "IDLE" + yolo_raw_count = 0 + merged_part_count = 0 + infer_ms = 0.0 + klt_valid = False + speed = 0.0 + det_every = 0 + approach_active = False + close_force_fullscan = False + fast_maneuver_guard = False + wavelet_active = False + best_wavelet_energy = 0.0 + best_wavelet_bonus = 0.0 + best_wavelet_hits = 0 + best_dist = 0.0 + best_residual_ok = False + guidance_candidate_box_eff = None + guidance_candidate_track_id = None + guidance_raw_yolo_box_eff = None + guidance_override_box_eff = None + used_prediction_hold = False + stale_lock_active = False + stale_lock_reason = "" + fast_handoff_active = False + guidance_reset_event = "" + guidance_force_neutral = False + soft_yolo_box_eff = None + soft_yolo_score = 0.0 + soft_yolo_track = None + soft_yolo_adopted = False + yolo_reanchor_box_eff = None + yolo_reanchor_score = 0.0 + yolo_reanchor_track = None + yolo_reanchor_adopted = False + yolo_reanchor_near_pred = False + yolo_reanchor_reason = "" + trajectory_hypotheses = [] + trajectory_roi_eff = None + trajectory_primary_box_eff = None + trajectory_det_label = "-" + trajectory_reanchor_used = False + A = None + ms_outliers = np.zeros((0, 2), dtype=np.float32) + if prev_gray_global is not None and USE_CAM_MOTION_COMP: + A, _ms_inliers, ms_outliers = estimate_global_affine_ex( + prev_gray_global, gray_now + ) + if A is not None and (not affine_is_plausible(A)): + A = None + ms_outliers = np.zeros((0, 2), dtype=np.float32) + if A is not None and kf.initialized: + cx0 = float(kf.x[0, 0]) + cy0 = float(kf.x[1, 0]) + ncx, ncy = apply_affine_to_point(A, cx0, cy0) + kf.x[0, 0] = ncx + kf.x[1, 0] = ncy + if locked_box_eff is not None: + x1, y1, x2, y2 = locked_box_eff + p1x, p1y = apply_affine_to_point(A, float(x1), float(y1)) + p2x, p2y = apply_affine_to_point(A, float(x2), float(y2)) + locked_box_eff[:] = np.array([p1x, p1y, p2x, p2y], dtype=np.float32) + locked_box_eff[:] = clip_box(locked_box_eff, ew, eh) + + motion_roi_eff = None + motion_active_zones = 0 + motion_mask = None + if flash_ttl > 0: + flash_ttl -= 1 + else: + flash_roi_eff = None + if wavelet_roi_ttl > 0: + wavelet_roi_ttl -= 1 + else: + wavelet_roi_eff = None + wavelet_roi_peak = 0.0 + if MOTION_ZONE_ENABLE and prev_gray_global is not None: + motion_mask = build_motion_mask( + prev_gray_global, + gray_now, + affine=A if USE_CAM_MOTION_COMP else None + ) + motion_roi_eff, motion_active_zones = motion_zones_to_roi(motion_mask, ew, eh) + + # --- Update motion saliency map --- + if prev_gray_global is not None: + motion_sal.update( + gray_now, + prev_gray_global, + A, + kind="affine" if A is not None else "none", + outlier_pts=ms_outliers, + ) + + prev_gray_global = gray_now + + iter_start = time.perf_counter() + + pred_box_eff = None + if kf.initialized: + kf_q_scale = 1.0 + if KALMAN_Q_DT_BOOST_ENABLE and dt > float(KALMAN_Q_DT_BOOST_START_SEC): + kf_q_scale += float(KALMAN_Q_DT_BOOST_SLOPE) * (dt - float(KALMAN_Q_DT_BOOST_START_SEC)) + kf_q_scale = min(kf_q_scale, float(KALMAN_Q_DT_BOOST_MAX)) + kf.predict(dt, q_scale=kf_q_scale) + vmax = float(KALMAN_CLAMP_V_PX_S) + kf.x[4, 0] = float(np.clip(kf.x[4, 0], -vmax, vmax)) + kf.x[5, 0] = float(np.clip(kf.x[5, 0], -vmax, vmax)) + pred_box_eff = clip_box(kf.to_box(), ew, eh) + + unc = 0.0 + if TURN_SAFE_ENABLE and pred_box_eff is not None: + unc = kf.uncertainty() + + speed = 0.0 + if kf.initialized: + vx = float(kf.x[4, 0]) + vy = float(kf.x[5, 0]) + speed = (vx * vx + vy * vy) ** 0.5 + unc = kf.uncertainty() + + pred_area_eff = 0.0 + if pred_box_eff is not None: + pred_area_eff = box_area(pred_box_eff) + elif locked_box_eff is not None: + pred_area_eff = box_area(locked_box_eff) + if ADAPTIVE_CHASE_ENABLE: + hyst = float(max(0.0, ADAPTIVE_STAGE_AREA_HYST)) + mid_on = float(ADAPTIVE_STAGE_MID_AREA) + close_on = float(APPROACH_CLOSE_AREA) + if adaptive_chase_stage == "close": + if pred_area_eff < (close_on - hyst): + adaptive_chase_stage = "mid" if pred_area_eff >= (mid_on - hyst) else "far" + elif adaptive_chase_stage == "mid": + if pred_area_eff >= (close_on + hyst): + adaptive_chase_stage = "close" + elif pred_area_eff < (mid_on - hyst): + adaptive_chase_stage = "far" + else: + if pred_area_eff >= (close_on + hyst): + adaptive_chase_stage = "close" + elif pred_area_eff >= (mid_on + hyst): + adaptive_chase_stage = "mid" + else: + adaptive_chase_stage = "close" if (APPROACH_MODE_ENABLE and (pred_area_eff >= float(APPROACH_FORCE_DET_AREA))) else "far" + + approach_active = APPROACH_MODE_ENABLE and (adaptive_chase_stage in ("mid", "close")) + approach_force_det_every = None + approach_kf_roi_scale = 1.0 + approach_max_area_ratio = float(TURN_MAX_AREA_RATIO) + approach_near_dist_diag = float(APPROACH_NEAR_DIST_DIAG) + approach_hsv_min_scale = 1.0 + approach_score_weight = 0.0 + approach_score_clip = 0.0 + approach_switch_extra_miss = 0 + approach_switch_extra_hits = 0 + if approach_active: + if adaptive_chase_stage == "mid": + approach_force_det_every = int(max(1, APPROACH_MID_FORCE_DET_EVERY)) + approach_kf_roi_scale = float(APPROACH_MID_KF_ROI_SCALE) + approach_max_area_ratio = float(APPROACH_MID_MAX_AREA_RATIO) + approach_near_dist_diag = float(APPROACH_MID_NEAR_DIST_DIAG) + approach_hsv_min_scale = float(APPROACH_MID_HSV_MIN_SCALE) + approach_score_weight = float(APPROACH_MID_SCORE_WEIGHT) + approach_score_clip = float(APPROACH_MID_SCORE_CLIP) + approach_switch_extra_miss = int(max(0, APPROACH_MID_SWITCH_EXTRA_MISS)) + approach_switch_extra_hits = int(max(0, APPROACH_MID_SWITCH_EXTRA_HITS)) + else: + approach_force_det_every = int(max(1, APPROACH_FORCE_DET_EVERY)) + approach_kf_roi_scale = float(APPROACH_KF_ROI_SCALE) + approach_max_area_ratio = float(APPROACH_MAX_AREA_RATIO) + approach_near_dist_diag = float(APPROACH_NEAR_DIST_DIAG) + approach_hsv_min_scale = float(APPROACH_HSV_MIN_SCALE) + approach_score_weight = float(APPROACH_SCORE_WEIGHT) + approach_score_clip = float(APPROACH_SCORE_CLIP) + approach_switch_extra_miss = int(max(0, APPROACH_CLOSE_SWITCH_EXTRA_MISS)) + approach_switch_extra_hits = int(max(0, APPROACH_CLOSE_SWITCH_EXTRA_HITS)) + + klt_valid = False + if confirmed and locked_box_eff is not None: + if klt.prev_gray is None or klt.pts is None or len(klt.pts) < KLT_REINIT_MIN_POINTS: + klt.init(frame_eff, locked_box_eff) + last_klt_init_frame = frame_id + + klt_box = klt.update(frame_eff) + if klt_box is not None: + klt_box = clip_box(klt_box, ew, eh) + klt_valid = (klt.quality >= KLT_OK_Q) and (klt.good_count >= KLT_OK_PTS) + if klt_valid: + cx, cy = box_center(klt_box) + bw, bh = box_wh(klt_box) + kf.update([cx, cy, bw, bh]) + locked_box_eff = klt_box + + if ( + TRAJ_PREDICT_ENABLE + and confirmed + and kf.initialized + and (miss_streak >= int(TRAJ_PREDICT_MISS_GE)) + ): + traj_ref_box = pred_box_eff if pred_box_eff is not None else locked_box_eff + trajectory_hypotheses = build_maneuver_hypotheses( + trajectory_obs_hist, traj_ref_box, kf, miss_streak, dt, ew, eh + ) + if trajectory_hypotheses: + trajectory_primary_box_eff = trajectory_hypotheses[0]["box"] + trajectory_roi_eff = make_union_roi_from_boxes( + [h["box"] for h in trajectory_hypotheses], ew, eh, + margin=TRAJ_ROI_MARGIN, min_side=TRAJ_ROI_MIN_SIDE + ) + + wavelet_roi_allowed = ( + (wavelet_cooldown <= 0) + and WAVELET_ROI_ENABLE and ( + ((not confirmed) and (miss_streak >= int(WAVELET_ROI_MISS_GE))) + or (confirmed and (miss_streak >= int(WAVELET_ROI_CONF_MISS_GE))) + ) + ) + if wavelet_roi_allowed: + wv_every = int(max(1, WAVELET_ROI_EVERY_N)) + compute_wavelet_now = (wavelet_roi_ttl <= 0) or ((frame_id % wv_every) == 0) + if compute_wavelet_now: + wv_t0 = time.perf_counter() + wave_pred_ref = pred_box_eff if pred_box_eff is not None else locked_box_eff + wroi, wpeak = wavelet_hot_roi( + gray_now, + ew, + eh, + motion_mask=motion_mask, + pred_ref=wave_pred_ref, + margin=WAVELET_ROI_MARGIN, + min_side=WAVELET_ROI_MIN_SIDE, + pred_scale=WAVELET_ROI_PRED_SCALE, + min_peak_ratio=WAVELET_ROI_MIN_PEAK_RATIO, + ) + wv_ms = (time.perf_counter() - wv_t0) * 1000.0 + if wv_ms > float(MODULE_TIME_BUDGET_MS): + wavelet_cooldown = max(int(wavelet_cooldown), int(WAVELET_COOLDOWN_FRAMES)) + if wroi is not None: + wavelet_roi_eff = wroi + wavelet_roi_peak = float(wpeak) + wavelet_roi_ttl = int(max(int(wavelet_roi_ttl), int(max(1, WAVELET_ROI_TTL)))) + + base = 6 if confirmed else RECOVER_FORCED_DET_EVERY + fast_bonus = int(clamp(speed / 28.0, 0, 4)) + unc_bonus = int(clamp(unc / 160.0, 0, 4)) + miss_bonus = int(clamp(miss_streak, 0, 3)) + klt_bonus = 2 if (confirmed and not klt_valid) else 0 + det_every = clamp(base - (fast_bonus + unc_bonus + miss_bonus + klt_bonus), 1, 10) + if confirmed and approach_active and (approach_force_det_every is not None): + det_every = min(int(det_every), int(approach_force_det_every)) + + yolo_fresh = (frame_ts - last_yolo_ok_ts) <= YOLO_FRESH_SEC + if YOLO_FORCE_DET_WHEN_WEAK and confirmed and (not klt_valid) and (not yolo_fresh): + det_every = 1 + + run_det = (frame_id - last_det_frame) >= det_every + need_fullscan = (not confirmed) and (frame_id - last_fullscan_frame) >= RECOVER_FULLSCAN_EVERY + + # Close-stage detector policy: + # older builds forced FULL-CLOSE on every detector pass while ch=close. + # That is safe but noisy: ByteTrack sees every false positive on the screen, + # even when KLT/Kalman are locked on the real target. Now close mode uses + # ROI-KF/ROI-TRAJ most of the time and falls back to FULL-CLOSE only when + # the lock is weak or on a periodic health-check. + close_periodic_due = bool( + CLOSE_PERIODIC_FULLSCAN_ENABLE + and confirmed + and (adaptive_chase_stage == "close") + and ((frame_id - last_fullscan_frame) >= int(CLOSE_PERIODIC_FULLSCAN_EVERY)) + ) + close_force_fullscan = bool( + confirmed + and CLOSE_FORCE_FULLSCAN + and (adaptive_chase_stage == "close") + and ( + (miss_streak >= int(CLOSE_FULLSCAN_MISS_GE)) + or (CLOSE_FULLSCAN_WHEN_KLT_INVALID and (not klt_valid)) + or (target_absent_frames >= int(CLOSE_FULLSCAN_TARGET_ABSENT_GE)) + or close_periodic_due + ) + ) + + if TURN_SAFE_ENABLE and TURN_SAFE_FORCE_FULLSCAN and confirmed and (miss_streak >= FULLSCAN_WHEN_MISS_GE): + need_fullscan = True + if ( + confirmed + and (miss_streak >= int(MOTION_CONF_MISS_GE)) + and ((frame_id - last_fullscan_frame) >= int(CONF_MISS_FORCE_FULLSCAN_EVERY)) + ): + need_fullscan = True + if (not confirmed) and (miss_streak >= int(RECOVER_FORCE_FULLSCAN_MISS_GE)): + need_fullscan = True + if close_force_fullscan: + need_fullscan = True + + autogaze_ref = pred_box_eff if pred_box_eff is not None else locked_box_eff + autogaze_roi_eff, autogaze_rois_eff, autogaze_info = autogaze_worker.get_latest( + frame_id, + ew, + eh, + pred_ref_box=autogaze_ref, + ) + autogaze_roi_allowed = ( + (autogaze_cooldown <= 0) + and autogaze_roi_eff is not None + and ( + ((not confirmed) and (miss_streak >= int(AUTOGAZE_MISS_GE))) + or (confirmed and (miss_streak >= int(AUTOGAZE_CONF_MISS_GE))) + ) + ) + if ( + autogaze_info is not None + and int(autogaze_info.get("age", -1)) == 0 + and float(autogaze_info.get("infer_ms", 0.0)) > float(MODULE_TIME_BUDGET_MS) + ): + autogaze_cooldown = max(int(autogaze_cooldown), int(AUTOGAZE_COOLDOWN_FRAMES)) + autogaze_roi_allowed = False + + if run_det: + roi_box = None + mode = "FULL" + force_motion_first = (not confirmed) and (miss_streak >= int(RECOVER_FORCE_MOTION_FIRST_MISS_GE)) + allow_kf_roi = confirmed or (miss_streak < int(RECOVER_ROI_KF_DISABLE_MISS_GE)) + prefer_motion_confirmed = ( + MOTION_CONF_ENABLE + and confirmed + and (miss_streak >= int(MOTION_CONF_MISS_GE)) + and (motion_roi_eff is not None) + and ((not MOTION_CONF_KLT_INVALID_ONLY) or (not klt_valid)) + ) + if ( + (not need_fullscan) + and prefer_motion_confirmed + ): + roi_box = motion_roi_eff.copy() + mode = "ROI-MOTION-C" + elif ( + (not need_fullscan) + and FLASH_ROI_ENABLE + and (flash_ttl > 0) + and (flash_roi_eff is not None) + and ((not confirmed) or (target_id is None) or (not klt_valid) or (miss_streak >= 1)) + ): + roi_box = clip_box(flash_roi_eff, ew, eh) + mode = "ROI-FLASH" + elif ( + (not need_fullscan) + and autogaze_roi_allowed + ): + roi_box = clip_box(autogaze_roi_eff, ew, eh) + mode = "ROI-GAZE" + elif ( + (not need_fullscan) + and wavelet_roi_allowed + and WAVELET_ROI_ENABLE + and (wavelet_roi_ttl > 0) + and (wavelet_roi_eff is not None) + ): + roi_box = clip_box(wavelet_roi_eff, ew, eh) + mode = "ROI-WAVE" + elif (not need_fullscan) and force_motion_first and MOTION_ZONE_ENABLE and (motion_roi_eff is not None): + roi_box = motion_roi_eff.copy() + mode = "ROI-MOTION" + elif ( + TRAJ_ROI_ENABLE + and (not need_fullscan) + and confirmed + and (miss_streak >= int(TRAJ_ROI_MISS_GE)) + and (trajectory_roi_eff is not None) + ): + roi_box = clip_box(trajectory_roi_eff, ew, eh) + mode = "ROI-TRAJ" + elif (not need_fullscan) and kf.initialized and allow_kf_roi: + cx = float(kf.x[0, 0]) + cy = float(kf.x[1, 0]) + vx = float(kf.x[4, 0]) + vy = float(kf.x[5, 0]) + vnorm = (vx * vx + vy * vy) ** 0.5 + 1e-6 + ux, uy = vx / vnorm, vy / vnorm + + # Адаптивный ROI: для мелких целей сохраняем + # запас на ошибку Kalman + контекст для YOLO. + # Для bbox 15×10 ROI ≈ 280px (после resize 640 + # объект занимает ~15% — оптимум для YOLO recall). + # Для bbox 30×20 ROI ≈ 550px (почти не сужается). + box_diag = 0.0 + if locked_box_eff is not None: + bw = float(locked_box_eff[2] - locked_box_eff[0]) + bh = float(locked_box_eff[3] - locked_box_eff[1]) + box_diag = (bw * bw + bh * bh) ** 0.5 + if box_diag >= 8.0: + target_roi_side = box_diag / 0.065 + target_roi_side = max(260.0, min(520.0, target_roi_side)) + else: + target_roi_side = float(BASE_RADIUS) + + radius_speed = BASE_RADIUS + SPEED_RADIUS_FACTOR * vnorm + radius_speed = clamp(radius_speed, BASE_RADIUS, MAX_RADIUS) + # Берём максимум — даём запас на смещение Kalman + radius = max(radius_speed, target_roi_side * 0.5) + radius = min(radius, MAX_RADIUS) + radius = max(radius, BASE_RADIUS) + + fx = ux * radius * (FORWARD_BIAS - 1.0) + fy = uy * radius * (FORWARD_BIAS - 1.0) + + scx = cx + fx * 0.35 + scy = cy + fy * 0.35 + rx = radius * FORWARD_BIAS + ry = radius * SIDE_BIAS + if approach_active: + rx *= float(approach_kf_roi_scale) + ry *= float(approach_kf_roi_scale) + + roi_box = clip_box([scx - rx, scy - ry, scx + rx, scy + ry], ew, eh) + mode = "ROI-KF" + elif (not need_fullscan) and (not confirmed) and MOTION_ZONE_ENABLE and (motion_roi_eff is not None): + roi_box = motion_roi_eff.copy() + mode = "ROI-MOTION" + + if need_fullscan: + roi_box = None + mode = "FULL-CLOSE" if close_force_fullscan else "FULL" + last_fullscan_frame = frame_id + + yolo_worker.submit(frame_eff, roi_box, mode, frame_ts) + last_det_frame = frame_id + + yolo = yolo_worker.try_get() + dets_eff = [] + infer_ms = 0.0 + used_roi = False + have_yolo = False + yolo_mode = "NONE" + yolo_raw_count = 0 + yolo_ts = None + merged_part_count = 0 + if yolo is not None: + have_yolo = True + dets_eff, yolo_ts, yolo_mode, infer_ms, used_roi = yolo + # Motion-aware re-weighting: буст движущимся детектам, + # штраф статичным (горизонт, ЛЭП, деревья). Защита + # от wrong target lock на статических объектах. + dets_eff = reweight_dets_by_motion(dets_eff, motion_sal) + yolo_raw_count = len(dets_eff) + if RECOVER_FILTER_ENABLE and (not confirmed) and (yolo_mode in ("ROI-MOTION", "ROI-WAVE", "ROI-MOTION-C", "ROI-GAZE")) and yolo_raw_count > 0: + raw_dets = dets_eff + filtered = [d for d in raw_dets if recover_det_is_valid(d, motion_mask)] + if len(filtered) > 0: + dets_eff = filtered + else: + raw_sorted = sorted(raw_dets, key=lambda dd: float(dd[4]), reverse=True) + dets_eff = raw_sorted[:min(3, len(raw_sorted))] + merge_ref = pred_box_eff if pred_box_eff is not None else locked_box_eff + if confirmed and (merge_ref is not None) and len(dets_eff) >= int(max(2, PART_MERGE_MIN_PARTS)): + dets_eff, merged_part_count = merge_close_part_dets(dets_eff, merge_ref, ew, eh) + if FLASH_ROI_ENABLE and len(dets_eff) > 0: + best_d = max(dets_eff, key=lambda dd: float(dd[4])) + if float(best_d[4]) >= float(FLASH_ROI_MIN_SCORE): + flash_roi_eff = make_focus_roi_from_box( + best_d[:4], + ew, + eh, + margin=FLASH_ROI_MARGIN, + min_side=FLASH_ROI_MIN_SIDE, + ) + flash_ttl = int(max(1, FLASH_ROI_TTL)) + yolo_hist.append(infer_ms) + + if len(dets_eff) == 0: + yolo_no_det += 1 + else: + yolo_no_det = 0 + if yolo_ts is not None: + last_yolo_ok_ts = float(yolo_ts) + else: + last_yolo_ok_ts = frame_ts + + preacq_det = None + if PREACQ_ENABLE and (not confirmed): + pre_ref = pred_box_eff if pred_box_eff is not None else locked_box_eff + if have_yolo: + preacq_det = pick_preacq_det(dets_eff, pre_ref, ew, eh) + preacq_hist.append(1 if preacq_det is not None else 0) + else: + preacq_hist.append(0) + preacq_hits = int(sum(preacq_hist)) + + if preacq_det is not None: + acquire_score = int(clamp(acquire_score + int(PREACQ_ACQ_BONUS), 0, 999)) + acquire_miss = 0 + + if PREACQ_INIT_KF_ON_MN and preacq_hits >= int(PREACQ_MIN_HITS): + pb = clip_box(preacq_det[:4], ew, eh) + if not kf.initialized: + kf.init_from_box(pb) + else: + cxp, cyp = box_center(pb) + bwp, bhp = box_wh(pb) + kf.update([cxp, cyp, bwp, bhp]) + locked_box_eff = pb + hit_streak = max(int(hit_streak), 1) + flash_roi_eff = make_focus_roi_from_box( + pb, ew, eh, margin=FLASH_ROI_MARGIN, min_side=FLASH_ROI_MIN_SIDE + ) + flash_ttl = int(max(int(flash_ttl), int(max(1, FLASH_ROI_TTL)))) + else: + preacq_hist.clear() + preacq_hits = 0 + + if DEBUG and dets_eff and (frame_id % 30 == 0): + print("sample det:", dets_eff[0]) + + if have_yolo and dets_eff: + guidance_raw_yolo_box_eff = pick_best_det(dets_eff, ew, eh) + + if have_yolo: + dets_np = np.array(dets_eff, dtype=np.float32) if dets_eff else np.zeros((0, 5), dtype=np.float32) + else: + dets_np = None + tracks = bt.update(dets_np, dt=dt) + tracks_for_select = [t for t in tracks if t.time_since_update <= BT_MAX_PREDICT_AGE] + fresh_tracks = [t for t in tracks_for_select if t.time_since_update == 0] + if SWITCH_STATIC_GUARD_ENABLE: + hist_window = max(2, int(SWITCH_TRACK_HIST_WINDOW)) + stale_thr = max(12, 3 * hist_window) + if track_center_last_seen: + stale_ids = [ + tid for tid, last_f in track_center_last_seen.items() + if (frame_id - int(last_f)) > int(stale_thr) + ] + for tid in stale_ids: + track_center_last_seen.pop(tid, None) + track_center_hist.pop(tid, None) + for t in fresh_tracks: + tid = int(t.track_id) + b_hist = clip_box(t.tlbr, ew, eh) + cc = box_center(b_hist) + hist = track_center_hist.get(tid) + if hist is None or hist.maxlen != hist_window: + hist = deque(maxlen=hist_window) + track_center_hist[tid] = hist + hist.append(np.array([float(cc[0]), float(cc[1])], dtype=np.float32)) + track_center_last_seen[tid] = int(frame_id) + + chosen = None + target_track = None + pred_ref = pred_box_eff if pred_box_eff is not None else (locked_box_eff if locked_box_eff is not None else None) + pred_diag = float(np.linalg.norm(box_wh(pred_ref))) if pred_ref is not None else 40.0 + + if SOFT_YOLO_HANDOFF_ENABLE and confirmed and have_yolo and dets_eff: + soft_ref = locked_box_eff if (klt_valid and locked_box_eff is not None) else pred_ref + if soft_ref is not None and ( + klt_valid + or miss_streak >= int(SOFT_YOLO_MIN_MISS) + or target_absent_frames >= int(SOFT_YOLO_MIN_ABSENT) + ): + soft_has_strong_klt_anchor = bool( + klt_valid + and locked_box_eff is not None + and float(klt.quality) >= float(SOFT_YOLO_KLT_MIN_Q) + ) + soft_yolo_box_eff, soft_yolo_score = pick_soft_yolo_handoff_det( + dets_eff, + soft_ref, + ew, + eh, + min_score=float(SOFT_YOLO_MIN_SCORE), + dist_diag=float(SOFT_YOLO_KLT_DIST_DIAG if soft_has_strong_klt_anchor else SOFT_YOLO_LOST_DIST_DIAG), + dist_min=float(SOFT_YOLO_KLT_DIST_MIN if soft_has_strong_klt_anchor else SOFT_YOLO_LOST_DIST_MIN), + iou_floor=float(SOFT_YOLO_KLT_IOU_FLOOR if soft_has_strong_klt_anchor else SOFT_YOLO_IOU_FLOOR), + max_area_ratio=float(SOFT_YOLO_KLT_MAX_AREA_RATIO if soft_has_strong_klt_anchor else SOFT_YOLO_MAX_AREA_RATIO), + max_aspect_ratio=float(SOFT_YOLO_MAX_ASPECT_RATIO), + ) + if soft_yolo_box_eff is not None: + match_dist = max(float(SOFT_YOLO_TRACK_DIST_MIN), float(SOFT_YOLO_TRACK_DIST_DIAG) * pred_diag) + soft_yolo_track = match_fresh_track_to_box( + fresh_tracks, + soft_yolo_box_eff, + ew, + eh, + min_iou=float(SOFT_YOLO_TRACK_IOU), + max_center_dist=match_dist, + ) + if (soft_yolo_track is not None) and ((target_id is None) or (soft_yolo_track.track_id != target_id)): + guidance_candidate_box_eff = soft_yolo_box_eff + guidance_candidate_track_id = int(soft_yolo_track.track_id) + elif target_id is not None: + guidance_candidate_box_eff = soft_yolo_box_eff + + if target_id is not None: + for t in tracks_for_select: + if t.track_id == target_id: + target_track = t + break + if target_track is None: + target_absent_frames += 1 + else: + if have_yolo and target_track.time_since_update > 0 and len(fresh_tracks) > 0: + target_track = None + target_absent_frames = TARGET_SWITCH_MISS_FRAMES + else: + target_absent_frames = 0 + else: + target_absent_frames = 0 + current_target_track = target_track + current_target_has_fresh_update = bool( + current_target_track is not None and current_target_track.time_since_update == 0 + ) + current_target_motion_ok = False + if ( + current_target_track is not None + and EGO_RESIDUAL_GATE_ENABLE + and motion_mask is not None + ): + current_target_motion_ok = track_residual_motion_ok( + current_target_track, + motion_mask, + ew, + eh, + ) + + # If KLT/Kalman are holding an old point while YOLO repeatedly sees a + # candidate elsewhere, allow a controlled re-anchor. This fixes stale + # KLT locks without reintroducing one-frame false-positive jumps. + reanchor_context = bool( + YOLO_REANCHOR_ENABLE + and confirmed + and have_yolo + and dets_eff + and ( + miss_streak >= int(YOLO_REANCHOR_MISS_GE) + or (not current_target_has_fresh_update) + or target_absent_frames >= int(YOLO_REANCHOR_ABSENT_GE) + ) + ) + if reanchor_context: + reanchor_ref = pred_box_eff if pred_box_eff is not None else locked_box_eff + ( + yolo_reanchor_box_eff, + yolo_reanchor_score, + yolo_reanchor_track, + yolo_reanchor_same, + yolo_reanchor_near_pred, + ) = pick_yolo_reanchor_candidate( + dets_eff, + ew=ew, + eh=eh, + pred_ref=reanchor_ref, + prev_candidate_box=yolo_reanchor_memory_box_eff, + fresh_tracks=fresh_tracks, + min_score=float(YOLO_REANCHOR_MIN_SCORE), + repeat_dist_min=float(YOLO_REANCHOR_REPEAT_DIST_MIN), + repeat_dist_diag=float(YOLO_REANCHOR_REPEAT_DIST_DIAG), + pred_dist_min=float(YOLO_REANCHOR_PRED_DIST_MIN), + pred_dist_diag=float(YOLO_REANCHOR_PRED_DIST_DIAG), + max_area_ratio=float(YOLO_REANCHOR_MAX_AREA_RATIO), + max_aspect_ratio=float(YOLO_REANCHOR_MAX_ASPECT_RATIO), + osd_reject=bool(YOLO_REANCHOR_OSD_REJECT), + osd_min_score=float(YOLO_REANCHOR_OSD_MIN_SCORE), + trajectory_hypotheses=trajectory_hypotheses, + traj_dist_min=float(TRAJ_REANCHOR_DIST_MIN), + traj_dist_diag=float(TRAJ_REANCHOR_DIST_DIAG), + ) + if ( + TRAJ_REANCHOR_ENABLE + and (yolo_reanchor_box_eff is None) + and trajectory_hypotheses + ): + (traj_box, traj_score, trajectory_det_label, _traj_pick_score) = pick_det_near_trajectory_hypotheses( + dets_eff, trajectory_hypotheses, ew, eh, + min_score=float(TRAJ_REANCHOR_MIN_SCORE), + dist_min=float(TRAJ_REANCHOR_DIST_MIN), + dist_diag=float(TRAJ_REANCHOR_DIST_DIAG), + ) + if traj_box is not None: + yolo_reanchor_box_eff = traj_box + yolo_reanchor_score = float(traj_score) + yolo_reanchor_near_pred = True + yolo_reanchor_track = match_fresh_track_to_box( + fresh_tracks, yolo_reanchor_box_eff, ew, eh, + min_iou=float(SOFT_YOLO_TRACK_IOU), + max_center_dist=max(float(SOFT_YOLO_TRACK_DIST_MIN), float(SOFT_YOLO_TRACK_DIST_DIAG) * 40.0), + ) + + if yolo_reanchor_box_eff is not None: + recent_same = bool( + yolo_reanchor_same + and ((frame_id - int(yolo_reanchor_memory_last_frame)) <= int(YOLO_REANCHOR_WINDOW)) + ) + if recent_same: + yolo_reanchor_memory_hits += 1 + else: + yolo_reanchor_memory_hits = 1 + yolo_reanchor_memory_box_eff = yolo_reanchor_box_eff.copy() + yolo_reanchor_memory_last_frame = int(frame_id) + + req_reanchor_hits = int(YOLO_REANCHOR_HITS) + if yolo_reanchor_track is not None: + req_reanchor_hits = min(req_reanchor_hits, int(YOLO_REANCHOR_TRACK_HITS)) + if ( + yolo_reanchor_near_pred + and float(yolo_reanchor_score) >= float(YOLO_REANCHOR_NEAR_PRED_ONE_SHOT_SCORE) + ): + req_reanchor_hits = 1 + + if yolo_reanchor_memory_hits >= req_reanchor_hits: + yolo_reanchor_reason = ( + f"yolo_reanchor:hits={yolo_reanchor_memory_hits}/{req_reanchor_hits}," + f"score={float(yolo_reanchor_score):.2f}," + f"nearPred={int(yolo_reanchor_near_pred)}," + f"traj={trajectory_det_label}," + f"trk={int(yolo_reanchor_track is not None)}" + ) + guidance_candidate_box_eff = yolo_reanchor_box_eff + if yolo_reanchor_track is not None: + guidance_candidate_track_id = int(yolo_reanchor_track.track_id) + elif (frame_id - int(yolo_reanchor_memory_last_frame)) > int(YOLO_REANCHOR_WINDOW): + yolo_reanchor_memory_box_eff = None + yolo_reanchor_memory_hits = 0 + elif (frame_id - int(yolo_reanchor_memory_last_frame)) > int(YOLO_REANCHOR_WINDOW): + yolo_reanchor_memory_box_eff = None + yolo_reanchor_memory_hits = 0 + + motion_switch_mode = confirmed and (yolo_mode in ("ROI-MOTION-C", "ROI-WAVE")) + fast_maneuver_guard = ( + SWITCH_FAST_MANEUVER_ENABLE + and confirmed + and (speed >= float(SWITCH_FAST_MANEUVER_SPEED)) + ) + switch_miss_need = int(TARGET_SWITCH_MISS_FRAMES) + if motion_switch_mode: + switch_miss_need = max(switch_miss_need, int(MOTION_CONF_SWITCH_MISS_FRAMES)) + if approach_active: + switch_miss_need += int(approach_switch_extra_miss) + if fast_maneuver_guard: + switch_miss_need += int(max(0, SWITCH_FAST_MANEUVER_EXTRA_MISS)) + stale_switch_ready = bool( + STALE_LOCK_BREAK_ENABLE + and confirmed + and (target_track is None) + and (len(fresh_tracks) > 0) + and ( + (miss_streak >= int(STALE_LOCK_BREAK_MIN_MISS)) + or (not klt_valid) + or (float(klt.quality) < float(STALE_LOCK_BREAK_KLT_QUALITY)) + ) + ) + can_switch = target_track is None and ( + (target_id is None) + or (not confirmed) + or (target_absent_frames >= switch_miss_need) + or stale_switch_ready + ) + + candidate_tracks = fresh_tracks if (have_yolo and len(fresh_tracks) > 0) else tracks_for_select + eligible_tracks = candidate_tracks + if (not confirmed) and RECOVER_FILTER_ENABLE and (yolo_mode in ("ROI-MOTION", "ROI-WAVE", "ROI-MOTION-C", "ROI-GAZE")): + eligible_tracks = [t for t in candidate_tracks if t.hits >= int(RECOVER_TARGET_MIN_HITS)] + if len(eligible_tracks) == 0: + eligible_tracks = candidate_tracks + + weak_reacq_guard = ( + WEAK_REACQ_GUARD_ENABLE + and confirmed + and (not klt_valid) + and (miss_streak >= int(WEAK_REACQ_MISS_GE)) + ) + if weak_reacq_guard: + eligible_tracks = [ + t for t in eligible_tracks + if (t.hits >= int(WEAK_REACQ_MIN_HITS)) and (float(t.score) >= float(WEAK_REACQ_MIN_SCORE)) + ] + if EGO_RESIDUAL_GATE_ENABLE and miss_streak >= int(EGO_RESIDUAL_MISS_GE) and motion_mask is not None: + motion_tracks = [t for t in eligible_tracks if track_residual_motion_ok(t, motion_mask, ew, eh)] + if len(motion_tracks) > 0: + eligible_tracks = motion_tracks + + wavelet_active = ( + (wavelet_cooldown <= 0) + and WAVELET_ASSIST_ENABLE + and (miss_streak >= int(WAVELET_ACTIVE_MISS_GE)) + and ((not WAVELET_ONLY_UNCONFIRMED) or (not confirmed)) + ) + if wavelet_active and WAVELET_ONLY_RECOVER_PHASE: + wavelet_active = ((not confirmed) or (miss_streak > 0)) + best_wavelet_energy = 0.0 + best_wavelet_bonus = 0.0 + best_wavelet_hits = 0 + + if WAVELET_MN_ENABLE and wavelet_track_last_seen: + stale = [ + tid for tid, last_f in wavelet_track_last_seen.items() + if (frame_id - int(last_f)) > int(max(12, 3 * int(WAVELET_MN_WINDOW))) + ] + for tid in stale: + wavelet_track_last_seen.pop(tid, None) + wavelet_track_hist.pop(tid, None) + + if can_switch and len(eligible_tracks) > 0: + best = None + best_score = -1e9 + best_hist = None + for t in eligible_tracks: + b = clip_box(t.tlbr, ew, eh) + c = box_center(b) + approach_bonus = 0.0 + is_switch_candidate = confirmed and (target_id is not None) and (t.track_id != target_id) + if not track_passes_score_gate( + track_score=float(t.score), + confirmed=confirmed, + is_switch_candidate=is_switch_candidate, + weak_reacq_guard=weak_reacq_guard, + acquire_floor=float(TRACK_SCORE_MIN_ACQUIRE), + reacquire_floor=float(TRACK_SCORE_MIN_REACQUIRE), + switch_floor=float(TRACK_SCORE_MIN_SWITCH), + ): + continue + residual_ok = False + if is_switch_candidate and EGO_RESIDUAL_GATE_ENABLE and motion_mask is not None: + residual_ok = track_residual_motion_ok(t, motion_mask, ew, eh) + + dist = 0.0 + i = 0.0 + if pred_ref is not None: + pc = box_center(pred_ref) + dist = float(np.linalg.norm(c - pc)) + max_dist = max(40.0, TARGET_REACQ_DIST_FACTOR * pred_diag) + if confirmed and (target_id is None): + noid_max = max(float(CONFIRMED_NO_ID_NEAR_MIN), float(CONFIRMED_NO_ID_NEAR_FACTOR) * pred_diag) + max_dist = min(max_dist, noid_max) + if not confirmed: + near_max = max(float(RECOVER_NEAR_DIST_MIN), float(RECOVER_NEAR_DIST_FACTOR) * pred_diag) + max_dist = min(max_dist, near_max) + if weak_reacq_guard: + weak_max = max(float(WEAK_REACQ_MAX_DIST_MIN), float(WEAK_REACQ_MAX_DIST_FACTOR) * pred_diag) + max_dist = min(max_dist, weak_max) + if dist > max_dist: + continue + i = iou(b, pred_ref) + + pred_area = box_area(pred_ref) + cand_area = box_area(b) + ar_ratio = safe_ratio(box_ar(b), box_ar(pred_ref)) + grow_ratio = cand_area / max(pred_area, 1e-3) + shrink_ratio = pred_area / max(cand_area, 1e-3) + allow_grow_ratio = float(TURN_MAX_AREA_RATIO) + if approach_active and cand_area >= pred_area: + close_growth = ( + dist <= (float(approach_near_dist_diag) * pred_diag) + or (cand_area >= float(APPROACH_CLOSE_AREA)) + ) + if close_growth: + allow_grow_ratio = max(allow_grow_ratio, float(approach_max_area_ratio)) + if grow_ratio > 1.0: + approach_bonus = float( + clamp( + float(approach_score_weight) * np.log2(grow_ratio), + 0.0, + float(approach_score_clip), + ) + ) + if ar_ratio > TURN_MAX_ASPECT_RATIO: + continue + if cand_area >= pred_area: + if grow_ratio > allow_grow_ratio: + continue + else: + if shrink_ratio > TURN_MAX_AREA_RATIO: + continue + if target_id is not None and t.track_id != target_id: + if i < TARGET_SWITCH_IOU_FLOOR and dist > (1.2 * pred_diag): + continue + + if ( + is_switch_candidate + and SWITCH_TRAJ_GATE_ENABLE + and (miss_streak >= int(SWITCH_TRAJ_MISS_GE)) + and (pred_ref is not None) + and kf.initialized + ): + vx = float(kf.x[4, 0]) + vy = float(kf.x[5, 0]) + vnorm = float(np.hypot(vx, vy)) + if vnorm >= float(SWITCH_TRAJ_MIN_SPEED): + dvec = c - box_center(pred_ref) + dnorm = float(np.linalg.norm(dvec)) + if dnorm > 1e-3: + ux = vx / vnorm + uy = vy / vnorm + cos_v = float((dvec[0] * ux + dvec[1] * uy) / dnorm) + perp = float(abs(dvec[0] * uy - dvec[1] * ux)) + perp_lim = max(float(SWITCH_TRAJ_PERP_MIN), float(SWITCH_TRAJ_PERP_DIAG) * pred_diag) + traj_ok = (cos_v >= float(SWITCH_TRAJ_COS_MIN)) and (perp <= perp_lim) + if (not traj_ok) and (not (SWITCH_TRAJ_REQUIRE_RESIDUAL_BYPASS and residual_ok)): + continue + + if is_switch_candidate and SWITCH_STATIC_GUARD_ENABLE: + hist = track_center_hist.get(int(t.track_id)) + min_hist = max(3, int(max(2, int(SWITCH_TRACK_HIST_WINDOW)) // 2)) + if hist is not None and len(hist) >= min_hist: + disp = float(np.linalg.norm(hist[-1] - hist[0])) + disp_lim = max(float(SWITCH_STATIC_MIN_DISP), float(SWITCH_STATIC_MIN_DIAG) * pred_diag) + if (disp < disp_lim) and (not (SWITCH_STATIC_REQUIRE_RESIDUAL_BYPASS and residual_ok)): + continue + + if motion_switch_mode and is_switch_candidate: + if int(t.hits) < int(MOTION_CONF_SWITCH_MIN_HITS): + continue + if float(t.score) < float(MOTION_CONF_SWITCH_MIN_SCORE): + continue + if pred_ref is not None: + if (i < float(MOTION_CONF_SWITCH_IOU_FLOOR)) and (dist > (float(MOTION_CONF_SWITCH_DIST_DIAG) * pred_diag)): + continue + if ( + MOTION_CONF_SWITCH_REQUIRE_RESIDUAL + and EGO_RESIDUAL_GATE_ENABLE + and motion_mask is not None + and (not residual_ok) + ): + continue + if ( + confirmed + and (target_id is not None) + and (t.track_id != target_id) + and OSD_SWITCH_BLOCK_ENABLE + and REJECT_OSD_ZONES + ): + in_osd = in_osd_zone(float(c[0]), float(c[1]), ew, eh) + if in_osd: + if pred_ref is None: + continue + if dist > (float(OSD_SWITCH_BLOCK_DIST_DIAG) * pred_diag): + continue + + app = 0.0 + cand_hist = None + if USE_HSV_GATE and ref_hist is not None: + cand_hist = compute_hsv_hist(frame_eff, b) + app = hsv_sim(ref_hist, cand_hist) + min_app = float(HSV_GATE_MIN_SIM) + if approach_active and pred_ref is not None: + pred_area = box_area(pred_ref) + cand_area = box_area(b) + grow_ratio = cand_area / max(pred_area, 1e-3) + if (cand_area >= float(APPROACH_CLOSE_AREA)) or (grow_ratio >= float(APPROACH_HSV_RELAX_GROW_RATIO)): + min_app *= float(approach_hsv_min_scale) + if app < min_app: + continue + + wv_energy = 0.0 + wv_bonus = 0.0 + wv_hits = 0 + if wavelet_active: + wv_energy = wavelet_energy_haar( + gray_now, + b, + margin=WAVELET_BOX_MARGIN, + min_side=WAVELET_MIN_SIDE, + ) + motion_ok = ( + EGO_RESIDUAL_GATE_ENABLE + and motion_mask is not None + and track_residual_motion_ok(t, motion_mask, ew, eh) + ) + tiny_box = box_area(b) <= float(WAVELET_GATE_TINY_AREA_MAX) + if tiny_box and (miss_streak >= int(WAVELET_GATE_MISS_GE)): + if (wv_energy < float(WAVELET_GATE_MIN_ENERGY)) and (not motion_ok): + continue + + if WAVELET_MN_ENABLE: + tid = int(t.track_id) + hist = wavelet_track_hist.get(tid) + if hist is None: + hist = deque(maxlen=max(1, int(WAVELET_MN_WINDOW))) + wavelet_track_hist[tid] = hist + wv_pass = (wv_energy >= float(WAVELET_GATE_MIN_ENERGY)) or motion_ok + hist.append(1 if wv_pass else 0) + wavelet_track_last_seen[tid] = int(frame_id) + wv_hits = int(sum(hist)) + if tiny_box and (miss_streak >= int(WAVELET_MN_MISS_GE)) and (wv_hits < int(WAVELET_MN_MIN_HITS)) and (not motion_ok): + continue + + wv_norm = (wv_energy - float(WAVELET_ENERGY_BASE)) / max(1e-6, float(WAVELET_ENERGY_BASE)) + wv_bonus = float(clamp(float(WAVELET_SCORE_WEIGHT) * wv_norm, -float(WAVELET_SCORE_CLIP), float(WAVELET_SCORE_CLIP))) + if WAVELET_MN_ENABLE and int(WAVELET_MN_WINDOW) > 0: + mn_ratio = float(wv_hits) / float(max(1, int(WAVELET_MN_WINDOW))) + wv_bonus += 0.08 * mn_ratio + + s = (2.2 * i) + (1.0 / (1.0 + dist)) + (0.25 * float(t.score)) + (0.9 * app) + wv_bonus + approach_bonus + if pred_ref is None: + s = (0.35 * float(t.score)) + (0.0005 * box_area(b)) + (0.9 * app) + wv_bonus + if target_id is not None and t.track_id == target_id: + s += TARGET_STICKY_SCORE_BONUS + + if s > best_score: + best_score = s + best = t + best_hist = cand_hist + best_wavelet_energy = wv_energy + best_wavelet_bonus = wv_bonus + best_wavelet_hits = wv_hits + best_dist = float(dist) + best_residual_ok = bool(residual_ok) + + if best is not None and best_score >= TARGET_PICK_MIN_SCORE: + if ( + confirmed + and (target_id is not None) + and (best.track_id != target_id) + and (best.time_since_update == 0) + ): + guidance_candidate_box_eff = clip_box(best.tlbr, ew, eh) + guidance_candidate_track_id = int(best.track_id) + if ( + STALE_LOCK_BREAK_ENABLE + and confirmed + and (target_id is not None) + and (best.track_id != target_id) + ): + klt_iou_now = 0.0 + if (klt.box is not None) and (locked_box_eff is not None): + klt_iou_now = float(iou(klt.box, locked_box_eff)) + handoff_decision = evaluate_stale_lock( + confirmed=confirmed, + have_fresh_candidate=(best.time_since_update == 0), + target_has_fresh_update=current_target_has_fresh_update, + candidate_dist_px=float(best_dist), + pred_diag_px=float(pred_diag), + miss_streak=int(miss_streak), + klt_valid=bool(klt_valid), + klt_quality=float(klt.quality), + klt_iou=klt_iou_now, + candidate_motion_ok=bool(best_residual_ok), + target_motion_ok=bool(current_target_motion_ok), + stale_break_dist_diag=float(STALE_LOCK_BREAK_DIST_DIAG), + stale_break_min_miss=int(STALE_LOCK_BREAK_MIN_MISS), + stale_break_klt_quality=float(STALE_LOCK_BREAK_KLT_QUALITY), + stale_break_klt_iou=float(STALE_LOCK_BREAK_KLT_IOU), + ) + stale_lock_active = bool(handoff_decision.stale_lock_active) + stale_lock_reason = handoff_decision.reason_text() + + if confirmed and weak_reacq_guard and ((target_id is None) or (best.track_id != target_id)): + if switch_candidate_id == best.track_id: + switch_candidate_hits += 1 + else: + switch_candidate_id = best.track_id + switch_candidate_hits = 1 + if switch_candidate_hits < int(WEAK_REACQ_ADOPT_HITS): + best = None + elif target_id is not None and confirmed and best.track_id != target_id: + req_switch_hits = compute_fast_handoff_hits( + stale_lock_active=bool(FAST_HANDOFF_ENABLE and stale_lock_active), + motion_switch_mode=bool(motion_switch_mode), + approach_extra_hits=int(approach_switch_extra_hits) if approach_active else 0, + fast_maneuver_extra_hits=int(max(0, SWITCH_FAST_MANEUVER_EXTRA_HITS)) if fast_maneuver_guard else 0, + default_switch_hits=int(TARGET_SWITCH_CONFIRM_HITS), + fast_handoff_hits=int(FAST_HANDOFF_CONFIRM_HITS), + motion_switch_hits=int(MOTION_CONF_SWITCH_CONFIRM_HITS), + ) + fast_handoff_active = bool( + FAST_HANDOFF_ENABLE + and stale_lock_active + and (req_switch_hits == int(FAST_HANDOFF_CONFIRM_HITS)) + ) + if switch_candidate_id == best.track_id: + switch_candidate_hits += 1 + else: + switch_candidate_id = best.track_id + switch_candidate_hits = 1 + if switch_candidate_hits < req_switch_hits: + best = None + else: + switch_candidate_id = None + switch_candidate_hits = 0 + + if best is not None and best_score >= TARGET_PICK_MIN_SCORE: + target_track = best + if USE_HSV_GATE: + ref_hist = blend_hist(ref_hist, best_hist, alpha=0.85) + target_absent_frames = 0 + + if target_track is None and target_id is None and len(eligible_tracks) > 0: + eligible_tracks = [ + t for t in eligible_tracks + if track_passes_score_gate( + track_score=float(t.score), + confirmed=confirmed, + is_switch_candidate=False, + weak_reacq_guard=weak_reacq_guard, + acquire_floor=float(TRACK_SCORE_MIN_ACQUIRE), + reacquire_floor=float(TRACK_SCORE_MIN_REACQUIRE), + switch_floor=float(TRACK_SCORE_MIN_SWITCH), + ) + ] + picked = None + if len(eligible_tracks) == 0: + picked = None + elif pred_ref is None: + picked = max(eligible_tracks, key=lambda tt: (float(tt.score), box_area(tt.tlbr))) + elif not confirmed: + pc = box_center(pred_ref) + near_lim = max(float(RECOVER_NEAR_DIST_MIN), float(RECOVER_NEAR_DIST_FACTOR) * pred_diag) + near_tracks = [] + for tt in eligible_tracks: + cc = box_center(clip_box(tt.tlbr, ew, eh)) + if float(np.linalg.norm(cc - pc)) <= near_lim: + near_tracks.append(tt) + if len(near_tracks) > 0: + picked = max(near_tracks, key=lambda tt: (float(tt.score), box_area(tt.tlbr))) + else: + if not weak_reacq_guard: + pc = box_center(pred_ref) + near_lim = max(float(CONFIRMED_NO_ID_NEAR_MIN), float(CONFIRMED_NO_ID_NEAR_FACTOR) * pred_diag) + near_tracks = [] + for tt in eligible_tracks: + cc = box_center(clip_box(tt.tlbr, ew, eh)) + if float(np.linalg.norm(cc - pc)) <= near_lim: + near_tracks.append(tt) + if len(near_tracks) > 0: + picked = max(near_tracks, key=lambda tt: (float(tt.score), box_area(tt.tlbr))) + + if picked is not None: + target_track = picked + target_absent_frames = 0 + switch_candidate_id = None + switch_candidate_hits = 0 + + if ( + SOFT_YOLO_HANDOFF_ENABLE + and confirmed + and (soft_yolo_box_eff is not None) + and ( + target_track is None + or miss_streak >= int(SOFT_YOLO_MIN_MISS) + or target_absent_frames >= int(SOFT_YOLO_MIN_ABSENT) + ) + ): + if soft_yolo_track is not None: + target_track = soft_yolo_track + target_absent_frames = 0 + else: + chosen = soft_yolo_box_eff + target_absent_frames = 0 + switch_candidate_id = None + switch_candidate_hits = 0 + soft_yolo_adopted = True + + if ( + YOLO_REANCHOR_ENABLE + and confirmed + and (yolo_reanchor_box_eff is not None) + and yolo_reanchor_reason + ): + # Bypass the normal KLT-anchor rejection only after M/N detector + # persistence says the old KLT anchor is stale. + if yolo_reanchor_track is not None: + target_track = yolo_reanchor_track + target_absent_frames = 0 + else: + chosen = yolo_reanchor_box_eff + target_absent_frames = 0 + switch_candidate_id = None + switch_candidate_hits = 0 + soft_yolo_adopted = False + yolo_reanchor_adopted = True + trajectory_reanchor_used = bool(trajectory_det_label != "-") + yolo_reanchor_last_reason = yolo_reanchor_reason + + suppress_target_id_update = False + klt_anchor_reject_reason = "" + if ( + BT_KLT_ANCHOR_GUARD_ENABLE + and (not yolo_reanchor_adopted) + and confirmed + and klt_valid + and locked_box_eff is not None + and float(klt.quality) >= float(BT_KLT_ANCHOR_MIN_Q) + ): + proposed_box_for_anchor = None + if target_track is not None: + proposed_box_for_anchor = clip_box(target_track.tlbr, ew, eh) + elif chosen is not None: + proposed_box_for_anchor = clip_box(chosen, ew, eh) + + if proposed_box_for_anchor is not None: + anchor_ok, klt_anchor_reject_reason = klt_anchor_accepts_box( + proposed_box_for_anchor, + locked_box_eff, + ew, + eh, + dist_diag=float(BT_KLT_ANCHOR_DIST_DIAG), + dist_min=float(BT_KLT_ANCHOR_DIST_MIN), + iou_floor=float(BT_KLT_ANCHOR_IOU_FLOOR), + max_area_ratio=float(BT_KLT_ANCHOR_MAX_AREA_RATIO), + max_aspect_ratio=float(BT_KLT_ANCHOR_MAX_ASPECT_RATIO), + ) + if not anchor_ok: + # KLT is still confidently sitting on the physical target. + # Do not let a one-frame ByteTrack/Yolo false positive drag + # Kalman/guidance away. Keep the KLT anchor as measurement. + target_track = None + soft_yolo_adopted = False + target_absent_frames = 0 + switch_candidate_id = None + switch_candidate_hits = 0 + if BT_KLT_ANCHOR_HOLD_ON_REJECT: + chosen = clip_box(locked_box_eff, ew, eh) + else: + chosen = None + if DEBUG and (frame_id % max(1, int(BT_KLT_ANCHOR_DEBUG_EVERY)) == 0): + print(f"[klt_anchor_guard] reject BT/Yolo box frame={frame_id} reason={klt_anchor_reject_reason}") + elif ( + BT_ID_STABILIZE_WHEN_KLT_VALID + and target_track is not None + and target_id is not None + and int(target_track.track_id) != int(target_id) + ): + # ByteTrack often creates a new id every frame for tiny FPV targets. + # Use its box as a detector measurement, but do not treat this as a + # physical target switch while KLT is strong. + suppress_target_id_update = True + target_absent_frames = 0 + switch_candidate_id = None + switch_candidate_hits = 0 + + prev_target_id = target_id + if target_track is not None: + if not suppress_target_id_update: + target_id = target_track.track_id + chosen = clip_box(target_track.tlbr, ew, eh) + switch_candidate_id = None + switch_candidate_hits = 0 + + chosen_valid = chosen is not None + if chosen_valid: + if not kf.initialized: + kf.init_from_box(chosen) + else: + cx, cy = box_center(chosen) + bw, bh = box_wh(chosen) + if pred_box_eff is not None: + pcx, pcy = box_center(pred_box_eff) + dist = float(np.hypot(cx - pcx, cy - pcy)) + diag = float(np.linalg.norm(box_wh(pred_box_eff))) + if dist > 10.0 * max(15.0, diag): + chosen = None + + if chosen is None and kf.initialized and TM_ENABLE and template_gray is not None: + pcx, pcy = box_center(pred_box_eff) if pred_box_eff is not None else (kf.x[0, 0], kf.x[1, 0]) + tm = tm_search(gray_now, template_gray, (pcx, pcy), miss_streak) + if tm is not None: + mcx, mcy, _, tw, th = tm + if pred_box_eff is not None: + bw, bh = box_wh(pred_box_eff) + else: + bw, bh = float(tw), float(th) + kf.update([float(mcx), float(mcy), float(bw), float(bh)]) + + if chosen is not None: + kf.update([cx, cy, bw, bh]) + + chosen_valid = chosen is not None + + if chosen_valid: + locked_box_eff = chosen + hit_streak += 1 + miss_streak = 0 + if not confirmed: + bonus = int(ACQUIRE_HIT_BONUS) + if target_track is not None and float(target_track.score) >= float(BT_HIGH): + bonus += 1 + acquire_score = int(clamp(acquire_score + bonus, 0, 999)) + acquire_miss = 0 + + if not confirmed and ((hit_streak >= CONFIRM_HITS) or (acquire_score >= ACQUIRE_CONFIRM_SCORE)): + confirmed = True + if locked_box_eff is not None: + klt.init(frame_eff, locked_box_eff) + last_klt_init_frame = frame_id + ref_hist = compute_hsv_hist(frame_eff, locked_box_eff) if USE_HSV_GATE else None + template_gray = tm_update_template(gray_now, locked_box_eff) + acquire_score = 0 + acquire_miss = 0 + preacq_hist.clear() + preacq_hits = 0 + + if USE_HSV_GATE and have_yolo: + cur_hist = compute_hsv_hist(frame_eff, locked_box_eff) + if ref_hist is None: + ref_hist = cur_hist + else: + if (frame_id % HSV_UPDATE_EVERY == 0) or (target_track is not None and float(target_track.score) >= BT_HIGH): + ref_hist = blend_hist(ref_hist, cur_hist, alpha=0.80) + + if TM_ENABLE and (have_yolo or frame_id % 5 == 0): + template_gray = tm_update_template(gray_now, locked_box_eff) + + if soft_yolo_adopted and SOFT_YOLO_REFRESH_KLT and confirmed and locked_box_eff is not None: + klt.init(frame_eff, locked_box_eff) + last_klt_init_frame = frame_id + + if yolo_reanchor_adopted and YOLO_REANCHOR_RESET_KLT and confirmed and locked_box_eff is not None: + klt.reset() + klt.init(frame_eff, locked_box_eff) + last_klt_init_frame = frame_id + yolo_reanchor_memory_box_eff = None + yolo_reanchor_memory_hits = 0 + + if ( + KLT_REFRESH_WITH_YOLO + and confirmed + and have_yolo + and (target_track is not None) + and (target_track.time_since_update == 0) + and (locked_box_eff is not None) + and (float(target_track.score) >= float(KLT_REFRESH_MIN_SCORE)) + ): + klt_age = int(frame_id - last_klt_init_frame) + klt_iou = iou(klt.box, locked_box_eff) if klt.box is not None else 0.0 + weak_anchor = ( + (not klt_valid) + or (klt.good_count < int(KLT_REINIT_MIN_POINTS)) + or (klt.quality < float(KLT_REFRESH_MIN_QUALITY)) + ) + stale_anchor = klt_age >= int(max(1, KLT_REFRESH_EVERY)) + drifted_anchor = (klt.box is None) or (klt_iou < float(KLT_REFRESH_MIN_IOU)) + if (klt_age > 0) and (weak_anchor or stale_anchor or drifted_anchor): + klt.init(frame_eff, locked_box_eff) + last_klt_init_frame = frame_id + + else: + used_prediction_hold = False + if confirmed and stale_lock_active: + guidance_ctrl.reset_for_target_switch( + np.array([0.5 * float(ew), 0.5 * float(eh)], dtype=np.float32), + cmd_damp=float(FAST_HANDOFF_GUIDANCE_CMD_DAMP), + ) + guidance_reset_event = "stale_lock_break" + guidance_force_neutral = True + + if TURN_SAFE_ENABLE and have_yolo and dets_eff and (miss_streak >= TURN_SAFE_MISS_BEFORE_RESET): + best_det, best_conf = pick_best_det_with_score(dets_eff, ew, eh) + if best_det is not None and float(best_conf) >= float(TURN_SAFE_MIN_SCORE): + adopted_track_id = None + adopted_track = None + best_iou = 0.0 + for tt in tracks_for_select: + if tt.time_since_update > 0: + continue + btt = clip_box(tt.tlbr, ew, eh) + i = iou(btt, best_det) + if i > best_iou: + best_iou = i + adopted_track = tt + if adopted_track is not None and best_iou >= 0.15: + adopted_track_id = adopted_track.track_id + + if (not TURN_SAFE_REQUIRE_TRACK_ID) or (adopted_track_id is not None): + kf.init_from_box(best_det) + locked_box_eff = best_det + confirmed = True + miss_streak = 0 + hit_streak = CONFIRM_HITS + target_id = adopted_track_id + target_absent_frames = 0 + switch_candidate_id = None + switch_candidate_hits = 0 + klt.reset() + if locked_box_eff is not None: + klt.init(frame_eff, locked_box_eff) + last_klt_init_frame = frame_id + if USE_HSV_GATE: + ref_hist = compute_hsv_hist(frame_eff, locked_box_eff) + if TM_ENABLE: + template_gray = tm_update_template(gray_now, locked_box_eff) + acquire_score = 0 + acquire_miss = 0 + preacq_hist.clear() + preacq_hits = 0 + + if confirmed and KLT_VALID_HOLD_ENABLE and klt_valid and locked_box_eff is not None: + miss_streak = min(int(miss_streak) + 1, int(KLT_VALID_HOLD_MAX_MISS)) + hit_streak = 0 + used_prediction_hold = True + elif confirmed and pred_box_eff is not None and (not stale_lock_active) and miss_streak < KALMAN_HOLD_MAX: + hold_box_eff = pred_box_eff + if ( + TRAJ_USE_PRIMARY_FOR_HOLD + and trajectory_primary_box_eff is not None + and (miss_streak >= int(TRAJ_HOLD_MISS_GE)) + ): + hold_box_eff = trajectory_primary_box_eff + locked_box_eff = hold_box_eff + miss_streak += 1 + hit_streak = 0 + used_prediction_hold = True + elif (not confirmed) and PROVISIONAL_HOLD_ENABLE and kf.initialized and pred_box_eff is not None and miss_streak < int(PROVISIONAL_HOLD_MAX): + locked_box_eff = pred_box_eff + miss_streak += 1 + hit_streak = max(0, int(hit_streak) - int(max(1, PROVISIONAL_HIT_DECAY))) + acquire_miss += 1 + acquire_score = max(0, int(acquire_score) - int(max(1, ACQUIRE_MISS_PENALTY))) + used_prediction_hold = True + elif not (confirmed and klt_valid): + miss_streak += 1 + if confirmed: + hit_streak = 0 + else: + hit_streak = max(0, int(hit_streak) - int(max(1, PROVISIONAL_HIT_DECAY))) + acquire_miss += 1 + acquire_score = max(0, int(acquire_score) - int(max(1, ACQUIRE_MISS_PENALTY))) + if acquire_miss >= int(ACQUIRE_MAX_MISS): + acquire_score = 0 + + if (not used_prediction_hold) and confirmed and (yolo_no_det >= YOLO_NO_DET_LIMIT) and (not klt_valid): + confirmed = False + locked_box_eff = None + ref_hist = None + template_gray = None + target_id = None + target_absent_frames = 0 + switch_candidate_id = None + switch_candidate_hits = 0 + klt.reset() + miss_streak = 0 + hit_streak = 0 + yolo_no_det = 0 + acquire_score = 0 + acquire_miss = 0 + preacq_hist.clear() + preacq_hits = 0 + + elif confirmed and (target_id is None) and (miss_streak >= int(CONFIRMED_NO_ID_MAX_MISS)): + confirmed = False + locked_box_eff = None + ref_hist = None + template_gray = None + target_id = None + target_absent_frames = 0 + switch_candidate_id = None + switch_candidate_hits = 0 + klt.reset() + hit_streak = 0 + acquire_score = 0 + acquire_miss = 0 + preacq_hist.clear() + preacq_hits = 0 + + elif miss_streak >= MAX_MISSES: + confirmed = False + locked_box_eff = None + ref_hist = None + template_gray = None + target_id = None + target_absent_frames = 0 + switch_candidate_id = None + switch_candidate_hits = 0 + klt.reset() + acquire_score = 0 + acquire_miss = 0 + preacq_hist.clear() + preacq_hits = 0 + + # ─── Stationary clutter killer ────────────────────── + # Независимый "полицейский": следит за движением центра + # confirmed трека и motion saliency в его зоне. Форсит + # сброс если трек прилип к статичному объекту (снег, + # горизонт, ЛЭП). + killer.update( + frame_id=frame_id, + target_id=target_id, + locked_box_eff=locked_box_eff, + motion_sal=motion_sal, + confirmed=confirmed, + frame_h=eh, + ) + if killer.should_kill(frame_id): + print(f"[stationary_killer] kill tid={target_id} " + f"reason={killer.last_kill_reason}") + killer.notify_kill_done(frame_id) + confirmed = False + locked_box_eff = None + ref_hist = None + template_gray = None + target_id = None + target_absent_frames = 0 + switch_candidate_id = None + switch_candidate_hits = 0 + klt.reset() + miss_streak = 0 + hit_streak = 0 + acquire_score = 0 + acquire_miss = 0 + preacq_hist.clear() + preacq_hits = 0 + + locked_box_orig = None + if locked_box_eff is not None: + locked_box_orig = unscale_box(locked_box_eff, sx, sy) + locked_box_orig = clip_box(locked_box_orig, frame_orig.shape[1], frame_orig.shape[0]) + + if GUIDANCE_OVERRIDE_ENABLE: + guidance_override_candidate_box_eff = pick_guidance_override_box( + track_candidate_box=guidance_candidate_box_eff, + raw_yolo_box=guidance_raw_yolo_box_eff, + ) + guidance_override_candidate_box_eff, guidance_override_memory_ttl = update_guidance_override_latch( + new_box=guidance_override_candidate_box_eff, + prev_box=guidance_override_memory_box_eff, + prev_ttl=guidance_override_memory_ttl, + max_ttl=int(GUIDANCE_OVERRIDE_TTL), + ) + guidance_override_memory_box_eff = guidance_override_candidate_box_eff + guidance_override_active = should_override_guidance( + confirmed=confirmed, + target_track_missing=(target_track is None), + have_fresh_candidate=(guidance_override_candidate_box_eff is not None), + candidate_matches_target=( + guidance_candidate_track_id is not None + and target_id is not None + and int(guidance_candidate_track_id) == int(target_id) + ), + miss_streak=int(miss_streak), + override_miss_ge=int(GUIDANCE_OVERRIDE_MISS_GE), + ) + if guidance_override_active: + guidance_override_box_eff = guidance_override_candidate_box_eff + else: + guidance_override_memory_box_eff = None + guidance_override_memory_ttl = 0 + + if DRAW_ALL_BOXES and dets_eff: + for d in dets_eff: + b_eff = d[:4] + b_orig = unscale_box(b_eff, sx, sy) + b_orig = clip_box(b_orig, frame_orig.shape[1], frame_orig.shape[0]) + x1, y1, x2, y2 = map(int, b_orig) + cv2.rectangle(frame_orig, (x1, y1), (x2, y2), (0, 255, 0), 1) + cv2.putText(frame_orig, f"{float(d[4]):.2f}", (x1, max(0, y1 - 6)), + cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 255, 0), 2) + + if DRAW_BT_TRACKS and tracks: + for t in tracks: + if BT_DRAW_ONLY_CONFIRMED and (t.hits < BT_MIN_HITS): + continue + if DRAW_BT_ONLY_FRESH and (int(t.time_since_update) > 0): + continue + b_eff = clip_box(t.tlbr, ew, eh) + b_orig = unscale_box(b_eff, sx, sy) + b_orig = clip_box(b_orig, frame_orig.shape[1], frame_orig.shape[0]) + x1, y1, x2, y2 = map(int, b_orig) + cv2.rectangle(frame_orig, (x1, y1), (x2, y2), (255, 255, 0), 2) + cv2.putText(frame_orig, f"T{t.track_id}:{t.score:.2f} a={int(t.time_since_update)}", + (x1, max(0, y1 - 8)), + cv2.FONT_HERSHEY_SIMPLEX, 0.6, (255, 255, 0), 2) + + if DRAW_LOCK_BOX and locked_box_orig is not None: + x1, y1, x2, y2 = map(int, locked_box_orig) + if miss_streak <= int(DRAW_KALMAN_WHEN_MISS_LE): + cv2.rectangle(frame_orig, (x1, y1), (x2, y2), (0, 0, 255), 2) + cv2.putText(frame_orig, f"ID={target_id}", (x1, max(0, y1 - 10)), + cv2.FONT_HERSHEY_SIMPLEX, 0.7, (0, 0, 255), 2) + + if DRAW_KALMAN and kf.initialized: + pb_eff = clip_box(kf.to_box(), ew, eh) + pb_orig = unscale_box(pb_eff, sx, sy) + cx, cy = box_center(pb_orig) + cv2.circle(frame_orig, (int(cx), int(cy)), 5, (255, 0, 0), -1) + + if DEBUG and DRAW_MOTION_ROI and (not confirmed) and (motion_roi_eff is not None): + m_orig = unscale_box(motion_roi_eff, sx, sy) + m_orig = clip_box(m_orig, frame_orig.shape[1], frame_orig.shape[0]) + mx1, my1, mx2, my2 = map(int, m_orig) + cv2.rectangle(frame_orig, (mx1, my1), (mx2, my2), (0, 165, 255), 1) + cv2.putText( + frame_orig, + f"MOTION zones={motion_active_zones}", + (mx1, max(0, my1 - 6)), + cv2.FONT_HERSHEY_SIMPLEX, + 0.5, + (0, 165, 255), + 2, + ) + + if DEBUG and DRAW_AUTOGAZE_ROI and len(autogaze_rois_eff) > 0: + for gi, g_eff in enumerate(autogaze_rois_eff): + g_orig = unscale_box(g_eff, sx, sy) + g_orig = clip_box(g_orig, frame_orig.shape[1], frame_orig.shape[0]) + gx1, gy1, gx2, gy2 = map(int, g_orig) + color = (255, 80, 20) if gi == 0 else (255, 160, 80) + cv2.rectangle(frame_orig, (gx1, gy1), (gx2, gy2), color, 1) + if gi == 0: + cv2.putText( + frame_orig, + "GAZE", + (gx1, max(0, gy1 - 6)), + cv2.FONT_HERSHEY_SIMPLEX, + 0.5, + color, + 2, + ) + + if DRAW_TRAJ: + if CLEAR_TRAJ_ON_RECOVER and (not confirmed): + traj.clear() + if (not DRAW_TRAJ_ONLY_WHEN_LOCKED) or (locked_box_eff is not None and confirmed): + traj_frame_i += 1 + if traj_frame_i % max(1, int(TRAIL_DRAW_EVERY_N)) == 0: + src_eff = locked_box_eff if locked_box_eff is not None else pred_box_eff + if src_eff is not None: + c_eff = box_center(src_eff) + c_orig = c_eff / np.array([sx, sy], dtype=np.float32) + pt = (int(c_orig[0]), int(c_orig[1])) + if (not traj) or (abs(pt[0] - traj[-1][0]) + abs(pt[1] - traj[-1][1]) >= int(TRAIL_MIN_STEP_PX)): + traj.append(pt) + if len(traj) >= 2: + overlay = frame_orig.copy() + for i in range(1, len(traj)): + p0 = traj[i - 1] + p1 = traj[i] + age = i / max(1, (len(traj) - 1)) + thickness = 1 if age < 0.85 else 2 + cv2.line(overlay, p0, p1, (0, 0, 255), thickness, cv2.LINE_AA) + a = float(clamp(TRAIL_ALPHA, 0.05, 0.6)) + frame_orig[:] = cv2.addWeighted(overlay, a, frame_orig, 1.0 - a, 0) + + if DEBUG and DRAW_TRAJ_PREDICTIONS and trajectory_hypotheses: + src_eff = locked_box_eff if locked_box_eff is not None else pred_box_eff + if src_eff is not None: + src_c = box_center(src_eff) / np.array([sx, sy], dtype=np.float32) + for h in trajectory_hypotheses: + hb_orig = unscale_box(h["box"], sx, sy) + hb_orig = clip_box(hb_orig, frame_orig.shape[1], frame_orig.shape[0]) + hx1, hy1, hx2, hy2 = map(int, hb_orig) + hc = box_center(h["box"]) / np.array([sx, sy], dtype=np.float32) + cv2.rectangle(frame_orig, (hx1, hy1), (hx2, hy2), (180, 0, 255), 1) + cv2.line(frame_orig, (int(src_c[0]), int(src_c[1])), (int(hc[0]), int(hc[1])), (180, 0, 255), 1, cv2.LINE_AA) + cv2.putText(frame_orig, str(h.get("label", "tr")), (hx1, max(0, hy1 - 4)), + cv2.FONT_HERSHEY_SIMPLEX, 0.4, (180, 0, 255), 1) + + status = "CONFIRMED" if confirmed else "RECOVER" + cv2.putText(frame_orig, status, (20, 40), cv2.FONT_HERSHEY_SIMPLEX, 1.0, (0, 255, 255), 2) + + ag_ready = int(getattr(autogaze_worker, "ready", False)) + ag_stale = int(autogaze_info["stale"]) if autogaze_info is not None else 1 + ag_age = int(autogaze_info["age"]) if autogaze_info is not None else -1 + ag_cells = int(autogaze_info["active_cells"]) if autogaze_info is not None else 0 + ag_ms = float(autogaze_info["infer_ms"]) if autogaze_info is not None else 0.0 + ag_rois = len(autogaze_rois_eff) + target_track_score = float(target_track.score) if target_track is not None else None + vx_guid = float(kf.x[4, 0]) if kf.initialized else 0.0 + vy_guid = float(kf.x[5, 0]) if kf.initialized else 0.0 + guidance_state = guidance_ctrl.update( + frame_id=frame_id, + frame_w=ew, + frame_h=eh, + confirmed=bool(confirmed and (not guidance_force_neutral)), + locked_box=None if guidance_force_neutral else locked_box_eff, + pred_box=None if guidance_force_neutral else pred_box_eff, + override_box=None if guidance_force_neutral else guidance_override_box_eff, + target_id=target_id, + target_track_score=target_track_score, + klt_valid=klt_valid, + klt_quality=float(klt.quality), + miss_streak=miss_streak, + vx=vx_guid, + vy=vy_guid, + ) + + if DEBUG: + cv2.putText(frame_orig, f"src={source_kind} p={ACTIVE_ANTI_FP_PROFILE} eff={ew}x{eh} miss={miss_streak} hit={hit_streak} acq={acquire_score} pH={preacq_hits} tAbs={target_absent_frames} swHit={switch_candidate_hits} rHit={yolo_reanchor_memory_hits} rAd={int(yolo_reanchor_adopted)} trH={len(trajectory_hypotheses)} trA={int(trajectory_reanchor_used)} trL={trajectory_det_label} stale={int(stale_lock_active)} fastH={int(fast_handoff_active)} detEvery={det_every} yNoDet={yolo_no_det} mZones={motion_active_zones} fT={flash_ttl} wRT={wavelet_roi_ttl} wRP={wavelet_roi_peak:.2f} aR={ag_ready} aS={ag_stale} aAge={ag_age} aC={ag_cells} aK={ag_rois}", + (20, 75), cv2.FONT_HERSHEY_SIMPLEX, 0.65, (0, 255, 255), 2) + cv2.putText(frame_orig, f"YOLO new={int(have_yolo)} dets={yolo_raw_count}->{len(dets_eff)} merge={merged_part_count} infer={infer_ms:.1f}ms mode={yolo_mode} gReset={guidance_reset_event or '-'}", + (20, 105), cv2.FONT_HERSHEY_SIMPLEX, 0.65, (0, 255, 255), 2) + cv2.putText(frame_orig, f"KLT valid={int(klt_valid)} q={klt.quality:.2f} pts={klt.good_count} speed={speed:.1f} dt={dt * 1000:.1f}ms dtSrc={dt_source} ch={adaptive_chase_stage} appr={int(approach_active)} fClose={int(close_force_fullscan)} swFast={int(fast_maneuver_guard)} wA={int(wavelet_active)} wCd={wavelet_cooldown} wE={best_wavelet_energy:.3f} wB={best_wavelet_bonus:.2f} wH={best_wavelet_hits} aMS={ag_ms:.1f} aCd={autogaze_cooldown}", + (20, 135), cv2.FONT_HERSHEY_SIMPLEX, 0.65, (0, 255, 255), 2) + guidance_ctrl.draw_overlay(frame_orig, guidance_state, sx, sy) + + track_logger.log_frame( + frame_id=frame_id, + timestamp_sec=frame_ts, + dt_sec=dt, + source_kind=source_kind, + status=status, + confirmed=confirmed, + target_id=target_id, + target_track_score=target_track_score, + locked=(locked_box_eff is not None), + locked_box=locked_box_eff, + pred_box=pred_box_eff, + hit_streak=hit_streak, + miss_streak=miss_streak, + acquire_score=acquire_score, + preacq_hits=preacq_hits, + switch_candidate_hits=switch_candidate_hits, + best_score=best_score, + have_yolo=have_yolo, + yolo_mode=yolo_mode, + yolo_raw_count=yolo_raw_count, + det_count=len(dets_eff) if dets_eff else 0, + merged_part_count=merged_part_count, + infer_ms=infer_ms, + stale_lock_active=stale_lock_active, + stale_lock_reason=stale_lock_reason, + fast_handoff_active=fast_handoff_active, + guidance_reset_event=guidance_reset_event, + used_prediction_hold=used_prediction_hold, + klt_valid=klt_valid, + klt_quality=klt.quality, + klt_points=klt.good_count, + speed=speed, + adaptive_chase_stage=adaptive_chase_stage, + approach_active=approach_active, + close_force_fullscan=close_force_fullscan, + fast_maneuver_guard=fast_maneuver_guard, + motion_active_zones=motion_active_zones, + wavelet_active=wavelet_active, + wavelet_energy=best_wavelet_energy, + wavelet_bonus=best_wavelet_bonus, + wavelet_hits=best_wavelet_hits, + autogaze_ready=ag_ready, + autogaze_stale=ag_stale, + autogaze_age=ag_age, + autogaze_cells=ag_cells, + autogaze_ms=ag_ms, + guidance_active=guidance_state["active"], + guidance_status=guidance_state["status"], + guidance_confidence=guidance_state["confidence"], + guidance_error_x=guidance_state["error_x"], + guidance_error_y=guidance_state["error_y"], + guidance_cmd_x=guidance_state["cmd_x"], + guidance_cmd_y=guidance_state["cmd_y"], + guidance_on_target=guidance_state["on_target"], + ) + + if TRAJ_PREDICT_ENABLE: + if confirmed and locked_box_eff is not None: + src_ok = bool( + (hit_streak > 0) + or klt_valid + or soft_yolo_adopted + or yolo_reanchor_adopted + or (miss_streak <= int(TRAJ_HISTORY_KEEP_MISS_LE)) + ) + if src_ok: + trajectory_obs_hist.append({ + "frame": int(frame_id), + "ts": float(frame_ts), + "center": box_center(locked_box_eff).astype(np.float32), + "box": clip_box(locked_box_eff, ew, eh).copy(), + }) + else: + trajectory_obs_hist.clear() + + if writer is not None: + writer.write(frame_orig) + + if SHOW_OUTPUT: + cv2.imshow(WINDOW_NAME, frame_orig) + + iter_ms = (time.perf_counter() - iter_start) * 1000.0 + perf_hist.append(iter_ms) + + elapsed = time.perf_counter() - iter_start + wait_ms = 1 + if VIDEO_REALTIME and target_out_fps > 0.0: + target_dt = 1.0 / target_out_fps + if elapsed < target_dt: + wait_ms = int((target_dt - elapsed) * 1000.0) + wait_ms = clamp(wait_ms, 1, 50) + + key = cv2.waitKey(int(wait_ms)) & 0xFF + if key == 27: + break + + if frame_id % 60 == 0 and perf_hist: + p50 = np.percentile(perf_hist, 50) + p95 = np.percentile(perf_hist, 95) + fps = 1000.0 / max(np.mean(perf_hist), 1e-6) + if yolo_hist: + yp50 = np.percentile(yolo_hist, 50) + yp95 = np.percentile(yolo_hist, 95) + else: + yp50, yp95 = 0.0, 0.0 + print(f"[perf] fps~{fps:.1f} iter p50={p50:.1f} p95={p95:.1f} | yolo p50={yp50:.1f} p95={yp95:.1f}") + + frame_id += 1 + + autogaze_worker.stop() + yolo_worker.stop() + track_summary = track_logger.close() + if track_summary is not None: + print( + f"[track-log] confirmed={track_summary['confirmed_frames']}/{track_summary['frames']} " + f"switches={track_summary['target_switches']} " + f"csv={track_summary['csv_path']}" + ) + if writer is not None: + writer.release() + cap.release() + cv2.destroyAllWindows() + + +if __name__ == "__main__": + main() diff --git a/motion_saliency.py b/motion_saliency.py new file mode 100644 index 0000000..3a5ccb5 --- /dev/null +++ b/motion_saliency.py @@ -0,0 +1,330 @@ +# ============================================================ +# motion_saliency.py +# Разделение независимого движения и ego-motion камеры. +# +# Поддерживает ОБА типа ego-motion: +# - affine 2x3 (через cv2.warpAffine) +# - homography 3x3 (через cv2.warpPerspective) +# +# Кроме того поддерживает режим "dual residual": считается +# residual от обеих моделей одновременно, и в каждой точке +# берётся МИНИМУМ — если хотя бы одна модель объясняет движение +# пикселя, он считается статичным. Устойчиво к вырождению любой +# из моделей на сложных сценах. +# ============================================================ + +import cv2 +import numpy as np + +from config import * +from helpers import clamp, clip_box + + +# ───────────────────────────────────────────────────────────── +# Параметры +# ───────────────────────────────────────────────────────────── +MS_ENABLE = True +MS_BLUR_KSIZE = 5 +MS_DIFF_THR = 18 +MS_MORPH_OPEN_KSIZE = 3 +MS_MORPH_CLOSE_KSIZE = 5 +MS_EMA_ALPHA = 0.55 +MS_OUTLIER_SIGMA_PX = 18.0 +MS_OUTLIER_WEIGHT = 0.55 +MS_DIFF_WEIGHT = 0.45 +MS_MIN_WARP_SHIFT_PX = 0.3 +MS_MASK_TOP_FRAC = 0.0 +MS_MASK_BOTTOM_FRAC = 0.0 +MS_DRAW_OVERLAY = True +MS_DRAW_ALPHA = 0.35 + +# Dual-residual mode: если True, motion_sal.update_dual() считает +# обе карты и берёт min — нужно если включаете и homography, и +# affine параллельно (устойчивее к вырождению). +MS_DUAL_MODE_DEFAULT = False + + +def _warp_prev(prev_gray, M, kind, out_size): + """Универсальный варп предыдущего кадра под текущий.""" + w, h = out_size + if M is None or kind == "none": + return prev_gray + + if kind == "affine": + tx = float(M[0, 2]) + ty = float(M[1, 2]) + if abs(tx) + abs(ty) < float(MS_MIN_WARP_SHIFT_PX): + return prev_gray + return cv2.warpAffine( + prev_gray, M, (w, h), + flags=cv2.INTER_LINEAR, + borderMode=cv2.BORDER_REPLICATE, + ) + + if kind == "homography": + return cv2.warpPerspective( + prev_gray, M, (w, h), + flags=cv2.INTER_LINEAR, + borderMode=cv2.BORDER_REPLICATE, + ) + + return prev_gray + + +class MotionSaliency: + """ + Карта независимого движения на кадре. + + Базовое использование: + ms = MotionSaliency() + ms.update(gray, prev_gray, M, kind="homography", outlier_pts=pts) + score = ms.score_box(box) + rois = ms.get_rois(thr=0.35) + + Dual-residual (устойчивый на сложных сценах): + ms.update_dual(gray, prev_gray, + H=homography, A=affine, + outlier_pts=pts) + """ + + def __init__(self): + self.enabled = bool(MS_ENABLE) + self.saliency = None + self.raw_diff = None + self.outlier_density = None + self.last_active_frac = 0.0 + self.last_kind = "none" + self._ema_alpha = float(MS_EMA_ALPHA) + + def reset(self): + self.saliency = None + self.raw_diff = None + self.outlier_density = None + self.last_active_frac = 0.0 + self.last_kind = "none" + + # ─── Основной update ───────────────────────────────────── + def update(self, gray, prev_gray, M, kind="affine", outlier_pts=None): + """ + M — матрица ego-motion (2x3 для affine, 3x3 для homography) + kind — "affine" | "homography" | "none" + """ + if not self.enabled or gray is None: + return None + + h, w = gray.shape[:2] + self.last_kind = kind + + diff_map = self._compute_diff_map(gray, prev_gray, M, kind) + outlier_map = self._compute_outlier_density(outlier_pts, h, w) + + combined = self._combine(diff_map, outlier_map, h, w) + self._apply_ema(combined) + self.last_active_frac = float((self.saliency > 0.25).mean()) + return self.saliency + + # ─── Dual residual update ──────────────────────────────── + def update_dual(self, gray, prev_gray, H=None, A=None, outlier_pts=None): + """ + Считает residual от homography и affine одновременно, в каждой + точке берёт min (если любая модель объясняет — пиксель статичен). + Устойчивее на сценах с несколькими плоскостями. + + H — homography 3x3 или None + A — affine 2x3 или None + """ + if not self.enabled or gray is None: + return None + + h, w = gray.shape[:2] + + diff_h = self._compute_diff_map(gray, prev_gray, H, "homography") if H is not None else None + diff_a = self._compute_diff_map(gray, prev_gray, A, "affine") if A is not None else None + + if diff_h is not None and diff_a is not None: + diff_map = np.minimum(diff_h, diff_a) + self.last_kind = "dual" + elif diff_h is not None: + diff_map = diff_h + self.last_kind = "homography" + elif diff_a is not None: + diff_map = diff_a + self.last_kind = "affine" + else: + diff_map = None + self.last_kind = "none" + + outlier_map = self._compute_outlier_density(outlier_pts, h, w) + combined = self._combine(diff_map, outlier_map, h, w) + self._apply_ema(combined) + self.last_active_frac = float((self.saliency > 0.25).mean()) + return self.saliency + + # ─── Scoring API ───────────────────────────────────────── + def score_box(self, box): + if self.saliency is None or box is None: + return 0.0 + h, w = self.saliency.shape[:2] + x1 = int(clamp(box[0], 0, w - 1)) + y1 = int(clamp(box[1], 0, h - 1)) + x2 = int(clamp(box[2], x1 + 1, w)) + y2 = int(clamp(box[3], y1 + 1, h)) + patch = self.saliency[y1:y2, x1:x2] + if patch.size == 0: + return 0.0 + return float(patch.mean()) + + def peak_box(self, box): + if self.saliency is None or box is None: + return 0.0 + h, w = self.saliency.shape[:2] + x1 = int(clamp(box[0], 0, w - 1)) + y1 = int(clamp(box[1], 0, h - 1)) + x2 = int(clamp(box[2], x1 + 1, w)) + y2 = int(clamp(box[3], y1 + 1, h)) + patch = self.saliency[y1:y2, x1:x2] + if patch.size == 0: + return 0.0 + return float(patch.max()) + + def get_rois(self, thr=0.35, min_area=30, max_rois=6, pad_px=12): + if self.saliency is None: + return [] + mask = (self.saliency >= float(thr)).astype(np.uint8) + if mask.sum() < min_area: + return [] + + n_labels, labels, stats, _ = cv2.connectedComponentsWithStats(mask, connectivity=8) + rois = [] + h, w = mask.shape[:2] + for lbl in range(1, int(n_labels)): + area = int(stats[lbl, cv2.CC_STAT_AREA]) + if area < min_area: + continue + x = int(stats[lbl, cv2.CC_STAT_LEFT]) + y = int(stats[lbl, cv2.CC_STAT_TOP]) + ww = int(stats[lbl, cv2.CC_STAT_WIDTH]) + hh = int(stats[lbl, cv2.CC_STAT_HEIGHT]) + box = np.array( + [x - pad_px, y - pad_px, x + ww + pad_px, y + hh + pad_px], + dtype=np.float32, + ) + box = clip_box(box, w, h) + rois.append((area, box)) + + rois.sort(key=lambda x: x[0], reverse=True) + return [b for _, b in rois[:max_rois]] + + # ─── Internals ─────────────────────────────────────────── + def _compute_diff_map(self, gray, prev_gray, M, kind): + """Ego-compensated frame differencing → float32 [0..1].""" + if prev_gray is None or gray.shape != prev_gray.shape: + return None + + k = int(MS_BLUR_KSIZE) + if k >= 3 and (k % 2) == 1: + g_cur = cv2.GaussianBlur(gray, (k, k), 0) + g_prv = cv2.GaussianBlur(prev_gray, (k, k), 0) + else: + g_cur = gray + g_prv = prev_gray + + h, w = g_cur.shape[:2] + warped = _warp_prev(g_prv, M, kind, (w, h)) + + diff = cv2.absdiff(g_cur, warped) + _, bin_mask = cv2.threshold(diff, int(MS_DIFF_THR), 255, cv2.THRESH_BINARY) + + ko = int(MS_MORPH_OPEN_KSIZE) + if ko >= 2: + kern = cv2.getStructuringElement(cv2.MORPH_ELLIPSE, (ko, ko)) + bin_mask = cv2.morphologyEx(bin_mask, cv2.MORPH_OPEN, kern) + + kc = int(MS_MORPH_CLOSE_KSIZE) + if kc >= 2: + kern = cv2.getStructuringElement(cv2.MORPH_ELLIPSE, (kc, kc)) + bin_mask = cv2.morphologyEx(bin_mask, cv2.MORPH_CLOSE, kern) + + amp = diff.astype(np.float32) / 255.0 + gate = (bin_mask > 0).astype(np.float32) + soft = cv2.GaussianBlur(amp * gate, (9, 9), 0) + + mx = float(soft.max()) + if mx > 1e-6: + soft = soft / mx + self.raw_diff = soft + return soft + + def _compute_outlier_density(self, outlier_pts, h, w): + """KDE density из outlier точек.""" + if outlier_pts is None or len(outlier_pts) == 0: + self.outlier_density = None + return None + + scale = 0.25 + sh = max(1, int(h * scale)) + sw = max(1, int(w * scale)) + acc = np.zeros((sh, sw), dtype=np.float32) + + pts = np.asarray(outlier_pts, dtype=np.float32).reshape(-1, 2) + for x, y in pts: + xi = int(clamp(x * scale, 0, sw - 1)) + yi = int(clamp(y * scale, 0, sh - 1)) + acc[yi, xi] += 1.0 + + sigma = float(MS_OUTLIER_SIGMA_PX) * scale + k = int(max(3, round(sigma * 6)) | 1) + blur = cv2.GaussianBlur(acc, (k, k), sigma) + + density = cv2.resize(blur, (w, h), interpolation=cv2.INTER_LINEAR) + mx = float(density.max()) + if mx > 1e-6: + density = density / mx + self.outlier_density = density + return density + + def _combine(self, diff_map, outlier_map, h, w): + combined = np.zeros((h, w), dtype=np.float32) + if diff_map is not None: + combined += float(MS_DIFF_WEIGHT) * diff_map + if outlier_map is not None: + combined += float(MS_OUTLIER_WEIGHT) * outlier_map + combined = np.clip(combined, 0.0, 1.0) + + if MS_MASK_TOP_FRAC > 0.0: + combined[: int(h * MS_MASK_TOP_FRAC), :] = 0.0 + if MS_MASK_BOTTOM_FRAC > 0.0: + combined[int(h * (1.0 - MS_MASK_BOTTOM_FRAC)) :, :] = 0.0 + return combined + + def _apply_ema(self, combined): + if self.saliency is None or self.saliency.shape != combined.shape: + self.saliency = combined + else: + a = self._ema_alpha + self.saliency = a * self.saliency + (1.0 - a) * combined + + # ─── Overlay ───────────────────────────────────────────── + def draw_overlay(self, frame_bgr, sx=1.0, sy=1.0): + if not MS_DRAW_OVERLAY or self.saliency is None: + return + h, w = frame_bgr.shape[:2] + sal = cv2.resize(self.saliency, (w, h), interpolation=cv2.INTER_LINEAR) + sal_u8 = np.clip(sal * 255.0, 0, 255).astype(np.uint8) + heat = cv2.applyColorMap(sal_u8, cv2.COLORMAP_JET) + cv2.addWeighted(heat, float(MS_DRAW_ALPHA), frame_bgr, 1.0 - float(MS_DRAW_ALPHA), 0, frame_bgr) + + cv2.putText( + frame_bgr, + f"MS[{self.last_kind}] active={self.last_active_frac*100:.1f}%", + (20, 230), + cv2.FONT_HERSHEY_SIMPLEX, 0.55, (255, 255, 255), 2, + ) + + def status_line(self): + if not self.enabled: + return "Motion saliency disabled" + return ( + f"Motion saliency ready: diff_w={MS_DIFF_WEIGHT} " + f"outlier_w={MS_OUTLIER_WEIGHT} ema={MS_EMA_ALPHA}" + ) diff --git a/operator_hud.py b/operator_hud.py new file mode 100644 index 0000000..96080d8 --- /dev/null +++ b/operator_hud.py @@ -0,0 +1,694 @@ +# ============================================================ +# operator_hud.py +# Операторский HUD для ручного перехвата +# +# Показывает пилоту: +# - Направление к цели (стрелка "рули сюда") +# - Расстояние и время до контакта (tau) +# - Индикатор качества захвата (lock quality) +# - Прицельный маркер с lead-предсказанием +# - Зону поражения (kill zone) +# - Звуковой сигнал при сближении (опционально) +# +# Интегрируется в main.py одним вызовом draw(). +# Не требует автопилота, моторов, IMU. +# ============================================================ + +import math +import time +import numpy as np +import cv2 + +from config import * +from helpers import clamp, box_center, box_wh, box_area, clip_box + + +# ───────────────────────────────────────────────────────────── +# КОНФИГУРАЦИЯ +# ───────────────────────────────────────────────────────────── + +# Камера (для оценки дальности) +HUD_FOCAL_PX = 315.0 # focal length в пикселях (откалибровать!) +HUD_TARGET_SIZE_M = 0.27 # типичный размер цели (м) + +# Фазы сближения (по tau, секунды) +HUD_PHASE_FAR_TAU = 999.0 # далеко, просто трек +HUD_PHASE_APPROACH_TAU = 5.0 # заход на цель +HUD_PHASE_ATTACK_TAU = 2.5 # атака, финальный бросок +HUD_PHASE_TERMINAL_TAU = 1.0 # терминальная фаза, не отвлекаться + +# Lead-предсказание для прицельного маркера +HUD_LEAD_SEC = 0.20 # упреждение (секунды вперёд) +HUD_LEAD_MAX_PX = 120.0 # максимум пикселей lead + +# Kill zone — при попадании цели в этот круг, пилот "на цели" +HUD_KILL_ZONE_RADIUS_NORM = 0.06 # доля от ширины кадра +HUD_ON_TARGET_MIN_FRAMES = 3 # стабильно на цели N кадров + +# Звук (через системный beep, работает на Linux/Windows) +HUD_AUDIO_ENABLE = False # включить звуковые подсказки +HUD_AUDIO_APPROACH_HZ = 2.0 # частота бипов при approach +HUD_AUDIO_ATTACK_HZ = 6.0 # частота при attack +HUD_AUDIO_TERMINAL_HZ = 15.0 # частота при terminal (почти непрерывно) + +# Элементы HUD +HUD_SHOW_STEER_ARROW = True # стрелка "рули сюда" +HUD_SHOW_RANGE = True # дальность +HUD_SHOW_TAU = True # время до контакта +HUD_SHOW_LOCK_QUALITY = True # полоска качества лока +HUD_SHOW_LEAD_MARKER = True # прицел с упреждением +HUD_SHOW_KILL_ZONE = True # зона поражения +HUD_SHOW_SPEED_BAR = True # скорость цели +HUD_SHOW_PHASE_BANNER = True # баннер фазы вверху экрана + +# Мигание при terminal +HUD_TERMINAL_BLINK_RATE = 8 # Гц мигания рамки + +# Tau: минимальный рост площади для оценки +HUD_TAU_MIN_AREA_RATE = 0.5 + + +class OperatorHUD: + """ + Всё что нужно пилоту для ручного перехвата. + Один вызов draw() в каждом кадре — рисует поверх frame_orig. + """ + + def __init__(self): + # Оценка дальности + self._area_hist = [] # [(timestamp, area), ...] + self._area_hist_max = 12 + self._range_m = None + self._tau = float('inf') + self._closing_vel = 0.0 + + # Фаза + self.phase = "SEARCH" # SEARCH | TRACK | APPROACH | ATTACK | TERMINAL + self._phase_start = time.perf_counter() + + # On-target счётчик + self._on_target_count = 0 + self.on_target = False + + # Аудио + self._last_beep_time = 0.0 + + # Lead smoothing + self._smooth_lead_x = 0.0 + self._smooth_lead_y = 0.0 + + def update(self, locked_box_eff, pred_box_eff, kf, + confirmed, miss_streak, hit_streak, + klt_quality, frame_ts, dt, ew, eh): + """ + Обновляет внутреннее состояние HUD. Вызывать каждый кадр. + + Args: + locked_box_eff: текущий locked bbox (effective pixels) или None + pred_box_eff: Kalman-predicted bbox или None + kf: Kalman8D или IMMFilter + confirmed: трек подтверждён + miss_streak: пропуски подряд + hit_streak: хиты подряд + klt_quality: качество KLT (0..1) + frame_ts: timestamp кадра + dt: дельта времени + ew, eh: размеры effective frame + + Returns: + dict с метриками для отображения + """ + ref_box = locked_box_eff if locked_box_eff is not None else pred_box_eff + + # ─── Range + Tau ──────────────────────────────────────── + if ref_box is not None: + w, h = box_wh(ref_box) + apparent = float(max(w, h)) + area = float(w * h) + + # Дальность + if apparent >= 3.0 and HUD_FOCAL_PX > 10.0: + self._range_m = (HUD_TARGET_SIZE_M * HUD_FOCAL_PX) / apparent + else: + self._range_m = None + + # Tau + self._area_hist.append((frame_ts, area)) + if len(self._area_hist) > self._area_hist_max: + self._area_hist = self._area_hist[-self._area_hist_max:] + self._tau = self._compute_tau() + self._closing_vel = self._compute_closing_vel() + else: + self._range_m = None + self._tau = float('inf') + self._closing_vel = 0.0 + self._area_hist.clear() + + # ─── Phase ────────────────────────────────────────────── + new_phase = self._classify_phase(confirmed, miss_streak) + if new_phase != self.phase: + self.phase = new_phase + self._phase_start = time.perf_counter() + + # ─── On-target ────────────────────────────────────────── + self._update_on_target(ref_box, ew, eh) + + # ─── Lead marker ──────────────────────────────────────── + lead_x, lead_y = 0.0, 0.0 + if kf.initialized and ref_box is not None: + vx = float(kf.x[4, 0]) + vy = float(kf.x[5, 0]) + lead_x = vx * HUD_LEAD_SEC + lead_y = vy * HUD_LEAD_SEC + mag = float(np.hypot(lead_x, lead_y)) + if mag > HUD_LEAD_MAX_PX: + lead_x *= HUD_LEAD_MAX_PX / mag + lead_y *= HUD_LEAD_MAX_PX / mag + + self._smooth_lead_x = 0.6 * self._smooth_lead_x + 0.4 * lead_x + self._smooth_lead_y = 0.6 * self._smooth_lead_y + 0.4 * lead_y + + # ─── Audio ────────────────────────────────────────────── + if HUD_AUDIO_ENABLE: + self._do_audio(frame_ts) + + # ─── Lock quality ─────────────────────────────────────── + lock_q = self._compute_lock_quality( + confirmed, miss_streak, hit_streak, klt_quality + ) + + # ─── Speed ────────────────────────────────────────────── + speed_px = 0.0 + if kf.initialized: + speed_px = float(np.hypot(kf.x[4, 0], kf.x[5, 0])) + + return { + "phase": self.phase, + "range_m": self._range_m, + "tau": self._tau, + "closing_vel": self._closing_vel, + "on_target": self.on_target, + "lock_quality": lock_q, + "lead_x": self._smooth_lead_x, + "lead_y": self._smooth_lead_y, + "speed_px": speed_px, + } + + def draw(self, frame_orig, locked_box_eff, pred_box_eff, kf, + hud_state, sx, sy): + """ + Рисует весь HUD поверх frame_orig. + + Args: + frame_orig: кадр для рисования (BGR) + locked_box_eff: locked bbox в effective pixels + pred_box_eff: predicted bbox в effective pixels + kf: Kalman/IMM (для скорости) + hud_state: dict из update() + sx, sy: масштаб effective → original + """ + oh, ow = frame_orig.shape[:2] + ref_box = locked_box_eff if locked_box_eff is not None else pred_box_eff + phase = hud_state["phase"] + + # Цвета по фазе + phase_colors = { + "SEARCH": (150, 150, 150), + "TRACK": (0, 220, 255), + "APPROACH": (0, 165, 255), + "ATTACK": (0, 80, 255), + "TERMINAL": (0, 0, 255), + } + color = phase_colors.get(phase, (150, 150, 150)) + + # ─── Kill zone ────────────────────────────────────────── + if HUD_SHOW_KILL_ZONE: + self._draw_kill_zone(frame_orig, ow, oh, color, phase) + + # ─── Стрелка наведения ────────────────────────────────── + if HUD_SHOW_STEER_ARROW and ref_box is not None: + self._draw_steer_arrow(frame_orig, ref_box, sx, sy, ow, oh, color) + + # ─── Lead marker ──────────────────────────────────────── + if HUD_SHOW_LEAD_MARKER and ref_box is not None: + self._draw_lead_marker(frame_orig, ref_box, hud_state, sx, sy, color) + + # ─── Lock quality bar ─────────────────────────────────── + if HUD_SHOW_LOCK_QUALITY: + self._draw_lock_quality(frame_orig, hud_state["lock_quality"], + ow, oh, color) + + # ─── Speed bar ────────────────────────────────────────── + if HUD_SHOW_SPEED_BAR and ref_box is not None: + self._draw_speed_bar(frame_orig, hud_state["speed_px"], + ow, oh, color) + + # ─── Range + Tau text ─────────────────────────────────── + if HUD_SHOW_RANGE or HUD_SHOW_TAU: + self._draw_range_tau(frame_orig, hud_state, ow, oh, color) + + # ─── Phase banner ─────────────────────────────────────── + if HUD_SHOW_PHASE_BANNER: + self._draw_phase_banner(frame_orig, phase, hud_state, ow, oh, color) + + # ─── Terminal blink ───────────────────────────────────── + if phase == "TERMINAL": + self._draw_terminal_blink(frame_orig, ow, oh) + + # ─── On-target indicator ──────────────────────────────── + if hud_state["on_target"]: + self._draw_on_target(frame_orig, ow, oh) + + # ───────────────────────────────────────────────────────── + # DRAW HELPERS + # ───────────────────────────────────────────────────────── + + def _draw_kill_zone(self, frame, ow, oh, color, phase): + """Кольцо kill zone в центре экрана.""" + cx, cy = ow // 2, oh // 2 + radius = int(float(ow) * HUD_KILL_ZONE_RADIUS_NORM) + + if phase in ("ATTACK", "TERMINAL"): + # Заливка полупрозрачная + overlay = frame.copy() + cv2.circle(overlay, (cx, cy), radius, color, 2) + cv2.addWeighted(overlay, 0.5, frame, 0.5, 0, frame) + else: + cv2.circle(frame, (cx, cy), radius, (80, 80, 80), 1) + + # Перекрестие + gap = 8 + arm = radius + 12 + cv2.line(frame, (cx - arm, cy), (cx - gap, cy), (80, 80, 80), 1) + cv2.line(frame, (cx + gap, cy), (cx + arm, cy), (80, 80, 80), 1) + cv2.line(frame, (cx, cy - arm), (cx, cy - gap), (80, 80, 80), 1) + cv2.line(frame, (cx, cy + gap), (cx, cy + arm), (80, 80, 80), 1) + + def _draw_steer_arrow(self, frame, ref_box, sx, sy, ow, oh, color): + """ + Стрелка от центра экрана к цели — показывает оператору + КУДА РУЛИТЬ. Размер стрелки пропорционален ошибке. + Когда цель в центре — стрелка исчезает. + """ + center_eff = box_center(ref_box) + # → original coords + tx = float(center_eff[0]) / max(1e-6, sx) + ty = float(center_eff[1]) / max(1e-6, sy) + + scr_cx = ow * 0.5 + scr_cy = oh * 0.5 + + dx = tx - scr_cx + dy = ty - scr_cy + dist = float(np.hypot(dx, dy)) + + # Мёртвая зона: если цель почти в центре, не рисуем стрелку + deadzone = float(ow) * 0.03 + if dist < deadzone: + return + + # Нормализуем + ux = dx / dist + uy = dy / dist + + # Длина стрелки: от 30 до 120px, пропорционально ошибке + max_arrow = min(120, ow // 6) + arrow_len = float(clamp(dist * 0.4, 30, max_arrow)) + + # Начало стрелки — на расстоянии от центра + start_r = 35 + sx_a = int(scr_cx + ux * start_r) + sy_a = int(scr_cy + uy * start_r) + ex_a = int(scr_cx + ux * (start_r + arrow_len)) + ey_a = int(scr_cy + uy * (start_r + arrow_len)) + + # Толщина зависит от фазы + thickness = 2 + if self.phase in ("ATTACK", "TERMINAL"): + thickness = 3 + + cv2.arrowedLine(frame, (sx_a, sy_a), (ex_a, ey_a), + color, thickness, cv2.LINE_AA, tipLength=0.3) + + def _draw_lead_marker(self, frame, ref_box, hud_state, sx, sy, color): + """ + Прицельный маркер с УПРЕЖДЕНИЕМ. + Показывает где цель БУДЕТ через HUD_LEAD_SEC секунд. + Оператор должен целиться сюда, а не в текущую позицию. + """ + center_eff = box_center(ref_box) + lead_x = hud_state["lead_x"] + lead_y = hud_state["lead_y"] + + # Lead point в effective → original + lx = float(center_eff[0] + lead_x) / max(1e-6, sx) + ly = float(center_eff[1] + lead_y) / max(1e-6, sy) + lx = int(lx) + ly = int(ly) + + # Ромб-маркер + size = 10 + pts = np.array([ + [lx, ly - size], + [lx + size, ly], + [lx, ly + size], + [lx - size, ly], + ], dtype=np.int32) + cv2.polylines(frame, [pts], True, color, 2, cv2.LINE_AA) + + # Линия от текущей позиции к lead + cx = int(float(center_eff[0]) / max(1e-6, sx)) + cy = int(float(center_eff[1]) / max(1e-6, sy)) + cv2.line(frame, (cx, cy), (lx, ly), color, 1, cv2.LINE_AA) + + def _draw_lock_quality(self, frame, quality, ow, oh, color): + """ + Вертикальная полоска слева — качество захвата. + Зелёная = отличный лок, жёлтая = нормально, красная = теряем. + """ + bar_x = 12 + bar_y = oh // 4 + bar_h = oh // 2 + bar_w = 8 + + # Фон + cv2.rectangle(frame, (bar_x, bar_y), + (bar_x + bar_w, bar_y + bar_h), (40, 40, 40), -1) + + # Заполнение снизу вверх + fill_h = int(bar_h * quality) + fill_y = bar_y + bar_h - fill_h + + if quality >= 0.7: + fill_color = (0, 200, 0) + elif quality >= 0.4: + fill_color = (0, 200, 200) + else: + fill_color = (0, 0, 200) + + if fill_h > 0: + cv2.rectangle(frame, (bar_x, fill_y), + (bar_x + bar_w, bar_y + bar_h), fill_color, -1) + + # Рамка + cv2.rectangle(frame, (bar_x, bar_y), + (bar_x + bar_w, bar_y + bar_h), (120, 120, 120), 1) + + def _draw_speed_bar(self, frame, speed_px, ow, oh, color): + """ + Горизонтальная полоска внизу — скорость цели. + Помогает оператору предвидеть манёвры. + """ + bar_x = ow // 4 + bar_y = oh - 30 + bar_w = ow // 2 + bar_h = 6 + + cv2.rectangle(frame, (bar_x, bar_y), + (bar_x + bar_w, bar_y + bar_h), (40, 40, 40), -1) + + # Нормализуем скорость (0-200 px/s → 0-1) + norm_speed = float(clamp(speed_px / 200.0, 0.0, 1.0)) + fill_w = int(bar_w * norm_speed) + + if norm_speed < 0.3: + fill_color = (0, 200, 0) + elif norm_speed < 0.7: + fill_color = (0, 200, 200) + else: + fill_color = (0, 80, 255) + + if fill_w > 0: + cv2.rectangle(frame, (bar_x, bar_y), + (bar_x + fill_w, bar_y + bar_h), fill_color, -1) + + cv2.putText(frame, f"SPD", (bar_x - 40, bar_y + bar_h), + cv2.FONT_HERSHEY_SIMPLEX, 0.35, (120, 120, 120), 1) + + def _draw_range_tau(self, frame, hud_state, ow, oh, color): + """Дальность и time-to-contact — крупно, справа.""" + range_m = hud_state["range_m"] + tau = hud_state["tau"] + closing = hud_state["closing_vel"] + + x = ow - 200 + y = oh // 2 - 30 + + if HUD_SHOW_RANGE and range_m is not None: + if range_m < 10: + range_txt = f"{range_m:.1f}m" + elif range_m < 100: + range_txt = f"{range_m:.0f}m" + else: + range_txt = f"{range_m:.0f}m" + + font_scale = 1.0 if range_m < 20 else 0.75 + cv2.putText(frame, range_txt, (x, y), + cv2.FONT_HERSHEY_SIMPLEX, font_scale, color, 2) + y += 35 + + if HUD_SHOW_TAU and tau < 30.0: + if tau < 2.0: + tau_txt = f"{tau:.1f}s" + tau_color = (0, 0, 255) + font_scale = 1.2 + elif tau < 5.0: + tau_txt = f"{tau:.1f}s" + tau_color = (0, 80, 255) + font_scale = 0.9 + else: + tau_txt = f"{tau:.0f}s" + tau_color = color + font_scale = 0.75 + + cv2.putText(frame, tau_txt, (x, y), + cv2.FONT_HERSHEY_SIMPLEX, font_scale, tau_color, 2) + y += 30 + + if closing > 0.5: + cv2.putText(frame, f"Vc {closing:.0f}m/s", (x, y), + cv2.FONT_HERSHEY_SIMPLEX, 0.55, color, 1) + + def _draw_phase_banner(self, frame, phase, hud_state, ow, oh, color): + """Баннер фазы в верхнем правом углу.""" + labels = { + "SEARCH": "SEARCH", + "TRACK": "TRACK", + "APPROACH": "APPROACH", + "ATTACK": "ATTACK", + "TERMINAL": "COMMIT", + } + label = labels.get(phase, phase) + + (tw, th), _ = cv2.getTextSize(label, cv2.FONT_HERSHEY_SIMPLEX, 0.8, 2) + tx = ow - tw - 16 + ty = 35 + + # Фон + pad = 6 + cv2.rectangle(frame, (tx - pad, ty - th - pad), + (tx + tw + pad, ty + pad), (0, 0, 0), -1) + cv2.rectangle(frame, (tx - pad, ty - th - pad), + (tx + tw + pad, ty + pad), color, 2) + cv2.putText(frame, label, (tx, ty), + cv2.FONT_HERSHEY_SIMPLEX, 0.8, color, 2) + + def _draw_terminal_blink(self, frame, ow, oh): + """Мигающая красная рамка в TERMINAL фазе.""" + t = time.perf_counter() + cycle = int(t * HUD_TERMINAL_BLINK_RATE) % 2 + if cycle == 0: + thick = 4 + cv2.rectangle(frame, (0, 0), (ow - 1, oh - 1), (0, 0, 255), thick) + + def _draw_on_target(self, frame, ow, oh): + """Индикатор "цель в прицеле" — мигающий кружок.""" + cx, cy = ow // 2, oh // 2 + radius = int(float(ow) * HUD_KILL_ZONE_RADIUS_NORM) + + t = time.perf_counter() + if int(t * 6) % 2 == 0: + cv2.circle(frame, (cx, cy), radius, (0, 255, 0), 3) + cv2.putText(frame, "ON TARGET", (cx - 60, cy + radius + 25), + cv2.FONT_HERSHEY_SIMPLEX, 0.65, (0, 255, 0), 2) + + # ───────────────────────────────────────────────────────── + # INTERNAL + # ───────────────────────────────────────────────────────── + + def _compute_tau(self): + """Time-to-contact из скорости роста площади.""" + if len(self._area_hist) < 4: + return float('inf') + + times = np.array([t for t, _ in self._area_hist], dtype=np.float64) + areas = np.array([a for _, a in self._area_hist], dtype=np.float64) + + t0 = times[0] + times = times - t0 + span = times[-1] - times[0] + if span < 0.05: + return float('inf') + + # Линрег для dA/dt + n = len(times) + st = np.sum(times) + sa = np.sum(areas) + stt = np.sum(times * times) + sta = np.sum(times * areas) + denom = n * stt - st * st + if abs(denom) < 1e-12: + return float('inf') + + area_rate = (n * sta - st * sa) / denom + + if area_rate < HUD_TAU_MIN_AREA_RATE: + return float('inf') + + current = float(areas[-1]) + if current < 1.0: + return float('inf') + + return max(0.0, current / (2.0 * area_rate)) + + def _compute_closing_vel(self): + """Closing velocity из изменения дальности.""" + if len(self._area_hist) < 4 or self._range_m is None: + return 0.0 + + # Грубая оценка: из tau и range + if self._tau > 0.1 and self._tau < 100.0 and self._range_m is not None: + return float(self._range_m / self._tau) + return 0.0 + + def _classify_phase(self, confirmed, miss_streak): + """Определяет фазу для оператора.""" + if not confirmed or miss_streak > 10: + return "SEARCH" + + tau = self._tau + range_m = self._range_m + + if tau <= HUD_PHASE_TERMINAL_TAU: + return "TERMINAL" + if tau <= HUD_PHASE_ATTACK_TAU: + return "ATTACK" + if tau <= HUD_PHASE_APPROACH_TAU: + return "APPROACH" + + # Fallback по дальности + if range_m is not None: + if range_m < 8.0: + return "TERMINAL" + if range_m < 30.0: + return "ATTACK" + if range_m < 80.0: + return "APPROACH" + + return "TRACK" + + def _update_on_target(self, ref_box, ew, eh): + """Проверяет, находится ли цель в kill zone.""" + if ref_box is None: + self._on_target_count = 0 + self.on_target = False + return + + center = box_center(ref_box) + scr_cx = ew * 0.5 + scr_cy = eh * 0.5 + err = float(np.hypot(center[0] - scr_cx, center[1] - scr_cy)) + threshold = float(ew) * HUD_KILL_ZONE_RADIUS_NORM + + if err <= threshold: + self._on_target_count += 1 + else: + self._on_target_count = max(0, self._on_target_count - 1) + + self.on_target = (self._on_target_count >= HUD_ON_TARGET_MIN_FRAMES) + + def _compute_lock_quality(self, confirmed, miss_streak, hit_streak, + klt_quality): + """0..1 — интегральное качество захвата для оператора.""" + if not confirmed: + return 0.0 + + q = 0.0 + q += 0.35 * float(clamp(hit_streak / 10.0, 0.0, 1.0)) + q += 0.30 * float(clamp(klt_quality, 0.0, 1.0)) + q += 0.20 * float(clamp(1.0 - miss_streak / 8.0, 0.0, 1.0)) + q += 0.15 # бонус за confirmed + return float(clamp(q, 0.0, 1.0)) + + def _do_audio(self, frame_ts): + """Звуковой сигнал через системный beep.""" + if self.phase == "SEARCH" or self.phase == "TRACK": + return + + hz_map = { + "APPROACH": HUD_AUDIO_APPROACH_HZ, + "ATTACK": HUD_AUDIO_ATTACK_HZ, + "TERMINAL": HUD_AUDIO_TERMINAL_HZ, + } + beep_rate = hz_map.get(self.phase, 0.0) + if beep_rate <= 0: + return + + interval = 1.0 / beep_rate + if (frame_ts - self._last_beep_time) >= interval: + self._last_beep_time = frame_ts + try: + # Кроссплатформенный beep + import sys + if sys.platform == "win32": + import winsound + freq = 800 if self.phase == "TERMINAL" else 600 + winsound.Beep(freq, 30) + else: + # Linux: \a через stdout + sys.stdout.write('\a') + sys.stdout.flush() + except Exception: + pass + + +# ============================================================= +# ИНТЕГРАЦИЯ В MAIN.PY +# ============================================================= +# +# 1. Импорт: +# from operator_hud import OperatorHUD +# +# 2. Инициализация (после guidance_ctrl): +# hud = OperatorHUD() +# +# 3. В главном цикле, ПОСЛЕ guidance_state (перед отрисовкой): +# +# hud_state = hud.update( +# locked_box_eff=locked_box_eff, +# pred_box_eff=pred_box_eff, +# kf=kf, +# confirmed=confirmed, +# miss_streak=miss_streak, +# hit_streak=hit_streak, +# klt_quality=klt.quality, +# frame_ts=frame_ts, +# dt=dt, +# ew=ew, +# eh=eh, +# ) +# +# 4. Отрисовка (ПОСЛЕ существующего draw, перед writer.write): +# +# hud.draw( +# frame_orig, +# locked_box_eff=locked_box_eff, +# pred_box_eff=pred_box_eff, +# kf=kf, +# hud_state=hud_state, +# sx=sx, +# sy=sy, +# ) +# +# Всё. Четыре строки в main.py. +# ============================================================= diff --git a/proportional_navigation.py b/proportional_navigation.py new file mode 100644 index 0000000..b27c086 --- /dev/null +++ b/proportional_navigation.py @@ -0,0 +1,387 @@ +# ============================================================ +# proportional_navigation.py +# Модуль 2: Пропорциональная навигация (Proportional Navigation) +# +# Реализует: +# - Pure Proportional Navigation (PPN) +# - Augmented Proportional Navigation (APN) с компенсацией +# ускорения цели +# - Screen-space fallback (когда нет оценки дальности) +# - Вычисление точки перехвата (intercept point) +# ============================================================ + +import numpy as np +from collections import deque + +from config import * +from config_intercept import * +from helpers import clamp, box_center, box_wh + +import cv2 + + +class ProportionalNavigator: + """ + Proportional Navigation — классический закон наведения ракет. + + Принцип: ускорение перехватчика перпендикулярно линии визирования (LOS) + пропорционально скорости вращения LOS и скорости сближения: + + a_cmd = N * Vc * dλ/dt + + где N — навигационная константа (3-5), + Vc — closing velocity, + dλ/dt — скорость изменения угла LOS. + + Augmented PN добавляет компенсацию ускорения цели: + + a_cmd = N * Vc * dλ/dt + (N/2) * a_target_normal + + При отсутствии 3D-информации работает в screen-space + (2D аналог: стабилизировать положение цели в центре кадра). + """ + + def __init__(self): + self.enabled = bool(PN_ENABLE) + self.N = float(PN_NAV_GAIN) + self.augmented = bool(PN_AUGMENTED) + + # Состояние LOS + self._prev_los = None # предыдущий угол LOS (рад) + self._prev_los_t = None + self._los_rate_smooth = 0.0 # сглаженная скорость LOS (рад/с) + self._los_rate_raw = 0.0 + + # Состояние LOS по обеим осям (для 2D PN в screen space) + self._prev_los_x = None + self._prev_los_y = None + + # Для Augmented PN: оценка ускорения цели + self._prev_target_vel = None + self._prev_target_vel_t = None + self._target_accel = np.zeros(2, dtype=np.float32) + + # История для отладки + self._cmd_hist = deque(maxlen=60) + + # Выходы + self.accel_cmd = np.zeros(2, dtype=np.float32) # [ax, ay] нормализованные + self.intercept_point = None # предсказанная точка перехвата + self.intercept_time = None # время до перехвата (сек) + self.los_rate = 0.0 + self.active = False + + def reset(self): + self._prev_los = None + self._prev_los_t = None + self._los_rate_smooth = 0.0 + self._los_rate_raw = 0.0 + self._prev_los_x = None + self._prev_los_y = None + self._prev_target_vel = None + self._prev_target_vel_t = None + self._target_accel = np.zeros(2, dtype=np.float32) + self.accel_cmd = np.zeros(2, dtype=np.float32) + self.intercept_point = None + self.intercept_time = None + self.los_rate = 0.0 + self.active = False + self._cmd_hist.clear() + + def update_3d(self, my_pos, my_vel, target_pos, target_vel, + closing_vel, timestamp): + """ + 3D Proportional Navigation (используется при наличии оценки дальности). + + Args: + my_pos: np.array([x, y]) — позиция перехватчика (метры) + my_vel: np.array([vx, vy]) — скорость перехватчика (м/с) + target_pos: np.array([x, y]) — позиция цели (метры) + target_vel: np.array([vx, vy]) — скорость цели (м/с) + closing_vel: float — скорость сближения (м/с), > 0 = сближаемся + timestamp: float — текущее время + + Returns: + dict с полями accel_cmd, los_rate, intercept_point, intercept_time + """ + result = self._idle_result() + if not self.enabled: + return result + + r = target_pos - my_pos + range_val = float(np.linalg.norm(r)) + if range_val < 0.01: + return result + + # Угол LOS + los_angle = float(np.arctan2(r[1], r[0])) + + # Скорость LOS + if self._prev_los is not None and self._prev_los_t is not None: + dt = timestamp - self._prev_los_t + if dt > 1e-4: + # Обработка перехода через ±π + d_los = los_angle - self._prev_los + if d_los > np.pi: + d_los -= 2.0 * np.pi + elif d_los < -np.pi: + d_los += 2.0 * np.pi + + self._los_rate_raw = d_los / dt + + alpha = float(PN_LOS_RATE_SMOOTH) + self._los_rate_smooth = ( + alpha * self._los_rate_smooth + + (1.0 - alpha) * self._los_rate_raw + ) + + self._prev_los = los_angle + self._prev_los_t = timestamp + self.los_rate = self._los_rate_smooth + + # Closing velocity + vc = max(float(closing_vel), float(PN_MIN_CLOSING_VEL)) + + # ─── Pure PN ───────────────────────────────────────────── + a_pn = self.N * vc * self._los_rate_smooth + + # ─── Augmented PN ──────────────────────────────────────── + a_aug = 0.0 + if self.augmented: + target_vel_np = np.array(target_vel, dtype=np.float32) + self._estimate_target_accel(target_vel_np, timestamp) + + # Нормальная компонента ускорения цели (перп. к LOS) + if range_val > 0.1: + los_unit = r / range_val + los_perp = np.array([-los_unit[1], los_unit[0]], dtype=np.float32) + a_target_normal = float(np.dot(self._target_accel, los_perp)) + a_aug = float(PN_AUG_GAIN) * self.N * 0.5 * a_target_normal + + # Суммарная команда + a_total = a_pn + a_aug + + # Нормализация + max_cmd = float(PN_MAX_ACCEL_CMD) + a_norm = float(clamp(a_total / max(20.0, vc), -max_cmd, max_cmd)) + + # Преобразование в декартовы координаты (перп. к LOS) + los_perp_dir = np.array([-np.sin(los_angle), np.cos(los_angle)], + dtype=np.float32) + self.accel_cmd = a_norm * los_perp_dir + + # Точка перехвата + self.intercept_point, self.intercept_time = self._compute_intercept( + my_pos, my_vel, target_pos, target_vel + ) + + self.active = True + self._cmd_hist.append(float(a_norm)) + + result.update({ + "active": True, + "accel_x": float(self.accel_cmd[0]), + "accel_y": float(self.accel_cmd[1]), + "accel_mag": float(abs(a_norm)), + "los_rate": float(self._los_rate_smooth), + "los_angle_deg": float(np.degrees(los_angle)), + "closing_vel": float(closing_vel), + "range_m": float(range_val), + "intercept_x": float(self.intercept_point[0]) if self.intercept_point is not None else None, + "intercept_y": float(self.intercept_point[1]) if self.intercept_point is not None else None, + "intercept_time": self.intercept_time, + "a_pn": float(a_pn), + "a_aug": float(a_aug), + }) + return result + + def update_screen(self, target_center, frame_w, frame_h, + target_vel_px, timestamp): + """ + Screen-space PN fallback (без 3D-информации). + + Вместо 3D LOS используем углы от центра кадра к цели. + LOS rate вычисляется из скорости смещения цели по экрану. + + Args: + target_center: (cx, cy) — центр bbox цели в пикселях + frame_w, frame_h: размеры кадра + target_vel_px: (vx, vy) — скорость цели в пикселях/сек (из Kalman) + timestamp: текущее время + + Returns: + dict — команды наведения в screen space + """ + result = self._idle_result() + if not self.enabled or not PN_SCREEN_FALLBACK: + return result + if target_center is None: + return result + + cx, cy = float(target_center[0]), float(target_center[1]) + scr_cx = float(frame_w) * 0.5 + scr_cy = float(frame_h) * 0.5 + + # Нормализованная ошибка (как LOS angle в screen space) + los_x = (cx - scr_cx) / max(1.0, scr_cx) + los_y = (cy - scr_cy) / max(1.0, scr_cy) + + # LOS rate по каждой оси + los_rate_x = 0.0 + los_rate_y = 0.0 + if self._prev_los_x is not None and self._prev_los_t is not None: + dt = timestamp - self._prev_los_t + if dt > 1e-4: + raw_x = (los_x - self._prev_los_x) / dt + raw_y = (los_y - self._prev_los_y) / dt + alpha = float(PN_LOS_RATE_SMOOTH) + los_rate_x = alpha * los_rate_x + (1.0 - alpha) * raw_x + los_rate_y = alpha * los_rate_y + (1.0 - alpha) * raw_y + + self._prev_los_x = los_x + self._prev_los_y = los_y + self._prev_los_t = timestamp + + self.los_rate = float(np.hypot(los_rate_x, los_rate_y)) + + # Screen-space "closing velocity" = 1 (нет 3D) + # Команда = N * los_rate (упрощённый PN) + cmd_x = float(clamp(self.N * los_rate_x + 0.8 * los_x, + -PN_MAX_ACCEL_CMD, PN_MAX_ACCEL_CMD)) + cmd_y = float(clamp(self.N * los_rate_y + 0.8 * los_y, + -PN_MAX_ACCEL_CMD, PN_MAX_ACCEL_CMD)) + + # Lead: добавляем упреждение по скорости цели + if target_vel_px is not None: + vx_n = float(target_vel_px[0]) / max(1.0, scr_cx) + vy_n = float(target_vel_px[1]) / max(1.0, scr_cy) + lead_t = float(PN_LEAD_TIME_SEC) + cmd_x += 0.3 * vx_n * lead_t + cmd_y += 0.3 * vy_n * lead_t + + cmd_x = float(clamp(cmd_x, -PN_MAX_ACCEL_CMD, PN_MAX_ACCEL_CMD)) + cmd_y = float(clamp(cmd_y, -PN_MAX_ACCEL_CMD, PN_MAX_ACCEL_CMD)) + + self.accel_cmd = np.array([cmd_x, cmd_y], dtype=np.float32) + self.active = True + + result.update({ + "active": True, + "accel_x": cmd_x, + "accel_y": cmd_y, + "accel_mag": float(np.hypot(cmd_x, cmd_y)), + "los_rate": self.los_rate, + "los_angle_deg": float(np.degrees(np.arctan2(los_y, los_x))), + "closing_vel": 0.0, + "range_m": None, + "intercept_x": None, + "intercept_y": None, + "intercept_time": None, + "mode": "screen", + }) + return result + + # ─── Private ───────────────────────────────────────────────── + + def _estimate_target_accel(self, target_vel, timestamp): + """Оценка ускорения цели из разности скоростей.""" + if self._prev_target_vel is not None and self._prev_target_vel_t is not None: + dt = timestamp - self._prev_target_vel_t + if dt > 1e-4: + raw_accel = (target_vel - self._prev_target_vel) / dt + alpha = 0.6 + self._target_accel = ( + alpha * self._target_accel + + (1.0 - alpha) * raw_accel + ).astype(np.float32) + + self._prev_target_vel = target_vel.copy() + self._prev_target_vel_t = timestamp + + def _compute_intercept(self, my_pos, my_vel, target_pos, target_vel, + max_iter=12): + """ + Итеративное вычисление точки перехвата. + + Находит время t, при котором перехватчик, летящий со своей скоростью, + достигает предсказанной позиции цели (target_pos + target_vel * t). + """ + my_speed = float(np.linalg.norm(my_vel)) + if my_speed < 0.1: + my_speed = 10.0 # fallback + + t = float(np.linalg.norm(target_pos - my_pos)) / max(my_speed, 0.1) + + for _ in range(max_iter): + predicted = target_pos + target_vel * t + dist = float(np.linalg.norm(predicted - my_pos)) + t_new = dist / max(my_speed, 0.1) + + if abs(t_new - t) < 0.01: + break + t = 0.5 * t + 0.5 * t_new # демпфированная итерация + + intercept = target_pos + target_vel * t + return intercept.astype(np.float32), float(t) + + @staticmethod + def _idle_result(): + return { + "active": False, + "accel_x": 0.0, + "accel_y": 0.0, + "accel_mag": 0.0, + "los_rate": 0.0, + "los_angle_deg": 0.0, + "closing_vel": 0.0, + "range_m": None, + "intercept_x": None, + "intercept_y": None, + "intercept_time": None, + } + + def status_line(self): + if not self.enabled: + return "Proportional Navigation disabled" + mode = "APN" if self.augmented else "PPN" + return f"PN ready: N={self.N:.1f} mode={mode}" + + def draw_overlay(self, frame_bgr, state, sx, sy): + """Рисует вектор наведения и точку перехвата.""" + if not self.enabled or not DRAW_PN_VECTOR: + return + if not state.get("active", False): + return + + h, w = frame_bgr.shape[:2] + cx = int(w * 0.5) + cy = int(h * 0.5) + + # Вектор команды + ax = float(state.get("accel_x", 0.0)) + ay = float(state.get("accel_y", 0.0)) + scale = 80.0 + ex = int(cx + ax * scale) + ey = int(cy + ay * scale) + + color = (0, 255, 128) if state.get("active") else (100, 100, 100) + cv2.arrowedLine(frame_bgr, (cx, cy), (ex, ey), color, 2, cv2.LINE_AA) + + # Точка перехвата + ix = state.get("intercept_x") + iy = state.get("intercept_y") + if ix is not None and iy is not None and DRAW_INTERCEPT_POINT: + ipx = int(float(ix) / max(1e-6, float(sx))) + ipy = int(float(iy) / max(1e-6, float(sy))) + cv2.drawMarker(frame_bgr, (ipx, ipy), (0, 255, 128), + cv2.MARKER_TILTED_CROSS, 14, 2) + + # Текст + los_r = state.get("los_rate", 0.0) + a_mag = state.get("accel_mag", 0.0) + it = state.get("intercept_time") + txt = f"PN: a={a_mag:.2f} LOS_r={los_r:.3f}" + if it is not None: + txt += f" t_int={it:.2f}s" + cv2.putText(frame_bgr, txt, (20, 260), + cv2.FONT_HERSHEY_SIMPLEX, 0.55, color, 2) diff --git a/range_estimation.py b/range_estimation.py new file mode 100644 index 0000000..3dcd1c9 --- /dev/null +++ b/range_estimation.py @@ -0,0 +1,343 @@ +# ============================================================ +# range_estimation.py +# Модуль 1: Оценка дальности до цели и time-to-contact (tau) +# +# Методы: +# 1) Монокулярная оценка по известному размеру цели +# 2) Tau — время до столкновения по скорости роста bbox +# 3) Скорость сближения (closing velocity) из производной range +# ============================================================ + +import numpy as np +from collections import deque + +from config import * +from config_intercept import * +from helpers import box_wh, box_area, box_center, clamp + + +class RangeEstimator: + """ + Оценка дальности до цели по размеру bounding box в монокулярной камере. + + Принцип: range = (known_size * focal_length) / apparent_size + Дополнительно вычисляет tau (time-to-contact) по скорости изменения + площади bbox, что не требует калибровки и даёт прямую оценку + времени до столкновения. + """ + + def __init__(self): + self.enabled = bool(RANGE_ENABLE) + + # Калибровка + self.focal_px = float(CAMERA_FOCAL_LENGTH_PX) + self.target_size_m = float(TARGET_KNOWN_SIZE_M) + self.min_apparent = float(RANGE_MIN_APPARENT_SIZE_PX) + + # Сглаженные оценки + self.range_m = None # текущая оценка дальности (метры) + self.range_raw = None # без сглаживания + self.closing_vel = 0.0 # скорость сближения (м/с), >0 = сближаемся + self.tau = float('inf') # time-to-contact (секунды) + self.tau_raw = float('inf') + + # Внутреннее состояние + self._prev_range = None + self._prev_area = None + self._prev_t = None + self._range_alpha = float(RANGE_SMOOTH_ALPHA) + self._tau_alpha = float(TAU_SMOOTH_ALPHA) + + # История для производных (скользящее окно) + self._area_hist = deque(maxlen=8) # (timestamp, area) + self._range_hist = deque(maxlen=8) # (timestamp, range) + + # Confidence: насколько доверяем оценке + self.confidence = 0.0 + + def reset(self): + """Полный сброс при потере цели.""" + self.range_m = None + self.range_raw = None + self.closing_vel = 0.0 + self.tau = float('inf') + self.tau_raw = float('inf') + self._prev_range = None + self._prev_area = None + self._prev_t = None + self._area_hist.clear() + self._range_hist.clear() + self.confidence = 0.0 + + def update(self, box, timestamp, dt, confirmed=False): + """ + Обновить оценку дальности и tau. + + Args: + box: bounding box цели [x1, y1, x2, y2] в effective pixels + timestamp: текущее время (секунды) + dt: дельта времени с предыдущего кадра + confirmed: цель подтверждена трекером + + Returns: + dict с полями: + range_m — дальность (метры) или None + range_raw — дальность без сглаживания + closing_vel — скорость сближения (м/с), >0 = сближаемся + tau — time-to-contact (сек), inf = не сближаемся + tau_raw — без сглаживания + confidence — 0..1 + phase — "far" | "approach" | "warn" | "critical" + """ + result = self._idle_result() + + if not self.enabled or box is None: + return result + + w, h = box_wh(box) + apparent = float(max(w, h)) + area = float(w * h) + + if apparent < self.min_apparent: + return result + + # ─── 1. Дальность по размеру ──────────────────────────── + range_raw = self._range_from_size(apparent) + range_raw = float(clamp(range_raw, RANGE_MIN_M, RANGE_MAX_M)) + self.range_raw = range_raw + + if self.range_m is None: + self.range_m = range_raw + else: + alpha = self._range_alpha + self.range_m = alpha * self.range_m + (1.0 - alpha) * range_raw + + # ─── 2. Closing velocity из dRange/dt ──────────────────── + self._range_hist.append((timestamp, range_raw)) + self.closing_vel = self._compute_closing_vel() + + # ─── 3. Tau из скорости изменения площади ──────────────── + self._area_hist.append((timestamp, area)) + tau_raw = self._compute_tau_from_area() + self.tau_raw = tau_raw + + if self.tau == float('inf'): + self.tau = tau_raw + else: + if tau_raw == float('inf'): + self.tau = self.tau * 1.1 # медленный fade к inf + else: + alpha = self._tau_alpha + self.tau = alpha * self.tau + (1.0 - alpha) * tau_raw + self.tau = max(0.0, self.tau) + + # ─── 4. Confidence ─────────────────────────────────────── + self.confidence = self._compute_confidence(apparent, confirmed) + + # ─── 5. Phase ──────────────────────────────────────────── + phase = self._classify_phase() + + self._prev_range = range_raw + self._prev_area = area + self._prev_t = timestamp + + result.update({ + "range_m": float(self.range_m), + "range_raw": float(self.range_raw), + "closing_vel": float(self.closing_vel), + "tau": float(self.tau), + "tau_raw": float(self.tau_raw), + "confidence": float(self.confidence), + "phase": phase, + "apparent_size_px": float(apparent), + "area_px": float(area), + }) + return result + + # ─── Private ───────────────────────────────────────────────── + + def _range_from_size(self, apparent_px): + """range = (real_size * focal_length) / apparent_size""" + if apparent_px < 1e-3: + return float(RANGE_MAX_M) + return (self.target_size_m * self.focal_px) / apparent_px + + def _compute_closing_vel(self): + """ + Скорость сближения из изменения дальности. + Положительная = сближаемся, отрицательная = удаляемся. + Использует линейную регрессию по последним точкам. + """ + if len(self._range_hist) < 3: + return 0.0 + + times = np.array([t for t, _ in self._range_hist], dtype=np.float64) + ranges = np.array([r for _, r in self._range_hist], dtype=np.float64) + + # Нормализация времени для числовой устойчивости + t0 = times[0] + times = times - t0 + dt_span = times[-1] - times[0] + if dt_span < 0.02: + return 0.0 + + # Линейная регрессия: range = a*t + b → closing_vel = -a + n = len(times) + sum_t = np.sum(times) + sum_r = np.sum(ranges) + sum_tt = np.sum(times * times) + sum_tr = np.sum(times * ranges) + + denom = n * sum_tt - sum_t * sum_t + if abs(denom) < 1e-12: + return 0.0 + + a = (n * sum_tr - sum_t * sum_r) / denom + return float(-a) # отрицательный наклон = сближение + + def _compute_tau_from_area(self): + """ + Time-to-contact из скорости роста площади bbox. + tau = area / (2 * dArea/dt) + + Физика: для объекта приближающегося со скоростью V на расстоянии R, + площадь изображения ∝ 1/R², поэтому dA/dt ∝ 2V/R³, + и tau ≈ A / (2 * dA/dt). + """ + if len(self._area_hist) < 3: + return float('inf') + + times = np.array([t for t, _ in self._area_hist], dtype=np.float64) + areas = np.array([a for _, a in self._area_hist], dtype=np.float64) + + t0 = times[0] + times = times - t0 + dt_span = times[-1] - times[0] + if dt_span < 0.02: + return float('inf') + + # Линейная регрессия для dArea/dt + n = len(times) + sum_t = np.sum(times) + sum_a = np.sum(areas) + sum_tt = np.sum(times * times) + sum_ta = np.sum(times * areas) + + denom = n * sum_tt - sum_t * sum_t + if abs(denom) < 1e-12: + return float('inf') + + area_rate = (n * sum_ta - sum_t * sum_a) / denom # dA/dt + + if area_rate < float(TAU_MIN_AREA_RATE): + return float('inf') # не сближаемся или слишком медленно + + current_area = float(areas[-1]) + if current_area < 1.0: + return float('inf') + + tau = current_area / (2.0 * area_rate) + return max(0.0, float(tau)) + + def _compute_confidence(self, apparent_px, confirmed): + """Confidence оценки дальности: 0..1""" + conf = 0.0 + + # Чем больше цель на экране — тем точнее оценка + size_conf = float(clamp((apparent_px - 4.0) / 50.0, 0.0, 0.4)) + conf += size_conf + + # Если трек подтверждён + if confirmed: + conf += 0.3 + + # Если есть достаточная история + hist_conf = float(clamp(len(self._range_hist) / 6.0, 0.0, 0.2)) + conf += hist_conf + + # Стабильность оценки (мало шума) + if len(self._range_hist) >= 4: + ranges = [r for _, r in self._range_hist] + std = float(np.std(ranges)) + mean = float(np.mean(ranges)) + if mean > 0.1: + cv = std / mean # coefficient of variation + stability = float(clamp(1.0 - cv * 5.0, 0.0, 0.1)) + conf += stability + + return float(clamp(conf, 0.0, 1.0)) + + def _classify_phase(self): + """Фаза сближения по дальности и tau.""" + tau = self.tau if self.tau != float('inf') else 999.0 + range_m = self.range_m if self.range_m is not None else 999.0 + + if tau <= float(TAU_CRITICAL_SEC) or range_m <= float(FSM_TERMINAL_RANGE_M): + return "critical" + if tau <= float(TAU_WARN_SEC) or range_m <= float(FSM_INTERCEPT_RANGE_M): + return "warn" + if range_m <= float(FSM_ACQUIRE_RANGE_M): + return "approach" + return "far" + + @staticmethod + def _idle_result(): + return { + "range_m": None, + "range_raw": None, + "closing_vel": 0.0, + "tau": float('inf'), + "tau_raw": float('inf'), + "confidence": 0.0, + "phase": "far", + "apparent_size_px": 0.0, + "area_px": 0.0, + } + + def status_line(self): + if not self.enabled: + return "Range estimation disabled" + return ( + f"Range estimation ready: focal={self.focal_px:.0f}px " + f"target_size={self.target_size_m:.2f}m" + ) + + def draw_overlay(self, frame_bgr, state, sx, sy): + """Рисует информацию о дальности на кадре.""" + if not self.enabled or not DRAW_RANGE_INFO: + return + + import cv2 + + range_m = state.get("range_m") + tau = state.get("tau", float('inf')) + closing = state.get("closing_vel", 0.0) + phase = state.get("phase", "far") + conf = state.get("confidence", 0.0) + + # Цвет по фазе + colors = { + "far": (180, 180, 180), + "approach": (0, 220, 255), + "warn": (0, 165, 255), + "critical": (0, 0, 255), + } + color = colors.get(phase, (180, 180, 180)) + + y0 = 200 + lines = [] + + if range_m is not None: + lines.append(f"RNG: {range_m:.1f}m Vc={closing:+.1f}m/s") + + if tau < 100.0: + lines.append(f"TAU: {tau:.2f}s phase={phase} conf={conf:.2f}") + else: + lines.append(f"TAU: --- phase={phase} conf={conf:.2f}") + + for i, line in enumerate(lines): + cv2.putText( + frame_bgr, line, + (20, y0 + i * 28), + cv2.FONT_HERSHEY_SIMPLEX, 0.60, color, 2, + ) diff --git a/requirements-docker.txt b/requirements-docker.txt new file mode 100644 index 0000000..6a625a3 --- /dev/null +++ b/requirements-docker.txt @@ -0,0 +1,5 @@ +numpy==1.26.4 +opencv-python==4.10.0.84 +ultralytics==8.4.75 +pymavlink==2.4.49 +pyserial==3.5 diff --git a/runtime_env.py b/runtime_env.py new file mode 100644 index 0000000..ced9c3c --- /dev/null +++ b/runtime_env.py @@ -0,0 +1,63 @@ +import os + + +def _parse_bool(value): + if isinstance(value, bool): + return value + text = str(value).strip().lower() + return text in {"1", "true", "yes", "on"} + + +def _parse_int(value): + return int(str(value).strip()) + + +def _parse_str(value): + return str(value) + + +def _parse_source(value): + text = str(value).strip() + if text and text.lstrip("+-").isdigit(): + return int(text) + return text + + +def _apply_env_overrides(namespace, spec): + for env_name, (key, parser) in spec.items(): + if env_name not in os.environ: + continue + namespace[key] = parser(os.environ[env_name]) + + +def apply_config_env_overrides(namespace): + _apply_env_overrides( + namespace, + { + "FPV_MODEL_PATH": ("MODEL_PATH", _parse_str), + "FPV_SOURCE": ("SOURCE", _parse_source), + "FPV_VIDEO_REALTIME": ("VIDEO_REALTIME", _parse_bool), + "FPV_SHOW_OUTPUT": ("SHOW_OUTPUT", _parse_bool), + "FPV_SAVE_INFER_VIDEO": ("SAVE_INFER_VIDEO", _parse_bool), + "FPV_OUT_VIDEO_PATH": ("OUT_VIDEO_PATH", _parse_str), + "FPV_GUIDANCE_EXPORT_ENABLE": ("GUIDANCE_EXPORT_ENABLE", _parse_bool), + "FPV_GUIDANCE_EXPORT_PATH": ("GUIDANCE_EXPORT_PATH", _parse_str), + }, + ) + + +def apply_intercept_env_overrides(namespace): + _apply_env_overrides( + namespace, + { + "FPV_AUTOPILOT_ENABLE": ("AUTOPILOT_ENABLE", _parse_bool), + "FPV_AUTOPILOT_BACKEND": ("AUTOPILOT_BACKEND", _parse_str), + "FPV_AUTOPILOT_JSON_PATH": ("AUTOPILOT_JSON_PATH", _parse_str), + "FPV_MAVLINK_CONNECTION": ("MAVLINK_CONNECTION", _parse_str), + "FPV_MSP_PORT": ("MSP_PORT", _parse_str), + "FPV_PROTO_UDP_ENABLE": ("PROTO_UDP_ENABLE", _parse_bool), + "FPV_PROTO_UDP_HOST": ("PROTO_UDP_HOST", _parse_str), + "FPV_PROTO_UDP_PORT": ("PROTO_UDP_PORT", _parse_int), + "FPV_PROTO_UDP_DESCRIPTOR": ("PROTO_UDP_DESCRIPTOR", _parse_int), + }, + ) diff --git a/stationary_killer.py b/stationary_killer.py new file mode 100644 index 0000000..e968543 --- /dev/null +++ b/stationary_killer.py @@ -0,0 +1,133 @@ +# ============================================================ +# stationary_killer.py +# Детектор прилипших треков на статичных объектах сцены. +# Защита от track hijacking на: +# - снежных фичах / следах / кустах +# - линии горизонта +# - ЛЭП / деревьях / ветках +# - отражениях от воды +# +# Работает как независимый "полицейский": следит за +# движением центра bbox'а confirmed трека и motion saliency +# score в его зоне. Если трек не движется И не даёт motion +# response — форсирует сброс target. +# ============================================================ + +from collections import deque +import numpy as np + + +# ─── Параметры детектора ───────────────────────────────────── +STAT_WINDOW_FRAMES = 45 # окно истории (~0.75s при 60fps) +STAT_MAX_CENTER_SHIFT = 12.0 # макс сдвиг центра bbox в пикселях +STAT_MAX_MOTION_SCORE = 0.12 # порог motion saliency +STAT_MIN_CONFIRMED_F = 20 # не трогать трек моложе этого +STAT_GRACE_PERIOD = 15 # после kill — не убивать N кадров + +# Для нижней половины кадра (parallax даёт false motion) +# пороги мягче — там движение фона более заметно +STAT_BOTTOM_FRAC = 0.55 +STAT_LOW_MAX_CENTER_SHIFT = 18.0 +STAT_LOW_MAX_MOTION = 0.20 + + +class StationaryKiller: + """ + Следит за движением confirmed трека и сигнализирует когда + трек "прилип" к статичной фиче (снег, горизонт, дерево). + + Работает независимо от ByteTrack / KLT / Kalman — просто + смотрит на историю locked_box_eff и motion_sal score. + """ + + def __init__(self): + self.target_id = None + self.history = deque(maxlen=STAT_WINDOW_FRAMES) + # history items: (frame_id, cx, cy, motion_score, frame_h) + self.confirmed_since = -1 + self.grace_until = -1 + self.kills_total = 0 + self.last_kill_reason = "" + + def reset(self): + self.target_id = None + self.history.clear() + self.confirmed_since = -1 + + def notify_kill_done(self, frame_id): + """Вызывается main.py после force reset.""" + self.kills_total += 1 + self.grace_until = int(frame_id) + int(STAT_GRACE_PERIOD) + self.reset() + + def update(self, frame_id, target_id, locked_box_eff, + motion_sal, confirmed, frame_h): + """Обновить историю трека. Вызывать каждый кадр.""" + if (not confirmed) or (target_id is None) or (locked_box_eff is None): + if self.target_id is not None: + self.reset() + return + + if target_id != self.target_id: + self.reset() + self.target_id = target_id + self.confirmed_since = int(frame_id) + + cx = 0.5 * (float(locked_box_eff[0]) + float(locked_box_eff[2])) + cy = 0.5 * (float(locked_box_eff[1]) + float(locked_box_eff[3])) + + if motion_sal is not None and motion_sal.saliency is not None: + ms_score = float(motion_sal.score_box(locked_box_eff)) + else: + ms_score = 0.5 # нет saliency — не штрафуем + + self.history.append((int(frame_id), cx, cy, ms_score, float(frame_h))) + + def should_kill(self, frame_id): + """Проверить нужно ли сбросить текущий target.""" + if int(frame_id) < int(self.grace_until): + return False + if self.target_id is None: + return False + if len(self.history) < int(STAT_WINDOW_FRAMES): + return False + if (int(frame_id) - int(self.confirmed_since)) < int(STAT_MIN_CONFIRMED_F): + return False + + cx0 = self.history[0][1] + cy0 = self.history[0][2] + max_shift = 0.0 + motion_scores = [] + frame_h = self.history[-1][4] + for _, cx, cy, ms, _fh in self.history: + s = float(np.hypot(cx - cx0, cy - cy0)) + if s > max_shift: + max_shift = s + motion_scores.append(ms) + + avg_motion = float(np.mean(motion_scores)) if motion_scores else 1.0 + last_cy = self.history[-1][2] + is_low_zone = last_cy >= float(frame_h) * float(STAT_BOTTOM_FRAC) + + if is_low_zone: + shift_lim = float(STAT_LOW_MAX_CENTER_SHIFT) + motion_lim = float(STAT_LOW_MAX_MOTION) + else: + shift_lim = float(STAT_MAX_CENTER_SHIFT) + motion_lim = float(STAT_MAX_MOTION_SCORE) + + if max_shift <= shift_lim and avg_motion <= motion_lim: + zone = "low" if is_low_zone else "normal" + self.last_kill_reason = ( + f"stationary:{zone} shift={max_shift:.1f}/{shift_lim:.0f} " + f"motion={avg_motion:.3f}/{motion_lim:.2f}" + ) + return True + return False + + def status_line(self): + return ( + f"StationaryKiller: monitoring tid={self.target_id} " + f"history={len(self.history)}/{STAT_WINDOW_FRAMES} " + f"kills={self.kills_total}" + ) diff --git a/target_handoff.py b/target_handoff.py new file mode 100644 index 0000000..80ffeea --- /dev/null +++ b/target_handoff.py @@ -0,0 +1,119 @@ +from dataclasses import dataclass + + +@dataclass(frozen=True) +class HandoffDecision: + stale_lock_active: bool + allow_fast_handoff: bool + required_switch_hits: int + disable_prediction_hold: bool + reasons: list[str] + + def reason_text(self) -> str: + return "|".join(self.reasons) + + +def evaluate_stale_lock( + *, + confirmed, + have_fresh_candidate, + target_has_fresh_update, + candidate_dist_px, + pred_diag_px, + miss_streak, + klt_valid, + klt_quality, + klt_iou, + candidate_motion_ok, + target_motion_ok, + stale_break_dist_diag, + stale_break_min_miss, + stale_break_klt_quality, + stale_break_klt_iou, +): + reasons = [] + if not confirmed or not have_fresh_candidate: + return HandoffDecision(False, False, 0, False, []) + + far_enough = candidate_dist_px >= max(40.0, stale_break_dist_diag * max(pred_diag_px, 1.0)) + weak_anchor = ( + (not klt_valid) + or (klt_quality < stale_break_klt_quality) + or (klt_iou < stale_break_klt_iou) + or (miss_streak >= stale_break_min_miss) + or (candidate_motion_ok and not target_motion_ok) + or (not target_has_fresh_update) + ) + + if not target_has_fresh_update: + reasons.append("target_not_fresh") + if not klt_valid: + reasons.append("klt_invalid") + if klt_quality < stale_break_klt_quality: + reasons.append("weak_klt_quality") + if klt_iou < stale_break_klt_iou: + reasons.append("weak_klt_iou") + if miss_streak >= stale_break_min_miss: + reasons.append("miss_streak") + if candidate_motion_ok and not target_motion_ok: + reasons.append("motion_conflict") + + stale = bool(far_enough and weak_anchor) + return HandoffDecision( + stale_lock_active=stale, + allow_fast_handoff=stale, + required_switch_hits=0, + disable_prediction_hold=stale, + reasons=reasons if stale else [], + ) + + +def compute_fast_handoff_hits( + *, + stale_lock_active, + motion_switch_mode, + approach_extra_hits, + fast_maneuver_extra_hits, + default_switch_hits, + fast_handoff_hits, + motion_switch_hits, +): + if stale_lock_active: + return int(fast_handoff_hits) + + hits = int(default_switch_hits) + int(approach_extra_hits) + int(fast_maneuver_extra_hits) + if motion_switch_mode: + hits = max(hits, int(motion_switch_hits)) + return int(hits) + + +def should_override_guidance( + *, + confirmed, + target_track_missing, + have_fresh_candidate, + candidate_matches_target, + miss_streak, + override_miss_ge, +): + return bool( + confirmed + and target_track_missing + and have_fresh_candidate + and (not candidate_matches_target) + and (int(miss_streak) >= int(override_miss_ge)) + ) + + +def pick_guidance_override_box(*, track_candidate_box, raw_yolo_box): + if track_candidate_box is not None: + return track_candidate_box + return raw_yolo_box + + +def update_guidance_override_latch(*, new_box, prev_box, prev_ttl, max_ttl): + if new_box is not None: + return new_box, int(max(0, max_ttl)) + if prev_box is not None and int(prev_ttl) > 0: + return prev_box, int(prev_ttl) - 1 + return None, 0 diff --git a/template_matching.py b/template_matching.py new file mode 100644 index 0000000..ae91aad --- /dev/null +++ b/template_matching.py @@ -0,0 +1,60 @@ +import cv2 + +from config import * +from helpers import clamp, box_center, box_wh + +# Template matching fallback +# ========================= + +def _crop_safe(img, x1, y1, x2, y2): + h, w = img.shape[:2] + x1 = int(clamp(x1, 0, w - 1)) + y1 = int(clamp(y1, 0, h - 1)) + x2 = int(clamp(x2, 0, w)) + y2 = int(clamp(y2, 0, h)) + if x2 <= x1 + 1 or y2 <= y1 + 1: + return None + return img[y1:y2, x1:x2] + + +def tm_update_template(gray_eff, box_eff): + if gray_eff is None or box_eff is None: + return None + cx, cy = box_center(box_eff) + half = TM_TEMPLATE_SIZE // 2 + patch = _crop_safe(gray_eff, cx - half, cy - half, cx + half, cy + half) + if patch is None: + return None + if patch.shape[0] < 12 or patch.shape[1] < 12: + return None + return patch.copy() + + +def tm_search(gray_eff, template, pred_center, miss_streak): + if (not TM_ENABLE) or gray_eff is None or template is None or pred_center is None: + return None + h, w = gray_eff.shape[:2] + cx, cy = float(pred_center[0]), float(pred_center[1]) + scale = TM_SEARCH_SCALE_BASE + TM_SEARCH_SCALE_PER_MISS * float(miss_streak) + th, tw = template.shape[:2] + win_w = int(max(tw * scale, tw + 20)) + win_h = int(max(th * scale, th + 20)) + x1 = int(cx - win_w / 2) + y1 = int(cy - win_h / 2) + x2 = int(cx + win_w / 2) + y2 = int(cy + win_h / 2) + roi = _crop_safe(gray_eff, x1, y1, x2, y2) + if roi is None or roi.shape[0] < th + 2 or roi.shape[1] < tw + 2: + return None + res = cv2.matchTemplate(roi, template, cv2.TM_CCOEFF_NORMED) + _, maxv, _, maxl = cv2.minMaxLoc(res) + if maxv < TM_MIN_SCORE: + return None + bx = x1 + maxl[0] + by = y1 + maxl[1] + mcx = bx + tw / 2 + mcy = by + th / 2 + return (mcx, mcy, float(maxv), tw, th) + + +# ========================= diff --git a/tests/__pycache__/test_autopilot_proto_udp.cpython-312-pytest-9.0.3.pyc b/tests/__pycache__/test_autopilot_proto_udp.cpython-312-pytest-9.0.3.pyc new file mode 100644 index 0000000..5b1fe02 Binary files /dev/null and b/tests/__pycache__/test_autopilot_proto_udp.cpython-312-pytest-9.0.3.pyc differ diff --git a/tests/__pycache__/test_decision_logger_events.cpython-312-pytest-9.0.3.pyc b/tests/__pycache__/test_decision_logger_events.cpython-312-pytest-9.0.3.pyc new file mode 100644 index 0000000..5c07759 Binary files /dev/null and b/tests/__pycache__/test_decision_logger_events.cpython-312-pytest-9.0.3.pyc differ diff --git a/tests/__pycache__/test_guidance_reset.cpython-312-pytest-9.0.3.pyc b/tests/__pycache__/test_guidance_reset.cpython-312-pytest-9.0.3.pyc new file mode 100644 index 0000000..511c3ee Binary files /dev/null and b/tests/__pycache__/test_guidance_reset.cpython-312-pytest-9.0.3.pyc differ diff --git a/tests/__pycache__/test_local_bytetrack_module.cpython-312-pytest-9.0.3.pyc b/tests/__pycache__/test_local_bytetrack_module.cpython-312-pytest-9.0.3.pyc new file mode 100644 index 0000000..a10dd12 Binary files /dev/null and b/tests/__pycache__/test_local_bytetrack_module.cpython-312-pytest-9.0.3.pyc differ diff --git a/tests/__pycache__/test_runtime_env.cpython-312-pytest-9.0.3.pyc b/tests/__pycache__/test_runtime_env.cpython-312-pytest-9.0.3.pyc new file mode 100644 index 0000000..925bb6f Binary files /dev/null and b/tests/__pycache__/test_runtime_env.cpython-312-pytest-9.0.3.pyc differ diff --git a/tests/__pycache__/test_target_handoff.cpython-312-pytest-9.0.3.pyc b/tests/__pycache__/test_target_handoff.cpython-312-pytest-9.0.3.pyc new file mode 100644 index 0000000..8900abf Binary files /dev/null and b/tests/__pycache__/test_target_handoff.cpython-312-pytest-9.0.3.pyc differ diff --git a/tests/__pycache__/test_track_score_gates.cpython-312-pytest-9.0.3.pyc b/tests/__pycache__/test_track_score_gates.cpython-312-pytest-9.0.3.pyc new file mode 100644 index 0000000..c6cbd07 Binary files /dev/null and b/tests/__pycache__/test_track_score_gates.cpython-312-pytest-9.0.3.pyc differ diff --git a/tests/test_autopilot_proto_udp.py b/tests/test_autopilot_proto_udp.py new file mode 100644 index 0000000..1419158 --- /dev/null +++ b/tests/test_autopilot_proto_udp.py @@ -0,0 +1,63 @@ +from pathlib import Path +import struct +import sys + + +PROJECT_ROOT = Path(__file__).resolve().parents[1] +if str(PROJECT_ROOT) not in sys.path: + sys.path.insert(0, str(PROJECT_ROOT)) + +from autopilot_bridge import ( + AutopilotCommand, + build_proto_udp_payload, + select_proto_udp_target_state, +) + + +def test_select_proto_udp_target_state_uses_tracker_state_when_detection_is_lost(): + assert select_proto_udp_target_state(confirmed=False, miss_streak=0) == 0 + assert select_proto_udp_target_state(confirmed=True, miss_streak=0) == 1 + assert select_proto_udp_target_state(confirmed=True, miss_streak=4) == 3 + + +def test_build_proto_udp_payload_encodes_offsets_and_bbox_area(): + cmd = AutopilotCommand() + cmd.target_id = 7 + cmd.target_state = 1 + cmd.frame_w = 1000 + cmd.frame_h = 500 + cmd.aim_x = 650.0 + cmd.aim_y = 150.0 + cmd.bbox_w = 200.0 + cmd.bbox_h = 50.0 + + payload = build_proto_udp_payload(cmd) + unpacked = struct.unpack(" acquire_floor + + +def test_weak_reacquire_uses_reacquire_floor(): + reacquire_floor = required_track_score( + confirmed=True, + is_switch_candidate=False, + weak_reacq_guard=True, + ) + + assert track_passes_score_gate( + track_score=reacquire_floor, + confirmed=True, + is_switch_candidate=False, + weak_reacq_guard=True, + ) + assert not track_passes_score_gate( + track_score=reacquire_floor - 0.01, + confirmed=True, + is_switch_candidate=False, + weak_reacq_guard=True, + ) + + +def test_switch_candidate_must_clear_switch_floor(): + switch_floor = required_track_score( + confirmed=True, + is_switch_candidate=True, + weak_reacq_guard=False, + ) + + assert track_passes_score_gate( + track_score=switch_floor, + confirmed=True, + is_switch_candidate=True, + weak_reacq_guard=False, + ) + assert not track_passes_score_gate( + track_score=switch_floor - 0.01, + confirmed=True, + is_switch_candidate=True, + weak_reacq_guard=False, + ) diff --git a/track_score_policy.py b/track_score_policy.py new file mode 100644 index 0000000..660426b --- /dev/null +++ b/track_score_policy.py @@ -0,0 +1,38 @@ +def required_track_score( + *, + confirmed, + is_switch_candidate, + weak_reacq_guard, + acquire_floor=0.08, + reacquire_floor=0.07, + switch_floor=0.10, +): + if not confirmed: + return float(acquire_floor) + + floor = float(reacquire_floor) + if weak_reacq_guard: + floor = max(floor, float(reacquire_floor)) + if is_switch_candidate: + floor = max(floor, float(switch_floor)) + return float(floor) + + +def track_passes_score_gate( + *, + track_score, + confirmed, + is_switch_candidate, + weak_reacq_guard, + acquire_floor=0.08, + reacquire_floor=0.07, + switch_floor=0.10, +): + return float(track_score) >= required_track_score( + confirmed=confirmed, + is_switch_candidate=is_switch_candidate, + weak_reacq_guard=weak_reacq_guard, + acquire_floor=acquire_floor, + reacquire_floor=reacquire_floor, + switch_floor=switch_floor, + ) diff --git a/trackers.py b/trackers.py new file mode 100644 index 0000000..4559810 --- /dev/null +++ b/trackers.py @@ -0,0 +1,197 @@ +import cv2 +import numpy as np + +from config import * +from helpers import clamp, box_center, box_wh, clip_box + +# Kalman and KLT trackers +# ========================= +# KALMAN 8D +# ========================= + +class Kalman8D: + def __init__(self): + self.x = np.zeros((8, 1), dtype=np.float32) + self.P = np.eye(8, dtype=np.float32) * 50.0 + + self.H = np.zeros((4, 8), dtype=np.float32) + self.H[0, 0] = 1.0 + self.H[1, 1] = 1.0 + self.H[2, 2] = 1.0 + self.H[3, 3] = 1.0 + + self.R = np.eye(4, dtype=np.float32) + # Снижено с 16→9 и 25→16: новая YOLO даёт точные детекты, + # Kalman должен им больше доверять и быстрее подстраиваться. + self.R[0, 0] = 9.0 + self.R[1, 1] = 9.0 + self.R[2, 2] = 16.0 + self.R[3, 3] = 16.0 + + self.initialized = False + # Процесс-шум повышен для реактивности на резкие манёвры. + # Было: q_pos=2.0, q_size=3.0, q_vel=4.0 — для плавного tracking'а + # Стало: q_pos=5.0, q_size=3.5, q_vel=12.0 — цель до 160 px/s со швырками + self.q_pos = 5.0 + self.q_size = 3.5 + self.q_vel = 12.0 + + def F(self, dt): + F = np.eye(8, dtype=np.float32) + F[0, 4] = dt + F[1, 5] = dt + F[2, 6] = dt + F[3, 7] = dt + return F + + def Q(self, dt, q_scale=1.0): + q_scale = float(max(0.25, q_scale)) + q = np.zeros((8, 8), dtype=np.float32) + s1 = dt * dt + s2 = dt + q[0, 0] = self.q_pos * s1 * q_scale + q[1, 1] = self.q_pos * s1 * q_scale + q[2, 2] = self.q_size * s1 * q_scale + q[3, 3] = self.q_size * s1 * q_scale + q[4, 4] = self.q_vel * s2 * q_scale + q[5, 5] = self.q_vel * s2 * q_scale + q[6, 6] = self.q_vel * s2 * q_scale + q[7, 7] = self.q_vel * s2 * q_scale + return q + + def predict(self, dt, q_scale=1.0): + dt = float(max(1e-3, dt)) + F = self.F(dt) + self.x = F @ self.x + self.P = F @ self.P @ F.T + self.Q(dt, q_scale=q_scale) + return self.x[:4].flatten() + + def update(self, z): + z = np.array(z, dtype=np.float32).reshape(4, 1) + y = z - self.H @ self.x + S = self.H @ self.P @ self.H.T + self.R + K = self.P @ self.H.T @ np.linalg.inv(S) + self.x = self.x + K @ y + I = np.eye(8, dtype=np.float32) + self.P = (I - K @ self.H) @ self.P + + def init_from_box(self, box): + cx, cy = box_center(box) + w, h = box_wh(box) + self.x[:] = 0 + self.x[0, 0] = cx + self.x[1, 0] = cy + self.x[2, 0] = w + self.x[3, 0] = h + self.P = np.eye(8, dtype=np.float32) * 50.0 + self.initialized = True + + def to_box(self): + cx = float(self.x[0, 0]) + cy = float(self.x[1, 0]) + w = float(max(2.0, self.x[2, 0])) + h = float(max(2.0, self.x[3, 0])) + return np.array([cx - w * 0.5, cy - h * 0.5, cx + w * 0.5, cy + h * 0.5], dtype=np.float32) + + def uncertainty(self): + return float(self.P[0, 0] + self.P[1, 1] + self.P[2, 2] + self.P[3, 3]) + + +# ========================= +# KLT TRACKER +# ========================= + +class KLTTracker: + def __init__(self): + self.prev_gray = None + self.pts = None + self.box = None + self.good_count = 0 + self.quality = 0.0 + + def reset(self): + self.prev_gray = None + self.pts = None + self.box = None + self.good_count = 0 + self.quality = 0.0 + + def init(self, frame_bgr, box): + gray = cv2.cvtColor(frame_bgr, cv2.COLOR_BGR2GRAY) + x1, y1, x2, y2 = map(int, box) + mask = np.zeros_like(gray) + cv2.rectangle(mask, (x1, y1), (x2, y2), 255, -1) + + pts = cv2.goodFeaturesToTrack( + gray, + maxCorners=KLT_MAX_CORNERS, + qualityLevel=KLT_QUALITY, + minDistance=KLT_MIN_DIST, + mask=mask + ) + self.prev_gray = gray + self.pts = pts + self.box = box.copy() + self.good_count = 0 + self.quality = 0.0 + + def update(self, frame_bgr): + if self.prev_gray is None or self.pts is None or len(self.pts) == 0 or self.box is None: + self.quality = 0.0 + self.good_count = 0 + return None + + gray = cv2.cvtColor(frame_bgr, cv2.COLOR_BGR2GRAY) + next_pts, st, err = cv2.calcOpticalFlowPyrLK( + self.prev_gray, gray, self.pts, None, + winSize=KLT_WIN, + maxLevel=KLT_MAX_LEVEL, + criteria=(cv2.TERM_CRITERIA_EPS | cv2.TERM_CRITERIA_COUNT, 30, 0.01) + ) + + if next_pts is None or st is None: + self.quality = 0.0 + self.good_count = 0 + self.prev_gray = gray + return None + + st = st.reshape(-1) + good_old = self.pts[st == 1].reshape(-1, 2) + good_new = next_pts[st == 1].reshape(-1, 2) + + self.good_count = int(len(good_new)) + if self.good_count < 6: + self.quality = 0.0 + self.prev_gray = gray + self.pts = good_new.reshape(-1, 1, 2) if self.good_count > 0 else None + return None + + disp = good_new - good_old + med = np.median(disp, axis=0) + d = np.linalg.norm(disp - med[None, :], axis=1) + inliers = d < KLT_OUTLIER_THRESH + in_cnt = int(np.count_nonzero(inliers)) + + if in_cnt < 6: + self.quality = 0.0 + self.prev_gray = gray + self.pts = good_new.reshape(-1, 1, 2) + return None + + disp_in = disp[inliers] + med = np.median(disp_in, axis=0) + dx, dy = float(med[0]), float(med[1]) + + x1, y1, x2, y2 = self.box + new_box = np.array([x1 + dx, y1 + dy, x2 + dx, y2 + dy], dtype=np.float32) + + frac = float(in_cnt) / float(len(inliers)) + self.quality = clamp(frac * (self.good_count / max(1, KLT_MAX_CORNERS)), 0.0, 1.0) + + self.prev_gray = gray + self.pts = good_new[inliers].reshape(-1, 1, 2) + self.box = new_box + return new_box + + +# ========================= diff --git a/trackers_hybrid.py b/trackers_hybrid.py new file mode 100644 index 0000000..c2eddc8 --- /dev/null +++ b/trackers_hybrid.py @@ -0,0 +1,456 @@ +# ============================================================ +# trackers_hybrid.py (v2 — affine motion for maneuvering targets) +# +# Ключевые отличия от v1: +# 1. KLT путь оценивает ПОЛНУЮ аффинку (translation + rotation +# + scale) через cv2.estimateAffinePartial2D, а не только +# медиану сдвигов. Это необходимо когда цель поворачивается +# в кадре — при rigid translation inlier-точки разлетаются +# и квалити падает. +# 2. Применение аффинки к центру и к размеру bbox с clamp'ом +# на изменение масштаба за кадр (защита от RANSAC-сбоев). +# 3. Более частая переинициализация features (8 вместо 15) +# для быстрого подстраивания под вращающуюся цель. +# 4. Phase correlation теперь работает на НЕСКОЛЬКИХ масштабах +# шаблона (0.9x, 1.0x, 1.1x) чтобы устойчиво ловить цель при +# приближении/удалении. +# ============================================================ + +import cv2 +import numpy as np + +from config import * +from helpers import clamp + + +# ─── Параметры ─────────────────────────────────────────────── +HYBRID_CLAHE_CLIP = 2.5 +HYBRID_CLAHE_TILE = 4 +HYBRID_KLT_QUALITY = 0.005 +HYBRID_KLT_QUALITY_RETRY = 0.002 +HYBRID_KLT_MIN_DIST = 2 +HYBRID_KLT_MAX_CORNERS = 50 +HYBRID_KLT_MIN_INLIERS = 3 +HYBRID_KLT_MIN_FOR_AFFINE = 6 # меньше — fallback на median translation +HYBRID_KLT_OUTLIER_THR = 3.5 +HYBRID_PAD_FACTOR = 0.25 +HYBRID_SEARCH_FACTOR = 2.4 # чуть больше search window для манёвров + +HYBRID_PHASE_MIN_RESP = 0.22 # чуть ослаблен +HYBRID_TM_MIN_SCORE = 0.42 +HYBRID_TM_SCALES = (0.9, 1.0, 1.1) + +# Защита от RANSAC-сбоев (sanity check, размер bbox НЕ обновляется) +HYBRID_MAX_SCALE_PER_FRAME = 1.25 # допустимый диапазон нормы аффинки +HYBRID_MIN_SCALE_PER_FRAME = 0.80 # иначе считаем RANSAC сбоем +HYBRID_MAX_TRANSLATION_PX = 80.0 # максимальный сдвиг центра за кадр + +HYBRID_TEMPLATE_UPDATE_EMA = 0.80 # чуть быстрее обновлять (было 0.85) +HYBRID_REINIT_FEATURES_EVERY = 8 # было 15 — для динамичной цели + + +class HybridTracker: + """ + Многоуровневый трекер для мелких манёвренных целей. + + API: + trk = HybridTracker() + trk.init(frame_bgr, box) + new_box = trk.update(frame_bgr) # None если потеряли + Поля: + self.box — текущий bbox + self.quality — 0..1 + self.last_method — "klt-affine" / "klt-translate" / "phasecorr" / "template" / "lost" + self.good_count — число inliers KLT на последнем кадре + """ + + def __init__(self): + self._clahe = cv2.createCLAHE( + clipLimit=float(HYBRID_CLAHE_CLIP), + tileGridSize=(int(HYBRID_CLAHE_TILE), int(HYBRID_CLAHE_TILE)), + ) + self.reset() + + # ─── Public API ────────────────────────────────────────── + def reset(self): + self.prev_gray = None + self.prev_gray_eq = None + self.pts = None + self.box = None + self.template = None + self.good_count = 0 + self.quality = 0.0 + self.last_method = "none" + self._frames_since_feature_init = 0 + self._success_streak = 0 + + def init(self, frame_bgr, box): + if frame_bgr is None or box is None: + return + gray = cv2.cvtColor(frame_bgr, cv2.COLOR_BGR2GRAY) + gray_eq = self._clahe.apply(gray) + + self.box = np.array(box, dtype=np.float32) + self.prev_gray = gray + self.prev_gray_eq = gray_eq + + self._extract_features(gray_eq, self.box) + self._update_template(gray_eq, self.box, reset=True) + self._frames_since_feature_init = 0 + self._success_streak = 0 + self.last_method = "init" + + def update(self, frame_bgr): + if self.box is None or self.prev_gray is None: + return None + + gray = cv2.cvtColor(frame_bgr, cv2.COLOR_BGR2GRAY) + gray_eq = self._clahe.apply(gray) + + # ── Попытка 1: KLT (affine или fallback translate) ── + new_box_klt, q_klt, klt_method = self._try_klt(gray, gray_eq) + + # ── Попытка 2: Phase correlation ──────────────────── + new_box_pc, q_pc = (None, 0.0) + if new_box_klt is None or q_klt < 0.25: + new_box_pc, q_pc = self._try_phase_correlation(gray_eq) + + # ── Попытка 3: Template matching (multi-scale) ────── + new_box_tm, q_tm = (None, 0.0) + if new_box_klt is None and new_box_pc is None: + new_box_tm, q_tm = self._try_template_match(gray_eq) + + # ── Выбор лучшего ───────────────────────────────────── + candidates = [] + if new_box_klt is not None: + candidates.append((klt_method, new_box_klt, q_klt)) + if new_box_pc is not None: + candidates.append(("phasecorr", new_box_pc, q_pc)) + if new_box_tm is not None: + candidates.append(("template", new_box_tm, q_tm)) + + if not candidates: + self.last_method = "lost" + self.quality = 0.0 + self.prev_gray = gray + self.prev_gray_eq = gray_eq + return None + + candidates.sort(key=lambda c: c[2], reverse=True) + method, new_box, q = candidates[0] + + self.box = new_box + self.quality = float(q) + self.last_method = method + self._success_streak += 1 + self._frames_since_feature_init += 1 + + # Переинициализация features и обновление template + if self._frames_since_feature_init >= int(HYBRID_REINIT_FEATURES_EVERY): + self._extract_features(gray_eq, self.box) + self._update_template(gray_eq, self.box, reset=False) + self._frames_since_feature_init = 0 + elif method in ("phasecorr", "template") and self._success_streak % 4 == 0: + self._extract_features(gray_eq, self.box) + self._update_template(gray_eq, self.box, reset=False) + + self.prev_gray = gray + self.prev_gray_eq = gray_eq + return self.box + + # ─── Level 1a: KLT with AFFINE motion ──────────────────── + def _try_klt(self, gray, gray_eq): + """ + Returns (new_box, quality, method_name). + method_name ∈ {"klt-affine", "klt-translate"} или (None, 0, "none"). + """ + if self.pts is None or len(self.pts) == 0: + return None, 0.0, "none" + + next_pts, st, err = cv2.calcOpticalFlowPyrLK( + self.prev_gray_eq, gray_eq, self.pts, None, + winSize=(21, 21), + maxLevel=3, + criteria=(cv2.TERM_CRITERIA_EPS | cv2.TERM_CRITERIA_COUNT, 30, 0.01), + ) + if next_pts is None or st is None: + self.good_count = 0 + return None, 0.0, "none" + + st = st.reshape(-1).astype(bool) + good_old = self.pts[st].reshape(-1, 2) + good_new = next_pts[st].reshape(-1, 2) + self.good_count = int(len(good_new)) + + if self.good_count < int(HYBRID_KLT_MIN_INLIERS): + return None, 0.0, "none" + + # ── Путь А: affine через estimateAffinePartial2D (RANSAC) ── + if self.good_count >= int(HYBRID_KLT_MIN_FOR_AFFINE): + M, inlier_mask = cv2.estimateAffinePartial2D( + good_old, good_new, + method=cv2.RANSAC, + ransacReprojThreshold=float(HYBRID_KLT_OUTLIER_THR), + maxIters=200, + confidence=0.99, + ) + if M is not None and inlier_mask is not None: + inlier_cnt = int(inlier_mask.sum()) + if inlier_cnt >= int(HYBRID_KLT_MIN_INLIERS): + new_box = self._apply_affine_to_box(M, self.box) + if new_box is not None and self._is_sane_box_update(self.box, new_box): + # quality: fraction of inliers * абсолютное число + frac = float(inlier_cnt) / max(1, self.good_count) + quality = float(clamp( + frac * (inlier_cnt / max(6.0, HYBRID_KLT_MAX_CORNERS * 0.4)), + 0.0, 1.0, + )) + # Обновляем pts только inliers для следующего кадра + mask_flat = inlier_mask.reshape(-1).astype(bool) + self.pts = good_new[mask_flat].reshape(-1, 1, 2).astype(np.float32) + return new_box, quality, "klt-affine" + + # ── Путь Б: fallback median translation ───────────── + disp = good_new - good_old + med = np.median(disp, axis=0) + d = np.linalg.norm(disp - med[None, :], axis=1) + inliers = d < float(HYBRID_KLT_OUTLIER_THR) + in_cnt = int(np.count_nonzero(inliers)) + + if in_cnt < int(HYBRID_KLT_MIN_INLIERS): + return None, 0.0, "none" + + disp_in = disp[inliers] + med = np.median(disp_in, axis=0) + dx, dy = float(med[0]), float(med[1]) + + if abs(dx) > HYBRID_MAX_TRANSLATION_PX or abs(dy) > HYBRID_MAX_TRANSLATION_PX: + return None, 0.0, "none" + + x1, y1, x2, y2 = self.box + new_box = np.array([x1 + dx, y1 + dy, x2 + dx, y2 + dy], dtype=np.float32) + + frac = float(in_cnt) / max(1, self.good_count) + quality = float(clamp( + frac * (in_cnt / max(6.0, HYBRID_KLT_MAX_CORNERS * 0.5)), + 0.0, 0.85, # translate даёт меньший макс quality чем affine + )) + + self.pts = good_new[inliers].reshape(-1, 1, 2).astype(np.float32) + return new_box, quality, "klt-translate" + + def _apply_affine_to_box(self, M, box): + """ + Применяет аффинку ТОЛЬКО к центру bbox (translation + rotation). + РАЗМЕР bbox НЕ ОБНОВЛЯЕТСЯ — остаётся фиксированным. + + Причина: когда KLT-трекер масштабировал бокс своим scale, он + постепенно расходился по размеру с тем, что детектирует YOLO, + в результате ByteTrack не ассоциировал детекты с треком + (hit_streak падал до 0). Размер бокса должен обновляться + только от YOLO — это единственный надёжный источник истинного + масштаба цели. + + scale от аффинки используется только для sanity check: если + он слишком далёк от 1.0, значит RANSAC сбился, возвращаем None. + """ + x1, y1, x2, y2 = [float(v) for v in box] + cx = 0.5 * (x1 + x2) + cy = 0.5 * (y1 + y2) + w = x2 - x1 + h = y2 - y1 + + # Центр через аффинку + new_cx = M[0, 0] * cx + M[0, 1] * cy + M[0, 2] + new_cy = M[1, 0] * cx + M[1, 1] * cy + M[1, 2] + + # Sanity check через норму столбца (должна быть ~1.0 для + # кадра к кадру; большое отклонение = RANSAC сбой) + scale = float(np.hypot(M[0, 0], M[1, 0])) + if not (float(HYBRID_MIN_SCALE_PER_FRAME) <= scale <= float(HYBRID_MAX_SCALE_PER_FRAME)): + return None + + # Размер НЕ меняем — используем старые w, h + return np.array( + [new_cx - w * 0.5, new_cy - h * 0.5, + new_cx + w * 0.5, new_cy + h * 0.5], + dtype=np.float32, + ) + + def _is_sane_box_update(self, old_box, new_box): + """Защита от RANSAC-выбросов.""" + ox1, oy1, ox2, oy2 = [float(v) for v in old_box] + nx1, ny1, nx2, ny2 = [float(v) for v in new_box] + ocx = 0.5 * (ox1 + ox2) + ocy = 0.5 * (oy1 + oy2) + ncx = 0.5 * (nx1 + nx2) + ncy = 0.5 * (ny1 + ny2) + shift = float(np.hypot(ncx - ocx, ncy - ocy)) + if shift > float(HYBRID_MAX_TRANSLATION_PX): + return False + return True + + # ─── Level 2: Phase correlation ────────────────────────── + def _try_phase_correlation(self, gray_eq): + if self.template is None or self.template.size == 0: + return None, 0.0 + + search, (sx, sy) = self._crop_search_window(gray_eq, self.box) + if search is None: + return None, 0.0 + h_t, w_t = self.template.shape[:2] + h_s, w_s = search.shape[:2] + if h_s < h_t or w_s < w_t: + return None, 0.0 + + cy_s = h_s // 2 + cx_s = w_s // 2 + y0 = max(0, cy_s - h_t // 2) + x0 = max(0, cx_s - w_t // 2) + y1 = min(h_s, y0 + h_t) + x1 = min(w_s, x0 + w_t) + patch = search[y0:y1, x0:x1] + if patch.shape != self.template.shape: + patch = cv2.copyMakeBorder( + patch, 0, h_t - patch.shape[0], 0, w_t - patch.shape[1], + cv2.BORDER_REPLICATE, + ) + + try: + t_f = self.template.astype(np.float32) + p_f = patch.astype(np.float32) + (shift_x, shift_y), response = cv2.phaseCorrelate(t_f, p_f) + except cv2.error: + return None, 0.0 + + if response < float(HYBRID_PHASE_MIN_RESP): + return None, 0.0 + if abs(shift_x) > HYBRID_MAX_TRANSLATION_PX or abs(shift_y) > HYBRID_MAX_TRANSLATION_PX: + return None, 0.0 + + x1b, y1b, x2b, y2b = self.box + new_box = np.array( + [x1b + shift_x, y1b + shift_y, x2b + shift_x, y2b + shift_y], + dtype=np.float32, + ) + quality = float(clamp(response * 0.75, 0.0, 0.75)) + return new_box, quality + + # ─── Level 3: Multi-scale template matching ────────────── + def _try_template_match(self, gray_eq): + if self.template is None or self.template.size == 0: + return None, 0.0 + + search, (sx, sy) = self._crop_search_window(gray_eq, self.box) + if search is None: + return None, 0.0 + + best = None # (score, new_box) + + for scale in HYBRID_TM_SCALES: + if scale == 1.0: + tmpl = self.template + else: + nh = max(4, int(self.template.shape[0] * scale)) + nw = max(4, int(self.template.shape[1] * scale)) + tmpl = cv2.resize(self.template, (nw, nh), interpolation=cv2.INTER_LINEAR) + + h_t, w_t = tmpl.shape[:2] + if search.shape[0] < h_t or search.shape[1] < w_t: + continue + + try: + result = cv2.matchTemplate(search, tmpl, cv2.TM_CCOEFF_NORMED) + min_val, max_val, min_loc, max_loc = cv2.minMaxLoc(result) + except cv2.error: + continue + + if max_val < float(HYBRID_TM_MIN_SCORE): + continue + + mx, my = max_loc + nx1 = sx + mx + ny1 = sy + my + new_box = np.array( + [nx1, ny1, nx1 + w_t, ny1 + h_t], dtype=np.float32 + ) + if best is None or max_val > best[0]: + best = (float(max_val), new_box) + + if best is None: + return None, 0.0 + + score, new_box = best + quality = float(clamp(score * 0.72, 0.0, 0.72)) + return new_box, quality + + # ─── Helpers ───────────────────────────────────────────── + def _extract_features(self, gray_eq, box): + h, w = gray_eq.shape[:2] + x1, y1, x2, y2 = box + bw = max(1.0, x2 - x1) + bh = max(1.0, y2 - y1) + pad_x = bw * float(HYBRID_PAD_FACTOR) + pad_y = bh * float(HYBRID_PAD_FACTOR) + + mx1 = int(clamp(x1 - pad_x, 0, w - 1)) + my1 = int(clamp(y1 - pad_y, 0, h - 1)) + mx2 = int(clamp(x2 + pad_x, 1, w)) + my2 = int(clamp(y2 + pad_y, 1, h)) + + mask = np.zeros_like(gray_eq) + mask[my1:my2, mx1:mx2] = 255 + + pts = cv2.goodFeaturesToTrack( + gray_eq, + maxCorners=int(HYBRID_KLT_MAX_CORNERS), + qualityLevel=float(HYBRID_KLT_QUALITY), + minDistance=int(HYBRID_KLT_MIN_DIST), + mask=mask, + ) + if pts is None or len(pts) < 6: + pts = cv2.goodFeaturesToTrack( + gray_eq, + maxCorners=int(HYBRID_KLT_MAX_CORNERS), + qualityLevel=float(HYBRID_KLT_QUALITY_RETRY), + minDistance=int(HYBRID_KLT_MIN_DIST), + mask=mask, + ) + self.pts = pts.astype(np.float32) if pts is not None else None + + def _update_template(self, gray_eq, box, reset=False): + h, w = gray_eq.shape[:2] + x1 = int(clamp(box[0], 0, w - 1)) + y1 = int(clamp(box[1], 0, h - 1)) + x2 = int(clamp(box[2], x1 + 2, w)) + y2 = int(clamp(box[3], y1 + 2, h)) + crop = gray_eq[y1:y2, x1:x2] + if crop.size == 0: + return + + if reset or self.template is None or self.template.shape != crop.shape: + self.template = crop.copy() + else: + a = float(HYBRID_TEMPLATE_UPDATE_EMA) + self.template = (a * self.template.astype(np.float32) + + (1.0 - a) * crop.astype(np.float32)).astype(np.uint8) + + def _crop_search_window(self, gray_eq, box): + h, w = gray_eq.shape[:2] + cx = 0.5 * (float(box[0]) + float(box[2])) + cy = 0.5 * (float(box[1]) + float(box[3])) + bw = max(8.0, float(box[2] - box[0])) + bh = max(8.0, float(box[3] - box[1])) + + sw = bw * float(HYBRID_SEARCH_FACTOR) + sh = bh * float(HYBRID_SEARCH_FACTOR) + + sx = int(clamp(cx - sw * 0.5, 0, w - 1)) + sy = int(clamp(cy - sh * 0.5, 0, h - 1)) + ex = int(clamp(cx + sw * 0.5, sx + 2, w)) + ey = int(clamp(cy + sh * 0.5, sy + 2, h)) + + search = gray_eq[sy:ey, sx:ex] + if search.size == 0: + return None, (0, 0) + return search, (sx, sy) diff --git a/trackers_safe.py b/trackers_safe.py new file mode 100644 index 0000000..d42b0b3 --- /dev/null +++ b/trackers_safe.py @@ -0,0 +1,290 @@ +# ============================================================ +# trackers_safe.py +# SafeKalman8D — drop-in замена для Kalman8D. +# +# Фиксит критический баг из логов v4: цель уходит вдаль, +# реально уменьшается по высоте, Kalman оценивает отрицательный +# vh, и без YOLO measurement updates экстраполирует h → 0. +# Bbox вырождается в 73×4, прилипает к линии горизонта, держится +# там 12 секунд, весь трек гибнет. +# +# Защита строится на трёх принципах: +# +# 1. ИЗМЕРЕНИЯМ ДОВЕРЯЕМ. Если YOLO говорит "цель 18×9" — бокс +# такой и становится. Реальная усадка не ограничивается. +# +# 2. ПРЕДСКАЗАНИЯМ — НЕТ. Между measurement'ами Kalman может +# сжимать bbox только с ограниченной скоростью (3.5% за +# кадр максимум). Это физически реалистично для удаляющейся +# цели и не даёт drift'у обрушить размер за 15 кадров. +# +# 3. ЖЁСТКИЕ ФИЗИЧЕСКИЕ ПРЕДЕЛЫ. Aspect ratio всегда 0.40–3.5 +# (нормальные пропорции дрона), минимальная сторона 8px +# (ниже трек бессмысленен), максимальная сторона 400px. +# +# Использование: +# было: from trackers import Kalman8D +# kf = Kalman8D() +# стало: from trackers_safe import SafeKalman8D +# kf = SafeKalman8D() +# +# Никаких других изменений в main.py не требуется. +# ============================================================ + +import numpy as np + +from trackers import Kalman8D + + +# ─── Физические пределы bbox ───────────────────────────────── +SAFE_KF_MIN_SIDE = 6.0 # минимальная сторона (пиксели) — снижено для далёких самолётов +SAFE_KF_MAX_SIDE = 400.0 # максимальная сторона +SAFE_KF_MAX_ASPECT = 6.0 # w/h максимум — самолёт в профиль с размахом крыла +SAFE_KF_MIN_ASPECT = 0.30 # w/h минимум — самолёт хвостом или узкий ракурс + +# ─── Ограничение скорости сжатия предсказанием ────────────── +# Между measurement'ами bbox может сжиматься/расти максимум на +# эту долю за кадр. Ослаблено для быстро маневрирующих целей: +# 0.945 = до 5.5% сжатия за кадр (было 3.5%) +# 1.060 = до 6.0% роста за кадр (было 4.0%) +SAFE_KF_MIN_SHRINK_PER_FRAME = 0.945 +SAFE_KF_MAX_GROW_PER_FRAME = 1.060 + +# ─── Velocity decay для долгих потерь измерений ───────────── +# После N predict'ов без update начинаем гасить velocity +# каждый кадр. Защита от "улёта" Kalman'а далеко от реального +# положения цели (особенно актуально для мелких далёких целей +# где YOLO не детектит регулярно). +SAFE_KF_DECAY_START_FRAMES = 8 # начинать после 8 кадров без update +SAFE_KF_VELOCITY_DECAY = 0.97 # 3% гашения за кадр после порога + + +class SafeKalman8D(Kalman8D): + """ + Kalman8D с sanity constraint'ами. Полностью API-совместим + с оригиналом. + + Дополнительные поля для диагностики: + self.sanity_reject_count — общий счётчик срабатываний + self.sanity_last_reason — причина последнего срабатывания + self.last_meas_w, last_meas_h — размер последнего + принятого measurement (для отладки) + """ + + def __init__(self): + super().__init__() + self.sanity_reject_count = 0 + self.sanity_last_reason = "" + self.last_meas_w = None + self.last_meas_h = None + # Сохраняем w, h после последнего update() — от этого + # значения отсчитывается допустимая скорость сжатия. + # Это "наше последнее подтверждённое знание о размере". + self._anchor_w = None + self._anchor_h = None + # Сколько predict'ов прошло с последнего measurement + self._frames_since_update = 0 + + # ─── Override: init_from_box ───────────────────────────── + def init_from_box(self, box): + if box is None: + return + box = np.asarray(box, dtype=np.float32) + box = self._force_valid_box(box) + super().init_from_box(box) + + w = float(self.x[2, 0]) + h = float(self.x[3, 0]) + self._anchor_w = w + self._anchor_h = h + self.last_meas_w = w + self.last_meas_h = h + self._frames_since_update = 0 + + # ─── Override: predict ─────────────────────────────────── + def predict(self, dt, q_scale=1.0): + """ + Predict + shrink-rate limit + aspect ratio constraint. + Плюс velocity decay: при долгом отсутствии measurements + постепенно гасим скорость, иначе Kalman "улетает" в + сторону по инерции (особенно для мелких далёких целей). + """ + # Считаем старые w, h ДО predict + prev_w = float(self.x[2, 0]) + prev_h = float(self.x[3, 0]) + + # ─── Velocity decay при долгой потере measurements ─ + if self._frames_since_update >= SAFE_KF_DECAY_START_FRAMES: + decay = float(SAFE_KF_VELOCITY_DECAY) + self.x[4, 0] *= decay # vx + self.x[5, 0] *= decay # vy + self.x[6, 0] *= decay # vw + self.x[7, 0] *= decay # vh + + # Стандартный Kalman predict + super().predict(dt, q_scale=q_scale) + self._frames_since_update += 1 + + new_w = float(self.x[2, 0]) + new_h = float(self.x[3, 0]) + + # ─── Ограничение скорости сжатия ──────────────────── + # Если Kalman хочет сжать bbox быстрее допустимого — + # откатываем к допустимому пределу и обнуляем velocity. + min_allowed_w = prev_w * SAFE_KF_MIN_SHRINK_PER_FRAME + min_allowed_h = prev_h * SAFE_KF_MIN_SHRINK_PER_FRAME + max_allowed_w = prev_w * SAFE_KF_MAX_GROW_PER_FRAME + max_allowed_h = prev_h * SAFE_KF_MAX_GROW_PER_FRAME + + rate_clamped = False + if new_w < min_allowed_w: + new_w = min_allowed_w + self.x[6, 0] = 0.0 # обнуляем vw + rate_clamped = True + elif new_w > max_allowed_w: + new_w = max_allowed_w + self.x[6, 0] = 0.0 + rate_clamped = True + + if new_h < min_allowed_h: + new_h = min_allowed_h + self.x[7, 0] = 0.0 # обнуляем vh + rate_clamped = True + elif new_h > max_allowed_h: + new_h = max_allowed_h + self.x[7, 0] = 0.0 + rate_clamped = True + + # ─── Абсолютные пределы + aspect ratio ────────────── + new_w, new_h, absolute_clamped = self._clamp_absolute(new_w, new_h) + + # Записываем обратно в состояние + self.x[2, 0] = new_w + self.x[3, 0] = new_h + + if rate_clamped: + self.sanity_reject_count += 1 + self.sanity_last_reason = "predict:rate_clamp" + elif absolute_clamped: + self.sanity_reject_count += 1 + self.sanity_last_reason = "predict:abs_clamp" + + return self.x[:4].flatten() + + # ─── Override: update ──────────────────────────────────── + def update(self, z): + """ + Measurement update. Измерениям доверяем, но проверяем + на явную вырожденность — если aspect ratio измерения + >6:1, это точно не дрон (мусорный детект), пропускаем. + """ + z = np.asarray(z, dtype=np.float32).reshape(-1) + if len(z) < 4: + return + + cx, cy, w, h = float(z[0]), float(z[1]), float(z[2]), float(z[3]) + + # Жёсткая проверка на явно битое измерение + if w < SAFE_KF_MIN_SIDE * 0.5 or h < SAFE_KF_MIN_SIDE * 0.5: + self.sanity_reject_count += 1 + self.sanity_last_reason = f"meas:too_small_{w:.0f}x{h:.0f}" + return + + if w > SAFE_KF_MAX_SIDE or h > SAFE_KF_MAX_SIDE: + self.sanity_reject_count += 1 + self.sanity_last_reason = f"meas:too_big_{w:.0f}x{h:.0f}" + return + + ar = w / max(1e-3, h) + # Для measurement пределы шире (YOLO на ракурсе сбоку может дать до 7:1 для самолёта) + if ar > 8.0 or ar < 0.18: + self.sanity_reject_count += 1 + self.sanity_last_reason = f"meas:bad_ar_{ar:.1f}" + return + + # Применяем measurement + z_clean = np.array([cx, cy, w, h], dtype=np.float32) + super().update(z_clean) + + # Обновляем anchor — это наш "последний достоверный размер" + self._anchor_w = float(self.x[2, 0]) + self._anchor_h = float(self.x[3, 0]) + self.last_meas_w = w + self.last_meas_h = h + self._frames_since_update = 0 + + # ─── Override: to_box ──────────────────────────────────── + def to_box(self): + """ + Последняя линия обороны: абсолютные пределы + aspect ratio. + Синхронизируем state с результатом clamp'а. + """ + cx = float(self.x[0, 0]) + cy = float(self.x[1, 0]) + w = float(self.x[2, 0]) + h = float(self.x[3, 0]) + + w, h, clamped = self._clamp_absolute(w, h) + + self.x[2, 0] = w + self.x[3, 0] = h + + return np.array( + [cx - w * 0.5, cy - h * 0.5, cx + w * 0.5, cy + h * 0.5], + dtype=np.float32, + ) + + # ─── Internal helpers ──────────────────────────────────── + def _clamp_absolute(self, w, h): + """Абсолютные пределы + aspect ratio. (w, h, clamped_flag).""" + clamped = False + + if w < SAFE_KF_MIN_SIDE: + w = SAFE_KF_MIN_SIDE + clamped = True + if h < SAFE_KF_MIN_SIDE: + h = SAFE_KF_MIN_SIDE + clamped = True + if w > SAFE_KF_MAX_SIDE: + w = SAFE_KF_MAX_SIDE + clamped = True + if h > SAFE_KF_MAX_SIDE: + h = SAFE_KF_MAX_SIDE + clamped = True + + # Aspect ratio clamp + ar = w / max(1e-3, h) + if ar > SAFE_KF_MAX_ASPECT: + # Слишком широкий → поднимаем h + h = w / SAFE_KF_MAX_ASPECT + clamped = True + elif ar < SAFE_KF_MIN_ASPECT: + # Слишком высокий → поднимаем w + w = h * SAFE_KF_MIN_ASPECT + clamped = True + + return w, h, clamped + + def _force_valid_box(self, box): + """Принудительно делает box валидным — используется в init.""" + x1, y1, x2, y2 = [float(v) for v in box] + cx = 0.5 * (x1 + x2) + cy = 0.5 * (y1 + y2) + w = max(SAFE_KF_MIN_SIDE, x2 - x1) + h = max(SAFE_KF_MIN_SIDE, y2 - y1) + w, h, _ = self._clamp_absolute(w, h) + return np.array( + [cx - w * 0.5, cy - h * 0.5, cx + w * 0.5, cy + h * 0.5], + dtype=np.float32, + ) + + # ─── Status ────────────────────────────────────────────── + def status_line(self): + anchor_w = self._anchor_w if self._anchor_w is not None else 0.0 + anchor_h = self._anchor_h if self._anchor_h is not None else 0.0 + return ( + f"SafeKalman8D: anchor={anchor_w:.0f}x{anchor_h:.0f} " + f"frames_no_meas={self._frames_since_update} " + f"rejects={self.sanity_reject_count} " + f"last={self.sanity_last_reason}" + ) diff --git a/yolo_worker.py b/yolo_worker.py new file mode 100644 index 0000000..12297f8 --- /dev/null +++ b/yolo_worker.py @@ -0,0 +1,140 @@ +import cv2 +import numpy as np +import time +import threading +from collections import deque +import torch + +from config import * +from helpers import clip_box, crop_roi, preprocess_for_yolo, filter_yolo_boxes_with_scores + +# Async YOLO worker +# ========================= + +class YOLOWorker: + def __init__(self, model): + self.model = model + self.req = deque(maxlen=YOLO_QUEUE_MAX) + self.res = deque(maxlen=1) + self.lock = threading.Lock() + self.running = False + self.thread = threading.Thread(target=self._loop, daemon=True) + + def start(self): + self.running = True + self.thread.start() + + def stop(self): + self.running = False + self.thread.join(timeout=1.0) + + def submit(self, frame_eff_bgr, roi_box_eff, mode, ts): + with self.lock: + self.req.append((frame_eff_bgr, roi_box_eff, mode, ts)) + + def try_get(self): + with self.lock: + if not self.res: + return None + return self.res.pop() + + def _loop(self): + while self.running: + item = None + with self.lock: + if self.req: + item = self.req.pop() + self.req.clear() + + if item is None: + time.sleep(0.001) + continue + + frame, roi_box, mode, ts = item + h, w = frame.shape[:2] + frame_infer = preprocess_for_yolo(frame) + dets = [] + infer_ms = 0.0 + used_roi = False + + try: + if roi_box is not None: + roi_box = clip_box(roi_box, w, h) + crop, ox, oy = crop_roi(frame_infer, roi_box) + if crop.size > 0: + used_roi = True + t0 = time.perf_counter() + with torch.inference_mode(): + r = self.model( + crop, + conf=YOLO_CONF_EFFECTIVE, + imgsz=IMG_SIZE_ROI, + verbose=False, + max_det=MAX_DET, + device=DEVICE, + half=USE_HALF + )[0] + infer_ms = (time.perf_counter() - t0) * 1000.0 + dets = filter_yolo_boxes_with_scores( + r, + frame_w=w, + frame_h=h, + offset_x=ox, + offset_y=oy, + min_conf=BT_LOW + ) + + else: + sh, sw = frame_infer.shape[:2] + short = min(sh, sw) + scale = 1.0 + target = IMG_SIZE_FULL + if short > target: + scale = target / float(short) + small = cv2.resize( + frame_infer, + (int(sw * scale), int(sh * scale)), + interpolation=cv2.INTER_AREA + ) + else: + small = frame_infer + + t0 = time.perf_counter() + with torch.inference_mode(): + r = self.model( + small, + conf=YOLO_CONF_EFFECTIVE, + imgsz=IMG_SIZE_FULL, + verbose=False, + max_det=MAX_DET, + device=DEVICE, + half=USE_HALF + )[0] + infer_ms = (time.perf_counter() - t0) * 1000.0 + + dets_s = filter_yolo_boxes_with_scores( + r, + frame_w=small.shape[1], + frame_h=small.shape[0], + offset_x=0, + offset_y=0, + min_conf=BT_LOW + ) + if scale != 1.0: + inv = 1.0 / scale + dets = [ + np.array([d[0] * inv, d[1] * inv, d[2] * inv, d[3] * inv, d[4]], dtype=np.float32) + for d in dets_s + ] + else: + dets = dets_s + + except Exception: + dets = [] + infer_ms = 0.0 + + with self.lock: + self.res.append((dets, ts, mode, infer_ms, used_roi)) + + +# =========================