670 lines
43 KiB
Python
670 lines
43 KiB
Python
"""Сборка конвейера: от облака точек до решения.
|
||
|
||
Порядок стадий повторяет путь сигнала в мозге мухи:
|
||
|
||
ретина → стабилизация → ламина → медулла/лобулярная пластинка
|
||
→ лобула → грибовидное тело → центральный комплекс → нисходящие нейроны
|
||
|
||
Конвейер держит состояние между кадрами (поза, коридор, собственное движение,
|
||
треки), поэтому один экземпляр обслуживает один поток лидарных данных.
|
||
"""
|
||
from __future__ import annotations
|
||
|
||
import threading
|
||
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, cluster_by_depth, 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, forward_azimuth, rotate_cloud
|
||
|
||
|
||
@dataclass
|
||
class Params:
|
||
"""Все настройки конвейера в одном месте."""
|
||
|
||
fov_deg: float = 30.0 # полусектор обработки по азимуту
|
||
# Полуширина габарита. 1.2, а не 1.6: так габарит задают организаторы в
|
||
# синтетике 24.09 — «у края» кончается на 1.13 м от оси, «вне габарита,
|
||
# но близко» начинается с 1.14. При 1.6 второй давал ложную тревогу, а
|
||
# в `doubleT_obstacle` в полосу заходили колонны между путями. Полигон
|
||
# парно +43 / −33 наблюдения, посторонних 793 → 742 (EXPERIMENTS п. 18.4).
|
||
half_width: float = 1.2
|
||
h_lo: float = 0.28 # нижняя граница габарита над рельсом, м
|
||
# Три правки габарита (Zhirik1337, 22.09), каждая мерилась отдельно на
|
||
# двух сценах и полигоне (EXPERIMENTS п. 16):
|
||
# h_lo_core — пол в колее: между рельсами (|u| ≤ core_width) нижняя
|
||
# граница опускается, чтобы не терять лежащего человека.
|
||
# ПРИНЯТО, но только дальше core_from: ближе в полосу
|
||
# 0.16…0.28 м попадают головки рельсов, рельс собирается
|
||
# в одну компоненту от кабины, и предмет выбрасывается
|
||
# вместе с ней. Лежачий P@50 0.27 → 0.50;
|
||
# k_sigma — габарит расширяется там, где ось пути известна хуже
|
||
# (в кривых вдали); 0 — не расширять. Отвергнуто;
|
||
# platform_filter — вырезать из габарита настил платформы. Отвергнуто:
|
||
# в тоннеле без платформы режет стену на обрывки.
|
||
h_lo_core: float = 0.16
|
||
core_width: float = 0.85 # полуширина колеи, м
|
||
core_from: float = 30.0 # ближе этой дальности пол в колее не опускается, м
|
||
k_sigma: float = 0.0
|
||
platform_filter: bool = False
|
||
h_hi: float = 2.3 # верхняя граница, м
|
||
# Верхняя секция габарита: от h_hi до h_top при |u| < half_width_top —
|
||
# то, что свисает со свода в путь вагона (см. lobula.gauge_mask). Вагон
|
||
# выше 3.5 м, а предмет «2×2 сверху» у организаторов висит низом на 3.0 м
|
||
# над рельсом — при верхе 2.3 м он был не виден вовсе. Верх 3.3, а не 3.7:
|
||
# у свода в настоящих тоннелях свои кабели и кронштейны, и ошибка наклона
|
||
# плоскости пути на 60–90 м опускает их на 0.1–0.3 м — при 3.7 они шли в
|
||
# тревоги (низ у них в основном выше 3.2 м, у предметов организаторов —
|
||
# 2.8 и 3.0). h_top <= h_hi — секции нет. EXPERIMENTS п. 18.4, 19.
|
||
h_top: float = 3.3
|
||
half_width_top: float = 1.0
|
||
top_d_max: float = 90.0 # дальше верхней секции нет, м
|
||
# Плоское у пола (см. central_complex._quality): вертикальный размер меньше
|
||
# flat_h, верх ниже flat_top над рельсом, не дальше flat_d — вес наблюдения
|
||
# умножается на flat_w. flat_h: 0 — выключено. EXPERIMENTS п. 19.
|
||
flat_h: float = 0.06
|
||
# Отдельный проход по верхней секции для свисающего со свода (см.
|
||
# lobula._hanging): в основном проходе оно склеено со сводом.
|
||
top_detect: bool = True
|
||
# Дальний трек (дальше far_confirm_from) поднимает тревогу, только прожив
|
||
# far_confirm кадров (см. descending.py). 0 — выключено. Замерено и
|
||
# отвергнуто: 15 кадров убирают короткие дальние фантомы, но полигон теряет
|
||
# 200 наблюдений, P@150 у человека стоя 0.46 → 0.37. EXPERIMENTS п. 19.
|
||
far_confirm: int = 0
|
||
far_confirm_from: float = 100.0
|
||
flat_top: float = 0.26
|
||
flat_d: float = 70.0
|
||
flat_w: float = 0.15
|
||
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
|
||
# Резкость: вес наблюдения p**power. Рабочая точка 1.5 выбрана сравнением
|
||
# при равной строгости (EXPERIMENTS п. 17.4): ложных треков на незнакомой
|
||
# линии 23.5 → 14.7 на км, на знакомой 6.5 → 5.7, на второй половине
|
||
# new_data 23.9 → 17.4, а полигон теряет 142 наблюдения из 7 600: 39
|
||
# вблизи у края габарита (пограничный случай п. 17.1–17.2) и 82 за 90 м.
|
||
# 2.0 срезает ещё 127 наблюдений ради 0.4 трека на км.
|
||
mbon_power: float = 1.5
|
||
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
|
||
# Висящее посреди габарита: пол вероятности считывания для кандидата,
|
||
# целиком внутри габарита (`hover_cont`), с низом выше `hover_h` над
|
||
# рельсом, |u| не больше `hover_u`, не дальше `hover_d`, компактного вдоль
|
||
# пути и не тоньше `hover_width`. Модель учили на стоящих предметах, и
|
||
# предмет в воздухе она гасит: куб 0.3 м на высоте 1.2 м у края габарита
|
||
# синтетики находился с 31 м, с правилом — с 63 м; ложных тревог на всех
|
||
# трёх проверках столько же. 0 — выключено. EXPERIMENTS п. 24.
|
||
hover_floor: float = 0.5
|
||
hover_h: float = 0.6
|
||
hover_cont: float = 0.9
|
||
hover_u: float = 1.2
|
||
hover_d: float = 80.0 # дальше — не действует: ось пути вдали неточна
|
||
hover_depth: float = 1.5 # протяжённость вдоль пути до, м: длиннее — труба, поручень
|
||
hover_width: float = 0.2 # ширина от, м: уже — провод, кронштейн, тонкая деталь
|
||
# Канал малых целей (LC11, см. lobula._small_targets): кандидат из двух-трёх
|
||
# лучей, если он висит в пустом пространстве габарита — вся связная
|
||
# компонента не больше `small_ctx` лучей, низ выше `small_h` над рельсом,
|
||
# |u| не больше `small_u`, фон за ним дальше `small_gap`, дальность от
|
||
# `small_d_from` до `small_d_to`. Куб 0.3 м посередине синтетики
|
||
# организаторов при обычном пороге в четыре луча находился с 45 м: с 57 м
|
||
# его точки ложатся в две ячейки образа. 0 — выключено. EXPERIMENTS п. 26.
|
||
small_rays: int = 2
|
||
small_ctx: int = 4
|
||
small_h: float = 0.5
|
||
small_u: float = 0.8
|
||
small_gap: float = 5.0
|
||
small_d_from: float = 45.0
|
||
small_d_to: float = 100.0
|
||
# Поправка оценки модели на распространённость предметов: за этой
|
||
# дальностью из логита вычитается типичный для полосы. 0 — не
|
||
# поправлять. Замер — EXPERIMENTS п. 12.3.
|
||
mbon_prior_from: float = 0.0
|
||
# Пороги решения (см. descending.py). Вынесены сюда, потому что обученное
|
||
# считывание меняет цену улики: когда ложные тревоги падают, запас можно
|
||
# потратить на дальность, опустив порог. Замер — EXPERIMENTS п. 11.
|
||
# Считывание по треку (см. track_readout.py): геометрическое смешивание
|
||
# обученного отсчёта с уликой перед сравнением с порогом. 0 — решает
|
||
# только улика, как раньше. Замер — EXPERIMENTS п. 15.
|
||
# Правки решения (Zhirik1337, 22.09), обе выключены по замеру (п. 16):
|
||
# lying_exempt — не штрафовать за вытянутость низкий предмет в колее
|
||
# (лежащий человек вытянут вдоль пути, как конструкция).
|
||
# Правит ручную формулу, а при mbon_blend = 1 вес целиком
|
||
# от модели — не действует;
|
||
# far_channel — дальний канал предупреждения в обход порога. Выигрыш
|
||
# настоящий, но та же задача, что warn_far, втрое дороже:
|
||
# +3.0 и +7.1 ложных трека на км на двух сценах.
|
||
lying_exempt: bool = False
|
||
far_channel: bool = False
|
||
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
|
||
# Порог тревоги, линейно опускаемый с 0.5 до `warn_far` на отрезке от
|
||
# `warn_far_from` до 200 м. В п. 12.2 ровно эта правка была отвергнута:
|
||
# тогда улика далёкого трека колебалась около нуля, и опускать порог под
|
||
# нулевым сигналом было бессмысленно. После гашения знакомости и порога
|
||
# по лучам улика перестала быть нулём, и та же правка дала P@150
|
||
# 0.33 → 0.37 без потерь на 50 и 100 м ценой нуля ложных треков на
|
||
# знакомой линии и одного на незнакомой. Экстренного торможения она не
|
||
# касается: у него свой порог и условие d ≤ max(тормозной путь, 25 м).
|
||
# EXPERIMENTS п. 15.9.
|
||
warn_far: float = 0.30
|
||
warn_far_from: float = 90.0
|
||
leak_far: float = 0.12 # утечка улики на дальнем краю; равна leak — выключено
|
||
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
|
||
# Скачок времени записи больше этого (или назад) — перемотка, повтор или
|
||
# выпадение данных: треки, одометрия и ось пути начинаются заново. 0 — не
|
||
# сбрасывать никогда.
|
||
reset_gap_s: float = 2.0
|
||
novelty_gate: float = 0.0 # ниже этой новизны кандидат отбрасывается сразу
|
||
# Где считать плотные стадии — сетчатку, ламину, кластеризацию (gpu.py):
|
||
# 'auto' — видеокарта, если PyTorch её видит, иначе процессор; 'cuda';
|
||
# 'cpu'. Результат одинаков: на видеокарте те же операции в тех же типах.
|
||
# Любой сбой видеокарты переводит конвейер на процессор до конца работы,
|
||
# кадр досчитывается там же. Узел поднимает видеокарту в фоне
|
||
# (`gpu_background`), считая первые кадры на процессоре. EXPERIMENTS п. 21.
|
||
device: str = "auto"
|
||
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,
|
||
gpu_background: bool = False):
|
||
self.p = params or Params()
|
||
self.device = "cpu"
|
||
self.gpu = None # плотные стадии на видеокарте (gpu.GpuStages)
|
||
self.gpu_error: str | None = None
|
||
self._gpu_ready = None # (стадии, устройство) — готовы, ждут начала кадра
|
||
# gpu_background: видеокарта поднимается в своём потоке, а кадры тем
|
||
# временем считает процессор. Первый запуск ядер CUDA бывает долгим —
|
||
# в Docker под WSL 15 с на каждый новый контейнер, — и узел всё это время
|
||
# не был подписан и терял кадры. Зависни драйвер совсем — узел так и
|
||
# останется на процессоре, а не встанет.
|
||
self.gpu_pending = str(self.p.device).strip().lower() != "cpu"
|
||
if self.gpu_pending:
|
||
if gpu_background:
|
||
threading.Thread(target=self._bring_up_gpu, name="flyguard-gpu",
|
||
daemon=True).start()
|
||
else:
|
||
self._bring_up_gpu()
|
||
self._take_gpu()
|
||
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._track_readout = track_readout
|
||
self._init_temporal()
|
||
self._calib: list[PointCloud2] = []
|
||
# Поворот облака вокруг вертикали, если у драйвера «вперёд» — не −Y
|
||
# (определяется по дальним эхам при калибровке, см. forward_azimuth)
|
||
self.forward_deg = 0.0
|
||
self.frames_seen = 0
|
||
self._last_stamp: float | None = None
|
||
self.time_jumps = 0 # сколько раз время записи скакало
|
||
|
||
def _init_temporal(self) -> None:
|
||
"""Всё, что копится от кадра к кадру: треки, одометрия, ось, улики.
|
||
|
||
Решётка лучей сюда не входит — она от сенсора, а не от места на пути.
|
||
"""
|
||
track_readout = self._track_readout
|
||
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,
|
||
lying_exempt=self.p.lying_exempt,
|
||
top_from=(self.p.h_hi if self.p.h_top > self.p.h_hi
|
||
else float("inf")),
|
||
flat=(self.p.flat_h, self.p.flat_top, self.p.flat_d,
|
||
self.p.flat_w),
|
||
hover=(self.p.hover_floor, self.p.hover_h,
|
||
self.p.hover_cont, self.p.hover_u,
|
||
self.p.hover_d, self.p.hover_depth,
|
||
self.p.hover_width))
|
||
# без накопления улик трек подтверждается первым же наблюдением
|
||
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,
|
||
far_channel=self.p.far_channel,
|
||
far_confirm=self.p.far_confirm,
|
||
far_confirm_from=self.p.far_confirm_from) if self.p.use_tracking
|
||
else DescendingNeurons(warn_evidence=0.0, clear_evidence=0.0,
|
||
emergency_evidence=0.0, min_hits=1))
|
||
|
||
# ------------------------------------------------------------------ видеокарта
|
||
|
||
def _bring_up_gpu(self) -> None:
|
||
"""Проверить CUDA, прогреть стадии и отдать их конвейеру (`_take_gpu`)."""
|
||
try:
|
||
from .device import get_device
|
||
dev = get_device(self.p.device)
|
||
if dev.startswith("cuda"):
|
||
from .gpu import GpuStages
|
||
stages = GpuStages(dev)
|
||
stages.warmup()
|
||
self._gpu_ready = (stages, dev)
|
||
except Exception as e: # нет драйвера, не та видеокарта, нет памяти
|
||
self._gpu_off(e)
|
||
finally:
|
||
self.gpu_pending = False
|
||
|
||
def _take_gpu(self) -> None:
|
||
"""Перейти на видеокарту, если она готова. Только между кадрами.
|
||
|
||
Посреди кадра нельзя: ламина и кластеризация берут образ, который
|
||
сетчатка оставила на видеокарте.
|
||
"""
|
||
if self._gpu_ready is not None:
|
||
self.gpu, self.device = self._gpu_ready
|
||
self._gpu_ready = None
|
||
|
||
@property
|
||
def gpu_active(self) -> bool:
|
||
"""Плотные стадии на видеокарте — уже или с ближайшего кадра."""
|
||
return self.gpu is not None or self._gpu_ready is not None
|
||
|
||
def _gpu_off(self, exc: Exception) -> None:
|
||
"""Сбой видеокарты: до конца работы считать на процессоре."""
|
||
from .device import notify_cuda_error
|
||
notify_cuda_error(exc)
|
||
self.gpu = None
|
||
self.device = "cpu"
|
||
self.gpu_error = f"{type(exc).__name__}: {exc}"
|
||
|
||
def _cluster(self, mask: np.ndarray, r: np.ndarray, **kw):
|
||
"""Кластеризация с учётом глубины: видеокарта, при сбое — процессор."""
|
||
if self.gpu is not None:
|
||
try:
|
||
return self.gpu.cluster_by_depth(mask, r, **kw)
|
||
except Exception as e:
|
||
self._gpu_off(e)
|
||
return cluster_by_depth(mask, r, **kw)
|
||
|
||
# ------------------------------------------------------------------ калибровка
|
||
|
||
@property
|
||
def calib_progress(self) -> int:
|
||
"""Сколько кадров уже набрано для калибровки решётки."""
|
||
return self.p.calib_frames if self.layout_full is not None else len(self._calib)
|
||
|
||
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.forward_deg = forward_azimuth(self._calib)
|
||
if self.forward_deg:
|
||
self._calib = [rotate_cloud(c, self.forward_deg) for c in self._calib]
|
||
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()
|
||
rotated = bool(self.forward_deg)
|
||
if rotated:
|
||
pc = rotate_cloud(pc, self.forward_deg)
|
||
if not self._ensure_layout(pc):
|
||
return None
|
||
if self.forward_deg and not rotated:
|
||
# поворот найден на этом кадре при калибровке: сам кадр ещё не повёрнут
|
||
pc = rotate_cloud(pc, self.forward_deg)
|
||
# Скачок времени записи — перемотка, `ros2 bag play --loop`, разрыв в
|
||
# записи. Треки, одометрия и ось пути относятся к другому месту, и
|
||
# с ними предмет с прошлого круга всплыл бы там, где его нет.
|
||
if (self._last_stamp is not None and self.p.reset_gap_s > 0
|
||
and not (-0.05 < pc.stamp - self._last_stamp < self.p.reset_gap_s)):
|
||
self._init_temporal()
|
||
self.time_jumps += 1
|
||
self._last_stamp = pc.stamp
|
||
self.frames_seen += 1
|
||
self._take_gpu()
|
||
|
||
with t("retina"):
|
||
img: RangeImage | None = None
|
||
if self.gpu is not None:
|
||
try:
|
||
img = self.gpu.project(self.layout_full, pc, self.cols)
|
||
except Exception as e:
|
||
self._gpu_off(e)
|
||
if img is None:
|
||
img = 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"):
|
||
lam = None
|
||
if self.gpu is not None:
|
||
try:
|
||
lam = self.gpu.lamina() # по образу кадра, уже лежащему на видеокарте
|
||
except Exception as e:
|
||
self._gpu_off(e)
|
||
if lam is None:
|
||
lam = lamina.process(tf.r, tf.valid, device="cpu")
|
||
|
||
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,
|
||
core_from=self.p.core_from,
|
||
platform_filter=self.p.platform_filter,
|
||
h_top=self.p.h_top,
|
||
half_width_top=self.p.half_width_top,
|
||
top_d_max=self.p.top_d_max)
|
||
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,
|
||
core_from=self.p.core_from,
|
||
platform_filter=self.p.platform_filter,
|
||
h_top=self.p.h_top,
|
||
half_width_top=self.p.half_width_top,
|
||
top_d_max=self.p.top_d_max,
|
||
top_detect=self.p.top_detect,
|
||
cluster=self._cluster,
|
||
small_rays=self.p.small_rays,
|
||
small_ctx=self.p.small_ctx,
|
||
small_h=self.p.small_h,
|
||
small_u=self.p.small_u,
|
||
small_gap=self.p.small_gap,
|
||
small_d=(self.p.small_d_from,
|
||
self.p.small_d_to))
|
||
|
||
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
|