forked from Dan4ick/Lidar_Muxa
Zhirik1337
062bdfd68c
- tools/make_benchmark.py: аугментации спавна d_start ∈ [40, 200] м, боковой дрейф v_lat, шум продольной координаты δs (устранение инверсии s_std), сценарии стоянки; - flyguard/lobula.py: зонный пол h_lo_core = 0.16 м в межрельсовой колее, динамическое расширение габарита в кривых W_eff(d) по Corridor.sigma(d), отсечение плоскости настила платформы; - flyguard/central_complex.py: поддержка лежащих препятствий в колее без штрафа за вытянутость формы; - flyguard/synth.py: добавлен класс «человек_лежа» (1.8×0.5×0.3 м); - flyguard/descending.py: дальний мягкий канал предупреждения на дистанциях >90 м; - flyguard/export.py: экспорт детекций в 3D BBox, уровни угрозы, маркеры RViz MarkerArray; - tests/test_pipeline.py, tests/run_tests.py: 38 юнит-тестов и автономный раннер.
377 lines
22 KiB
Python
377 lines
22 KiB
Python
"""CENTRAL COMPLEX — накопление улик и треки в мировой системе координат.
|
||
|
||
Эллипсоидное тело мухи держит **кольцевой аттрактор**: клетки EPG образуют один
|
||
«бугор» активности, кодирующий текущий курс, клетки PEN сдвигают этот бугор по
|
||
сигналам собственного вращения, а взаимное торможение не даёт возникнуть второму
|
||
бугру. Так муха помнит направление, даже когда ориентир пропал из виду.
|
||
|
||
Здесь тот же механизм решает другую задачу. Кандидат на дальности 150 м — это
|
||
пять-десять лучей, и по одному кадру он неотличим от шума. Но поезд едет, и
|
||
объект, если он настоящий, остаётся **на одном и том же месте в тоннеле**, а не в
|
||
поле зрения. Поэтому треки живут в координате «расстояние по пути от точки
|
||
старта», сдвигаемой оценкой собственного движения (роль PEN), каждое совпадение
|
||
подкачивает улику (локальное возбуждение), несовпадение — утечка, а конкуренция
|
||
за одно и то же место не даёт плодить дубликаты (глобальное торможение APL/Δ7).
|
||
|
||
Это и есть подтверждение по нескольким кадрам, которого требует ТЗ, — но не как
|
||
эвристический фильтр, а как накопитель, который вытягивает слабый сигнал из шума.
|
||
"""
|
||
from __future__ import annotations
|
||
|
||
import itertools
|
||
from dataclasses import dataclass, field
|
||
|
||
import numpy as np
|
||
|
||
from .lobula import Candidate
|
||
|
||
|
||
@dataclass
|
||
class Track:
|
||
"""Подтверждаемая гипотеза о препятствии."""
|
||
|
||
id: int
|
||
s_world: float # положение вдоль пути от начала записи, м
|
||
u: float # смещение от оси пути, м
|
||
h: float # высота над рельсом, м
|
||
width: float
|
||
height: float
|
||
evidence: float = 0.0 # накопленная улика, 0…1
|
||
hits: int = 0
|
||
misses: int = 0
|
||
age: int = 0
|
||
first_d: float = 0.0 # на какой дальности впервые замечен
|
||
last_d: float = 0.0
|
||
last_n_rays: int = 0
|
||
novelty: float = 1.0
|
||
speed_lat: float = 0.0 # поперечная скорость, м/с
|
||
p_track: float = 0.0 # отсчёт считывания по треку, если оно подключено
|
||
history: list = field(default_factory=list)
|
||
|
||
# Сводка по всем наблюдениям трека, накапливаемая на лету. Считать её из
|
||
# `history` нельзя: описание трека нужно каждый кадр для всех живых
|
||
# треков, а разворачивать ради этого полсотни списков в массивы — заметная
|
||
# доля бюджета кадра. Все обновления здесь O(1).
|
||
n_obs: int = 0
|
||
s_sum: float = 0.0 # мировая привязка, подразумеваемая наблюдением
|
||
s_sq: float = 0.0
|
||
u_sum: float = 0.0
|
||
u_sq: float = 0.0
|
||
w_sum: float = 0.0 # вес наблюдения — ровно то, что идёт в улику
|
||
w_max: float = 0.0
|
||
p_sum: float = 0.0 # отсчёт считывания по кандидату, если оно есть
|
||
p_max: float = 0.0
|
||
p_n: int = 0 # сколько раз он вообще был
|
||
w_ema: float = 0.0 # он же, но со скользящим забыванием
|
||
r_sum: float = 0.0 # лучи, нормированные на ожидаемые для дальности
|
||
r_last: float = 0.0
|
||
ri_sum: float = 0.0 # суммы для наклона нормированных лучей по времени
|
||
i_sum: float = 0.0
|
||
i_sq: float = 0.0
|
||
|
||
@property
|
||
def confirmed(self) -> bool:
|
||
return self.evidence >= 0.5
|
||
|
||
@property
|
||
def w_mean(self) -> float:
|
||
"""Средний вес наблюдения за жизнь трека.
|
||
|
||
Улика — это тот же вес, но просуммированный и обрезанный единицей, и
|
||
обрезание её убивает: на реальном объекте медиана улики 1.000 и у
|
||
предмета, и у ложных треков (AUC 0.624), а у среднего веса 0.998
|
||
против 0.269 (AUC 1.000). Сумма отвечает «давно ли я на это смотрю»,
|
||
среднее — «на что именно я смотрю». EXPERIMENTS п. 15.
|
||
"""
|
||
return self.w_sum / max(self.n_obs, 1)
|
||
|
||
@property
|
||
def p_mean(self) -> float:
|
||
"""Средний покадровый отсчёт считывания MBON за жизнь трека.
|
||
|
||
Без переданной конвейеру модели отсчёта нет вовсе, и возвращать тут
|
||
ноль значило бы тихо погасить все треки разом. Поэтому в таком случае
|
||
отдаётся средний вес наблюдения — он считается всегда.
|
||
"""
|
||
return self.p_sum / self.p_n if self.p_n else self.w_mean
|
||
|
||
def distance(self, s_now: float) -> float:
|
||
return self.s_world - s_now
|
||
|
||
def observe(self, c: Candidate, s_obs: float, w: float) -> None:
|
||
"""Записать наблюдение в сводку.
|
||
|
||
Полезное здесь — `w_sum` и `p_sum`: улика суммируется и упирается в
|
||
потолок, а среднее того же веса различает предмет и обстановку и после
|
||
насыщения (EXPERIMENTS п. 15.1).
|
||
|
||
`s_obs` — место в тоннеле, которое подразумевает наблюдение: путь
|
||
плюс дальность. Копится ТОЛЬКО ради замеров: на полигоне его разброс
|
||
выглядит сильным признаком, а на настоящем объекте он перевёрнут,
|
||
потому что вставка стоит на дальности, посчитанной из той же оценки
|
||
движения (п. 15.4). В решении не участвует и участвовать не должен.
|
||
"""
|
||
i = float(self.n_obs)
|
||
self.n_obs += 1
|
||
self.s_sum += s_obs
|
||
self.s_sq += s_obs * s_obs
|
||
self.u_sum += c.u
|
||
self.u_sq += c.u * c.u
|
||
self.w_sum += w
|
||
self.w_max = max(self.w_max, w)
|
||
# Среднее за всю жизнь отстаёт у СБЛИЖАЮЩЕГОСЯ предмета: он
|
||
# появляется далеко и слабым, и первые десятки наблюдений тянут
|
||
# среднее вниз ещё долго после того, как он стал отчётливым.
|
||
# Скользящее забывание (полупериод около трёх наблюдений) отвечает
|
||
# на вопрос «каков он сейчас», а не «каким был с самого начала».
|
||
self.w_ema = w if self.n_obs == 1 else 0.8 * self.w_ema + 0.2 * w
|
||
p = c.extra.get("mbon") if c.extra else None
|
||
if p is not None:
|
||
self.p_n += 1
|
||
self.p_sum += float(p)
|
||
self.p_max = max(self.p_max, float(p))
|
||
r = c.n_rays / expected_rays(c.d)
|
||
self.r_sum += r
|
||
self.r_last = r
|
||
self.ri_sum += i * r
|
||
self.i_sum += i
|
||
self.i_sq += i * i
|
||
|
||
|
||
class CentralComplex:
|
||
"""Накопитель улик и менеджер треков."""
|
||
|
||
def __init__(self, *, gate_d: float = 4.0, gate_u: float = 1.2,
|
||
gain: float = 0.34, leak: float = 0.12,
|
||
leak_far: float | None = None, leak_far_from: float = 90.0,
|
||
inhibition: float = 0.05, max_misses: int = 12,
|
||
max_tracks: int = 48, use_shape: bool = True,
|
||
mbon_power: float = 1.0, mbon_blend: float = 1.0,
|
||
nov_fade_from: float = 0.0, nov_fade_to: float = 160.0):
|
||
self.use_shape = use_shape
|
||
self.mbon_power = mbon_power
|
||
self.mbon_blend = mbon_blend
|
||
self.nov_fade_from = nov_fade_from
|
||
self.nov_fade_to = nov_fade_to
|
||
self.gate_d = gate_d
|
||
self.gate_u = gate_u
|
||
self.gain = gain
|
||
self.leak = leak
|
||
# Утечка улики за промах, слабеющая с дальностью. Промах на 150 м
|
||
# — не свидетельство отсутствия, а свойство решётки: предмет там
|
||
# даёт единицы лучей и виден через кадр. Ровно по этой причине
|
||
# число лучей уже нормируется на ожидаемое (см. _quality), и
|
||
# промах нормировать надо так же.
|
||
#
|
||
# Считать легко: при доле попаданий p улика не убывает, только
|
||
# если p·gain·w > (1-p)·leak. На 120…160 м кандидат появляется в
|
||
# 62 % наблюдений, и при gain 0.34 и leak 0.12 нужно качество
|
||
# выше 0.22 — столько далёкое наблюдение не даёт никогда. Поэтому
|
||
# улика у далёкого трека не «чуть ниже порога», а около нуля, и
|
||
# снижением порога это не лечится (замер: 0.5 → 0.3 не изменил
|
||
# ничего). None — зависимости нет, как было.
|
||
self.leak_far = leak if leak_far is None else leak_far
|
||
self.leak_far_from = leak_far_from
|
||
self.inhibition = inhibition
|
||
self.max_misses = max_misses
|
||
self.max_tracks = max_tracks
|
||
self.tracks: list[Track] = []
|
||
self.s_world = 0.0
|
||
self._ids = itertools.count(1)
|
||
|
||
# ------------------------------------------------------------------ обновление
|
||
|
||
def _leak_at(self, d: float) -> float:
|
||
"""Утечка на дальности d: линейно от leak до leak_far к 200 м."""
|
||
span = 200.0 - self.leak_far_from
|
||
if span <= 0 or d <= self.leak_far_from:
|
||
return self.leak
|
||
f = min((d - self.leak_far_from) / span, 1.0)
|
||
return self.leak + (self.leak_far - self.leak) * f
|
||
|
||
def update(self, candidates: list[Candidate], ds: float, dt: float) -> list[Track]:
|
||
"""Сдвинуть мир на `ds`, сопоставить кандидатов, обновить улики."""
|
||
self.s_world += ds
|
||
|
||
# допуск по дальности растёт с расстоянием: там и разрешение грубее,
|
||
# и ошибка оценки собственного движения успевает накопиться
|
||
for t in self.tracks:
|
||
t.age += 1
|
||
|
||
used = set()
|
||
for t in self.tracks:
|
||
d_pred = t.distance(self.s_world)
|
||
best, best_cost = None, None
|
||
for k, c in enumerate(candidates):
|
||
if k in used:
|
||
continue
|
||
gd = self.gate_d + 0.05 * max(c.d, 0.0)
|
||
dd = abs(c.d - d_pred)
|
||
du = abs(c.u - t.u)
|
||
if dd > gd or du > self.gate_u + 0.4:
|
||
continue
|
||
cost = dd / gd + du / (self.gate_u + 0.4)
|
||
if best_cost is None or cost < best_cost:
|
||
best, best_cost = k, cost
|
||
if best is None:
|
||
t.misses += 1
|
||
t.evidence = max(0.0, t.evidence
|
||
- self._leak_at(t.distance(self.s_world)))
|
||
continue
|
||
|
||
c = candidates[best]
|
||
used.add(best)
|
||
t.hits += 1
|
||
t.misses = 0
|
||
w = _quality(c, self.use_shape, self.mbon_power, self.mbon_blend,
|
||
self.nov_fade_from, self.nov_fade_to)
|
||
t.observe(c, self.s_world + c.d, w)
|
||
t.evidence = min(1.0, t.evidence + self.gain * w)
|
||
if dt > 1e-3:
|
||
t.speed_lat = 0.6 * t.speed_lat + 0.4 * (c.u - t.u) / dt
|
||
# положение сглаживается: ближние наблюдения точнее дальних
|
||
a = float(np.clip(0.5 * (60.0 / max(c.d, 20.0)), 0.15, 0.6))
|
||
t.s_world = (1 - a) * t.s_world + a * (self.s_world + c.d)
|
||
t.u = (1 - a) * t.u + a * c.u
|
||
t.h = (1 - a) * t.h + a * c.h
|
||
t.width = max(t.width * 0.7, c.width)
|
||
t.height = max(t.height * 0.7, c.height)
|
||
t.last_d = c.d
|
||
t.last_n_rays = c.n_rays
|
||
t.novelty = 0.7 * t.novelty + 0.3 * c.novelty
|
||
t.history.append((c.d, c.u, c.n_rays))
|
||
if len(t.history) > 64:
|
||
del t.history[:-64]
|
||
|
||
# новые гипотезы из несопоставленных кандидатов
|
||
for k, c in enumerate(candidates):
|
||
if k in used or len(self.tracks) >= self.max_tracks:
|
||
continue
|
||
t = Track(id=next(self._ids), s_world=self.s_world + c.d, u=c.u, h=c.h,
|
||
width=c.width, height=c.height, first_d=c.d, last_d=c.d,
|
||
last_n_rays=c.n_rays, novelty=c.novelty)
|
||
w = _quality(c, self.use_shape, self.mbon_power, self.mbon_blend,
|
||
self.nov_fade_from, self.nov_fade_to)
|
||
t.observe(c, self.s_world + c.d, w)
|
||
t.evidence = self.gain * w
|
||
t.hits = 1
|
||
self.tracks.append(t)
|
||
|
||
# глобальное торможение: сильный трек подавляет соседей по месту
|
||
self._inhibit()
|
||
|
||
self.tracks = [t for t in self.tracks
|
||
if t.misses <= self.max_misses and t.evidence > 0.02
|
||
and t.distance(self.s_world) > -5.0]
|
||
self.tracks.sort(key=lambda t: -t.evidence)
|
||
del self.tracks[self.max_tracks:]
|
||
return self.tracks
|
||
|
||
def _inhibit(self) -> None:
|
||
if len(self.tracks) < 2 or self.inhibition <= 0:
|
||
return
|
||
order = sorted(self.tracks, key=lambda t: -t.evidence)
|
||
for i, strong in enumerate(order):
|
||
for weak in order[i + 1:]:
|
||
if (abs(strong.s_world - weak.s_world) < self.gate_d
|
||
and abs(strong.u - weak.u) < self.gate_u):
|
||
weak.evidence = max(0.0, weak.evidence - self.inhibition)
|
||
|
||
def confirmed(self) -> list[Track]:
|
||
out = [t for t in self.tracks if t.confirmed and t.distance(self.s_world) > 0]
|
||
out.sort(key=lambda t: t.distance(self.s_world))
|
||
return out
|
||
|
||
|
||
def expected_rays(d: float) -> float:
|
||
"""Сколько лучей предмет даёт на дальности d просто по устройству решётки.
|
||
|
||
Нужна в двух местах — в весе наблюдения и в описании трека, — и оба
|
||
раза за одним и тем же: дальний предмет даёт мало лучей не потому, что
|
||
он сомнительный, а потому, что так устроена развёртка.
|
||
"""
|
||
return max(3.0, 2500.0 / max(d, 5.0) ** 1.4)
|
||
|
||
|
||
def _quality(c: Candidate, use_shape: bool = True, mbon_power: float = 1.0,
|
||
mbon_blend: float = 1.0, nov_fade_from: float = 0.0,
|
||
nov_fade_to: float = 160.0) -> float:
|
||
"""Вес одного наблюдения: сколько улики оно добавляет.
|
||
|
||
Дальний объект даёт мало лучей не потому, что он сомнительный, а потому что
|
||
так устроена решётка, — поэтому число лучей нормируется на ожидаемое для
|
||
этой дальности. Остальные множители отделяют предмет от конструкции тоннеля:
|
||
предмет целиком помещается в габарит и компактен вдоль пути, а лоток или
|
||
стена тянутся дальше и в стороны. Новизна из грибовидного тела входит сюда
|
||
же множителем.
|
||
"""
|
||
support = float(np.clip(c.n_rays / expected_rays(c.d), 0.25, 1.0))
|
||
|
||
# Контраст к фону — главная улика вблизи и недоступная вдали. На 170 м
|
||
# кольцо окружения ламины упирается в стену тоннеля, стоящую на той же
|
||
# дальности, и `gap` структурно равен нулю: предмет не «ближе окружения»,
|
||
# он «на той же дальности, что окружение». Замер это подтверждает —
|
||
# у вставленного человека на 140…185 м gap = 0.0 во ВСЕХ кадрах.
|
||
#
|
||
# Там, где контраст не измеряется, его заменяет опора веерного тела:
|
||
# возвращались ли лучи из этой точки пути кадр за кадром. Берётся
|
||
# максимум — вблизи решает контраст, вдали накопление.
|
||
contrast = float(np.clip(c.gap / 3.0, 0.2, 1.0))
|
||
acc = float(c.extra.get("acc_support", 0.0)) if c.extra else 0.0
|
||
if acc > 0.0:
|
||
contrast = max(contrast, float(np.clip(acc, 0.2, 1.0)))
|
||
if use_shape:
|
||
whole = float(np.clip((c.containment - 0.25) / 0.45, 0.05, 1.0))
|
||
span = max(c.width, c.height, 0.2)
|
||
# Предмет или человек, лежащий в колее вдоль пути (depth ~ 1.8 м при малой высоте),
|
||
# лежит прямо на полотне перед поездом — не штрафуем его как конструкцию тоннеля:
|
||
is_lying_on_track = (abs(c.u) <= 0.85 and c.h <= 0.40 and c.depth <= 2.2)
|
||
if is_lying_on_track:
|
||
compact = 1.0
|
||
else:
|
||
compact = float(np.clip(1.5 - c.depth / (3.0 * span), 0.1, 1.0))
|
||
# опора снизу: упавший предмет, человек, камень стоят на полотне, а знак,
|
||
# лоток или кронштейн висят на стене, и под ними пусто
|
||
grounded = float(np.clip(1.25 - c.h_min / 1.2, 0.15, 1.0))
|
||
else:
|
||
whole = compact = grounded = 1.0
|
||
# Знакомость на большой дальности не просто бесполезна, а вредна. Замер:
|
||
# на 120…185 м у вставленного человека новизна 0.150, а у окружающей
|
||
# обстановки 0.199, разделяющая способность AUC 0.293 — то есть
|
||
# признак перевёрнут. Причина понятна: на шести лучах дескриптор
|
||
# вырождается, и память узнаёт в предмете любую далёкую конструкцию.
|
||
# Поэтому вклад новизны гасится с дальностью, а вблизи, где она и
|
||
# работает, остаётся как был. 0 — не гасить (EXPERIMENTS п. 12.3).
|
||
novel = novelty_gain(c.novelty)
|
||
if nov_fade_from > 0.0 and c.d > nov_fade_from:
|
||
span = max(nov_fade_to - nov_fade_from, 1e-3)
|
||
w = 1.0 - min((c.d - nov_fade_from) / span, 1.0)
|
||
novel = novel ** w
|
||
hand = support * contrast * whole * compact * grounded
|
||
|
||
# Обученное считывание MBON, если оно есть. Модель видит те же признаки,
|
||
# что и шесть множителей выше, плюс тень и интенсивность, которых в ручной
|
||
# формуле нет вовсе. Знакомость в неё НЕ входит и остаётся отдельным
|
||
# каналом: физику решает модель, конкретный тоннель — память.
|
||
#
|
||
# `mbon_blend` — геометрическое смешивание с ручной формулой: 1 — только
|
||
# модель, 0 — только руками, между ними всё промежуточное. Нужно затем,
|
||
# чтобы размен «модель против ручной формулы» мерился, а не объявлялся.
|
||
p = c.extra.get("mbon") if c.extra else None
|
||
if p is not None and mbon_blend > 0.0:
|
||
pm = float(np.clip(p, 1e-4, 1.0)) ** max(mbon_power, 1e-3)
|
||
b = float(np.clip(mbon_blend, 0.0, 1.0))
|
||
hand = pm ** b * max(hand, 1e-4) ** (1.0 - b)
|
||
return float(np.clip(hand * novel, 0.0, 1.0))
|
||
|
||
|
||
def novelty_gain(novelty: float, lo: float = 0.15, hi: float = 0.50,
|
||
floor: float = 0.05) -> float:
|
||
"""Ответ MBON → множитель улики.
|
||
|
||
Пороги взяты по измеренному разделению (`tools/tune_memory.py`): знакомая
|
||
обстановка даёт новизну около 0.14, реальный объект — около 0.50. Отображение
|
||
делает разницу резкой, но оставляет ненулевой пол: даже похожий на привычную
|
||
конструкцию предмет должен накапливать улику, просто медленнее.
|
||
"""
|
||
return float(np.clip((novelty - lo) / (hi - lo), floor, 1.0))
|