forked from Dan4ick/Lidar_Muxa
204 lines
11 KiB
Python
204 lines
11 KiB
Python
"""DESCENDING NEURONS — решение.
|
||
|
||
Весь разбор сцены у мухи сходится на нескольких десятках нисходящих нейронов,
|
||
идущих в грудной ганглий. Два из них работают как раз по надвигающемуся объекту:
|
||
|
||
* **Giant Fiber (DNp01)** — один толстый аксон с высоким порогом. Срабатывает
|
||
только на близкое и быстрое надвигание и запускает немедленный аварийный
|
||
взлёт, жертвуя устойчивостью ради скорости.
|
||
* **DNp02/DNp11** — порог ниже, реакция раньше и мягче: муха успевает
|
||
подготовиться, не срываясь в паническое движение.
|
||
|
||
Поезду нужна ровно такая же пара уровней: заблаговременное предупреждение с
|
||
запасом по дальности и экстренное торможение по надёжному близкому объекту.
|
||
Гистерезис здесь — не украшение: без него трек на пороге даёт дребезг, а
|
||
дребезжащая команда торможения хуже её отсутствия.
|
||
"""
|
||
from __future__ import annotations
|
||
|
||
from dataclasses import dataclass, field
|
||
|
||
from .central_complex import CentralComplex, Track
|
||
|
||
BRAKING_DECEL = 1.0 # м/с², служебное торможение метропоезда (оценка)
|
||
REACTION_TIME = 1.5 # с, задержка канала «решение → тормоз»
|
||
|
||
|
||
@dataclass
|
||
class DetectedObject:
|
||
distance: float
|
||
lateral: float
|
||
height: float
|
||
width: float
|
||
size_v: float
|
||
confidence: float
|
||
novelty: float
|
||
n_rays: int
|
||
track_id: int
|
||
ttc: float
|
||
|
||
|
||
@dataclass
|
||
class Decision:
|
||
"""Выход системы за один кадр."""
|
||
|
||
detected: bool = False
|
||
emergency: bool = False
|
||
distance: float = float("inf")
|
||
ttc: float = float("inf")
|
||
confidence: float = 0.0
|
||
stopping_distance: float = 0.0
|
||
objects: list[DetectedObject] = field(default_factory=list)
|
||
speed: float = 0.0
|
||
|
||
@property
|
||
def clear(self) -> bool:
|
||
return not self.detected
|
||
|
||
|
||
class DescendingNeurons:
|
||
"""Два порога с гистерезисом поверх подтверждённых треков."""
|
||
|
||
def __init__(self, *, warn_evidence: float = 0.5, clear_evidence: float = 0.3,
|
||
emergency_evidence: float = 0.75, min_hits: int = 2,
|
||
novelty_floor: float = 0.10, max_range: float = 200.0,
|
||
warn_far: float | None = None, warn_far_from: float = 90.0,
|
||
track_blend: float = 0.0, track_score: str = "w_mean",
|
||
track_gate: float = 0.0, track_readout=None,
|
||
far_channel: bool = False):
|
||
self.warn_evidence = warn_evidence
|
||
self.clear_evidence = clear_evidence
|
||
self.emergency_evidence = emergency_evidence
|
||
self.min_hits = min_hits
|
||
self.novelty_floor = novelty_floor
|
||
self.max_range = max_range
|
||
# Порог предупреждения, ослабевающий с дальностью. Пара нисходящих
|
||
# нейронов у мухи не равноправна: DNp02/DNp11 срабатывают рано и
|
||
# мягко, Giant Fiber — поздно и жёстко. Здесь то же самое по
|
||
# дальности: далёкая улика физически слабее (4…8 лучей вместо
|
||
# сотни), а цена ошибки ниже — до предмета ещё сотня метров и
|
||
# десятки кадров на подтверждение. Экстренное торможение своего
|
||
# порога не снижает. None — послабления нет.
|
||
self.warn_far = warn_evidence if warn_far is None else warn_far
|
||
self.warn_far_from = warn_far_from
|
||
# Считывание по ТРЕКУ (см. track_readout.py). Улика — это сумма
|
||
# покадровых весов, и она ничего не знает ни о том, держится ли
|
||
# место в тоннеле, ни о том, растёт ли число лучей по закону
|
||
# решётки. Модель знает. Смешивание геометрическое, как и для
|
||
# покадрового считывания: 0 — решает только улика, ровно как
|
||
# раньше, 1 — только модель. Так размен меряется, а не объявляется.
|
||
self.track_blend = track_blend
|
||
# Жёсткий порог по тому же отсчёту — вместо смешивания или вместе с
|
||
# ним. Разница существенная: смешивание опускает отсчёт И предмету
|
||
# тоже (обе величины меньше единицы), а порог ничего не отнимает у
|
||
# того, кто его прошёл. На реальном объекте средний вес 0.998 у
|
||
# предмета против 0.269 у ложных треков — разделение, под которое
|
||
# порог и просится. 0 — не проверять.
|
||
self.track_gate = track_gate
|
||
# Дальний канал предупреждения (Zhirik1337, 22.09): далёкий трек с
|
||
# хорошими наблюдениями поднимает тревогу в обход порога, даже если
|
||
# улика просела на пропусках. Та же задача, что у `warn_far`, другим
|
||
# способом; включать вместе с ним — ослабить дальний край дважды.
|
||
# Замер — EXPERIMENTS п. 16.
|
||
self.far_channel = far_channel
|
||
self.track_score = track_score
|
||
self.track_readout = track_readout
|
||
self._latched: set[int] = set()
|
||
|
||
@property
|
||
def _hyst(self) -> float:
|
||
return self.clear_evidence / max(self.warn_evidence, 1e-6)
|
||
|
||
def _warn_at(self, d: float) -> float:
|
||
"""Порог тревоги на дальности d: линейно от warn до warn_far."""
|
||
span = self.max_range - self.warn_far_from
|
||
if span <= 0 or d <= self.warn_far_from:
|
||
return self.warn_evidence
|
||
f = min((d - self.warn_far_from) / span, 1.0)
|
||
return self.warn_evidence + (self.warn_far - self.warn_evidence) * f
|
||
|
||
def decide(self, cx: CentralComplex, speed: float) -> Decision:
|
||
out = Decision(speed=speed)
|
||
stop = speed * REACTION_TIME + speed * speed / (2 * BRAKING_DECEL)
|
||
out.stopping_distance = stop
|
||
|
||
# Дешёвые проверки сначала: модель считается только по тем трекам,
|
||
# которые их прошли, — обычно это единицы, а не полсотни.
|
||
ready: list[tuple[Track, float]] = []
|
||
for t in cx.tracks:
|
||
d = t.distance(cx.s_world)
|
||
far_floor = 0.05 if self.far_channel else self.novelty_floor
|
||
nov_ok = (t.novelty >= self.novelty_floor if d <= self.warn_far_from
|
||
else t.novelty >= far_floor)
|
||
if (not (0.0 < d <= self.max_range) or t.hits < self.min_hits
|
||
or not nov_ok):
|
||
self._latched.discard(t.id)
|
||
continue
|
||
ready.append((t, d))
|
||
|
||
use_track = ((self.track_blend > 0.0 or self.track_gate > 0.0)
|
||
and bool(ready))
|
||
if use_track and self.track_score == "model":
|
||
if self.track_readout is None:
|
||
use_track = False
|
||
else:
|
||
self.track_readout.annotate_tracks([t for t, _ in ready],
|
||
cx.s_world)
|
||
|
||
live: list[tuple[Track, float]] = []
|
||
for t, d in ready:
|
||
warn = self._warn_at(d)
|
||
# гистерезис: попавший в тревогу трек держится до нижнего порога
|
||
on = warn if t.id not in self._latched else warn * self._hyst
|
||
score = t.evidence
|
||
if use_track:
|
||
q = (t.p_track if self.track_score == "model"
|
||
else t.p_mean if self.track_score == "p_mean"
|
||
else t.w_ema if self.track_score == "w_ema"
|
||
else t.w_mean)
|
||
if self.track_gate > 0.0 and q < self.track_gate:
|
||
self._latched.discard(t.id)
|
||
continue
|
||
if self.track_blend > 0.0:
|
||
b = min(max(self.track_blend, 0.0), 1.0)
|
||
score = (max(q, 1e-4) ** b
|
||
* max(t.evidence, 1e-4) ** (1.0 - b))
|
||
|
||
# Дальний канал раннего предупреждения (DNp02/DNp11):
|
||
# На дальности d > warn_far_from при наличии подтверждений (hits >= min_hits)
|
||
# трек с устойчивыми признаками (p_mean, w_mean, w_ema или p_track)
|
||
# квалифицируется как предупреждение, даже если улика просела из-за пропусков.
|
||
is_far = (self.far_channel and d > self.warn_far_from
|
||
and t.hits >= self.min_hits)
|
||
quality = max(t.p_mean, t.w_mean, t.w_ema)
|
||
far_qualified = is_far and (
|
||
(quality >= 0.40 and t.evidence >= 0.15)
|
||
or (getattr(t, "p_track", 0.0) >= 0.65)
|
||
)
|
||
|
||
if score < on and not far_qualified:
|
||
self._latched.discard(t.id)
|
||
continue
|
||
self._latched.add(t.id)
|
||
live.append((t, d))
|
||
|
||
if not live:
|
||
return out
|
||
|
||
live.sort(key=lambda p: p[1])
|
||
for t, d in live:
|
||
ttc = d / speed if speed > 0.5 else float("inf")
|
||
out.objects.append(DetectedObject(
|
||
distance=d, lateral=t.u, height=t.h, width=t.width, size_v=t.height,
|
||
confidence=min(1.0, t.evidence * t.novelty + 0.0),
|
||
novelty=t.novelty, n_rays=t.last_n_rays, track_id=t.id, ttc=ttc))
|
||
|
||
nearest, d0 = live[0]
|
||
out.detected = True
|
||
out.distance = d0
|
||
out.ttc = d0 / speed if speed > 0.5 else float("inf")
|
||
out.confidence = max(o.confidence for o in out.objects)
|
||
out.emergency = any(
|
||
t.evidence >= self.emergency_evidence and (d <= max(stop, 25.0))
|
||
for t, d in live)
|
||
return out
|