MAI/range_estimation.py

# ============================================================
# 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,
            )