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