422 lines
26 KiB
Python
422 lines
26 KiB
Python
"""Сборка конвейера: от облака точек до решения.
|
||
|
||
Порядок стадий повторяет путь сигнала в мозге мухи:
|
||
|
||
ретина → стабилизация → ламина → медулла/лобулярная пластинка
|
||
→ лобула → грибовидное тело → центральный комплекс → нисходящие нейроны
|
||
|
||
Конвейер держит состояние между кадрами (поза, коридор, собственное движение,
|
||
треки), поэтому один экземпляр обслуживает один поток лидарных данных.
|
||
"""
|
||
from __future__ import annotations
|
||
|
||
import time
|
||
from dataclasses import dataclass, field
|
||
|
||
import numpy as np
|
||
|
||
from . import lamina
|
||
from .cdr import PointCloud2
|
||
from .central_complex import CentralComplex
|
||
from .descending import Decision, DescendingNeurons
|
||
from .geometry import (STRAIGHT, Corridor, RailPlane, TrackFrame, fit_corridor,
|
||
fit_rail_plane)
|
||
from .fan_body import FanBody
|
||
from .lobula import Candidate, find_candidates, gauge_mask
|
||
from .medulla import EgoMotion, EgoMotionEstimator, EmdBank, looming
|
||
from .mbon_readout import MbonReadout
|
||
from .mushroom_body import Habituation, HabituationConfig, MushroomBody, NoMemory
|
||
from .retina import RangeImage, ScanLayout
|
||
|
||
|
||
@dataclass
|
||
class Params:
|
||
"""Все настройки конвейера в одном месте."""
|
||
|
||
fov_deg: float = 30.0 # полусектор обработки по азимуту
|
||
half_width: float = 1.6 # полуширина габарита, м
|
||
h_lo: float = 0.28 # нижняя граница габарита над рельсом, м
|
||
h_lo_core: float = 0.16 # нижняя граница в колее (|u| <= core_width)
|
||
core_width: float = 0.85 # полуширина междупутья/колеи, м
|
||
k_sigma: float = 0.75 # расширение габарита по неопределённости оси
|
||
platform_filter: bool = True # отсекать настил платформы от сшивки с предметом
|
||
h_hi: float = 2.3 # верхняя граница, м
|
||
ctx_up: float = 4.0 # насколько контекст лезет выше габарита, м
|
||
# Разделение фигуры и фона по движению (см. lobula.split_by_figure):
|
||
# порог скорости сближения вдоль луча, в долях пройденного пути.
|
||
# Поднимает рабочую дальность с 62 до 80 м, но вчетверо увеличивает ложные
|
||
# тревоги: в тоннеле полно поверхностей, которые приближаются так же, как
|
||
# предмет. 0 — выключено; рабочее значение 0.6. Кривая — EXPERIMENTS п. 9.4.
|
||
split_adv: float = 0.0
|
||
# Разделение фигуры и фона по контрасту ламины (см. lobula.split_by_figure):
|
||
# порог «насколько ближе своего окружения», м. Гладкая стена даёт ноль по
|
||
# построению центр-окружения, предмет на стене — ступеньку. Именно это
|
||
# вытаскивает предмет из компоненты, растёкшейся вдоль стены круглого
|
||
# тоннеля, где покадровый тракт слеп целиком. 0 — выключено.
|
||
split_gap: float = 6.0
|
||
split_near: float = 55.0 # ближе этого резать только растянутое
|
||
# Разброс дальности внутри компоненты, выше которого её режут и в
|
||
# ближней зоне: предмет по дальности занимает десятки сантиметров,
|
||
# кромка платформы и стена — десятки метров. 0 — не резать вблизи
|
||
# вовсе. Замер — EXPERIMENTS п. 12.7.
|
||
near_long: float = 0.0
|
||
split_top: int = 1 # сколько фигур выносить из одной компоненты
|
||
# Накопление слабых улик в координатах пути (см. fan_body.py). Заменяет
|
||
# недоступный на большой дальности контраст к фону: на 140…185 м `gap`
|
||
# структурно равен нулю, и без накопления улика не растёт вовсе.
|
||
enable_accumulator: bool = True
|
||
acc_near: float = 55.0 # ближе этого покадровый тракт и так справляется
|
||
acc_gain: float = 1.5 # опора накопителя, засчитываемая за полный контраст
|
||
# Привыкание внутри проезда (см. mushroom_body.Habituation). Замысел: гасить
|
||
# формы, встретившиеся в НЕСКОЛЬКИХ разных точках пути, то есть штатную
|
||
# повторяющуюся обстановку, без единого прошлого проезда.
|
||
#
|
||
# ПО УМОЛЧАНИЮ ВЫКЛЮЧЕНО — измерено, что избирательности у механизма нет.
|
||
# При ёмкости, достаточной чтобы популяция не насыщалась (20 000 клеток),
|
||
# привыкание не меняет ничего: 7.5 ложных трека на км против тех же 7.5.
|
||
# Весь эффект маленькой популяции (5.9 на км) оказался насыщением, которое
|
||
# давит без разбора: новизна вставленного предмета падала до 0.24 при
|
||
# медианной новизне кандидата 0.43…0.71, то есть предмет подавлялся
|
||
# СИЛЬНЕЕ обстановки. Огрубление признаков и резкий отсчёт MBON не помогли.
|
||
# Разбор — EXPERIMENTS п. 10.
|
||
enable_habituation: bool = False
|
||
hab_rate: float = 0.25 # депрессия за одно новое место
|
||
hab_place_m: float = 5.0 # насколько разнести места, чтобы считать разными
|
||
hab_recover_m: float = 800.0 # путь, за который привыкание сходит наполовину
|
||
hab_n_kc: int = 20000 # ёмкость короткой памяти, клеток Кеньона
|
||
hab_read_q: float = 0.75 # квантиль отсчёта MBON
|
||
hab_quant: float = 0.5 # огрубление признаков, в долях разброса
|
||
# Обученное считывание MBON (см. mbon_readout.py). Заменяет ручную формулу
|
||
# веса улики в центральном комплексе там, где модель передана конвейеру;
|
||
# без модели всё работает по-прежнему.
|
||
enable_mbon: bool = True
|
||
mbon_power: float = 1.0 # резкость: p**power перед смешиванием
|
||
mbon_blend: float = 1.0 # 1 — только модель, 0 — только ручная формула
|
||
# Гашение знакомости с дальностью: за `nov_fade_from` её вклад падает
|
||
# до нуля к `nov_fade_to`. За 90 м новизна измеримо ВРЕДНА — у
|
||
# вставленного человека на 120…185 м она 0.150 против 0.199 у
|
||
# обстановки, AUC 0.293, то есть признак перевёрнут. Гашение подняло
|
||
# P@100 с 0.31 до 0.48 и P@150 с нуля до 0.32 ценой 3.3 → 5.6 ложных
|
||
# трека на км на ЗНАКОМОЙ линии и ноля на незнакомой. 0 — не гасить,
|
||
# это тихий режим для линии с обученной памятью. EXPERIMENTS п. 12.3.
|
||
nov_fade_from: float = 90.0
|
||
nov_fade_to: float = 160.0
|
||
# Поправка оценки модели на распространённость предметов: за этой
|
||
# дальностью из логита вычитается типичный для полосы. 0 — не
|
||
# поправлять. Замер — EXPERIMENTS п. 12.3.
|
||
mbon_prior_from: float = 0.0
|
||
# Пороги решения (см. descending.py). Вынесены сюда, потому что обученное
|
||
# считывание меняет цену улики: когда ложные тревоги падают, запас можно
|
||
# потратить на дальность, опустив порог. Замер — EXPERIMENTS п. 11.
|
||
# Считывание по треку (см. track_readout.py): геометрическое смешивание
|
||
# обученного отсчёта с уликой перед сравнением с порогом. 0 — решает
|
||
# только улика, как раньше. Замер — EXPERIMENTS п. 15.
|
||
track_blend: float = 0.0
|
||
track_gate: float = 0.0 # жёсткий порог по тому же отсчёту
|
||
# Чем считать «насколько это предмет» по треку: `w_mean` — средний вес
|
||
# наблюдения за всю жизнь, `w_ema` — он же со скользящим забыванием,
|
||
# `p_mean` — средний покадровый отсчёт считывания, `model` — обученное
|
||
# считывание по треку (нужен файл модели). Замер — п. 15.
|
||
track_score: str = "w_mean"
|
||
warn_evidence: float = 0.5 # улика, с которой трек поднимает тревогу
|
||
clear_evidence: float = 0.3 # и до которой держится (гистерезис)
|
||
emergency_evidence: float = 0.75
|
||
# Наблюдений, без которых трек не считается. Три подтверждения стоили
|
||
# дальности на предметах среднего размера: на 60…100 м они видны через
|
||
# кадр, и третьего приходится ждать слишком долго. Два подтверждения
|
||
# подняли рабочую дальность человека сидя с 20 до 80 м, а чемодана —
|
||
# с 20 до 62 м, и не стоили ни одного лишнего ложного трека
|
||
# (3.3 на км в обоих случаях). Замер — EXPERIMENTS п. 11.3.
|
||
min_hits: int = 2
|
||
novelty_floor: float = 0.10
|
||
warn_far: float = 0.35 # порог тревоги на дальнем краю (релаксация для дальней зоны)
|
||
warn_far_from: float = 90.0 # с какой дальности он начинает падать
|
||
leak_far: float = 0.06 # утечка улики на дальнем краю (компенсирует прореживание лучей)
|
||
leak_far_from: float = 90.0 # с какой дальности она начинает падать
|
||
d_min: float = 4.0
|
||
d_max: float = 220.0
|
||
# Сколько лучей образуют кандидата. Вблизи обрывок из трёх лучей — это
|
||
# кусок чего-то большего, и он дробит настоящий предмет: разрешить
|
||
# тройки везде стоит P@50 у человека 0.70 → 0.64. Вдали три луча —
|
||
# всё, что есть (медиана на 150 м — шесть). Порог по дальности берёт
|
||
# дальний выигрыш целиком (P@150 0.32 → 0.37, человек сидя 80 → 100 м
|
||
# рабочей дальности) и не стоит ни одного лишнего ложного трека ни на
|
||
# знакомой линии, ни на незнакомой. EXPERIMENTS п. 12.5.
|
||
min_rays: int = 4
|
||
min_rays_far: int = 3 # 0 — не различать
|
||
min_rays_far_from: float = 90.0
|
||
calib_frames: int = 12
|
||
novelty_gate: float = 0.0 # ниже этой новизны кандидат отбрасывается сразу
|
||
device: str = "auto" # вычислительное устройство: 'auto' (NVIDIA GPU при наличии), 'cuda', 'cpu'
|
||
enable_motion: bool = True
|
||
enable_memory: bool = True
|
||
enable_looming: bool = False # T4/T5 + LPLC2: нужны для оценки надвигания и для
|
||
# схемы мозга; на решение пока не влияют
|
||
|
||
# Переключатели для абляции: каждый отключает один механизм, оставляя
|
||
# остальные. Нужны, чтобы вклад механизма измерялся, а не декларировался.
|
||
use_corridor: bool = True # ось пути; False — прямой коридор вперёд
|
||
use_shape: bool = True # целостность, компактность, опора снизу
|
||
use_tracking: bool = True # накопление улик по кадрам
|
||
|
||
|
||
@dataclass
|
||
class FrameResult:
|
||
"""Всё, что конвейер узнал за один кадр."""
|
||
|
||
stamp: float
|
||
decision: Decision
|
||
candidates: list[Candidate]
|
||
ego: EgoMotion | None
|
||
plane: RailPlane
|
||
corridor: Corridor
|
||
tf: TrackFrame | None = None
|
||
lam: lamina.LaminaOutput | None = None
|
||
emd: np.ndarray | None = None # (4, h, w) отклики T4/T5
|
||
loom: np.ndarray | None = None # (h, w) отклик LPLC2
|
||
hab: float = 0.0 # привыкание популяции KC, 0…1
|
||
timings: dict = field(default_factory=dict)
|
||
|
||
@property
|
||
def total_ms(self) -> float:
|
||
return sum(self.timings.values())
|
||
|
||
|
||
class FlyGuard:
|
||
"""Полный конвейер обнаружения посторонних объектов."""
|
||
|
||
def __init__(self, params: Params | None = None,
|
||
memory: MushroomBody | None = None,
|
||
layout: ScanLayout | None = None,
|
||
readout: "MbonReadout | None" = None,
|
||
track_readout=None):
|
||
self.p = params or Params()
|
||
from .device import get_device
|
||
self.device = get_device(self.p.device)
|
||
self.memory = memory if (memory is not None and self.p.enable_memory) else NoMemory()
|
||
self.readout = readout if (readout is not None and self.p.enable_mbon) else None
|
||
self.layout_full = layout
|
||
self.layout: ScanLayout | None = None
|
||
self.cols: slice | None = None
|
||
|
||
self.plane: RailPlane | None = None
|
||
self._prev_r: np.ndarray | None = None
|
||
self.corridor: Corridor | None = None
|
||
self.ego_est = EgoMotionEstimator()
|
||
self.fan: FanBody | None = None
|
||
self.s_path = 0.0 # пройденный путь, м — координата привыкания
|
||
self.hab = (Habituation(HabituationConfig(
|
||
n_kc=self.p.hab_n_kc, sparsity=80.0 / max(self.p.hab_n_kc, 1),
|
||
rate=self.p.hab_rate, place_m=self.p.hab_place_m,
|
||
read_q=self.p.hab_read_q, quant=self.p.hab_quant,
|
||
recover_m=self.p.hab_recover_m))
|
||
if self.p.enable_habituation else None)
|
||
self.emd_bank = EmdBank()
|
||
self.cx = CentralComplex(use_shape=self.p.use_shape,
|
||
mbon_power=self.p.mbon_power,
|
||
mbon_blend=self.p.mbon_blend,
|
||
leak_far=self.p.leak_far,
|
||
leak_far_from=self.p.leak_far_from,
|
||
nov_fade_from=self.p.nov_fade_from,
|
||
nov_fade_to=self.p.nov_fade_to)
|
||
# без накопления улик трек подтверждается первым же наблюдением
|
||
self.dn = (DescendingNeurons(
|
||
warn_evidence=self.p.warn_evidence,
|
||
clear_evidence=self.p.clear_evidence,
|
||
emergency_evidence=self.p.emergency_evidence,
|
||
min_hits=self.p.min_hits,
|
||
novelty_floor=self.p.novelty_floor,
|
||
warn_far=self.p.warn_far,
|
||
warn_far_from=self.p.warn_far_from,
|
||
track_blend=self.p.track_blend,
|
||
track_gate=self.p.track_gate,
|
||
track_score=self.p.track_score,
|
||
track_readout=track_readout) if self.p.use_tracking
|
||
else DescendingNeurons(warn_evidence=0.0, clear_evidence=0.0,
|
||
emergency_evidence=0.0, min_hits=1))
|
||
self._calib: list[PointCloud2] = []
|
||
self.frames_seen = 0
|
||
|
||
# ------------------------------------------------------------------ калибровка
|
||
|
||
def _ensure_layout(self, pc: PointCloud2) -> bool:
|
||
"""Накопить кадры и откалибровать решётку. False — ещё не готов."""
|
||
if self.layout is not None:
|
||
return True
|
||
if self.layout_full is None:
|
||
self._calib.append(pc)
|
||
if len(self._calib) < self.p.calib_frames:
|
||
return False
|
||
self.layout_full = ScanLayout.calibrate(self._calib)
|
||
self._calib.clear()
|
||
self.cols = self.layout_full.column_slice(self.p.fov_deg)
|
||
self.layout = self.layout_full.sub(self.cols)
|
||
return True
|
||
|
||
# ------------------------------------------------------------------ накопление
|
||
|
||
def _accumulate(self, tf, inside, lat, ds: float,
|
||
cands: list[Candidate]) -> None:
|
||
"""Досыпать кадр в веерное тело и проставить опору каждому кандидату."""
|
||
if not self.p.enable_accumulator:
|
||
return
|
||
if self.fan is None:
|
||
self.fan = FanBody(d_max=self.p.d_max, half_width=self.p.half_width,
|
||
h_lo=self.p.h_lo, h_hi=self.p.h_hi,
|
||
d_near=self.p.acc_near)
|
||
self.fan.update(tf.d[inside], lat[inside], tf.h[inside], ds)
|
||
if not cands:
|
||
return
|
||
d = np.array([c.d for c in cands], np.float32)
|
||
u = np.array([c.u for c in cands], np.float32)
|
||
g = max(self.p.acc_gain, 1e-3)
|
||
for c, s in zip(cands, self.fan.support_at(d, u)):
|
||
c.extra["acc_support"] = float(s) / g
|
||
|
||
# ------------------------------------------------------------------ фигура и фон
|
||
|
||
MIN_DS = 0.25 # м: меньше — шум дальности забивает разницу
|
||
|
||
def _advance(self, tf, ds: float):
|
||
"""Скорость сближения вдоль каждого луча, в долях пройденного пути.
|
||
|
||
Вдоль фиксированного луча стена, параллельная движению, не приближается
|
||
вовсе: поезд едет, точка пересечения скользит по стене, дальность
|
||
остаётся прежней. Предмет, обращённый к поезду, приближается ровно на
|
||
пройденный путь. Отсюда 0 у фона и 1 у фигуры.
|
||
|
||
Сравниваются два соседних кадра в одной решётке: столбец отвечает
|
||
фиксированному азимуту, поэтому достаточно вычитания без перепроекции.
|
||
Рысканье в кривой за кадр меньше шага решётки (0.06° при радиусе
|
||
1300 м против шага 0.1°), поэтому им пренебрегаем.
|
||
"""
|
||
cur = np.where(tf.valid, tf.r, np.nan).astype(np.float32)
|
||
prev, self._prev_r = self._prev_r, cur
|
||
if prev is None or prev.shape != cur.shape or ds < self.MIN_DS:
|
||
return None
|
||
with np.errstate(invalid="ignore"):
|
||
a = (prev - cur) / ds
|
||
return np.nan_to_num(a, nan=0.0, posinf=0.0, neginf=0.0)
|
||
|
||
# ------------------------------------------------------------------ основной такт
|
||
|
||
def process(self, pc: PointCloud2, *, keep_debug: bool = False) -> FrameResult | None:
|
||
"""Обработать один кадр. None, пока идёт калибровка решётки."""
|
||
t = _Timer()
|
||
if not self._ensure_layout(pc):
|
||
return None
|
||
self.frames_seen += 1
|
||
|
||
with t("retina"):
|
||
img: RangeImage = self.layout_full.project(pc, self.cols)
|
||
|
||
with t("stabilize"):
|
||
self.plane = fit_rail_plane(img, self.layout, prev=self.plane)
|
||
tf = TrackFrame(img, self.layout, self.plane)
|
||
|
||
with t("corridor"):
|
||
self.corridor = (fit_corridor(tf, prev=self.corridor) if self.p.use_corridor
|
||
else STRAIGHT)
|
||
|
||
with t("lamina"):
|
||
dev = self.device
|
||
if dev.startswith("cuda"):
|
||
from .device import is_cuda_available
|
||
if not is_cuda_available():
|
||
self.device = dev = "cpu"
|
||
lam = lamina.process(tf.r, tf.valid, device=dev)
|
||
|
||
with t("ego"):
|
||
ego = (self.ego_est.update(tf, pc.stamp) if self.p.enable_motion
|
||
else EgoMotion(0.0, 0.0, 0.0, 0.0, 0.1))
|
||
|
||
with t("figure"):
|
||
advance = self._advance(tf, ego.ds)
|
||
|
||
emd = loom = None
|
||
if self.p.enable_looming:
|
||
with t("medulla"):
|
||
emd = self.emd_bank.update(lam.on)
|
||
loom = looming(emd)
|
||
|
||
with t("lobula"):
|
||
masks = gauge_mask(tf, self.corridor, half_width=self.p.half_width,
|
||
h_lo=self.p.h_lo, h_hi=self.p.h_hi,
|
||
d_min=self.p.d_min, d_max=self.p.d_max,
|
||
k_sigma=self.p.k_sigma,
|
||
h_lo_core=self.p.h_lo_core,
|
||
core_width=self.p.core_width,
|
||
platform_filter=self.p.platform_filter)
|
||
cands = find_candidates(tf, lam, self.corridor, masks=masks,
|
||
half_width=self.p.half_width, h_lo=self.p.h_lo,
|
||
h_hi=self.p.h_hi, ctx_up=self.p.ctx_up,
|
||
split_adv=self.p.split_adv,
|
||
split_gap=self.p.split_gap,
|
||
split_near=self.p.split_near,
|
||
near_long=self.p.near_long,
|
||
split_top=self.p.split_top, advance=advance,
|
||
d_min=self.p.d_min,
|
||
d_max=self.p.d_max, min_rays=self.p.min_rays,
|
||
min_rays_far=self.p.min_rays_far,
|
||
min_rays_far_from=self.p.min_rays_far_from,
|
||
k_sigma=self.p.k_sigma,
|
||
h_lo_core=self.p.h_lo_core,
|
||
core_width=self.p.core_width,
|
||
platform_filter=self.p.platform_filter)
|
||
|
||
with t("mushroom"):
|
||
cands = self.memory.annotate(cands)
|
||
self.s_path += ego.ds
|
||
if self.hab is not None:
|
||
self.hab.update(cands, self.s_path, ego.ds)
|
||
if self.p.novelty_gate > 0:
|
||
cands = [c for c in cands if c.novelty >= self.p.novelty_gate]
|
||
|
||
with t("fan"):
|
||
# Накопитель идёт ПОСЛЕ памяти и мимо неё: память обучена на
|
||
# покадровых кандидатах, а у накопленного скопления нет ни
|
||
# контраста к фону, ни интенсивности — её признаки для него
|
||
# не определены. Судить о нём надо по тому, что у него есть:
|
||
# по числу попаданий в одну точку пути и по размеру.
|
||
self._accumulate(tf, masks[0], masks[1], ego.ds, cands)
|
||
|
||
with t("readout"):
|
||
# Строго после накопителя: опора веерного тела входит в признаки
|
||
# считывания, и на большой дальности это его главный вход.
|
||
if self.readout is not None:
|
||
self.readout.annotate(cands, self.p.mbon_prior_from)
|
||
|
||
with t("central"):
|
||
self.cx.update(cands, ego.ds, ego.dt)
|
||
|
||
with t("descending"):
|
||
decision = self.dn.decide(self.cx, ego.speed)
|
||
|
||
return FrameResult(stamp=pc.stamp, decision=decision, candidates=cands,
|
||
ego=ego, plane=self.plane, corridor=self.corridor,
|
||
tf=tf if keep_debug else None,
|
||
lam=lam if keep_debug else None,
|
||
emd=emd, loom=loom,
|
||
hab=self.hab.level if self.hab is not None else 0.0,
|
||
timings=t.result)
|
||
|
||
|
||
class _Timer:
|
||
"""Замер времени по стадиям — нужен для отчёта о задержке."""
|
||
|
||
def __init__(self):
|
||
self.result: dict[str, float] = {}
|
||
self._name = None
|
||
self._t0 = 0.0
|
||
|
||
def __call__(self, name: str):
|
||
self._name = name
|
||
return self
|
||
|
||
def __enter__(self):
|
||
self._t0 = time.perf_counter()
|
||
return self
|
||
|
||
def __exit__(self, *exc):
|
||
self.result[self._name] = (time.perf_counter() - self._t0) * 1e3
|
||
return False
|