SUDOZOVCHIK/Brainrot_Muxa
1
0
Fork 0
forked from Dan4ick/Lidar_Muxa
Code Pull requests Activity

добавил ROS 2-пакет, Docker-образ, вид мозга и полную документацию

Browse source
This commit is contained in:
Данил Омелечко 2026-09-26 22:41:05 +03:00
parent bfc0bb2a1d
commit e3d511d7ce
90 changed files with 16138 additions and 227 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

12
.gitignore vendored
View file

@ -2,11 +2,21 @@ __pycache__/
*.py[cod]
.venv/
.pytest_cache/
data/
# записи лидара — только в корне: flyguard/data и .../flyguard/data нужны пакету
/data/
artifacts/mbon_folds/
artifacts/track_folds/
docs/figures/*.png
!docs/figures/curve_gauge.png
!docs/figures/doubleT_mount_shadow.png
docs/figures/*.mp4
# сборка ROS 2 и колёса PyTorch для сборки образа без сети
ros2_ws/build/
ros2_ws/install/
ros2_ws/log/
docker/wheels/*.whl
# Project memory & chat transfer
PROJECT_MEMORY.md
CHAT_MEMORY.md

857
README.md
View file

@ -1,241 +1,656 @@
# FlyGuard · ML-ядро
# FlyGuard — обнаружение посторонних объектов в тоннеле метро по данным 3D-лидара
Обнаружение посторонних объектов в тоннеле метро по данным 3D-лидара
Hesai Pandar128. Кейс 05, ЛЦТ-2026.
Решение кейса «Система обнаружения посторонних объектов для беспилотных поездов в тоннеле метро»
(хакатон «Лидеры цифровой трансформации 2026», направление «Город»; заказчик — Департамент
транспорта Москвы / ГУП «Московский метрополитен»).
Это **ядро обработки**: облако точек на входе, решение о препятствии на выходе.
Узел ROS 2, транспорт, контейнер и визуализация живут отдельно и сюда не
входят — ядро от них не зависит и проверяется без ROS вообще.
На вход — поток облаков точек Hesai Pandar128. На выход — ответ на единственный важный вопрос:
**«путь свободен» или «впереди препятствие на N метров»**.
---
## Чем это не является
## В двух словах
Не нейросетевой детектор общего назначения. Конвейер собран по схемам
зрительной системы дрозофилы, и каждая стадия — это конкретный нейропиль с
конкретной функцией, а не слой, подобранный перебором:
ТЗ формулирует главный вызов так:
```
облако точек 0.3–0.9 млн, 10 Гц
│
├─ RETINA омматидиальная решётка → дальностный образ 128 × N
├─ HALTERES плоскость рельсов: крен, тангаж, высота сенсора
├─ LAMINA диспаритет 1/R → ON/OFF, центр-окружение на 3 масштабах
├─ MEDULLA / LP T4/T5 → LPTC: скорость поезда без одометрии
├─ LOBULA LC11: кандидаты; разрез компоненты по контрасту
├─ MUSHROOM BODY KC → APL → MBON: новизна формы (без меток)
│ + обученное считывание MBON (с метками из физики)
├─ FAN-SHAPED BODY накопление лучей в координатах пути
├─ CENTRAL COMPLEX накопление улик в координатах пути, треки
└─ DESCENDING два порога с гистерезисом → решение
```
> «Возможно, нужно научиться хорошо описывать нормальный тоннель, а затем искать всё, что в него
> не вписывается.»
Ничего про геометрию сенсора не захардкожено: решётка лучей, высота установки,
крен и тангаж **калибруются по самим данным** на первых кадрах. В записях
встречаются две раскладки скана (3600 азимутов на 360° и 1200 на 120°) и две
высоты установки (1.31 и 1.70 м) — ядро работает с обеими без правок.
Это дословное описание того, чем занимается мозг **Drosophila melanogaster**. Муха решает ровно
нашу задачу — на лету, без разметки, без одометрии, с мизерным бюджетом нейронов и в жёстком
реальном времени. Поэтому архитектура FlyGuard собрана из её вычислительных схем, взятых из
коннектома (FlyWire / hemibrain):
---
## Что нужно интеграции: один класс, один вызов
```python
from flyguard.pipeline import FlyGuard, Params
from flyguard.mushroom_body import MushroomBody
from flyguard.mbon_readout import MbonReadout
fg = FlyGuard(Params(),
memory=MushroomBody.load("artifacts/mushroom_body.npz"),
readout=MbonReadout.load("artifacts/mbon_readout.npz"))
res = fg.process(cloud) # cloud: flyguard.cdr.PointCloud2
if res is None:
... # первые 12 кадров уходят на калибровку решётки
else:
d = res.decision # detected, distance, confidence, emergency, objects
res.total_ms # время обработки кадра
```
Требования к входу: `flyguard.cdr.PointCloud2` — поля `x, y, z, intensity`,
порядок точек как в сыром CDR. Конвейер **хранит состояние между кадрами**
(решётка, плоскость пути, ось пути, треки, накопитель), поэтому один экземпляр
обслуживает один поток данных; для параллельных сценариев нужны разные
экземпляры.
Выход `Decision`: `detected`, `distance` (м), `confidence` (0…1),
`emergency` (флаг экстренного торможения), `objects` (список подтверждённых
треков с id, дистанцией и габаритами).
Всё считается на **CPU**, GPU не требуется. Медиана обработки кадра — 43 мс
при бюджете 100 мс (замер по стадиям, один процесс, EXPERIMENTS п. 7.4).
Ламину можно перенести на **NVIDIA GPU** (`Params(device="auto")` или
`"cuda"`, модуль `flyguard/device.py` с переходом на CPU при любом сбое карты).
Замерено на RTX 5070 Ti: ламина 5.8 → 2.9 мс с копированием туда и обратно,
то есть кадр 43 → ~40 мс, — остальные 80 % времени кластеризация и геометрия,
которые видеокарта не ускоряет. Поэтому по умолчанию `device="cpu"`: выигрыш
в 3 мс не стоит отдельного образа и `--gpus all` на машине проверки.
---
## Структура
```
flyguard/ ядро: стадии обработки, память, считывание
bag.py cdr.py чтение rosbag2 и разбор CDR без ROS
device.py выбор CPU / NVIDIA GPU и переход на CPU при сбое
retina.py geometry.py решётка лучей, плоскость рельсов, ось пути
lamina.py medulla.py контраст (на CPU или GPU), движение
lobula.py кандидаты
mushroom_body.py память тоннеля (без меток)
mbon_readout.py обученное считывание (с метками)
track_readout.py считывание по истории трека — инструмент замера
fan_body.py накопление в координатах пути
central_complex.py треки и улики
descending.py решение
pipeline.py сборка
export.py 3D-рамки, время до столкновения, маркеры RViz
synth.py вставка предметов трассировкой лучей
tools/ обучение, оценка, разбор, полигон с аугментациями
tests/ 53 теста, запускаются без данных и без ROS
docs/ методика и результаты
artifacts/ обученные модели
```
---
## Как запустить
```bash
pip install -r requirements.txt
pytest tests -q # или без pytest: python tests/run_tests.py
```
Через Docker (подробно — [docs/DOCKER.md](docs/DOCKER.md)):
```bash
./docker-run.sh build
docker compose run --rm test # тесты
docker compose run --rm info # что видно из контейнера: CPU, GPU, CUDA
docker compose run --rm evaluate # ложные тревоги
docker compose run --rm benchmark # полигон
```
Для обучения на видеокарте — `pip install -r requirements-gpu.txt` и
`--device cuda` у `train_mbon.py`, `train_mushroom_body.py`, `evaluate.py`.
Записи лидара в репозиторий не кладутся. Положите их рядом
(`../data/for_hackathon/...`) или укажите путь:
```bash
set FLYGUARD_DATA=D:\lidar\data
```
Обучение и оценка:
```bash
python tools/make_training_set.py # размеченная выборка
python tools/train_mbon.py --device cuda --baseline # считывание MBON
python tools/evaluate.py --mbon-dir artifacts/mbon_folds # ложные тревоги
python tools/make_benchmark.py --memory artifacts/mushroom_body.npz \
--mbon-dir artifacts/mbon_folds # дальность обнаружения
python tools/plot_benchmark.py # кривые и график
python tools/compare_benchmark.py было.json стало.json # правка парно
```
Большой бэг `new_data` (90 ГБ) не распаковывается целиком — инструменты
читают его кусками прямо из архива (`--tar`, по умолчанию
`../датасет/new_data` или `FLYGUARD_NEW_DATA`):
```bash
python tools/make_training_set.py --new-data 0:110 --out data/cache/training_set_nd.npz
python tools/train_mbon.py --device cuda --data data/cache/training_set.npz \
--data data/cache/training_set_nd.npz --train-only new_data_ --out nd.npz
python tools/eval_new_data.py --shards 110: \
--readout было=artifacts/mbon_readout.npz --readout стало=nd.npz --out nd.json
```
Проверка на второй половине честная: вставки идут только в первую, а
вторую модель не видит ни в каком виде (EXPERIMENTS п. 17.3).
Синтетику организаторов (`cloud_with_fake_obj`, 10 предметов) сверяет с
эталоном отдельный инструмент. Разметки к бэгу нет, но вставленные точки не
лежат на элевациях колец, и эталон извлекается из самого бэга (п. 18.2):
```bash
python tools/eval_org_synth.py # итоговые настройки
python tools/eval_org_synth.py --set half_width=1.6 --set h_top=0
```
В этом бэге нет поля `ring`, а порядок точек сбит вставками: `retina.py`
восстанавливает кольца по элевации и раскладывает такие кадры по углам точек.
До этой правки ядро на нём не обрабатывало ни одного кадра (п. 18.1).
Любое поле `Params` меняется без правки кода: `--set h_lo_core=0.28 --set
k_sigma=0.75` у `make_benchmark.py`, `evaluate.py`, `make_training_set.py` и
`check_obstacle.py`. Два прогона полигона сравнивайте только парно —
`compare_benchmark.py` считает, сколько наблюдений перевернулось в каждую
сторону на одних и тех же вставках. Итоговые таблицы двух прогонов шумят
сильнее, чем меняет их большинство правок (EXPERIMENTS п. 16.1).
Тяжёлые шаги сами раскладываются по бэгам на процессы — записей пять, физических
ядер шесть, и это вся доступная зернистость: конвейер держит состояние между
кадрами, поэтому разрезать одну запись нельзя. Замерено: полигон 134 → 36 с,
сбор выборки 96 → 26 с на облегчённой конфигурации, то есть 3.7–3.8×, и файл на
выходе совпадает с последовательным **побайтово**. Отключается `--jobs 1`.
Для замера задержки кадра `--jobs 1` обязателен: под пятью процессами время
кадра растёт с 32 до 56 мс. Это свойство замера, а не конвейера, поэтому
`evaluate.py` в параллельном режиме печатает задержку как `nan` — чтобы такое
число нельзя было случайно привести в отчёте.
---
## Где мы сейчас
| Метрика | Значение | Чем измерено |
| Проблема кейса | Схема мухи | Модуль |
|---|---|---|
| Реальный объект 0.67 × 1.35 м на 55 м | **99.5 %** кадров | `tools/check_obstacle.py` |
| Синтетика организаторов, 10 предметов | **9 из 10** верно | `tools/eval_org_synth.py` |
| Ложные тревоги, leave-one-bag-out | **5.7 трека на км**, 8.4 % кадров | `tools/evaluate.py --mbon-dir` |
| То же без обученного считывания | 11.9 на км, 20.3 % кадров | там же, без `--mbon-dir` |
| На незнакомой линии (памяти нет) | **12.8 на км**; без считывания 36.1 | `tools/evaluate.py --no-memory` |
| Вторая половина `new_data`: другой день, 3.41 км, не видена при обучении | **11.7 на км**, 17.9 % кадров | `tools/eval_new_data.py` |
| Дальность (полигон, 15 560 наблюдений) | человек стоя: рабочая дальность **100 м**, P@50 = 0.72, P@100 = 0.52, P@150 = 0.46 | `tools/plot_benchmark.py` |
| Человек, упавший на пути | P@50 = **0.50** (было 0.27), рабочая дальность 20 м | там же |
| Обработка кадра | медиана 33 мс из бюджета 100 мс; на ядре уровня стенда жюри 55 / 65 мс (медиана / p95) | контейнер, EXPERIMENTS п. 18.5, 19.5 |
| Разделение «знакомое / новое» (без учителя) | ROC AUC 0.905 | `tools/tune_memory.py` |
| Считывание MBON «предмет / тоннель» (с учителем) | ROC AUC **0.986** | `tools/train_mbon.py` |
| Разметки нет, почти всё — пустой тоннель | грибовидное тело: KC + APL + MBON, подавление **знакомого** | `mushroom_body.py` |
| Нельзя путать своё движение с чужим объектом | T4/T5 → LPTC, широкопольный оптический поток | `medulla.py` |
| Увидеть заранее, независимо от размера | LPLC2 — детектор надвигания | `medulla.py` |
| Мелкий объект = 5 лучей на кадр | кольцевой аттрактор эллипсоидного тела: накопление улик | `central_complex.py` |
| На 170 м контраст к фону равен нулю | веерное тело: улики копятся в координатах мира, а не кадра | `fan_body.py` |
| Не реагировать на штатные конструкции | депрессия синапсов KC→MBON | `mushroom_body.py` |
| Сенсор закреплён нежёстко | жужжальца и оцеллии: стабилизация «взгляда» | `geometry.py` |
| 100 мс на кадр, слабый CPU | разрежённый бинарный код, 0.1 % активных клеток | `mushroom_body.py` |
Проверка всегда **leave-one-bag-out**: память обучается на всех записях, кроме
проверяемой. Иначе цифры лгут — подавлять конструкции, которые сам же и
запомнил, умеет кто угодно, а на приватном тесте будет новый участок.
Подробный разбор с привязкой к типам нейронов — в [docs/ALGORITHM.md](docs/ALGORITHM.md)
и [docs/CONNECTOME.md](docs/CONNECTOME.md).
---
## Что честно не работает
## Архитектура
Разобрано замерами, подробности — в `docs/EXPERIMENTS.md`:
```
ROS 2 bag → /lidar_points (PointCloud2, 0.3–0.9 млн точек, 10 Гц)
│
├─ RETINA омматидиальная решётка → дальностный образ 128 × N
├─ HALTERES плоскость пути: крен, тангаж, высота сенсора
├─ LAMINA диспаритет 1/R, ON/OFF, центр-окружение на 3 масштабах
├─ MEDULLA/LP T4/T5 → LPTC: скорость поезда без одометрии; LPLC2: надвигание
├─ LOBULA LC11: кандидаты; разрез по контрасту отделяет предмет от стены
├─ MUSHROOM BODY новизна: подавление знакомой обстановки тоннеля
├─ FAN-SHAPED BODY накопление лучей в координатах пути: улика там, где нет контраста
├─ CENTRAL COMPLEX накопление улик в координатах пути, треки
└─ DESCENDING два порога: предупреждение и экстренное торможение
│
▼
/flyguard/obstacle · /flyguard/markers · /flyguard/brain · /flyguard/diagnostics
```
* **За 200 м на этих участках не увидит никто**: прямая видимость в тоннелях
121–167 м, дальше линия взгляда упирается в стену кривой. На отдельных
перегонах и того меньше — 49–90 м.
* **Мелкие предметы на большой дальности невозможны с этим сенсором**: ведро
(0.1 м²) на 160–190 м даёт один луч, каска и бутылка — ноль.
* **За 80–90 м прирельсовая зона не наблюдается вовсе**: луч скользит по
полотну, и самая низкая видимая точка у оси пути оказывается выше головки
рельса на 0.1–0.6 м.
* Привыкание внутри проезда сделано и **отвергнуто замером** — п. 10.
* **Яркость как признак мы себе не засчитываем.** Абсолютной шкалы
интенсивности в записях нет: медиана по кандидатам обстановки 3…7 в пяти
бэгах и 23.5 в шестом. Вставка берёт яркость реальных возвратов с тех же
лучей, то есть признак намеренно обесточен, и дальность из-за этого
занижена — настоящий предмет был в 1.47 раза ярче окружения. Разбор — п. 11.1.
Детально — [docs/ARCHITECTURE.md](docs/ARCHITECTURE.md).
---
## Документация
## Сборка
* `docs/ALGORITHM.md` — что делает каждая стадия и почему именно так.
* `docs/EXPERIMENTS.md` — все замеры, включая отрицательные результаты.
* `docs/CONNECTOME.md` — что взято из коннектома как число, а что как идея.
```bash
docker build -t flyguard -f docker/Dockerfile .
```
Образ основан на `ros:humble-ros-base-jammy` (Ubuntu 22.04 + ROS 2 Humble). Все зависимости
ставятся на этапе сборки; **во время работы сеть не нужна**. Обе обученные части лежат в
образе и подключаются сами: память тоннеля (`artifacts/mushroom_body.npz`) и считывание MBON
(`artifacts/mbon_readout.npz`) — ровно та конфигурация, что замерена в разделе «Результаты».
Собранный образ проверяется одной командой:
```bash
docker run --rm -v "$PWD/docker:/smoke:ro" flyguard bash /smoke/smoke_test.sh
```
### Стенд без интернета
Организаторы подтвердили: на тестовом сервере сети нет. Узлу она и не нужна — всё
ставится при сборке. Но сама `docker build` без сети не пройдёт: ей нужны базовый образ
`ros:humble-ros-base-jammy` и пакеты apt. Поэтому образ собирается там, где сеть есть, и
переносится файлом:
```bash
docker save flyguard | gzip > flyguard_image.tar.gz # на машине с сетью
```
```bash
docker load -i flyguard_image.tar.gz # на стенде
```
После `docker load` все команды ниже работают как есть. Образ — 9.3 ГБ, из них 6.6 ГБ —
PyTorch с библиотеками CUDA (раздел «Видеокарта»); архив — около 5 ГБ. Наш готовый архив —
`flyguard_image.tar`, 4.7 ГБ: слои в нём уже сжаты, поэтому `gzip` поверх не нужен, а
`docker load -i` принимает и `.tar`, и `.tar.gz`. Проверено на чистом движке: архив загружен в
Docker Desktop 29.8 (Windows 11, WSL 2) и работает с `--gpus all` и без него.
Если сети нет и там, где собирается образ, колёса PyTorch скачиваются на соседней машине
(`python docker/fetch_wheels.py`) и отдаются сборке любым HTTP-сервером — аргумент
`TORCH_WHEELS`, подробности в [docker/wheels/README.md](docker/wheels/README.md).
## Запуск
Одной командой — детектор и проигрывание бэга вместе:
```bash
docker run --rm -it --gpus all --network host --ipc host -v /path/to/bags:/data flyguard \
ros2 launch flyguard detect.launch.py bag:=/data/doubleT_obstacle
```
Или раздельно — узел в контейнере, а `ros2 bag play` на хосте, как на поезде:
```bash
docker run --rm -it --gpus all --network host --ipc host flyguard
```
```bash
ros2 bag play --read-ahead-queue-size 10 /path/to/bags/doubleT_obstacle
```
**`--ipc host` обязателен, если bag проигрывается на хосте.** С `--network host` Fast DDS
считает контейнер и хост одной машиной и передаёт кадры через разделяемую память
`/dev/shm`, а без `--ipc host` у контейнера она своя. Кадры тогда теряются молча: замерено,
узел не получает **ни одного** кадра, а с флагом — 252 из 252. Если флаг всё же забыт, узел
через пять секунд после появления издателя пишет в журнал, в чём дело. Проигрывание внутри
контейнера (первая команда или второй контейнер из этого же образа) работает и без флага:
точка входа видит собственную `/dev/shm` и переводит транспорт на UDP.
Профиль транспорта для кадров в 24 МБ лежит в образе и подключается сам
(`docker/fastdds_large.xml`); снять — `FLYGUARD_DDS_PROFILE=0`.
`--read-ahead-queue-size 10` тоже не косметика: по умолчанию проигрыватель читает вперёд
1000 сообщений, при кадре в 24 МБ это 24 ГБ, и пока он их читает, первые секунды записи
успевают «просрочиться» и не публикуются вовсе (без флага доходит 119 кадров из 201).
В первой команде launch ставит его сам.
С RViz окну нужен доступ к экрану хоста:
```bash
xhost +local:
docker run --rm -it --network host --ipc host -e DISPLAY=$DISPLAY \
-v /tmp/.X11-unix:/tmp/.X11-unix -v /path/to/bags:/data flyguard \
ros2 launch flyguard detect.launch.py bag:=/data/doubleT_obstacle rviz:=true
```
**RViz в контейнере без доступа к видеокарте рисует программно** (Mesa `llvmpipe` на
процессоре): вращение мышью идёт рывками по 2–5 кадров в секунду, хотя в настройках стоит
`Frame Rate: 30` — это потолок, а не факт. Замер на `doubleT_obstacle`: программный RViz
занимает 1.9 ядра, с видеокартой — 0.2 ядра, в 9 раз меньше. Видеокарта пробрасывается так:
| Где | Добавить к `docker run` |
|---|---|
| Linux, NVIDIA (нужен nvidia-container-toolkit) | `--gpus all` (графика драйвера включена в образе: `NVIDIA_DRIVER_CAPABILITIES=all`) |
| Linux, встроенная Intel/AMD | `--device /dev/dri` |
| Windows: Docker Desktop или Docker в WSL 2 | `--device /dev/dxg -v /usr/lib/wsl:/usr/lib/wsl:ro -e LD_LIBRARY_PATH=/usr/lib/wsl/lib` (этих флагов хватает и для CUDA; одного `--gpus all` в Docker Desktop мало — CUDA будет, а RViz рисует процессором) |
Проверено на Windows 11 + WSL 2 (RTX 5070 Ti, `D3D12`) — в Docker внутри WSL и в Docker
Desktop 29.8; строки для Linux — стандартные флаги, на стенде не проверялись. Экран на
Windows даёт WSLg: из терминала Ubuntu — `-e DISPLAY=:0 -v /tmp/.X11-unix:/tmp/.X11-unix`,
из PowerShell с Docker Desktop — `-e DISPLAY=:0 -v /run/desktop/mnt/host/wslg/.X11-unix:/tmp/.X11-unix`.
Записи на Windows держите в файловой системе WSL (`/root/bags` и т. п.): с диска `C:` через
общую папку Docker Desktop кадры по 24 МБ идут около одного в секунду — замерено, за минуту
дошло 62 кадра из 201. Чем рисует OpenGL, показывает проверка без окна:
```bash
docker run --rm -e DISPLAY=$DISPLAY -v /tmp/.X11-unix:/tmp/.X11-unix <флаги видеокарты> \
-v "$PWD/docker:/smoke:ro" flyguard python3 /smoke/gl_probe.py
```
Можно и вовсе запустить RViz на хосте с нашим конфигом, если там стоит ROS 2:
`rviz2 -d ros2_ws/src/flyguard/config/flyguard.rviz`. На обнаружение всё это не влияет:
детектор с программным RViz и с аппаратным обработал одни и те же 190 кадров из 190.
RViz показывает не сырое облако, а **облако обзора**, которое публикует сам узел
(`/flyguard/view_cloud`): лучи сектора обработки, выровненные по плоскости рельсов, — до
77 тысяч точек вместо 900 тысяч. Рамки препятствий стоят в тех же координатах, пол пути
лежит на сетке. Сырое облако RViz не тянет (24 МБ на кадр), и топик у записей разный —
`/lidar_points` в пяти записях и `/sensing/lidar/hesai128/pointcloud` в `doubleT_obstacle`, —
а облако обзора есть всегда, из какого бы топика узел ни читал. Публикуется, только когда на
него кто-то подписан, и в замеры задержки не входит.
**Оранжевый контур — габарит, который узел проверяет.** Ось пути впереди узел оценивает в
каждом кадре по сечению тоннеля, и в кривой габарит изгибается вместе с ней. Проверяется
объединение изогнутого габарита и прямого, вдоль оси лидара: неточная оценка оси не должна
сужать зону поиска. На прямом пути контуры совпадают, в кривой рисуются оба — изогнутый ярко,
прямой бледно ([рисунок](docs/figures/curve_gauge.png)). Рамки препятствий стоят у своих
точек. На кривой синтетики организаторов (радиус около 760 м) ось сходится с положением
предмета до 0.04 м на 22 м, до 0.15 м на 39 м и до 0.27 м на 64 м (EXPERIMENTS п. 20).
Чтобы не ждать до нужного места, проигрывание начинается с любой секунды: `start:=18`.
### Видеокарта
Раскладка точек по решётке, ламина и кластеризация — три стадии, работающие с целым
образом 128 × 600 лучей, — считаются на видеокарте, если узел её видит; остальное на
процессоре. Для этого контейнеру нужен `--gpus all` (на Linux — с nvidia-container-toolkit).
Без флага, без видеокарты или при любом её сбое на ходу узел считает на процессоре — **с тем
же результатом**: на видеокарте те же операции в тех же типах, и решения совпадают с
процессорными покадрово на синтетике и шести записях ([EXPERIMENTS](docs/EXPERIMENTS.md),
п. 21). Где идёт счёт, узел пишет в журнал:
```
вычисления: пока процессор — видеокарту проверяю и прогреваю в фоне (в Docker под WSL до 20 с), результат тот же
вычисления: видеокарта NVIDIA GeForce RTX 5070 Ti (16 ГБ, CUDA 12.8) — сетчатка, ламина, кластеризация; остальное на процессоре (готова через 16.0 с после старта)
```
**Видеокарта поднимается в фоне**, а узел подписывается на лидар сразу и первые кадры считает
на процессоре; на видеокарту он переходит между кадрами. Первый запуск ядер CUDA в новом
контейнере под WSL длится 15 с (на Windows без контейнера — 0.9 с), и раньше узел всё это
время не был подписан: запись, пущенная сразу, теряла первые 15 с. Теперь запуск одновременно
с записью даёт те же 201 кадр из 201 и 189 обнаружений из 190. Зависни драйвер совсем, узел
останется на процессоре, а не встанет.
Кадр в контейнере (медиана / p95), видеокарта против процессора:
| Где | `doubleT_obstacle` | синтетика организаторов |
|---|---|---|
| Docker Desktop 29.8 (Windows 11, WSL 2), `--gpus all` | 23.9 / 35.2 мс против 34.8 / 39.4 | 26.4 / 42.6 мс против 42.7 / 48.0 |
| Docker Engine в Ubuntu под WSL, `/dev/dxg` | 23.4 / 30.2 мс против 31.8 / 34.9 | — |
Решения одинаковые: 189 обнаружений из 190 и 398 кадров с тревогой из 1499 в обоих режимах.
Принудительно на процессоре — `device:=cpu`. В Docker Engine, поставленном прямо в Ubuntu под
WSL (без Docker Desktop), вместо `--gpus all`:
`--device /dev/dxg -v /usr/lib/wsl:/usr/lib/wsl:ro -e LD_LIBRARY_PATH=/usr/lib/wsl/lib`.
Под WSL драйвер CUDA пишет в каждый новый контейнер свой кэш, около 330 МБ, а виртуальный диск
Docker от этого растёт и сам не сжимается; если на диске тесно — `-e CUDA_CACHE_DISABLE=1`, на
скорость это не влияет.
### Пауза, перемотка, покадрово
Проигрыватель, запущенный из launch (`bag:=...`), клавиатуры не слышит. Для него в образе
есть пульт: контейнер запускается с именем, а пульт — во втором терминале.
```bash
docker run --rm -it --name flyguard_demo --network host --ipc host -e DISPLAY=$DISPLAY \
-v /tmp/.X11-unix:/tmp/.X11-unix -v /path/to/bags:/data flyguard \
ros2 launch flyguard detect.launch.py bag:=/data/doubleT_obstacle rviz:=true
```
```bash
docker exec -it flyguard_demo flyguard-keys /data/doubleT_obstacle
```
| Клавиша | Действие |
|---|---|
| пробел | пауза / продолжить |
| ← → | на 3 секунды назад / вперёд |
| `.` | один кадр вперёд (на паузе) |
| ↑ ↓ | быстрее / медленнее (×0.1 … ×4) |
| `0` | в начало записи |
| `q` | выйти, запись играет дальше |
На паузе RViz держит последний кадр и рамки, и сцену можно крутить. После перемотки узел сам
замечает скачок времени записи и начинает треки, одометрию и ось пути заново — иначе
предмет с прошлого места всплыл бы там, где его нет; то же самое происходит на каждом круге
`ros2 bag play --loop`. Если `ros2 bag play` запущен вручную в своём терминале, пульт не нужен:
у проигрывателя свои клавиши (пробел — пауза, → — следующий кадр, ↑ ↓ — скорость).
Все записи подряд, по строке сводки на каждую (узел поднимается заново для каждой записи):
```bash
docker run --rm --network host --ipc host -v /path/to/bags:/data:ro \
-v "$PWD/docker:/smoke:ro" flyguard bash /smoke/check_all.sh
```
Итог узел пишет прямо в консоль контейнера — смену состояния и, пока тревога держится,
ближайшую дальность раз в секунду; когда тревога снимается, пишет `путь свободен`. Так
выглядит запись с настоящим препятствием (`doubleT_obstacle`, объект на 55–57 м):
```
[detector-1] [WARN] [1790249076.653486850] [flyguard]: ПРЕПЯТСТВИЕ: 55.7 м, уверенность 0.36, объектов 1
[detector-1] [WARN] [1790249327.983883704] [flyguard]: препятствие: 55.9 м, уверенность 0.51, объектов 1
[detector-1] [WARN] [1790249336.988973904] [flyguard]: препятствие: 56.0 м, уверенность 0.68, объектов 2
```
Если поезд движется, к строке добавляется время до столкновения.
Полный программный выход без RViz:
```bash
ros2 topic echo /flyguard/obstacle
```
### Облака без поля `ring` и с нарушенным порядком точек
Синтетический бэг организаторов (`cloud_with_fake_obj`, 24.09) устроен не так, как записи с
поезда: в облаке только `x, y, z, intensity`, без `ring` и `timestamp`, а там, где вставлен
предмет, заслонённые им точки удалены и точки предмета вписаны в середину массива. Число точек
в кадре гуляет от 214 до 353 тысяч, и порядок «столбец · эхо · кольцо» нарушен даже в кадрах
ровно на 307 200 точек — ни в одном из первых двенадцати он не цел. Узел это переносит сам:
* кольца восстанавливаются по гистограмме элевации (у лазерного канала она постоянна до
0.0001°, соседние каналы Pandar128 разнесены на 0.086°);
* каждый кадр проверяется: у целого кадра элевация каждой точки совпадает с элевацией её
кольца. Такой кадр идёт быстрым путём по порядку точек, а кадр с нарушенным порядком
раскладывается в ту же решётку **по углам каждой точки**. На целом кадре оба пути дают
образ бит в бит; раскладка по углам дороже: 11 мс против 6 на синтетике (скан 120°),
22 против 6 на круговом скане. В контейнере кадр синтетики — 43.7 / 50.2 мс (медиана / p95).
До этой правки узел на синтетике не обработал бы ни одного кадра: калибровка решётки
требовала поле `ring`.
### Демонстрация со схемой мозга мухи
```bash
docker run --rm -it --network host --ipc host -e DISPLAY=$DISPLAY \
-v /tmp/.X11-unix:/tmp/.X11-unix -v /path/to/bags:/data flyguard \
ros2 launch flyguard detect.launch.py bag:=/data/doubleT_obstacle rviz:=true brain_view:=true
```
Топик `/flyguard/brain` отдаёт мозг дрозофилы, подсвеченный **живой активностью**: видно,
как загорается ламина на контрасте, лобулярная пластинка на скорости, доли грибовидного тела
на новизне и гигантское волокно в момент решения. Вид отключён по умолчанию, чтобы не попадать
в замеры задержки; когда включён, рисуется пять раз в секунду по 15 мс. С `brain_view:=true`
RViz берёт свой конфиг: схема справа во всю высоту, панель Displays свёрнута — открывается
стрелкой у левого края окна.
Три варианта, `brain_style`:
| Значение | Что показывает |
|---|---|
| `scheme` | нарисованная схема нейропилей с подписями |
| `cloud` | **139 255 настоящих нейронов** коннектома FlyWire на своих анатомических местах |
| `hybrid` | по умолчанию: сверху панели решётки лучей (дальность, ON/OFF ламины, кандидаты), снизу облако нейронов |
Облако — не симуляция. Мембранные потенциалы 139 тысяч клеток никто не интегрирует: коннектом
даёт анатомию и принадлежность клеток, конвейер даёт активность по стадиям, а вид накладывает
одно на другое. Привязка держится на именах типов клеток, и все они есть в выгрузке поимённо —
LC11 (127 нейронов), LPLC2 (210), HS/VS (22), T4/T5 (12 245), клетки Кеньона (5177),
MBON (96), APL (2), гигантское волокно DNp01 (2). Ровно те схемы, из которых собран FlyGuard.
Для экрана и видео вид рисуется крупнее: `brain_scale:=2` — 2360 × 1572 (под 2K),
`brain_scale:=3` — 3540 × 2358 (под 4K). Сомы берутся из атласа своего размера, это настоящие
координаты, а не растянутая картинка; шрифты, линии и отступы растут вместе с масштабом.
Кадр вида рисуется 14 мс в 1×, 57 мс в 2× и 113 мс в 3×, пять раз в секунду; в 2× на записи
с поезда и на синтетике узел принял все кадры. По умолчанию масштаб 1 — прежние 1180 пикселей
по ширине.
Атласы (по 620–660 КБ, три размера) лежат в пакете и пересобираются из публичных выгрузок Codex:
```bash
python tools/build_brain_atlas.py
python tools/build_brain_atlas.py --width 2360 --height 1240 --margin 36 --out ros2_ws/src/flyguard/flyguard/data/brain_atlas_x2.npz
```
Данные FlyWire — CC-BY 4.0 (Dorkenwald et al., Schlegel et al., Nature 2024).
---
## Выходные данные
| Топик | Тип | Назначение |
|---|---|---|
| `/flyguard/obstacle` | `flyguard_msgs/ObstacleStatus` | основной программный выход |
| `/flyguard/detected` | `std_msgs/Bool` | бинарный статус для простой интеграции |
| `/flyguard/distance` | `std_msgs/Float32` | расстояние до ближайшего объекта, м (−1 — свободно) |
| `/flyguard/markers` | `visualization_msgs/MarkerArray` | рамки объектов для RViz |
| `/flyguard/view_cloud` | `sensor_msgs/PointCloud2` | облако обзора для RViz: сектор обработки в координатах пути, только при подписчике |
| `/flyguard/brain` | `sensor_msgs/Image` | мозг мухи с живой активностью (схема, облако нейронов или гибрид) |
| `/flyguard/diagnostics` | `diagnostic_msgs/DiagnosticArray` | задержки по стадиям, скорость, радиус кривой |
`ObstacleStatus` содержит: `detected`, `emergency`, `distance`, `time_to_collision`,
`confidence`, `speed`, `stopping_distance`, `processing_ms` и список объектов с габаритами,
числом лучей, новизной и стабильным `track_id`.
---
## Параметры
Все настройки — в [ros2_ws/src/flyguard/config/flyguard.yaml](ros2_ws/src/flyguard/config/flyguard.yaml),
переопределяются при запуске:
```bash
ros2 launch flyguard detect.launch.py fov_deg:=35.0 half_width:=1.5
```
| Параметр | По умолчанию | Смысл |
|---|---|---|
| `input_topic` | `/lidar_points` | топик лидара. Узел слушает и запасные имена (`/sensing/lidar/hesai128/pointcloud`, `/points_raw`), а если облако идёт в топик с другим именем, через секунду находит его сам, подписывается и пишет об этом в журнал |
| `device` | `auto` | где считать сетчатку, ламину и кластеризацию: `auto` — видеокарта, если есть (контейнер с `--gpus all`), иначе процессор; `cuda`; `cpu`. Видеокарта поднимается в фоне, первые кадры считает процессор. Результат одинаков, см. раздел «Видеокарта» |
| `mbon_path` | из образа | обученное считывание MBON (`.npz`); пусто — ручная формула веса улики |
| `mbon_power` | `1.5` | резкость считывания: вес наблюдения — вероятность в этой степени, см. [EXPERIMENTS](docs/EXPERIMENTS.md) п. 17.4 |
| `half_width` | `1.2` | полуширина габарита, м. Так его задают организаторы в синтетике 24.09: «у края» — до 1.13 м от оси, «вне габарита, но близко» — с 1.14 м. При прежних 1.6 второй давал ложную тревогу, см. [EXPERIMENTS](docs/EXPERIMENTS.md) п. 18.4 |
| `h_lo` / `h_hi` | `0.28` / `2.3` | границы основной части габарита по высоте над головкой рельса, м |
| `h_top` / `half_width_top` / `top_d_max` | `3.3` / `1.0` / `90` | верхняя секция: от `h_hi` до `h_top`, уже основной и не дальше `top_d_max` — то, что свисает со свода в путь вагона. Ищется отдельным проходом (`top_detect`), в котором свод не склеивается со свисающим; стоящее, что уходит вниз к полу, остаётся основному проходу. Верх 3.3, а не выше: у свода свои кабели и кронштейны, и на 60–90 м ошибка наклона опускает их в секцию. `h_top: 0` — выключить |
| `flat_h` / `flat_top` / `flat_d` / `flat_w` | `0.06` / `0.26` / `70` / `0.15` | плоское у пола: высотой меньше `flat_h`, целиком ниже `flat_top` над рельсом, не дальше `flat_d` — вес наблюдения умножается на `flat_w`. Пластины на полотне, края жёлоба, порог гермозатвора: организаторы подтвердили, что в жёлобе — не препятствие. `flat_h: 0` — выключить |
| `h_lo_core` / `core_from` | `0.16` / `30.0` | пол между рельсами и дальность, с которой он опущен: иначе упавший на пути человек (0.30 м) виден верхушкой в два сантиметра. Ближе 30 м пол прежний — там в полосу попадают головки рельсов, см. [EXPERIMENTS](docs/EXPERIMENTS.md) п. 16.3 |
| `d_min` / `d_max` | `4.0` / `220.0` | зона поиска по дальности, м |
| `fov_deg` | `30.0` | полусектор обработки по азимуту, ° |
| `min_rays` | `4` | минимум лучей на кандидата |
| `memory_path` | из образа | обученная память тоннеля (`.npz`) |
| `brain_view` | `false` | публиковать вид мозга |
| `brain_style` | `hybrid` | `scheme` · `cloud` · `hybrid` — что именно рисовать |
| `brain_scale` | `1` | масштаб вида мозга: `2` — 2360 × 1572 для 2K, `3` — для 4K (облако и гибрид) |
| `ctx_up` | `4.0` | насколько кластеризация смотрит выше габарита, м; меньше — и колонна, срезанная по верхней границе, выглядит предметом |
| `split_adv` | `0.0` | разделение фигуры и фона по скорости сближения: рабочая дальность 62 → 80 м ценой вчетверо больших ложных тревог. По умолчанию выключено, см. [EXPERIMENTS](docs/EXPERIMENTS.md) п. 9.4 |
| `enable_accumulator` | `true` | накопление лучей в координатах пути: удваивает рабочую дальность там, где локальный контраст равен нулю |
| `acc_near` / `acc_gain` | `55.0` / `1.5` | с какой дальности включается накопление и какая опора считается полным контрастом |
| `split_gap` | `6.0` | разрез компоненты, растёкшейся вдоль стены, по контрасту ламины: гладкая стена даёт ноль по построению, предмет на ней — ступеньку. `0` — выключить |
| `split_near` / `split_top` | `55.0` / `1` | ближе какой дальности не резать и сколько фигур выносить из компоненты; одна фигура вместо всех — 6.4 против 9.4 ложных трека на км при той же дальности |
| `enable_habituation` | `false` | привыкание внутри проезда. Выключено: измерено, что избирательности нет, см. [EXPERIMENTS](docs/EXPERIMENTS.md) п. 10 |
| `best_effort` | `false` | QoS: RELIABLE. При BEST_EFFORT потеря одного UDP-фрагмента отбрасывает весь 24-мегабайтный кадр |
| `queue_depth` | `10` | глубина очереди подписки |
| `raw_subscription` | `true` | брать кадр сырыми байтами CDR, минуя сборку Python-объекта `sensor_msgs` |
| `async_worker` | `false` | обрабатывать в отдельном потоке; по умолчанию в колбэке — поток борется за GIL с приёмом |
Ничего про геометрию сенсора не захардкожено: решётка лучей, высота установки, крен и тангаж
**калибруются по самим данным** на первых кадрах. В предоставленных записях встречаются две
разные раскладки скана (3600 азимутов на 360° и 1200 на 120°) и две высоты установки сенсора
(1.31 м и 1.70 м) — решение работает с обеими без единой правки.
---
## Результаты
Полный разбор с методикой — [docs/EXPERIMENTS.md](docs/EXPERIMENTS.md).
| Метрика | Значение |
|---|---|
| Реальный объект (0.67 × 1.35 м на 55 м) | обнаружен в **99.5 %** кадров |
| Синтетика организаторов (`cloud_with_fake_obj`, 10 предметов) | **9 из 10** верно: из восьми предметов в габарите найдены семь (2×2 посередине — с 95 м, 2×2 сверху — с 87 м, длинный на рельсах — с 79 м, 0.3 на рельсе — с 76 м, стержень 5 см с потолка — только с 13 м), оба предмета вне габарита — без тревоги; не найден 2×2, заходящий в габарит на 0.3 м. Ложных тревог за 151 с — три коротких (`tools/eval_org_synth.py`, EXPERIMENTS п. 18–19) |
| Ложные тревоги, leave-one-bag-out | **5.7 разных ложных трека на километр** (8.4 % кадров) — без обученного считывания 11.9 |
| То же на незнакомой линии, памяти нет | **12.8 на км** (20.7 % кадров) — без считывания 36.1 |
| То же на второй половине `new_data` — другой день, 3.41 км, ни в каком виде не видена при обучении, памяти нет | **11.7 на км** (17.9 % кадров) |
| Время обработки кадра | итоговый образ в контейнере: `doubleT_obstacle` **33.4 / 37.9 мс** (медиана / p95), синтетика **43.5 / 48.8 мс** (требование 100 мс) |
| То же на ядре уровня стенда жюри (i7-9700E ≈ 0.61 нашего по PassMark, `docker/jury_cpu_test.sh`) | реальная запись **55 / 65 мс** (медиана / p95), 201 из 201; синтетика **68 / 80 мс**, 1510 из 1510 |
| Приём в контейнере | **201 из 201** и **252 из 252**, отброшено **0** |
| Рабочая дальность, размеченный полигон | **100 м** для человека стоя, **122 м** сидя, **100 м** для чемодана, 20 м для человека лёжа |
| Обнаружение на 50 м (при видимости) | **0.72** человек стоя; **0.50** человек лёжа — против 0.27 до пола в колее |
| Обнаружение на 100 м (при видимости) | **0.52** стоя и **0.54** сидя — против 0.19 без обученного считывания; лёжа **0.19** против 0.00 |
| Обнаружение на 150 м (при видимости) | **0.46** стоя и **0.39** сидя — против 0.39 и 0.20 до пола в колее |
| Оценка скорости без одометрии | согласие перепроекции 0.76–0.95 |
| Разделение «знакомое / новое» | ROC AUC 0.905 |
| Обобщение на форму, которой не было в обучении | ROC AUC **0.965–0.997**, потеря не больше 0.028 |
| Вклад накопления улик | без него ложных объектов в 13.5 раза больше |
| Вклад памяти тоннеля | вдвое меньше ложных тревог, обнаружение не страдает |
| Вклад обученного считывания MBON | при итоговых настройках ложных вдвое меньше: 11.9 → 5.7 на км на знакомой линии, 36.1 → 14.7 на незнакомой |
| Вклад гашения знакомости на дальности | P@100 0.31 → 0.53, P@150 0.00 → 0.33, рабочая дальность 80 → 100 м |
| Вклад дальнего порога тревоги | P@150 0.33 → 0.37, человек сидя 0.22 → 0.24; ложных на знакомой линии 8.0 → 8.0, на незнакомой 20.3 → 21.3 |
| Вклад резкости считывания 1.5 | ложных на незнакомой линии 23.5 → 14.7 на км, на знакомой 6.5 → 5.7, на второй половине `new_data` 23.9 → 17.4; полигон парно +1 / −142 из 7 600 наблюдений (39 вблизи у края габарита, 82 за 90 м). Сравнение при равной строгости с моделью, обученной на `new_data`, — EXPERIMENTS п. 17.4 |
| Вклад габарита 1.2 м и верхней секции (тогда до 3.7 м, теперь до 3.3 — п. 19) | синтетика организаторов 6 → 8 из 10; полигон парно +43 / −33, посторонних 793 → 742; ложных на незнакомой линии 14.7 → 13.8 на км, на знакомой 5.7 → 5.7; цена — ящик на 100 м (P@100 0.50 → 0.40). EXPERIMENTS п. 18.4 |
| Вклад сброса при скачке времени | вторая половина `new_data` (70 с выпавших кадров): 17.4 → 13.8 ложного трека на км; на записях без разрывов не меняет ничего. EXPERIMENTS п. 18.9 |
| Вклад штрафа за плоское у пола и прохода по верхней секции | синтетика 8 → 9 из 10, фантомов на ней 5 → 3; пустые записи 3 → 2 ложных трека (73 → 45 кадров); незнакомая линия 13.8 → 12.8 на км, вторая половина `new_data` 14.4 → 11.7; знакомая 5.7 → 5.7; полигон парно 0 / −3 (каска), посторонних 742 → 758. EXPERIMENTS п. 19 |
| Вклад пола в колее (с переобучением считывания) | лёжа P@50 0.27 → 0.50, P@150 стоя 0.39 → 0.49, ящик 0.06 → 0.26; парно +685 наблюдений против −33; ложных на знакомой линии 8.0 → 8.0, на незнакомой 21.3 → 23.5 |
| Вклад разреза по контрасту | 9.1 → 7.5 ложных трека на км *(замер до обученного считывания)*, и слепая полоса 40–90 м на `roundT_pressureGate_roundT` 0.00 → 0.45…1.00 |
Строки «вклад …» для отдельных механизмов измерены до появления обученного
считывания, и опора у них 7.4–7.5 трека на километр, а не 3.3.
**Про полигон честно.** 23.09 он пересобран: у каждого сценария теперь свой
генератор случайности, а в каталоге появился человек лёжа. При **тех же**
настройках человек стоя дал 80 м рабочей дальности и P@100 = 0.48 вместо
прежних 100 м и 0.53 — это шум полигона, а не поломка: две реализации
случайности расходятся на P@150 до 0.07. Поэтому правки теперь меряются
**парно**, на одних и тех же вставках (`tools/compare_benchmark.py`): считается,
сколько наблюдений перевернулось из «не видел» в «видел» и обратно. Разбор —
[EXPERIMENTS](docs/EXPERIMENTS.md), п. 16.1.
**Про слепые участки честно.** Размеченный полигон вскрыл то, чего не видно на одном
реальном объекте: на части участков предмет сливается со стеной по дальности, попадает
с ней в одну связную компоненту и отбрасывается вместе с ней. Человек на оси пути
обнаруживался в 100 % кадров на трёх бэгах из пяти и в 0 % на двух — при 65 лучах на
предмете, то есть не из-за видимости.
**Один из двух слепых бэгов прозрел.** На `roundT_pressureGate_roundT` — той самой
записи, где компонента из 42 000 лучей течёт вдоль стены от 4 до 99 м и уносит
предмет с собой, — полностью слепая полоса 40–90 м стала 0.45 / 1.00 / 0.57.
Помог разрез компоненты по контрасту ламины: гладкая стена даёт нулевой
центр-окружение по построению, а предмет на ней — ступеньку. Из переглубокой
компоненты выносится ровно одна, сильнейшая фигура; без этого ограничения разрез
отрезает фон, протяжённость кандидата падает с 6.9 до 0.8 м, вместе с ней пропадает
множитель компактности в весе улики, и каждое наблюдение начинает весить вдесятеро
больше. Измеренная цена одной фигуры против всех — 6.4 против 9.4 ложных трека на км
при одинаковой дальности. Второй слепой бэг, `roundT_doubleT`, за 40 м остаётся
слепым, и это разобрано покадрово: за 50 м до предмета **не доходит линия взгляда** —
98 % лучей упираются в преграду ближе него независимо от того, куда поперёк его
ставить, — а на 35–52 м мешает наш порог `split_near`, снижать который вышло
слишком дорого (7.4 → 17.1 ложного трека на км ради девяти метров). Подробно —
[EXPERIMENTS](docs/EXPERIMENTS.md), п. 9.6.
Два предыдущих подхода к тому же — разрез по допуску глубины и разрез по скорости
сближения (`split_adv`) — тоже возвращали зрение, но стоили 25.8 и 21.5 трека на
километр и остались выключенными. Разбор всех трёх — [EXPERIMENTS](docs/EXPERIMENTS.md),
п. 9.3–9.5.
**Про новый участок честно.** Обученная память на незнакомой линии бесполезна по
определению: без неё ложных треков 21.8 на километр против 9.1. Мы попробовали
закрыть это привыканием внутри проезда — гасить форму, встретившуюся в нескольких
разных точках пути, то есть штатную повторяющуюся обстановку. Механизм сделан,
доведён до работы и **отвергнут по замеру**: при ёмкости, достаточной чтобы
популяция не насыщалась, он не меняет ничего (7.5 против 7.5 ложных треков на км),
а весь видимый эффект маленькой популяции оказался глобальным глушением, которое
давит предмет сильнее обстановки (новизна вставленного предмета 0.24 при медианной
новизне кандидата 0.43…0.71). Код оставлен и выключен, полный разбор с таблицами —
[EXPERIMENTS](docs/EXPERIMENTS.md), п. 10.
Работает на новом участке то, что и работало: перенос долговременной памяти. В
дескрипторе намеренно смешаны признаки формы и углового размера (переносятся на
любой тоннель) с положением в сечении (запоминает конкретную обстановку), и первая
половина снижает ложные тревоги с 21.8 до 9.1 трека на километр на бэге, которого
память не видела.
**Про обученное считывание честно.** Метки для него сделаны вставкой предметов
трассировкой лучей, и первые две модели сенсора оказались неверными: сначала
яркость вставки считалась по ламбертовой ρ·cosθ/r², и за 110 м предмет выходил
тусклее тоннеля, потом — постоянной, и он стал ярче тоннеля впятеро. В обоих
случаях модель училась узнавать вставку по яркости, а не по форме, и полигонная
дальность была завышена. Абсолютной шкалы интенсивности в этих записях нет вовсе:
медиана по кандидатам обстановки 3…7 в пяти бэгах и 23.5 в шестом. Сейчас вставка
берёт яркость реальных возвратов с тех же лучей, признак стал неинформативным, и
все цифры выше получены уже так. Заявленные до этого разбора 100 м рабочей
дальности и P@100 = 0.53 были получены на полигоне с артефактом: после починки
модели сенсора те же замеры дали 80 м и 0.31. Нынешние 100 м и 0.53 —
совпадение по величине, но получены они уже на честном полигоне и другими
средствами (гашение знакомости и порог по лучам, зависящий от дальности).
Разбор с таблицами — [EXPERIMENTS](docs/EXPERIMENTS.md), п. 11.
**Про дальность и ложные честно.** Улика далёкого предмета — произведение
нескольких множителей, и один из них на дальности оказался перевёрнутым:
у вставленного человека на 120–185 м новизна 0.150 против 0.199 у окружающей
обстановки (AUC 0.293). На шести лучах дескриптор вырождается, и память
тоннеля узнаёт в предмете любую далёкую конструкцию — то есть мы сами гасили
свой сигнал. После гашения вклада новизны за 90 м обнаружение на 150 м
выросло с нуля до 0.33, а на 100 м с 0.31 до 0.53. Вместе с порогом по числу
лучей, зависящим от дальности (четыре вблизи, три за 90 м), рабочая дальность
по человеку выросла с 80 до 100 м. Цена — ложные тревоги на **знакомой**
линии 3.5 → 8.0 трека на километр; на незнакомой плата нулевая, потому что
там подавлять нечем. Кому дороже тишина, тот ставит `nov_fade_from: 0` и
получает 3.5 на километр, теряя дальнюю зону. Лишние далёкие треки дают предупреждения, а
не торможение: экстренный уровень требует близкой дистанции. Разбор —
[EXPERIMENTS](docs/EXPERIMENTS.md), п. 12.
**Про решение по треку честно.** Замышлялось обученное считывание по истории
трека — и оно **не обогнало ни один из признаков, которые ему же и дали**
(AUC 0.880 против 0.895 у одного среднего отсчёта). Зато по дороге нашлось, что
улика насыщается: на настоящем объекте она 1.000 и у предмета, и у ложных
треков (AUC 0.624), а средний вес наблюдения, из которого она складывается, —
0.998 против 0.269. Смешивать его с уликой оказалось бесполезно: при равном
числе ложных тревог простой порог даёт обнаружение не хуже, а на незнакомой
линии смешивание вытаскивает лишние треки (20.3 → 24.0 на км). В итоге приняли
не модель, а ключ, **отвергнутый тремя разделами раньше**, — порог тревоги,
опускаемый с 0.5 до 0.3 за 90 м. Тогда он ничего не давал, потому что улика
далёкого трека была нулём; после гашения знакомости она им быть перестала, и
тот же ключ поднял P@150 с 0.33 до 0.37. Отрицательный результат верен только
для конфигурации, в которой получен. Разбор — [EXPERIMENTS](docs/EXPERIMENTS.md),
п. 15.
**Про упавшего на пути человека честно.** Самый важный для метро случай был
виден хуже всех крупных предметов: при высоте 0.30 м и поле габарита 0.28 м в
габарит попадала верхушка в два сантиметра (P@50 = 0.27, на 100 м — ноль). Пол
между рельсами опущен до 0.16 м (предложение Zhirik1337), но не везде: без
порога по дальности обнаружение **вблизи падало вдвое** — в полосу 0.16…0.28 м
попадают головки рельсов, рельс собирается в одну компоненту от самой кабины, и
предмет выбрасывается вместе с ней. Дальше 30 м вреда нет, и после
переобучения считывания на кандидатах нового пола лежачий человек P@50 = 0.50,
рабочая дальность 32 м; ложных на знакомой линии столько же, на незнакомой
+2.2 на км. Ещё четыре правки из того же набора замерены и отвергнуты: каждая
давала меньше, чем стоила. Разбор — [EXPERIMENTS](docs/EXPERIMENTS.md), п. 16.
**Про ось пути честно.** Ось берётся не из рельсов, а из дрейфа центра сечения
тоннеля с дальностью: на 100 м рельсы дают единицы точек, а свод — тысячи.
Поэтому ось не дрожит: сдвиг между соседними кадрами в одной точке пути на
100 м — p90 не хуже 0.35 м при полуширине габарита 1.6 м. Но наблюдается
сечение только до 62–107 м, дальше ось продолжается по касательной, и
касательная расходится с кривой до 1.19 м на 150 м. Продолжать вместо неё
измеренную кривизну пробовали — хуже: кривизна оценивается на коротком плече,
гуляет между кадрами и даёт до 6 м расхождения.
**Про край габарита честно.** На синтетике организаторов граница между «у края, внутри» и
«вне, но близко» — сантиметры: 1.13 и 1.14 м от оси. Ошибка нашей оси на 40–80 м —
0.2–0.4 м, проверено по их предметам (EXPERIMENTS п. 20.1). Поэтому ящик 2×2, заходящий в
габарит на 0.06–0.28 м, мы не берём: по форме и перепаду дальности он неотличим от
настоящих конструкций у края, которые из-за той же ошибки оси оказываются у нас на
1.0–1.1 м. Правило, которое его бы нашло, на записях без препятствий срабатывает в 16
местах, одно держится 451 кадр (п. 20.3).
Из этого следует оговорка к дальним цифрам полигона. Предмет вставляется на
**нашу же** оценку оси, поэтому ошибку оси полигон не мерит в принципе. В
эксплуатации предмет на 150 м может оказаться в метре от того места, где мы
считаем путь, и у края габарита из него выпасть. Дальние цифры оптимистичны
именно по этой причине, а не из-за обнаружения. Разбор —
[EXPERIMENTS](docs/EXPERIMENTS.md), п. 13.
**Про дальность честно.** Паспортный максимум Pandar128 — 200 м, максимальное эхо в датасете —
208.8 м. Но реальная **прямая видимость в этих тоннелях 121–167 м**: тоннели кривые (радиусы
1300–8700 м), и дальше линия взгляда упирается в стену. Заявленные в ТЗ «300 м → отлично» на
предоставленных участках физически недостижимы никаким алгоритмом.
Что мешает дотянуться до 200 м там, где видимость позволяет, тоже измерено и оказалось не
тем, чего ожидаешь. Человек на 170 м освещён **в каждом кадре** (5 лучей) и за проход набирает
около 84 попаданий в одну точку мира; кандидат формируется, размер определяется верно. Не
растёт улика: кольцо окружения ламины на 170 м упирается в стену тоннеля, которая там же, и
локальный контраст обнуляется. Лечится не порогом, а накоплением лучей в координатах пути и
геометрической картой линии — разбор в [docs/EXPERIMENTS.md](docs/EXPERIMENTS.md), п. 7.3.
**Про стекло честно.** Лидар светит на 905 нм, и прозрачное стекло для него почти
прозрачно: луч уходит насквозь и возвращается от того, что за стеклом, а гладкая
поверхность отражает зеркально — в сторону, а не назад к прибору. Стеклянный предмет
виден только тем, что в нём не прозрачно: этикеткой, пробкой, рамой, грязью, бликом
там, где луч падает на поверхность почти по нормали. Отдельного приёма для стекла у нас
нет, и честно его не сделать: из пустоты предмет не восстановить. Для масштаба — даже
непрозрачная бутылка (0.03 м²) на полигоне почти не видна: на 40–55 м на ней 3 луча, а
ниже примерно десяти лучей предмет не отличить от шума (EXPERIMENTS п. 9).
---
## Разработка без ROS
Весь конвейер работает и офлайн, прямо по `.db3`, без установленного ROS — это удобно для
экспериментов на Windows и для воспроизведения метрик:
```bash
python tools/inspect_bags.py --root data/for_hackathon # калибровка решётки
python tools/run_pipeline.py --all --memory artifacts/mushroom_body.npz
python tools/evaluate.py --device cuda # leave-one-bag-out
python tools/make_benchmark.py --memory artifacts/mushroom_body.npz
python tools/render_brain.py --bag data/for_hackathon/doubleT_obstacle --video brain.mp4
```
`evaluate.py`, `make_benchmark.py` и `make_training_set.py` раскладывают работу по
бэгам на процессы (`--jobs`, по умолчанию — по числу записей, но не больше
физических ядер). Результат совпадает с последовательным побайтово: случайность
у каждой записи своя, общей изменяемой памяти между ними нет. Задержку кадра при
этом мерить нельзя — нужен `--jobs 1`.
Обучение памяти тоннеля (без единой метки, на пустых проездах):
```bash
python tools/train_mushroom_body.py --device cuda \
--extra-cache data/cache/new_data_candidates.npz
```
---
## Состав репозитория
```
ros2_ws/src/flyguard/ ROS 2-пакет: конвейер, узел, launch, конфиги, RViz, вид мозга
ros2_ws/src/flyguard_msgs/ сообщения ObstacleStatus и DetectedObject
docker/ Dockerfile образа, точка входа, проверки в контейнере
flyguard/ то же ядро, что в ROS-пакете, — для инструментов и тестов без ROS
tools/ офлайн-инструменты: калибровка, обучение, метрики, полигон
tests/ тесты без ROS: pytest tests
docs/ архитектура, алгоритм, эксперименты, коннектом
artifacts/ обученная память тоннеля и результаты замеров
```
Ядро в `flyguard/` и в `ros2_ws/src/flyguard/flyguard/` — один и тот же код из одного
источника. Сборка и запуск по этому README — через `docker/Dockerfile`. В корне лежат ещё
`Dockerfile`, `Dockerfile.gpu` и `docker-compose.yml` для прогона ядра по записям без ROS
и `Dockerfile.ros2` с узлом `flyguard/flyguard_ros2_node.py`.
## Лицензия
MIT.

37
artifacts/ablation.json Normal file
View file

@ -0,0 +1,37 @@
[
{
"variant": "полная система",
"alarm_rate": 0.10717703349282297,
"fp_tracks": 6,
"obj_rate": 0.9894736842105263,
"seconds": 70.2
},
{
"variant": "− память тоннеля",
"alarm_rate": 0.24114832535885167,
"fp_tracks": 16,
"obj_rate": 0.9894736842105263,
"seconds": 61.4
},
{
"variant": "− ось пути (прямой коридор)",
"alarm_rate": 0.04401913875598086,
"fp_tracks": 3,
"obj_rate": 0.9894736842105263,
"seconds": 54.6
},
{
"variant": "− признаки формы",
"alarm_rate": 0.30526315789473685,
"fp_tracks": 21,
"obj_rate": 0.9894736842105263,
"seconds": 61.0
},
{
"variant": "− накопление улик",
"alarm_rate": 0.3741626794258373,
"fp_tracks": 81,
"obj_rate": 1.0,
"seconds": 60.9
}
]

BIN
artifacts/track_readout.npz Normal file
View file

Binary file not shown.

109
docker/Dockerfile Normal file
View file

@ -0,0 +1,109 @@
# FlyGuard — обнаружение посторонних объектов в тоннеле метро по данным 3D-лидара.
#
# Образ самодостаточен: во время работы сеть не нужна, все зависимости ставятся
# на этапе сборки. Базовый образ — официальный ROS 2 Humble на Ubuntu 22.04,
# ровно та среда, что указана в ТЗ.
#
# docker build -t flyguard -f docker/Dockerfile .
# docker run --rm -it --gpus all --network host --ipc host -v /path/to/bags:/data flyguard
#
# Без --gpus all (или на машине без видеокарты NVIDIA) тот же образ считает на
# процессоре — с тем же результатом.
FROM ros:humble-ros-base-jammy
ARG DEBIAN_FRONTEND=noninteractive
# Численное ядро берётся из репозиториев Ubuntu, а не из pip: так образ собирается
# без выхода в PyPI и получает numpy/scipy, слинкованные с системным BLAS.
RUN apt-get update && apt-get install -y --no-install-recommends \
python3-numpy \
python3-scipy \
python3-opencv \
python3-pil \
fonts-dejavu-core \
python3-colcon-common-extensions \
ros-humble-rviz2 \
ros-humble-rosbag2-storage-mcap \
ros-humble-diagnostic-msgs \
&& rm -rf /var/lib/apt/lists/*
# PyTorch с CUDA 12.8 — плотные стадии конвейера на видеокарте (сетчатка,
# ламина, кластеризация; flyguard/gpu.py). Колёса несут свою среду CUDA, от
# машины нужен только драйвер NVIDIA (у стенда 580 — это до CUDA 13.0).
# 2.9.1 — последняя сборка под Python 3.10 из Ubuntu 22.04; CUDA 12.8 нужна
# для Blackwell (RTX 50xx) и годится для Ada (RTX 40xx). Весит PyTorch с
# библиотеками CUDA 6.5 ГБ, поэтому ставится одним слоем и без кэша pip.
#
# Колёса — из индекса PyTorch, а если задан TORCH_WHEELS — с этого адреса
# (сборка там, где сети нет: колёса скачиваются на соседней машине и отдаются
# любым HTTP-сервером, docker/wheels/README.md). В контекст сборки колёса не
# кладутся: это ещё 4 ГБ копий на диске.
ARG TORCH_WHEELS=
RUN set -e; \
if [ -n "$TORCH_WHEELS" ]; then \
python3 -c "import re, urllib.request as u; b = '$TORCH_WHEELS'.rstrip('/'); \
f = re.findall(r'href=\"(pip-[^\"]+\.whl)\"', u.urlopen(b + '/').read().decode())[0]; \
u.urlretrieve(b + '/' + f, '/tmp/pip.whl')"; \
python3 /tmp/pip.whl/pip install --no-cache-dir --no-index \
--find-links "$TORCH_WHEELS" torch==2.9.1; \
rm -f /tmp/pip.whl; \
else \
apt-get update && apt-get install -y --no-install-recommends python3-pip; \
pip3 install --no-cache-dir --index-url https://download.pytorch.org/whl/cu128 \
torch==2.9.1; \
rm -rf /var/lib/apt/lists/*; \
fi; \
python3 -c "import torch; print('torch', torch.__version__, 'CUDA', torch.version.cuda)"
# Видеокарта для CUDA и для RViz (OpenGL через драйвер NVIDIA при --gpus all)
ENV NVIDIA_VISIBLE_DEVICES=all \
NVIDIA_DRIVER_CAPABILITIES=all
WORKDIR /opt/flyguard
# Сначала манифесты — слой с зависимостями переживает правки кода
COPY ros2_ws/src/flyguard/package.xml ros2_ws/src/flyguard/package.xml
COPY ros2_ws/src/flyguard_msgs/package.xml ros2_ws/src/flyguard_msgs/package.xml
COPY ros2_ws/src/flyguard_msgs/CMakeLists.txt ros2_ws/src/flyguard_msgs/CMakeLists.txt
COPY ros2_ws/src ros2_ws/src
# Память тоннеля и обученное считывание MBON. Без считывания узел работает
# на ручной формуле веса улики, а у неё ложных тревог в полтора раза больше
# (13.6 против 8.0 на км, README) — замеренная конфигурация и образ должны совпадать.
COPY artifacts/mushroom_body.npz ros2_ws/src/flyguard/artifacts/mushroom_body.npz
COPY artifacts/mbon_readout.npz ros2_ws/src/flyguard/artifacts/mbon_readout.npz
# Обычная установка, без --symlink-install: тот режим оставляет в install/
# ссылки внутрь build/ и src/, и последующее удаление build/ ломает пакет.
RUN . /opt/ros/humble/setup.sh \
&& cd ros2_ws \
&& colcon build --cmake-args -DCMAKE_BUILD_TYPE=Release \
&& rm -rf build log
COPY docker/entrypoint.sh /entrypoint.sh
COPY docker/fastdds_large.xml /opt/flyguard/fastdds_large.xml
COPY docker/fastdds_udp.xml /opt/flyguard/fastdds_udp.xml
# Пульт проигрывания для `docker exec` (пауза, перемотка, скорость)
COPY docker/flyguard-keys.sh /usr/local/bin/flyguard-keys
# Снятие CR: при клонировании на Windows git может подставить CRLF, и тогда
# ядро ищет интерпретатор «bash\r». Ошибка неочевидная, а стоит одной строки.
RUN sed -i 's/\r$//' /entrypoint.sh /usr/local/bin/flyguard-keys \
&& chmod +x /entrypoint.sh /usr/local/bin/flyguard-keys
# BLAS в один поток. Матрицы конвейера мелкие, и потоки OpenBLAS не ускоряют
# кадр, а крутятся вхолостую в ожидании работы: при 4 потоках узел занимал
# 3 ядра, при одном — 0.4 ядра, а кадр даже чуть быстрее (33.9 / 39.3 мс против
# 33.2 / 40.5, медиана / p95; синтетика 43.7 / 50.2 против 45.9 / 54.7).
ENV OMP_NUM_THREADS=1 \
OPENBLAS_NUM_THREADS=1 \
MKL_NUM_THREADS=1 \
PYTHONUNBUFFERED=1 \
RCUTILS_COLORIZED_OUTPUT=1
# Транспорт под кадр в 24 МБ: умолчания Fast DDS рассчитаны на сотни килобайт
# и теряли половину кадров ещё до подписчика. Профиль поднимает сегмент
# разделяемой памяти и буферы сокетов; снимается через FLYGUARD_DDS_PROFILE=0.
ENV FASTRTPS_DEFAULT_PROFILES_FILE=/opt/flyguard/fastdds_large.xml
ENTRYPOINT ["/entrypoint.sh"]
CMD ["ros2", "launch", "flyguard", "detect.launch.py"]

67
docker/bagrate_test.sh Normal file
View file

@ -0,0 +1,67 @@
#!/usr/bin/env bash
# С какой частотой `ros2 bag play` реально отдаёт кадры — без нашей ноды.
# Нужно, чтобы отличить потери в нашем коде от предела самого транспорта.
set -u
BAG="${1:?укажите бэг}"
TOPIC="${2:-/sensing/lidar/hesai128/pointcloud}"
python3 - "$TOPIC" > /tmp/rate.txt 2>/dev/null <<'PY' &
import sys, time
import rclpy
from rclpy.node import Node
from rclpy.qos import (QoSProfile, QoSHistoryPolicy, QoSReliabilityPolicy,
QoSDurabilityPolicy)
from sensor_msgs.msg import PointCloud2
topic = sys.argv[1]
qos = QoSProfile(history=QoSHistoryPolicy.KEEP_LAST, depth=10,
reliability=QoSReliabilityPolicy.RELIABLE,
durability=QoSDurabilityPolicy.VOLATILE)
class Sink(Node):
def __init__(self):
super().__init__("sink")
self.n = 0
self.first = self.last = None
self.bytes = 0
self.create_subscription(PointCloud2, topic, self.cb, qos, raw=True)
def cb(self, raw):
now = time.perf_counter()
if self.first is None:
self.first = now
self.last = now
self.n += 1
self.bytes += len(raw)
rclpy.init()
s = Sink()
try:
rclpy.spin(s)
except KeyboardInterrupt:
pass
finally:
span = (s.last - s.first) if (s.first and s.last and s.last > s.first) else 0
print(f"принято сообщений: {s.n}")
if span > 0:
print(f"за {span:.1f} с → {s.n / span:.2f} Гц")
print(f"поток: {s.bytes / span / 1e6:.0f} МБ/с, размер кадра "
f"{s.bytes / max(s.n, 1) / 1e6:.1f} МБ")
PY
SINK=$!
sleep 3
echo "=== проигрывание (приёмник только считает, ничего не обрабатывает) ==="
T0=$(date +%s.%N)
ros2 bag play "$BAG" >/dev/null 2>&1
T1=$(date +%s.%N)
awk -v a="$T0" -v b="$T1" 'BEGIN{printf "бэг проигран за %.1f с
", b-a}'
sleep 2
kill -INT $SINK 2>/dev/null || true
wait $SINK 2>/dev/null || true
echo
cat /tmp/rate.txt

71
docker/brain_test.sh Normal file
View file

@ -0,0 +1,71 @@
#!/usr/bin/env bash
# Проверка топика /flyguard/brain: он публикуется и содержит настоящую картинку.
# Несколько кадров сохраняются в PNG — их же можно показать в презентации.
#
# docker run --rm --network host --shm-size=1g -v <бэги>:/data \
# -v <репо>/docker:/t:ro -v <куда>:/out flyguard bash /t/brain_test.sh /data/<бэг>
set -u
BAG="${1:?укажите путь к бэгу}"
OUT="${OUT:-/out}"
mkdir -p "$OUT"
ros2 launch flyguard detect.launch.py brain_view:=true > /tmp/node.log 2>&1 &
LAUNCH_PID=$!
for i in $(seq 1 30); do
ros2 topic list 2>/dev/null | grep -q '^/flyguard/brain$' && break
sleep 1
done
echo "топик /flyguard/brain объявлен через ${i} с"
python3 - "$OUT" > /tmp/brain.txt 2>&1 <<'PY' &
import sys, rclpy
from rclpy.node import Node
from sensor_msgs.msg import Image
import numpy as np, cv2
out = sys.argv[1]
class Grab(Node):
def __init__(self):
super().__init__("brain_grab")
self.n = 0
self.shapes = set()
self.create_subscription(Image, "/flyguard/brain", self.cb, 10)
def cb(self, m):
self.n += 1
a = np.frombuffer(m.data, np.uint8).reshape(m.height, m.width, -1)
self.shapes.add((m.height, m.width, m.encoding, a.shape[2]))
# берём каждый двадцатый кадр, чтобы картинки отличались
if self.n % 20 == 1 and self.n < 200:
img = a if m.encoding == "bgr8" else cv2.cvtColor(a, cv2.COLOR_RGB2BGR)
cv2.imwrite(f"{out}/brain_{self.n:04d}.png", img)
rclpy.init()
g = Grab()
try:
rclpy.spin(g)
except KeyboardInterrupt:
pass
finally:
print(f"кадров схемы принято: {g.n}")
for s in g.shapes:
print(f"размер {s[1]}x{s[0]}, кодировка {s[2]}, каналов {s[3]}")
PY
GRAB_PID=$!
sleep 2
ros2 bag play --read-ahead-queue-size 10 "$BAG" > /dev/null 2>&1
sleep 2
kill -INT $GRAB_PID 2>/dev/null || true
wait $GRAB_PID 2>/dev/null || true
kill $LAUNCH_PID 2>/dev/null || true
sleep 1
echo
cat /tmp/brain.txt
echo
ls -la "$OUT" | head -15

23
docker/check_all.sh Normal file
View file

@ -0,0 +1,23 @@
#!/usr/bin/env bash
# Все записи подряд: для каждой — узел, проигрывание и строка сводки.
#
# docker run --rm --network host --ipc host -v <бэги>:/data:ro \
# -v <репо>/docker:/smoke:ro flyguard bash /smoke/check_all.sh
#
# Узел поднимается заново на каждую запись: это разные проезды, и состояние
# одного (треки, ось пути) не должно перетекать в другой. «Кадров с тревогой»
# на записи без препятствия — это ложные тревоги.
set -u
here="$(dirname "$0")"
printf '%-38s %8s %9s %22s %26s\n' "запись" "принято" "обработ." "кадров с тревогой" "кадр: медиана / p95, мс"
for bag in /data/*/; do
[ -f "$bag/metadata.yaml" ] || continue
name=$(basename "$bag")
out=$(bash "$here/demo_test.sh" "$bag" 2>&1)
recv=$(echo "$out" | grep -o "frames_received=[0-9]*" | cut -d= -f2)
proc=$(echo "$out" | grep -o "frames_processed=[0-9]*" | cut -d= -f2)
hits=$(echo "$out" | grep -o "кадров с обнаружением: [0-9]* ([0-9.]*%)" | sed 's/кадров с обнаружением: //')
ms=$(echo "$out" | grep -o "медиана [0-9.]* мс, p95 [0-9.]*" | sed 's/медиана //; s/ мс, p95 / \/ /')
first=$(echo "$out" | grep -o "ПРЕПЯТСТВИЕ: [0-9.]* м" | head -1)
printf '%-38s %8s %9s %22s %26s %s\n' "$name" "${recv:--}" "${proc:--}" "${hits:-0}" "${ms:--}" "$first"
done

14
docker/ddsrate_test.sh Normal file
View file

@ -0,0 +1,14 @@
#!/usr/bin/env bash
# Сравнение транспорта DDS: умолчания против профиля для крупных сообщений.
# Приёмник ничего не обрабатывает, поэтому меряется именно транспорт.
set -u
BAG="${1:?укажите бэг}"
TOPIC="${2:-/sensing/lidar/hesai128/pointcloud}"
PROFILE="${FASTRTPS_DEFAULT_PROFILES_FILE:-<умолчания>}"
echo "профиль: $PROFILE"
echo "/dev/shm: $(df -h /dev/shm | awk 'NR==2{print $2}')"
echo "rmem_max: $(cat /proc/sys/net/core/rmem_max)"
echo
bash "$(dirname "$0")/bagrate_test.sh" "$BAG" "$TOPIC"

135
docker/demo_test.sh Normal file
View file

@ -0,0 +1,135 @@
#!/usr/bin/env bash
# Сквозная проверка внутри контейнера: нода + проигрывание бэга + разбор вывода.
#
# docker run --rm --network host -v <бэги>:/data -v <репо>/docker:/smoke:ro flyguard \
# bash /smoke/demo_test.sh /data/doubleT_obstacle
set -u
# Управление заданиями: без него фоновые процессы неинтерактивного bash глухи к
# SIGINT, и узел не гас по `kill -INT` — его добивал `kill -9`, без итога.
set -m
BAG="${1:?укажите путь к бэгу}"
shift || true
LAUNCH_ARGS=("$@")
# NODE=external — узел уже запущен снаружи (другим контейнером, см.
# jury_cpu_test.sh): здесь только проигрывание и подсчёт.
LAUNCH_PID=""
if [ "${NODE:-}" != "external" ]; then
echo "=== запуск ноды ==="
ros2 launch flyguard detect.launch.py "${LAUNCH_ARGS[@]}" > /tmp/node.log 2>&1 &
LAUNCH_PID=$!
echo "параметры: ${LAUNCH_ARGS[*]:-по умолчанию}"
else
: > /tmp/node.log
fi
for i in $(seq 1 30); do
if ros2 topic list 2>/dev/null | grep -q '^/flyguard/obstacle$'; then
echo "топик /flyguard/obstacle появился за ${i} с"
break
fi
sleep 1
done
# Видеокарта поднимается в фоне, и узел принимает кадры, не дожидаясь её. Для
# замера скорости ждём её здесь, иначе первые кадры посчитает процессор:
# решения те же, а время кадра смешается. WAIT_GPU=0 — не ждать, как на стенде.
if [ "${WAIT_GPU:-1}" = 1 ] && grep -q "вычисления: пока процессор" /tmp/node.log 2>/dev/null; then
for i in $(seq 1 90); do
grep -qE "вычисления: (видеокарта|процессор \()|не поднялась" /tmp/node.log && break
sleep 1
done
GPU_STATE=$(grep -oE 'готова через [0-9.]+ с|не поднялась|CUDA недоступна' /tmp/node.log | head -1)
echo "видеокарта: ${GPU_STATE:-нет ответа за 90 с}"
fi
# Счётчик на rclpy: `ros2 topic echo` сам не успевает за потоком и теряет
# сообщения, из-за чего занижает оценку частоты.
python3 - > /tmp/count.txt 2>/dev/null <<'PY' &
import rclpy
from rclpy.node import Node
from flyguard_msgs.msg import ObstacleStatus
from diagnostic_msgs.msg import DiagnosticArray
class Counter(Node):
def __init__(self):
super().__init__("counter")
self.n = self.hits = 0
self.dist, self.ms, self.diag = [], [], {}
self.create_subscription(ObstacleStatus, "/flyguard/obstacle", self.on_obs, 50)
self.create_subscription(DiagnosticArray, "/flyguard/diagnostics", self.on_diag, 50)
def on_obs(self, m):
self.n += 1
self.ms.append(m.processing_ms)
if m.detected:
self.hits += 1
self.dist.append(m.distance)
def on_diag(self, m):
for st in m.status:
for kv in st.values:
if kv.key in ("dropped_frames", "frames_processed",
"frames_received", "frames_calibration",
"cycle_ms"):
self.diag[kv.key] = kv.value
rclpy.init()
c = Counter()
try:
rclpy.spin(c)
except KeyboardInterrupt:
pass
finally:
import statistics
ms = sorted(c.ms)
print(f"сообщений: {c.n}")
if c.n:
print(f"кадров с обнаружением: {c.hits} ({c.hits / c.n:.1%})")
if c.dist:
print(f"дистанция: медиана {statistics.median(c.dist):.1f} м, "
f"мин {min(c.dist):.1f} м, макс {max(c.dist):.1f} м")
if ms:
print(f"обработка кадра: медиана {statistics.median(ms):.1f} мс, "
f"p95 {ms[int(0.95 * (len(ms) - 1))]:.1f} мс, макс {max(ms):.1f} мс")
print("по данным самой ноды: " + ", ".join(f"{k}={v}" for k, v in sorted(c.diag.items())))
PY
COUNTER_PID=$!
sleep 2
echo "=== проигрывание бэга ==="
# read-ahead-queue-size: проигрыватель по умолчанию набивает очередь на 1000
# сообщений ДО старта. При кадре в 24 МБ это 24 ГБ, чтение занимает секунды,
# а часы записи уже идут — всё, что за это время «просрочилось», не публикуется.
PLAY_ARGS="${PLAY_ARGS:---read-ahead-queue-size 10}"
echo "аргументы проигрывания: $PLAY_ARGS"
T0=$(date +%s.%N)
ros2 bag play $PLAY_ARGS "$BAG" 2>&1 | grep -Ei "duration|Opened database" | head -3
T1=$(date +%s.%N)
awk -v a="$T0" -v b="$T1" 'BEGIN{printf "проиграно за %.1f с" ORS, b-a}'
sleep 3
kill -INT $COUNTER_PID 2>/dev/null || true
wait $COUNTER_PID 2>/dev/null || true
# SIGINT, а не SIGTERM: так launch гасит и узел, а узел печатает итог. От
# SIGTERM детектор переживал launch, и в check_all.sh узлы копились от записи
# к записи, считая кадры соседей и отбирая у них процессор. `kill -9` ниже —
# только страховка: с `set -m` узел гаснет за полсекунды.
if [ -n "$LAUNCH_PID" ]; then
kill -INT $LAUNCH_PID 2>/dev/null || true
for i in $(seq 1 10); do pgrep -f "lib/flyguard/detector" >/dev/null || break; sleep 1; done
pkill -9 -f "lib/flyguard/detector" 2>/dev/null || true
wait $LAUNCH_PID 2>/dev/null || true
fi
echo
echo "=== результат ==="
cat /tmp/count.txt
echo
echo "=== журнал ноды ==="
grep -Ei "вычисления|видеокарт|память|считывание|решётк|кадр лидара|подписка|итог|ошибк|error|traceback" /tmp/node.log | head -20
echo
echo "=== что узел сказал в консоль ==="
grep -E "ПРЕПЯТСТВИЕ|препятствие|ЭКСТРЕННОЕ|экстренное|путь свободен" /tmp/node.log | head -30

60
docker/entrypoint.sh Normal file
View file

@ -0,0 +1,60 @@
#!/usr/bin/env bash
# Точка входа контейнера: окружение ROS + прогрев конвейера.
set -e
source /opt/ros/humble/setup.bash
source /opt/flyguard/ros2_ws/install/setup.bash
# Профиль транспорта можно снять для сравнения: FLYGUARD_DDS_PROFILE=0
if [ "${FLYGUARD_DDS_PROFILE:-1}" = "0" ]; then
unset FASTRTPS_DEFAULT_PROFILES_FILE
fi
# Контейнер без --ipc host. Docker монтирует ему свою /dev/shm с источником
# «shm», а с --ipc host видна хостовая. С --network host Fast DDS считает
# контейнер и хост одной машиной и шлёт кадры через /dev/shm, но сегмент
# собеседника отсюда не виден — и кадры пропадают молча: от `ros2 bag play`
# на хосте узел не получает НИ ОДНОГО (замерено, и с нашим профилем, и без
# него). В таком контейнере транспорт переводится на UDP: тогда работают
# проигрывание внутри контейнера и второй контейнер из этого же образа.
# Проигрыватель на хосте со своими умолчаниями всё равно пойдёт через память,
# и для него выход один — запускать контейнер с --ipc host.
shm_source=$(awk '$5 == "/dev/shm" { for (i = 7; i <= NF; i++) if ($i == "-") { print $(i + 2); exit } }' \
/proc/self/mountinfo 2>/dev/null || true)
if [ -n "${FASTRTPS_DEFAULT_PROFILES_FILE:-}" ] && [ "$shm_source" = "shm" ]; then
export FASTRTPS_DEFAULT_PROFILES_FILE=/opt/flyguard/fastdds_udp.xml
echo "ВНИМАНИЕ: контейнер запущен без --ipc host — транспорт переключён на UDP." >&2
echo " Проигрывание внутри контейнера работает, но ros2 bag play на хосте" >&2
echo " кадры сюда не доставит: для него запускайте с --ipc host." >&2
fi
# Сегмент разделяемой памяти не поместится в /dev/shm размером 64 МБ, который
# Docker даёт по умолчанию, и Fast DDS молча свалится обратно на UDP.
if [ "${FASTRTPS_DEFAULT_PROFILES_FILE:-}" = "/opt/flyguard/fastdds_large.xml" ]; then
shm_mb=$(df -m /dev/shm 2>/dev/null | awk 'NR==2{print $2}')
if [ -n "$shm_mb" ] && [ "$shm_mb" -lt 512 ]; then
echo "ВНИМАНИЕ: /dev/shm всего ${shm_mb} МБ — запустите с --shm-size=1g," >&2
echo " иначе крупные кадры пойдут через UDP и часть потеряется." >&2
fi
fi
# Прогрев: первый вызов numpy/scipy тянет за собой загрузку библиотек и
# первичное размещение буферов, и без прогрева первый же кадр уехал бы
# за бюджет в 100 мс. Здесь это делается на синтетическом кадре до старта ноды.
if [ "${FLYGUARD_WARMUP:-1}" = "1" ]; then
python3 - <<'PY' || echo "прогрев пропущен (не критично)"
import numpy as np
from scipy import ndimage
from scipy.sparse.csgraph import connected_components
from scipy.sparse import coo_matrix
a = np.random.rand(128, 1200).astype(np.float32)
ndimage.uniform_filter(a, size=(7, 29), mode="nearest")
g = coo_matrix((np.ones(3, np.int8), ([0, 1, 2], [1, 2, 0])), shape=(4, 4))
connected_components(g, directed=False)
np.linalg.lstsq(np.random.rand(64, 3), np.random.rand(64), rcond=None)
print("прогрев выполнен")
PY
fi
exec "$@"

77
docker/fastdds_large.xml Normal file
View file

@ -0,0 +1,77 @@
<?xml version="1.0" encoding="UTF-8" ?>
<!-- Профиль Fast DDS для крупных сообщений.
Кадр Pandar128 весит 8-24 МБ, а умолчания Fast DDS рассчитаны на сотни
килобайт: сегмент разделяемой памяти 512 КБ, сокетные буферы ОС 208 КБ.
Кадр в них не помещается, транспорт разваливает его на десятки тысяч
фрагментов, и на 10 Гц часть теряется ещё до подписчика.
Здесь сегмент поднят до 128 МБ (пять-шесть кадров в полёте), буферы
сокетов - до 64 МБ, очередь портов увеличена. Разделяемая память идёт
первой: внутри одной машины кадр передаётся без копирования через сокет.
Сегмент заводит КАЖДЫЙ участник DDS, а их в сеансе обычно три: нода,
проигрыватель бэга и наблюдатель. Больше 128 МБ брать нельзя - при
--shm-size=1g третий участник уже не поместится, и Fast DDS откатится
на UDP, потеряв половину кадров.
Подключается переменной FASTRTPS_DEFAULT_PROFILES_FILE (см. entrypoint.sh).
Контейнеру нужен --shm-size не меньше сегмента, иначе /dev/shm в 64 МБ
обрежет его молча. -->
<dds xmlns="http://www.eprosima.com/XMLSchemas/fastRTPS_Profiles">
<profiles>
<transport_descriptors>
<transport_descriptor>
<transport_id>shm_large</transport_id>
<type>SHM</type>
<segment_size>134217728</segment_size>
<maxMessageSize>4194304</maxMessageSize>
<port_queue_capacity>8192</port_queue_capacity>
<healthy_check_timeout_ms>2000</healthy_check_timeout_ms>
</transport_descriptor>
<transport_descriptor>
<transport_id>udp_large</transport_id>
<type>UDPv4</type>
<sendBufferSize>67108864</sendBufferSize>
<receiveBufferSize>67108864</receiveBufferSize>
<maxMessageSize>65000</maxMessageSize>
</transport_descriptor>
</transport_descriptors>
<participant profile_name="flyguard_large_data" is_default_profile="true">
<rtps>
<userTransports>
<transport_id>shm_large</transport_id>
<transport_id>udp_large</transport_id>
</userTransports>
<useBuiltinTransports>false</useBuiltinTransports>
</rtps>
</participant>
<!-- Асинхронная запись обязательна: синхронная блокирует поток публикации
на время отправки всех фрагментов кадра. -->
<data_writer profile_name="flyguard_writer" is_default_profile="true">
<qos>
<publishMode>
<kind>ASYNCHRONOUS</kind>
</publishMode>
<reliability>
<kind>RELIABLE</kind>
</reliability>
</qos>
<historyMemoryPolicy>PREALLOCATED_WITH_REALLOC</historyMemoryPolicy>
</data_writer>
<data_reader profile_name="flyguard_reader" is_default_profile="true">
<qos>
<reliability>
<kind>RELIABLE</kind>
</reliability>
</qos>
<historyMemoryPolicy>PREALLOCATED_WITH_REALLOC</historyMemoryPolicy>
</data_reader>
</profiles>
</dds>

57
docker/fastdds_udp.xml Normal file
View file

@ -0,0 +1,57 @@
<?xml version="1.0" encoding="UTF-8" ?>
<!-- Профиль Fast DDS без разделяемой памяти — запасной, для контейнера без
--ipc host.
С --network host Fast DDS считает контейнер и хост одной машиной и шлёт
кадры через /dev/shm. Но без --ipc host у контейнера своя /dev/shm, сегмент
собеседника не виден, и кадры пропадают молча: узел не получает НИ ОДНОГО
кадра от `ros2 bag play`, запущенного на хосте (замерено). Здесь остаётся
только UDP: кадры доходят, хотя при системных сокетных буферах часть
теряется. Точка входа подключает этот профиль сама, когда видит свою
/dev/shm; правильный запуск — с --ipc host, и тогда работает основной
профиль fastdds_large.xml. -->
<dds xmlns="http://www.eprosima.com/XMLSchemas/fastRTPS_Profiles">
<profiles>
<transport_descriptors>
<transport_descriptor>
<transport_id>udp_large</transport_id>
<type>UDPv4</type>
<sendBufferSize>67108864</sendBufferSize>
<receiveBufferSize>67108864</receiveBufferSize>
<maxMessageSize>65000</maxMessageSize>
</transport_descriptor>
</transport_descriptors>
<participant profile_name="flyguard_udp_only" is_default_profile="true">
<rtps>
<userTransports>
<transport_id>udp_large</transport_id>
</userTransports>
<useBuiltinTransports>false</useBuiltinTransports>
</rtps>
</participant>
<data_writer profile_name="flyguard_writer" is_default_profile="true">
<qos>
<publishMode>
<kind>ASYNCHRONOUS</kind>
</publishMode>
<reliability>
<kind>RELIABLE</kind>
</reliability>
</qos>
<historyMemoryPolicy>PREALLOCATED_WITH_REALLOC</historyMemoryPolicy>
</data_writer>
<data_reader profile_name="flyguard_reader" is_default_profile="true">
<qos>
<reliability>
<kind>RELIABLE</kind>
</reliability>
</qos>
<historyMemoryPolicy>PREALLOCATED_WITH_REALLOC</historyMemoryPolicy>
</data_reader>
</profiles>
</dds>

59
docker/fetch_wheels.py Normal file
View file

@ -0,0 +1,59 @@
"""Скачать колёса PyTorch с CUDA для сборки образа без сети (в docker/wheels).
Нужно, когда у машины, где собирается образ, нет выхода в интернет (или он
сломан, как у WSL за VPN), а у соседней машины есть. Скачивать можно хоть на
Windows: колёса берутся под Linux x86_64 и Python 3.10 — ровно под образ.
Тонкость: `pip download --platform` не пересчитывает маркеры окружения и на
Windows молча пропускает зависимости «только для Linux» — все библиотеки CUDA
и triton. Поэтому их список берётся из метаданных самого колеса torch и
докачивается явно, с точными версиями.
python docker/fetch_wheels.py
"""
from __future__ import annotations
import re
import subprocess
import sys
import zipfile
from pathlib import Path
TORCH = "torch==2.9.1"
INDEX = "https://download.pytorch.org/whl/cu128"
OUT = Path(__file__).resolve().parent / "wheels"
TARGET = ["--platform", "manylinux_2_28_x86_64", "--platform", "manylinux_2_27_x86_64",
"--platform", "manylinux_2_17_x86_64", "--platform", "manylinux2014_x86_64",
"--python-version", "3.10", "--implementation", "cp", "--abi", "cp310",
"--only-binary=:all:"]
def pip_download(*args: str) -> None:
cmd = [sys.executable, "-m", "pip", "download", "--index-url", INDEX,
"--extra-index-url", "https://pypi.org/simple", "-d", str(OUT), *args]
subprocess.run(cmd, check=True)
def main() -> None:
OUT.mkdir(exist_ok=True)
pip_download(TORCH, *TARGET)
wheel = next(OUT.glob("torch-" + TORCH.split("==")[1] + "*.whl"))
with zipfile.ZipFile(wheel) as z:
meta = next(n for n in z.namelist() if n.endswith(".dist-info/METADATA"))
lines = z.read(meta).decode("utf-8").splitlines()
linux = []
for line in lines:
m = re.match(r"Requires-Dist: ([A-Za-z0-9_.\-]+==[^;\s]+); platform_system == \"Linux\"$",
line)
if m:
linux.append(m.group(1))
print(f"зависимости только для Linux: {len(linux)}")
pip_download("--no-deps", *TARGET, *linux)
subprocess.run([sys.executable, "-m", "pip", "download", "pip", "--only-binary=:all:",
"--no-deps", "-d", str(OUT)], check=True)
size = sum(p.stat().st_size for p in OUT.glob("*.whl")) / 2**30
print(f"готово: {len(list(OUT.glob('*.whl')))} колёс, {size:.1f} ГБ в {OUT}")
if __name__ == "__main__":
main()

10
docker/flyguard-keys.sh Normal file
View file

@ -0,0 +1,10 @@
#!/usr/bin/env bash
# Пульт проигрывания для уже запущенного контейнера (из второго терминала):
#
# docker exec -it flyguard_demo flyguard-keys /data/<запись>
#
# `docker exec` не проходит через точку входа, поэтому окружение ROS
# поднимается здесь. Клавиши — в flyguard/player_keys.py.
source /opt/ros/humble/setup.bash
source /opt/flyguard/ros2_ws/install/setup.bash
exec ros2 run flyguard player_keys "$@"

51
docker/gl_probe.py Normal file
View file

@ -0,0 +1,51 @@
"""Чем рисует OpenGL в контейнере: видеокарта или программный llvmpipe.
docker run --rm -e DISPLAY=:0 -v /tmp/.X11-unix:/tmp/.X11-unix \
-v "$PWD/docker:/smoke:ro" flyguard python3 /smoke/gl_probe.py
Окно не открывается: контекст создаётся на pbuffer 1×1 через EGL.
"""
import ctypes
import sys
EGL_OPENGL_API = 0x30A2
EGL_SURFACE_TYPE, EGL_PBUFFER_BIT = 0x3033, 0x0001
EGL_RENDERABLE_TYPE, EGL_OPENGL_BIT = 0x3040, 0x0008
EGL_WIDTH, EGL_HEIGHT, EGL_NONE = 0x3057, 0x3056, 0x3038
GL_VENDOR, GL_RENDERER, GL_VERSION = 0x1F00, 0x1F01, 0x1F02
egl = ctypes.CDLL("libEGL.so.1")
gl = ctypes.CDLL("libGL.so.1")
egl.eglGetDisplay.restype = ctypes.c_void_p
egl.eglGetDisplay.argtypes = [ctypes.c_void_p]
egl.eglInitialize.argtypes = [ctypes.c_void_p, ctypes.c_void_p, ctypes.c_void_p]
egl.eglChooseConfig.argtypes = [ctypes.c_void_p, ctypes.c_void_p, ctypes.c_void_p,
ctypes.c_int, ctypes.c_void_p]
egl.eglCreatePbufferSurface.restype = ctypes.c_void_p
egl.eglCreatePbufferSurface.argtypes = [ctypes.c_void_p, ctypes.c_void_p, ctypes.c_void_p]
egl.eglCreateContext.restype = ctypes.c_void_p
egl.eglCreateContext.argtypes = [ctypes.c_void_p, ctypes.c_void_p, ctypes.c_void_p,
ctypes.c_void_p]
egl.eglMakeCurrent.argtypes = [ctypes.c_void_p] * 4
gl.glGetString.restype = ctypes.c_char_p
dpy = egl.eglGetDisplay(None)
if not dpy or not egl.eglInitialize(dpy, None, None):
sys.exit("EGL не инициализировался (нет DISPLAY?)")
egl.eglBindAPI(EGL_OPENGL_API)
attrs = (ctypes.c_int * 5)(EGL_SURFACE_TYPE, EGL_PBUFFER_BIT,
EGL_RENDERABLE_TYPE, EGL_OPENGL_BIT, EGL_NONE)
cfg, n = ctypes.c_void_p(), ctypes.c_int()
if not egl.eglChooseConfig(dpy, attrs, ctypes.byref(cfg), 1, ctypes.byref(n)) or n.value < 1:
sys.exit("нет подходящей конфигурации EGL")
surf = egl.eglCreatePbufferSurface(dpy, cfg, (ctypes.c_int * 5)(EGL_WIDTH, 1, EGL_HEIGHT, 1,
EGL_NONE))
ctx = egl.eglCreateContext(dpy, cfg, None, None)
if not ctx or not egl.eglMakeCurrent(dpy, surf, surf, ctx):
sys.exit("контекст OpenGL не создан")
renderer = gl.glGetString(GL_RENDERER).decode()
print("vendor: ", gl.glGetString(GL_VENDOR).decode())
print("renderer:", renderer)
print("version: ", gl.glGetString(GL_VERSION).decode())
print("ПРОГРАММНО (процессор)" if "llvmpipe" in renderer or "softpipe" in renderer
else "ВИДЕОКАРТА")

41
docker/jury_cpu_test.sh Normal file
View file

@ -0,0 +1,41 @@
#!/usr/bin/env bash
# Узел на урезанном процессоре — имитация стенда жюри (i7-9700E).
#
# bash docker/jury_cpu_test.sh /root/bags/doubleT_obstacle # доля 0.61
# bash docker/jury_cpu_test.sh /root/bags/cloud_with_fake_obj 0.5
#
# Запускается на хосте (в WSL), не в контейнере. Узел идёт в отдельном
# контейнере с квотой процессора, а проигрыватель и подсчёт — во втором, без
# квоты: на стенде у проигрывателя свои ядра, и делить с ним квоту узла было бы
# строже, чем на самом деле.
#
# Доля по умолчанию — отношение однопоточной производительности (PassMark
# single thread): 2511 у i7-9700E против 4129 у Ryzen 5 7600X, на котором
# писались замеры. Узел однопоточный (BLAS в один поток), поэтому доля одного
# ядра и есть «ядро в 1.64 раза медленнее». Для другой машины передайте свою
# долю. Квота нарезается периодами по 10 мс, а не по 100 мс по умолчанию, —
# иначе узел работал бы рывками по 61 мс и стоял бы по 39 мс, что искажает
# задержку кадра сильнее, чем настоящий медленный процессор.
set -u
BAG="${1:?укажите путь к бэгу на хосте}"
SHARE="${2:-0.61}"
here="$(cd "$(dirname "$0")" && pwd)"
bag_dir="$(cd "$(dirname "$BAG")" && pwd)"
bag_name="$(basename "$BAG")"
quota=$(awk -v s="$SHARE" 'BEGIN{printf "%d", s * 10000}')
docker rm -f fg_slow >/dev/null 2>&1 || true
docker run -d --name fg_slow --network host --ipc host \
--cpu-period 10000 --cpu-quota "$quota" flyguard > /dev/null
echo "узел: квота $SHARE ядра ($quota мкс из 10000)"
docker run --rm --network host --ipc host -e NODE=external \
-v "$bag_dir:/data:ro" -v "$here:/smoke:ro" flyguard \
bash /smoke/demo_test.sh "/data/$bag_name" 2>&1 | grep -v "^\s*$"
docker kill --signal=INT fg_slow > /dev/null 2>&1
sleep 3
echo
echo "=== итог узла ==="
docker logs fg_slow 2>&1 | grep -E "итог|ошибк|Traceback" | sed 's/.*\] //'
docker rm -f fg_slow > /dev/null 2>&1

64
docker/setup_wsl_docker.sh Normal file
View file

@ -0,0 +1,64 @@
#!/usr/bin/env bash
# Установка Docker Engine внутрь WSL Ubuntu, без Docker Desktop.
#
# Зачем: Docker Desktop 4.69 не стартует, если имя пользователя Windows содержит
# кириллицу — его Inference manager не может создать unix-сокет по пути
# C:\Users\<кириллица>\AppData\Local\Docker\run\dockerInference. Docker Engine
# внутри WSL обходит проблему целиком и для нашей задачи даже уместнее:
# нужен Linux с ROS 2, а WSL и есть Linux.
#
# Запуск (от root внутри WSL):
# wsl -d Ubuntu -u root -- bash /mnt/c/Games/Study/AI_Lidar/docker/setup_wsl_docker.sh
#
# Откат:
# apt-get remove --purge -y docker.io containerd && apt-get autoremove -y
set -euo pipefail
# Docker Desktop оставляет в PATH заглушку `docker`, которая только печатает
# совет включить WSL-интеграцию. Поэтому всё делается по абсолютным путям.
DOCKER=/usr/bin/docker
DOCKERD=/usr/bin/dockerd
echo "== состояние =="
. /etc/os-release && echo "дистрибутив: ${PRETTY_NAME}"
echo "ядер: $(nproc), свободно на /: $(df -h / | awk 'NR==2 {print $4}')"
if [ -x "$DOCKERD" ]; then
echo "docker уже установлен: $($DOCKER --version)"
else
echo "== установка docker.io =="
export DEBIAN_FRONTEND=noninteractive
apt-get update -qq
apt-get install -y -qq --no-install-recommends docker.io iptables
fi
# В WSL нет systemd, демон поднимается напрямую. iptables переводится в
# legacy-режим: nft-бэкенд в WSL обычно не работает, и docker падает на
# настройке сети.
update-alternatives --set iptables /usr/sbin/iptables-legacy >/dev/null 2>&1 || true
update-alternatives --set ip6tables /usr/sbin/ip6tables-legacy >/dev/null 2>&1 || true
if $DOCKER info >/dev/null 2>&1; then
echo "демон уже работает"
else
echo "== запуск демона =="
mkdir -p /var/log
nohup "$DOCKERD" >/var/log/dockerd.log 2>&1 &
for i in $(seq 1 40); do
if $DOCKER info >/dev/null 2>&1; then
echo "демон поднялся за ${i} с"
break
fi
sleep 1
done
fi
if ! $DOCKER info >/dev/null 2>&1; then
echo "ОШИБКА: демон не поднялся, последние строки журнала:" >&2
tail -40 /var/log/dockerd.log >&2 || true
exit 1
fi
echo "== готово =="
$DOCKER version --format 'клиент {{.Client.Version}}, сервер {{.Server.Version}} ({{.Server.Os}}/{{.Server.Arch}})'
$DOCKER info --format 'ядер {{.NCPU}}, память {{.MemTotal}}, хранилище {{.Driver}}'

69
docker/smoke_test.sh Normal file
View file

@ -0,0 +1,69 @@
#!/usr/bin/env bash
# Проверка собранного образа: на месте ли пакеты, сообщения, зависимости.
# Запускается внутри контейнера, окружение ROS уже поднято точкой входа.
set -u
echo "=== пакеты ==="
ros2 pkg list | grep -i flyguard || echo " НЕТ пакетов flyguard"
echo
echo "=== исполняемые ==="
ros2 pkg executables flyguard || echo " НЕТ исполняемых"
echo
echo "=== сообщение ObstacleStatus ==="
ros2 interface show flyguard_msgs/msg/ObstacleStatus 2>&1 | grep -v '^#' | grep -v '^$' | head -12
echo
echo "=== python-зависимости ==="
python3 - <<'PY'
import numpy, scipy
print("numpy", numpy.__version__, "| scipy", scipy.__version__)
try:
import cv2
print("opencv", cv2.__version__)
except Exception as e:
print("opencv НЕТ:", e)
PY
echo
echo "=== импорт конвейера ==="
python3 - <<'PY'
from flyguard.pipeline import FlyGuard, Params
from flyguard.mushroom_body import MushroomBody
from pathlib import Path
import glob
fg = FlyGuard(Params())
print("конвейер импортируется, стадий в параметрах:", len(Params().__dataclass_fields__))
hits = glob.glob("/opt/ros/**/flyguard/artifacts/mushroom_body.npz", recursive=True) \
+ glob.glob("/opt/flyguard/**/mushroom_body.npz", recursive=True)
if hits:
mb = MushroomBody.load(hits[0])
print(f"память тоннеля: {hits[0]}")
print(f" клеток Кеньона {mb.cfg.n_kc}, активных {mb.n_active}, "
f"обучена на {mb.n_seen} примерах, признаков {mb.n_pn}")
else:
print("память тоннеля НЕ НАЙДЕНА в образе")
from flyguard.mbon_readout import MbonReadout
hits = glob.glob("/opt/ros/**/flyguard/artifacts/mbon_readout.npz", recursive=True) \
+ glob.glob("/opt/flyguard/**/mbon_readout.npz", recursive=True)
if hits:
rd = MbonReadout.load(hits[0])
print(f"считывание MBON: {hits[0]}")
print(f" клеток Кеньона {rd.cfg.n_kc}, признаков {rd.n_pn}")
else:
print("считывание MBON НЕ НАЙДЕНО в образе")
PY
echo
echo "=== rosbag ==="
if ros2 bag --help >/dev/null 2>&1; then
echo "ros2 bag доступен"
python3 -c "import rosbag2_py; print('rosbag2_py ok')" 2>&1 | tail -1
else
echo "ros2 bag НЕДОСТУПЕН"
fi
echo
echo "=== launch-файл ==="
ros2 launch flyguard detect.launch.py --show-args 2>&1 | head -20

1
docker/wheels/.gitignore vendored Normal file
View file

@ -0,0 +1 @@
*.whl

28
docker/wheels/README.md Normal file
View file

@ -0,0 +1,28 @@
# Колёса для сборки без сети
Если там, где собирается образ, нет доступа к download.pytorch.org, колёса PyTorch
скачиваются на соседней машине и отдаются сборке по HTTP — аргумент `TORCH_WHEELS`
в docker/Dockerfile. Без аргумента сборка берёт PyTorch из индекса, как обычно.
1. Скачать на машине с сетью — хоть на Windows, колёса берутся под Linux:
python docker/fetch_wheels.py
Обычного `pip download --platform ...` мало: на Windows он молча пропускает
зависимости «только для Linux» — все библиотеки CUDA и triton. Скрипт берёт их
список из метаданных колеса torch и докачивает явно, заодно кладёт колесо pip
(в базовом образе его нет).
2. Отдать папку любым HTTP-сервером и собрать образ с сетью хоста:
python3 -m http.server 8765 --bind 127.0.0.1 --directory docker/wheels &
docker build --network host --build-arg TORCH_WHEELS=http://127.0.0.1:8765 \
-t flyguard -f docker/Dockerfile .
`--network host` нужен, чтобы сборка видела сервер на 127.0.0.1. Базовый образ
и пакеты apt сборка всё равно берёт из сети (или из своего кэша, если эти слои
уже собирались) — колёса снимают только самую тяжёлую часть, 3.8 ГБ.
Колёса в контекст сборки не копируются (`.dockerignore`): это ещё 4 ГБ копий на
диске. В репозиторий они тоже не кладутся (.gitignore) — около 4 ГБ, 26 файлов.
После сборки папку можно очистить.

178
docs/ARCHITECTURE.md Normal file
View file

@ -0,0 +1,178 @@
# Архитектура решения
Цепочка, которую просит ТЗ: **ROS 2 bag → обработка облака → алгоритм обнаружения →
результат детекции → визуализация.**
---
## 1. Общая схема
```
ros2 bag play / реальный лидар
│
sensor_msgs/PointCloud2
0.3–0.9 млн точек, 10 Гц
│
┌──────────────────────────▼──────────────────────────────┐
│ flyguard/node.py — ROS 2-нода │
│ подписка (BEST_EFFORT) → очередь на 1 кадр │
│ обработка в отдельном потоке, старые кадры отброшены │
└──────────────────────────┬──────────────────────────────┘
│ flyguard.cdr.PointCloud2 (без копирования)
┌──────────────────────────▼──────────────────────────────┐
│ flyguard/pipeline.py — конвейер, хранит состояние │
└──────────────────────────┬──────────────────────────────┘
│
retina.py RETINA облако → дальностный образ 128 × N
│ выпрямление скоса каналов, слияние эх
▼
geometry.py HALTERES плоскость рельсов: крен, тангаж, высота
│ ось пути: дуга u(d) = c₁d + c₂d²
▼
lamina.py LAMINA диспаритет 1/R → ON/OFF, центр-окружение ×3
│
├────────────────► medulla.py MEDULLA / LOBULA PLATE
│ T4/T5 → LPTC: скорость без одометрии
│ LPLC2: надвигание
▼
lobula.py LOBULA связность с учётом глубины → кандидаты
│ разрез по контрасту: фигура из компоненты,
│ растёкшейся вдоль стены
│ признаки: габариты, целостность, тень, опора
▼
mushroom_body MUSHROOM BODY PN→KC (случайно, 6 входов) → APL → MBON
│ новизна: 1 — незнакомо, 0 — штатная обстановка
│ (привыкание внутри проезда сделано и выключено: п. 10
│ EXPERIMENTS — избирательности у механизма нет)
▼
fan_body.py FAN-SHAPED BODY сетка в координатах пути: опора для кандидата
│ там, где контраст структурно равен нулю
▼
central_complex CENTRAL COMPLEX накопление улик в координатах пути, треки
│
▼
descending.py DESCENDING два порога с гистерезисом → решение
│
▼
┌──────────────────────────────────────────────────────────┐
│ /flyguard/obstacle ObstacleStatus — программный выход │
│ /flyguard/detected Bool — бинарный статус │
│ /flyguard/distance Float32 — расстояние, м │
│ /flyguard/markers MarkerArray — рамки для RViz2 │
│ /flyguard/brain Image — схема мозга мухи │
│ /flyguard/diagnostics DiagnosticArray — задержки, скорость│
└──────────────────────────────────────────────────────────┘
```
---
## 2. Состояние между кадрами
Конвейер не обрабатывает кадры независимо. Между вызовами он хранит:
| Что | Где | Зачем |
|---|---|---|
| решётка лучей | `FlyGuard.layout` | калибруется по первым 12 кадрам, дальше не меняется |
| плоскость пути | `FlyGuard.plane` | сглаживание по кадрам, устойчивость к качке |
| ось пути | `FlyGuard.corridor` | сглаживание и ограничение скорости изменения |
| профиль и точки | `EgoMotionEstimator` | сопоставление с предыдущим кадром → скорость |
| задержанный сигнал | `EmdBank` | вторая половина коррелятора T4/T5 |
| сетка пути | `FanBody` | накопление лучей в координатах мира |
| треки | `CentralComplex` | накопление улик в координатах пути |
| гистерезис | `DescendingNeurons` | защёлка тревоги |
Поэтому один экземпляр `FlyGuard` обслуживает один поток данных. Для офлайн-экспериментов
с несколькими сценариями одновременно создаётся несколько экземпляров.
---
## 3. Потоки и реальное время
Нода разделена на два потока:
* **поток ROS** принимает облака и кладёт в слот на один кадр; если предыдущий ещё не
обработан, он **отбрасывается** и счётчик `dropped_frames` растёт;
* **рабочий поток** берёт последний кадр и гоняет конвейер.
Так система реального времени отвечает на текущую обстановку, а не доедает накопившееся
прошлое. Число отброшенных кадров публикуется в диагностике: если оно растёт, значит
машина не тянет, и это видно сразу, а не проявляется скрытой задержкой.
Замер по стадиям ведётся всегда и публикуется в `/flyguard/diagnostics`, поэтому
профилировать решение можно прямо на стенде, не пересобирая его.
---
## 4. Разделение на пакеты
```
ros2_ws/src/
├── flyguard_msgs/ ament_cmake — только сообщения
│ └── msg/ObstacleStatus.msg, msg/DetectedObject.msg
└── flyguard/ ament_python — конвейер и нода
├── flyguard/
│ ├── cdr.py разбор PointCloud2 без ROS (офлайн-режим)
│ ├── bag.py чтение rosbag2 sqlite3 без ROS
│ ├── ros_conv.py sensor_msgs → внутреннее представление
│ ├── retina.py решётка лучей, дальностный образ
│ ├── geometry.py плоскость пути, ось, координаты (d, u, h)
│ ├── lamina.py ON/OFF, центр-окружение
│ ├── medulla.py T4/T5, LPTC, LPLC2, оценка движения
│ ├── lobula.py кандидаты, связность с учётом глубины
│ ├── mushroom_body.py новизна
│ ├── central_complex.py треки
│ ├── descending.py решение
│ ├── synth.py синтетические препятствия (для полигона)
│ ├── brain_view.py схема мозга мухи
│ ├── pipeline.py сборка
│ ├── node.py ROS 2-нода
│ └── data/pandar128_channels.csv поканальная таблица из руководства
├── launch/detect.launch.py
├── config/flyguard.yaml, config/flyguard.rviz
└── test/test_pipeline.py
```
Ключевое решение: **ядро не зависит от ROS**. `rclpy` импортируется только в `node.py`.
Благодаря этому весь конвейер запускается офлайн прямо по `.db3`, что дало возможность
отлаживать и мерить качество на Windows без ROS и быстро гонять полигон в несколько
параллельных сценариев.
---
## 5. Артефакты
| Файл | Что это | Как получен |
|---|---|---|
| `artifacts/mushroom_body.npz` | память тоннеля, без учителя | `tools/train_mushroom_body.py` на пустых проездах |
| `artifacts/mbon_readout.npz` | обученное считывание MBON | `tools/train_mbon.py` на размеченной вставками выборке |
| `artifacts/mbon_folds/` | по модели на складку, для честной проверки | он же, ключ `--save-folds` |
| `data/cache/training_set.npz` | размеченная выборка кандидатов | `tools/make_training_set.py` |
| `artifacts/generalisation.json` | leave-one-bag-out | `tools/evaluate.py` |
| `artifacts/benchmark.json` | кривые дальности | `tools/make_benchmark.py` |
| `flyguard/data/pandar128_channels.csv` | 128 каналов лидара | `tools/extract_channel_table.py` из руководства |
Память тоннеля копируется в Docker-образ и подхватывается launch-файлом автоматически;
путь переопределяется параметром `memory_path`.
---
## 6. Потоки данных в цифрах
Для кадра 128 × 3600 × 2 эха (полный круговой скан, 921 600 точек):
| Стадия | Объём на входе | Время, мс |
|---|---|---|
| приём и разбор сообщения | 24 МБ | ~2 (без копирования) |
| retina (оконная проекция) | только нужный сектор | 6–9 |
| стабилизация | ~150 тыс. точек | 3–4 |
| ось пути | 30 срезов | 4–5 |
| ламина | 128 × 600 × 3 масштаба | 6–7 |
| оценка движения | 6000 точек × ~20 проб | 5–8 |
| лобула | связность по маске | 3–5 |
| грибовидное тело | единицы кандидатов | 1–2 |
| центральный комплекс и решение | десятки треков | <0.3 |
Оконная проекция — важная оптимизация: при секторе обработки ±30° тяжёлая арифметика
выполняется только над теми сырыми столбцами, которые в него попадут с учётом скоса
каналов. На круговом скане это сократило стадию ретины с 29 до 7 мс без изменения
результата (проверено побитовым сравнением).

30
docs/CONNECTOME.md
View file

@ -147,6 +147,36 @@ T4 получает вход из ON-пути, T5 — из OFF-пути; каж
---
## 3.5. Атлас: те же клетки, видимые глазом
Проверить, что архитектура действительно собрана из заявленных схем, можно не на
словах. `tools/build_brain_atlas.py` берёт публичные выгрузки Codex (FAFB v783) и
сводит их в атлас: положение сомы каждого нейрона и номер стадии FlyGuard, которой
он соответствует. Привязка идёт по **имени типа клетки**, а не по догадке, и все
типы находятся поимённо:
| Тип | В коннектоме | Где используется |
|---|---|---|
| LC11 | 127 | `lobula.py` — кандидаты как компактные группы лучей |
| LPLC2 | 210 | `medulla.py` — надвигание как дивергенция потока |
| HS (HSE/HSN/HSS) | 6 | `medulla.py` — широкопольный поток |
| VS1…VS8 | 16 | там же, вертикальный канал |
| T4a–d / T5a–d | 6243 / 6002 | `medulla.py` — коррелятор Хассенштайна–Райхардта |
| L1 / L2 | 3503 | `lamina.py` — каналы ON и OFF |
| Клетки Кеньона | 5177 | `mushroom_body.py` — разрежённый код |
| MBON | 96 | там же, выход памяти |
| APL | 2 | там же, глобальное торможение |
| EPG | 47 | `central_complex.py` — кольцевой аттрактор |
| DNp01 | 2 | `descending.py` — гигантское волокно |
Совпадение с литературой полное: LC11 около 110–130 клеток, HS по три на сторону,
VS по восемь, APL и гигантское волокно — по одной на полушарие.
Вид `brain_style: cloud` рисует эти 139 255 нейронов на их анатомических местах и
подсвечивает активностью соответствующей стадии. Симуляции по-прежнему нет:
коннектом даёт анатомию, конвейер даёт активность, вид накладывает одно на другое
за 8 мс на кадр.
## 4. Источники
* Dorkenwald S. и др. **Neuronal wiring diagram of an adult brain.** Nature, 2024 —

82
docs/EXPERIMENTS.md
View file

@ -252,6 +252,86 @@ numba являются ламина и оценка движения — вме
проекция сократила стадию с **29 до 7 мс**, причём результат совпадает с полной
проекцией **побитово** — проверено сравнением массивов.
### 7.1. Замеры в собранном контейнере
Всё выше измерено офлайн. Ниже — то же, но внутри Docker-образа, через настоящий
ROS 2 и `ros2 bag play`. Счётчики берутся из самой ноды и публикуются в
`/flyguard/diagnostics`: внешний подписчик сам теряет сообщения и занижает оценку.
| Бэг | Кадр | Всего в бэге | Принято | На калибровку | Обработано | Отброшено | Медиана | p95 |
|---|---|---|---|---|---|---|---|---|
| `roundT_doubleT` | 8 МБ | 252 | **252** | 11 | 241 | **0** | 43.0 мс | 54.4 мс |
| `doubleT_obstacle` | 24 МБ | 201 | **201** | 11 | 190 | **0** | 31.9 мс | 41.2 мс |
Принимается **всё**. Одиннадцать кадров уходят на восстановление решётки лучей по самим
данным — это цена того, что в решении не захардкожена геометрия сенсора.
Нода обрабатывает **всё, что до неё доходит**, на полных 10 Гц и с трёхкратным
запасом по бюджету. Реальный объект в контейнере обнаруживается в **98.9 %** кадров —
ровно столько же, сколько офлайн. Ложные тревоги на `roundT_doubleT` в контейнере
**1.2 %** кадров (было 40.6 % до исправления контекста, см. 5.1).
### 7.2. Где на самом деле теряются кадры
Сначала до ноды на `doubleT_obstacle` доходило 106 сообщений из 201. Проверка показала,
что алгоритм тут ни при чём: **подписчик, который вообще ничего не делает**, получал
ровно столько же (99 из 201). Потери были в транспорте, и у них нашлись три причины.
**1. Умолчания Fast DDS рассчитаны на сотни килобайт.** Сегмент разделяемой памяти —
512 КБ, буферы сокетов ОС — 208 КБ, при кадре в 24 МБ. Кадр не помещается, разделяемая
память не используется, всё идёт по UDP десятками тысяч фрагментов. Профиль
`docker/fastdds_large.xml` поднимает сегмент до 128 МБ, буферы до 64 МБ и ставит
разделяемую память первой; он прописан в образ через `FASTRTPS_DEFAULT_PROFILES_FILE`.
**2. `/dev/shm` в контейнере по умолчанию 64 МБ.** Сегмент туда не влезает, Fast DDS
молча откатывается на UDP. Нужен `--shm-size=1g`; точка входа предупреждает, если
меньше. Сегмент заводит каждый участник DDS, а их в сеансе обычно три (нода,
проигрыватель, наблюдатель) — поэтому 128 МБ, а не 512: при 512 третий участник
не помещался и получал `Unable to Register SHM Transport`.
**3. Проигрыватель набивает очередь чтения до старта.** `ros2 bag play` по умолчанию
читает вперёд 1000 сообщений; при кадре в 24 МБ это 24 ГБ. Пока он читает, часы записи
уже идут, и первые секунды кадров «просрочиваются» и не публикуются вовсе. Помогает
`--read-ahead-queue-size 10`.
Итог по 24-мегабайтному бэгу (201 кадр всего), приёмник без обработки и полный конвейер:
| Конфигурация | Пустой приёмник | Полный конвейер |
|---|---|---|
| умолчания | 99 | 106 |
| + профиль DDS и `--shm-size=1g` | **201** | 125 |
| + `--read-ahead-queue-size 10` | — | 181 |
| + быстрая калибровка решётки | — | **201** |
**4. Калибровка решётки блокировала колбэк на 2.8 секунды.** Оставшиеся два десятка
кадров терялись не в транспорте: восстановление решётки по 12 кадрам считало медиану
по стопке из 12 массивов 128 × 7200 — 1.4 с только на два `nanmedian`. Пока колбэк
занят, очередь на 10 кадров переполняется.
Направление луча задано сенсором и в каждом кадре одно и то же; кадры нужны лишь
чтобы закрыть лучи без эха. Медиана заменена на первое конечное значение, счёт —
на float32: **2806 мс → 373 мс**. Сверка с паспортом после замены не изменилась ни в
одном знаке (медиана ошибки 0.0645° по элевации, 0.0435° по сдвигу — как и раньше).
Очередь поднята до 20 кадров как страховка.
Заодно исправлен счётчик: `frames_received` считался как «обработано + отброшено» и
не включал кадры калибровки, из-за чего в отчёте выглядела потеря там, где её не было.
Отдельно применено в самой ноде:
* подписка по умолчанию **RELIABLE** — при BEST_EFFORT потеря одного UDP-фрагмента
отбрасывает всё 24-мегабайтное сообщение целиком;
* кадр берётся **сырыми байтами CDR** (`raw_subscription`), минуя сборку
Python-объекта `sensor_msgs` — на 24 МБ она стоит дороже всей нашей обработки;
* обработка идёт **в колбэке, а не в отдельном потоке**: поток боролся за GIL
с приёмом и ронял выработку с 10 до 2 Гц при неизменном такте в 32 мс.
Почему не переписали конвейер на компилируемый язык: он тратит 32 мс из 100 и
простаивает две трети такта, а терялись кадры **до** его вызова. Ускорение обработки
не вернуло бы ни одного кадра; настройка транспорта вернула 82.
---
## 7.3. Почему далёкий предмет теряется — и что нужно для 200 м
ТЗ просит 300 м как «отлично» и 200 м как «очень хорошо». Разберём честно, чего
@ -339,7 +419,7 @@ numba являются ламина и оценка движения — вме
| Кадров с тревогой | **9.4 %** | 17.2 % |
| Посторонних тревог на кадр (полигон) | **0.055** | 0.171 |
| Реальный объект на 55 м | 98.9 % | 98.9 % |
| Задержка, медиана | 32 мс | 33 мс |
| Задержка, медиана в контейнере | 32 мс | 33 мс |
**Включено по умолчанию.** Размен здесь принципиально лучше, чем у разделения фигуры
и фона (п. 9.4): там было вчетверо больше ложных за +29 % дальности, здесь — в 1.75

BIN
docs/figures/curve_gauge.png Normal file
View file

Binary file not shown.
Side by side

After

Width:  |  Height:  |  Size: 124 KiB

BIN
docs/figures/doubleT_mount_shadow.png Normal file
View file

Binary file not shown.
Side by side

After

Width:  |  Height:  |  Size: 263 KiB

364
flyguard/brain_atlas.py Normal file
View file

@ -0,0 +1,364 @@
"""Мозг как облако из 139 255 нейронов, подсвеченное живой активностью.
Отличие от `brain_view` — не в красоте, а в том, чем нарисовано. Там схема,
нарисованная руками; здесь каждая точка — **настоящий нейрон из коннектома**
FlyWire FAFB v783, стоящий на своём анатомическом месте, а его яркость берётся
от той стадии FlyGuard, которой он соответствует.
Симуляции нет и не нужно. Мембранные потенциалы 139 тысяч клеток никто не
интегрирует: коннектом даёт анатомию и принадлежность клеток, конвейер даёт
активность по стадиям, а здесь одно накладывается на другое. Стоимость кадра —
взвешенная сумма десяти заранее посчитанных карт плотности.
Привязка держится на именах типов клеток, а не на догадках. FlyGuard с самого
начала собран из конкретных схем, и все они в выгрузке есть поимённо:
LC11 127 нейронов детектор мелкого объекта → lobula.py
LPLC2 210 детектор надвигания → medulla.py
HS/VS 22 широкопольный поток → medulla.py
T4/T5 12245 элементарные детекторы движения
KC 5177 клетки Кеньона, разрежённый код → mushroom_body.py
MBON 96 выход памяти, новизна
APL 2 глобальное торможение
DNp01 2 гигантское волокно, экстренное решение → descending.py
Атлас собирается `tools/build_brain_atlas.py` и лежит в `data/brain_atlas.npz`
(605 КБ). Данные FlyWire — CC-BY 4.0, Dorkenwald et al. и Schlegel et al.,
Nature 2024.
"""
from __future__ import annotations
from collections import deque
from pathlib import Path
import numpy as np
try:
import cv2
except ImportError:
cv2 = None
from .brain_view import BG, INK, _colour, put_text, text_size
ATLAS = Path(__file__).with_name("data") / "brain_atlas.npz"
# Какая стадия конвейера освещает какую группу нейронов. Ключи справа — из
# `BrainView.activity`; так обе картинки всегда показывают одно и то же.
STAGE_SOURCE: dict[str, tuple[str, ...]] = {
"retina": ("retina",),
"lamina": ("lamina",),
"medulla": ("medulla",),
"lptc": ("lobula_plate",),
"looming": ("looming",),
"lobula": ("lobula",),
"mushroom": ("mb_lobes", "mb_calyx"),
"central": ("eb", "pb", "fb"),
"descending": ("dn",),
"other": (),
}
OTHER_BASE = 0.045 # остальной мозг: виден, но не отвлекает
TRACE_N = 100 # длина ритмограммы: 10 с при 10 Гц
RU = {
"retina": "сетчатка R1–R8",
"lamina": "ламина L1 / L2",
"medulla": "медулла T4 / T5",
"lptc": "лоб. пластинка HS / VS",
"looming": "LPLC2 надвигание",
"lobula": "лобула LC11",
"mushroom": "грибовидное тело KC → MBON",
"central": "центральный комплекс",
"descending": "нисходящие DNp01",
}
class NeuronCloud:
"""Облако нейронов в анатомических координатах, яркость — активность стадии."""
def __init__(self, labels: bool = True, gain: float = 1.0, scale: int = 1):
self.enabled = cv2 is not None and ATLAS.exists()
self.labels = labels
self.gain = gain
# Масштаб отрисовки: 1 — для узла (1180 × 620), 2–3 — для экрана и
# видео в 2K/4K. Сомы берутся из атласа своего размера
# (brain_atlas_x2.npz …) — это настоящие координаты, а не растянутая
# картинка; шрифты, линии и отступы растут вместе с масштабом.
self.s = max(1, int(scale))
self._smooth: dict[str, float] = {}
self._trace: deque = deque(maxlen=TRACE_N)
if not self.enabled:
return
path = ATLAS.with_name(f"brain_atlas_x{self.s}.npz") if self.s > 1 else ATLAS
d = np.load(path if path.exists() else ATLAS, allow_pickle=False)
self.h, self.w = (int(v) for v in d["shape"])
stages = [str(s) for s in d["stages"]]
px, py = d["px"].astype(np.int64), d["py"].astype(np.int64)
stage, side, depth = d["stage"], d["side"], d["depth"]
if not path.exists(): # атласа нужного размера нет — растянуть обычный
px, py, self.h, self.w = px * self.s, py * self.s, self.h * self.s, self.w * self.s
ok = (px >= 0) & (px < self.w) & (py >= 0) & (py < self.h)
flat = py * self.w + px
# Глубина: дальние сомы тусклее, иначе фронтальная проекция выглядит плоской
shade = (0.55 + 0.45 * (1.0 - depth / 255.0)).astype(np.float32)
# Сомы занимают шестую часть холста; считать по всему полотну незачем.
# Карты сразу строятся по занятым пикселям: это убирает из каждого
# кадра 80 % работы, а в 4K — ещё и сотни мегабайт промежуточных карт.
self.support = np.unique(flat[ok])
at = np.searchsorted(self.support, flat)
# Две карты на стадию — левое и правое полушарие: так видно, с какой
# стороны пришёл объект, ровно как у мухи с её двумя оптическими долями.
self.keys: list[str] = []
maps = []
for i, name in enumerate(stages):
for sd, tag in ((0, "L"), (1, "R"), (2, "C")):
m = ok & (stage == i) & (side == sd)
if m.sum() == 0:
continue
self.keys.append(f"{name}|{tag}")
maps.append(np.bincount(at[m], weights=shade[m],
minlength=self.support.size).astype(np.float32))
self.maps = np.ascontiguousarray(np.stack(maps)) # (K, занятые пиксели)
self.stage_of = [k.split("|")[0] for k in self.keys]
self.side_of = [k.split("|")[1] for k in self.keys]
# Якоря подписей — центры тяжести стадии в своём полушарии. Общий
# центр тяжести у всех стадий приходится на середину мозга и ничего
# не показывает: стадии парные.
self.anchor: dict[str, tuple[tuple[int, int], tuple[int, int]]] = {}
for i, name in enumerate(stages):
if name not in RU:
continue
a = []
for sd in (0, 1):
m = ok & (stage == i) & (side == sd)
m = m if m.sum() > 30 else (ok & (stage == i))
a.append((int(px[m].mean()), int(py[m].mean())))
self.anchor[name] = tuple(a)
self._lut = self._build_lut()
self._base = self._draw_base()
# ------------------------------------------------------------------ статика
@staticmethod
def _build_lut() -> np.ndarray:
"""Палитра «покой → работа → тревога», та же, что у схемы."""
lut = np.zeros((256, 3), np.uint8)
for i in range(256):
lut[i] = _colour(i / 255.0)
return lut
def _draw_base(self) -> np.ndarray:
s = self.s
img = np.full((self.h, self.w, 3), BG, np.uint8)
put_text(img, "FlyGuard — 139 255 нейронов коннектома FlyWire, "
"подсвеченных активностью конвейера",
(26 * s, 34 * s), 0.6 * s, (205, 205, 215))
cv2.line(img, (26 * s, 46 * s), (self.w - 26 * s, 46 * s), (52, 52, 62), s)
return img
# ------------------------------------------------------------------ кадр
def weights(self, act: dict[str, float], bias: float) -> np.ndarray:
"""Яркость каждой полукарты. `bias` < 0 — объект слева, > 0 — справа."""
w = np.empty(len(self.keys), np.float32)
for i, (stage, side) in enumerate(zip(self.stage_of, self.side_of)):
src = STAGE_SOURCE.get(stage, ())
v = max((self._smooth.get(k, 0.0) for k in src), default=0.0)
v = OTHER_BASE if not src else max(v, OTHER_BASE * 0.7)
if side in ("L", "R") and stage in ("retina", "lamina", "medulla",
"lptc", "looming", "lobula"):
# У мухи зрительный мир перекрещен: предмет справа сильнее
# освещает левую долю. Для демонстрации важно, что асимметрия
# видна и соответствует стороне объекта.
v *= 1.0 + 0.10 * (bias if side == "L" else -bias)
w[i] = np.clip(v, 0.0, 1.0)
return w
def render(self, res) -> np.ndarray | None:
if not self.enabled:
return None
from .brain_view import BrainView
for k, v in BrainView.activity(res).items():
self._smooth[k] = 0.6 * self._smooth.get(k, v) + 0.4 * v
# Ритмограмма: у дрозофилы ритмы мозга меряют электродом, из наших
# данных они не выводятся. Поэтому здесь честный след собственного
# состояния — сколько работает зрительный тракт, сколько памяти и
# когда сработало решение.
vis = float(np.mean([self._smooth.get(k, 0.0) for k in
("lamina", "medulla", "lobula", "lobula_plate")]))
self._trace.append((float(vis),
float(self._smooth.get("dopamine", 0.0)),
bool(res.decision.detected),
float(res.total_ms),
float(self._smooth.get("habituation", 0.0))))
lat = [c.u for c in res.candidates] or [0.0]
bias = float(np.clip(np.mean(lat) / 1.6, -1.0, 1.0))
vals = self.weights(self.activity_smoothed(), bias) @ self.maps
img = self._base.copy()
if self.s == 1:
idx = np.clip(vals * (255.0 * self.gain), 0, 255).astype(np.uint8)
img.reshape(-1, 3)[self.support] = self._lut[idx]
# Точки одиночные и на большом холсте теряются: лёгкое размытие делает
# облако читаемым, не превращая его в заливку.
cv2.GaussianBlur(img, (3, 3), 0, dst=img)
else:
self._paint_fine(img, vals)
if self.labels:
self._draw_labels(img)
self._draw_rhythm(img)
self._draw_readout(img, res)
return img
def _paint_fine(self, img, vals) -> None:
"""Облако в высоком разрешении: каждая сома на своём месте.
В масштабе 1 пиксель холста собирает несколько сом, и яркость — их
сумма. Здесь сома — отдельная точка, поэтому сумма берётся по
окрестности той же площади (гауссово окно, нормированное так, что
одиночная сома сохраняет свою яркость), а края точек смешиваются с
фоном по доле покрытия. Облако выглядит как в узле, только чётче.
"""
sig = 0.55 * self.s
norm = 2.0 * np.pi * sig * sig
field = np.zeros((2, self.h * self.w), np.float32)
field[0, self.support] = vals
field[1, self.support] = 1.0
v = cv2.GaussianBlur(field[0].reshape(self.h, self.w), (0, 0), sig) * norm
a = np.clip(cv2.GaussianBlur(field[1].reshape(self.h, self.w), (0, 0), sig)
* norm, 0.0, 1.0)
idx = np.clip(v * (255.0 * self.gain), 0, 255).astype(np.uint8)
col = self._lut[idx]
img[:] = cv2.blendLinear(col, img, a, 1.0 - a)
def activity_smoothed(self) -> dict[str, float]:
return self._smooth
def _draw_labels(self, img) -> None:
"""Подписи столбиком у ближнего края с выноской к центру тяжести стадии.
Центры тяжести половины стадий лежат в середине мозга и налезают друг
на друга, поэтому подписи разводятся по вертикали, а привязка остаётся
честной — линия ведёт ровно в центр тяжести.
"""
# Зрительный путь подписывается слева, память и решение — справа:
# так выноски не пересекают мозг и идут в своё же полушарие.
s = self.s
left_side = ("retina", "lamina", "medulla", "lptc")
cols = {True: 76 * s, False: 76 * s}
for name in ("retina", "lamina", "medulla", "lptc",
"looming", "lobula", "mushroom", "central", "descending"):
if name not in self.anchor:
continue
left = name in left_side
ax, ay = self.anchor[name][0 if left else 1]
txt = RU[name]
tw, th = text_size(txt, 0.42 * s)
x = 26 * s if left else self.w - tw - 26 * s
y = cols[left]
cols[left] += 27 * s
box = img[max(y - th - 5 * s, 0):y + 5 * s, max(x - 6 * s, 0):x + tw + 6 * s]
if box.size:
box[:] = (box * 0.2).astype(np.uint8)
put_text(img, txt, (x, y), 0.42 * s, INK)
tip = (x + tw + 7 * s, y - th // 2) if left else (x - 7 * s, y - th // 2)
cv2.line(img, tip, (ax, ay), (82, 80, 96), s, cv2.LINE_AA)
cv2.circle(img, (ax, ay), 3 * s, (162, 160, 178), s, cv2.LINE_AA)
def _draw_rhythm(self, img) -> None:
"""Ритмограмма: след активности и дофамина за последние десять секунд.
Это не ритм мозга дрозофилы — тот меряют электродом, и из лидарных
данных он не выводится. Это след собственного состояния системы:
зрительный тракт, сигнал обучения и отметки моментов решения.
"""
if len(self._trace) < 3:
return
s = self.s
x0, y0, w, hgt = 250 * s, self.h - 96 * s, 380 * s, 56 * s
cv2.rectangle(img, (x0, y0), (x0 + w, y0 + hgt), (40, 39, 48), -1)
cv2.rectangle(img, (x0, y0), (x0 + w, y0 + hgt), (60, 58, 70), s)
put_text(img, "ритм · 10 с", (x0 + 4 * s, y0 - 6 * s), 0.37 * s, (128, 126, 140))
n = len(self._trace)
xs = x0 + (np.arange(n) * (w - 6 * s) / max(TRACE_N - 1, 1)).astype(int) + 3 * s
tr = np.array([(a, b, float(c), e) for a, b, c, _, e in self._trace],
np.float32)
lx = x0 + 78 * s
traces = [(0, (176, 196, 92), "зрение"), (1, (86, 178, 240), "дофамин")]
if tr[:, 3].max() > 0.0:
traces.append((3, (150, 120, 226), "привыкание"))
for col, colour, name in traces:
ys = (y0 + hgt - 8 * s - tr[:, col] * (hgt - 16 * s)).astype(int)
cv2.polylines(img, [np.stack([xs, ys], 1)], False, colour, s, cv2.LINE_AA)
cv2.line(img, (lx, y0 - 10 * s), (lx + 11 * s, y0 - 10 * s), colour, 2 * s,
cv2.LINE_AA)
put_text(img, name, (lx + 15 * s, y0 - 6 * s), 0.35 * s, (132, 130, 144))
lx += 15 * s + text_size(name, 0.35 * s)[0] + 16 * s
# Полоска решений внизу: где система говорила «препятствие».
hot = tr[:, 2] > 0.5
if hot.any():
for i in np.flatnonzero(hot):
cv2.line(img, (int(xs[i]), y0 + hgt - 5 * s), (int(xs[i]), y0 + hgt - 2 * s),
(66, 66, 248), s)
cv2.line(img, (lx, y0 - 10 * s), (lx + 11 * s, y0 - 10 * s), (66, 66, 248), 2 * s)
put_text(img, "решение", (lx + 15 * s, y0 - 6 * s), 0.35 * s, (132, 130, 144))
def _draw_readout(self, img, res) -> None:
d = res.decision
hab = float(getattr(res, "hab", 0.0))
rows = [("скорость", f"{abs(d.speed) * 3.6:.1f} км/ч"),
("кандидатов", f"{len(res.candidates)}"),
("новизна MBON", f"{max((c.novelty for c in res.candidates), default=0.0):.2f}"),
("дофамин PPL1/PAM", f"{self._smooth.get('dopamine', 0.0):.2f}")]
# привыкание по умолчанию выключено — строка появляется, только если оно
# включено и уже что-то накопило
if hab > 0.0:
rows.append(("привыкание", f"{hab:.2f}"))
rows.append(("обработка", f"{res.total_ms:.1f} мс"))
s = self.s
y0 = self.h - 126 * s - 23 * s * (len(rows) - 5)
# Столбик лежит поверх облака, и серые подписи в нём тонули в точках
# оптической доли — подложка, как под выносками стадий.
vw = max(text_size(v, 0.46 * s)[0] for _, v in rows)
right = min(196 * s + vw + 10 * s, 244 * s) # не заходить на ритмограмму
box = img[y0 - 18 * s:y0 + 23 * s * (len(rows) - 1) + 8 * s, 18 * s:right]
box[:] = (box * 0.25).astype(np.uint8)
for i, (k, v) in enumerate(rows):
put_text(img, k, (28 * s, y0 + i * 23 * s), 0.44 * s, (118, 116, 128))
put_text(img, v, (196 * s, y0 + i * 23 * s), 0.46 * s, (206, 206, 216))
if d.detected:
txt = f"ПРЕПЯТСТВИЕ {d.distance:.0f} м"
sub = ("ЭКСТРЕННОЕ ТОРМОЖЕНИЕ" if d.emergency
else f"уверенность {d.confidence:.2f}")
colour = (66, 66, 248) if d.emergency else (72, 196, 240)
else:
txt, sub, colour = "ПУТЬ СВОБОДЕН", "", (120, 214, 130)
put_text(img, txt, (self.w - 470 * s, self.h - 62 * s), 0.9 * s, colour, 2)
if sub:
put_text(img, sub, (self.w - 470 * s, self.h - 32 * s), 0.54 * s, colour)
x = self.w - 300 * s
for lvl, name in ((0.10, "покой"), (0.45, "работа"), (0.85, "тревога")):
cv2.circle(img, (x, 34 * s), 5 * s, _colour(lvl), -1, cv2.LINE_AA)
put_text(img, name, (x + 12 * s, 39 * s), 0.4 * s, (150, 148, 162))
x += 14 * s + text_size(name, 0.4 * s)[0] + 22 * s
# ------------------------------------------------------------------ ROS
@staticmethod
def to_msg(img: np.ndarray, stamp, frame_id: str):
from .brain_view import BrainView
return BrainView.to_msg(img, stamp, frame_id)

145
flyguard/brain_hybrid.py Normal file
View file

@ -0,0 +1,145 @@
"""Гибридный вид: что видит глаз — и что от этого происходит в мозге.
Сверху четыре панели ретинотопической решётки, как их видит муха: дальностный
образ, ON-канал ламины (что ближе окружения), OFF-канал (что дальше) и
выделенные кандидаты. Снизу — те же 139 255 нейронов коннектома, подсвеченные
активностью соответствующих стадий.
Смысл именно в паре: видно не только «мозг светится», но и от чего. Контраст
в ламине появляется на панели — и в ту же секунду загорается полоса ламины в
облаке; предмет попадает в габарит — вспыхивает лобула; трек набирает улику —
разгорается центральный комплекс и уходит вниз к гигантскому волокну.
Стоит около 12 мс и включается флагом, чтобы не попадать в замеры задержки.
"""
from __future__ import annotations
import numpy as np
try:
import cv2
except ImportError:
cv2 = None
from .brain_atlas import NeuronCloud
from .brain_view import BG, put_text
PANEL_H = 132 # высота строки панелей, px
PAD = 10
def _norm(a: np.ndarray, lo: float, hi: float) -> np.ndarray:
return np.clip((a - lo) / max(hi - lo, 1e-6), 0.0, 1.0)
class BrainHybrid:
"""Панели решётки + облако нейронов в одном кадре."""
def __init__(self, labels: bool = True, scale: int = 1):
self.cloud = NeuronCloud(labels=labels, scale=scale)
self.s = self.cloud.s
self.enabled = cv2 is not None and self.cloud.enabled
if not self.enabled:
return
self.w = self.cloud.w
self.strip_h = (PANEL_H + 34) * self.s
self.h = self.cloud.h + self.strip_h
# ------------------------------------------------------------------ панели
def _panel(self, img: np.ndarray, title: str, hint: str, slot: int,
out: np.ndarray) -> None:
n, s = 4, self.s
pad, ph, top = PAD * s, PANEL_H * s, 28 * s
pw = (self.w - pad * (n + 1)) // n
x = pad + slot * (pw + pad)
# INTER_AREA, а не NEAREST: столбцы чередуются по эхам, и при
# прореживании соседями решётка превращается в шахматку.
small = cv2.resize(img, (pw, ph), interpolation=cv2.INTER_AREA)
out[top:top + ph, x:x + pw] = small
cv2.rectangle(out, (x, top), (x + pw, top + ph), (58, 56, 68), s)
from .brain_view import text_size
put_text(out, title, (x + 2 * s, 22 * s), 0.4 * s, (188, 188, 200))
if hint:
tw = text_size(hint, 0.36 * s)[0]
# подпись справа рисуется только если не налезает на заголовок
if x + pw - tw - 8 * s > x + 2 * s + text_size(title, 0.4 * s)[0]:
put_text(out, hint, (x + pw - tw - 2 * s, 22 * s), 0.36 * s, (112, 110, 124))
def _strip(self, res) -> np.ndarray:
out = np.full((self.strip_h, self.w, 3), BG, np.uint8)
tf, lam = res.tf, res.lam
if tf is None or lam is None:
put_text(out, "панели решётки доступны только с keep_debug=True",
(PAD * self.s, 24 * self.s), 0.44 * self.s, (140, 138, 152))
return out
# 1. Дальностный образ: ближнее ярче, «нет эха» — ровный тёмный фон,
# иначе пустые лучи красятся палитрой и спорят с данными.
r = np.where(tf.valid, tf.r, np.nan)
depth = 1.0 - _norm(np.nan_to_num(r, nan=220.0), 4.0, 140.0)
rng = cv2.applyColorMap((depth * 255).astype(np.uint8), cv2.COLORMAP_TURBO)
rng[~tf.valid] = (34, 32, 38)
self._panel(rng, "дальностный образ", "R1–R8", 0, out)
# 2. ON: ближе окружения — то, что торчит навстречу
on = _norm(lam.on, 0.0, 0.02)
self._panel(cv2.applyColorMap((on * 255).astype(np.uint8),
cv2.COLORMAP_INFERNO),
"ламина L1 · ON-контраст", "ближе фона", 1, out)
# 3. OFF: дальше окружения — провалы, ниши, окклюзионная тень.
# Шкала шире, чем у ON: провалы по дальности много глубже выступов.
off = _norm(lam.off, 0.0, 0.06)
off_img = cv2.applyColorMap((off * 255).astype(np.uint8), cv2.COLORMAP_BONE)
off_img[~tf.valid] = (34, 32, 38)
self._panel(off_img, "ламина L2 · OFF-контраст", "дальше фона", 2, out)
# 4. Кандидаты: лучи, попавшие в габарит, поверх приглушённой сцены.
# Цвет по новизне: знакомое тусклое, незнакомое горит.
base = np.where(tf.valid, 40 + depth * 95, 22).astype(np.uint8)
cand = cv2.cvtColor(base, cv2.COLOR_GRAY2BGR)
mark = np.zeros(cand.shape[:2], np.uint8)
hot_of = np.zeros(cand.shape[:2], np.float32)
for c in res.candidates:
rays = c.extra.get("rays") if isinstance(c.extra, dict) else None
if rays is None:
continue
ii, jj = rays
keep = (ii < cand.shape[0]) & (jj < cand.shape[1])
mark[ii[keep], jj[keep]] = 1
hot_of[ii[keep], jj[keep]] = float(np.clip(c.novelty, 0.0, 1.0))
if mark.any():
# утолщаем: одиночные лучи на уменьшенной панели не видно
k = np.ones((3, 5), np.uint8)
grown = cv2.dilate(mark, k)
hot_g = cv2.dilate(hot_of, k)
m = grown.astype(bool)
cand[m] = np.stack([np.full(m.sum(), 60),
(110 + 130 * hot_g[m]).astype(int),
(70 + 185 * hot_g[m]).astype(int)], axis=1)
self._panel(cand, "лобула LC11 · кандидаты",
f"{len(res.candidates)} шт.", 3, out)
s = self.s
cv2.line(out, (PAD * s, self.strip_h - 2 * s), (self.w - PAD * s, self.strip_h - 2 * s),
(52, 52, 62), s)
return out
# ------------------------------------------------------------------ кадр
def render(self, res) -> np.ndarray | None:
if not self.enabled:
return None
cloud = self.cloud.render(res)
if cloud is None:
return None
out = np.empty((self.h, self.w, 3), np.uint8)
out[:self.strip_h] = self._strip(res)
out[self.strip_h:] = cloud
return out
@staticmethod
def to_msg(img: np.ndarray, stamp, frame_id: str):
from .brain_view import BrainView
return BrainView.to_msg(img, stamp, frame_id)

369
flyguard/brain_view.py Normal file
View file

@ -0,0 +1,369 @@
"""Схема мозга дрозофилы, подсвеченная живой активностью конвейера.
Каждая стадия FlyGuard соответствует конкретному нейропилю, поэтому состояние
системы показывается не графиками, а анатомической схемой: где сейчас «горит»,
там и идёт работа. На демонстрации это замыкает цепочку из ТЗ —
тоннель → облако точек → мозг мухи → «препятствие, 87 м».
Вид дорсальный, схематичный, но с правильным взаимным расположением: по краям
оптические доли (ламина → медулла → лобула и лобулярная пластинка), в центре
грибовидные тела (чашечка → ножка → доли) и центральный комплекс
(протоцеребральный мост, веерное тело, эллипсоидное тело, нодули), снизу —
нисходящие нейроны с гигантским волокном.
Отрисовка стоит 2–4 мс и вынесена под флаг `brain_view`, чтобы не попадать
в замеры задержки основного тракта.
"""
from __future__ import annotations
import array
import numpy as np
try:
import cv2
except ImportError: # без OpenCV вид просто выключен
cv2 = None
# Встроенные шрифты OpenCV (Hershey) знают только латиницу: любая кириллица
# выходит рядом вопросительных знаков. Подписи здесь русские и составляют
# половину смысла схемы, поэтому текст рисуется через PIL шрифтом DejaVu.
try:
from PIL import Image, ImageDraw, ImageFont
except ImportError:
Image = ImageDraw = ImageFont = None
_FONT_PATHS = (
"/usr/share/fonts/truetype/dejavu/DejaVuSans.ttf",
"/usr/share/fonts/truetype/dejavu/DejaVuSansCondensed.ttf",
"/usr/share/fonts/TTF/DejaVuSans.ttf",
"C:/Windows/Fonts/segoeui.ttf",
"C:/Windows/Fonts/arial.ttf",
)
_font_cache: dict[int, object] = {}
def _font(px: int):
"""Шрифт нужного кегля; None — если ни одного файла не нашлось."""
if ImageFont is None:
return None
if px not in _font_cache:
f = None
for path in _FONT_PATHS:
try:
f = ImageFont.truetype(path, px)
break
except OSError:
continue
_font_cache[px] = f or ImageFont.load_default()
return _font_cache[px]
def _px(scale: float) -> int:
"""Кегль PIL, дающий примерно ту же высоту, что и Hershey с этим масштабом."""
return max(9, int(round(scale * 30)))
def text_size(txt: str, scale: float) -> tuple[int, int]:
f = _font(_px(scale))
if f is None:
(tw, th), _ = cv2.getTextSize(txt, cv2.FONT_HERSHEY_SIMPLEX, scale, 1)
return tw, th
box = f.getbbox(txt)
return box[2] - box[0], box[3] - box[1]
def put_text(img, txt: str, org, scale: float, colour, weight: int = 1) -> None:
"""Надпись с кириллицей. `org` — левый нижний угол, как у cv2.putText.
Рисуется по вырезанному прямоугольнику, а не по всему полотну: PIL копирует
изображение при каждом преобразовании, и на полном кадре полтора десятка
подписей стоили 90 мс — больше, чем весь конвейер.
"""
f = _font(_px(scale))
if f is None or Image is None:
cv2.putText(img, txt, org, cv2.FONT_HERSHEY_SIMPLEX, scale, colour,
weight, cv2.LINE_AA)
return
x, y = int(org[0]), int(org[1])
box = f.getbbox(txt)
pad = 2
# Чернила лягут в x … x+(box[2]-box[0]) по горизонтали и
# y-(box[3]-box[1]) … y по вертикали — вырез берётся ровно по ним.
x0 = max(x - pad, 0)
y0 = max(y - (box[3] - box[1]) - pad, 0)
x1 = min(x + (box[2] - box[0]) + pad + 1, img.shape[1])
y1 = min(y + pad + 1, img.shape[0])
if x1 <= x0 or y1 <= y0:
return
crop = img[y0:y1, x0:x1]
pil = Image.fromarray(crop[:, :, ::-1])
ImageDraw.Draw(pil).text((x - box[0] - x0, y - box[3] - y0), txt, font=f,
fill=tuple(int(c) for c in colour[::-1]))
crop[:, :, :] = np.asarray(pil)[:, :, ::-1]
W, H = 1180, 620
BG = (22, 20, 26)
INK = (150, 150, 165)
# шкала «покой → работа → тревога» в BGR
_STOPS = (
(0.00, (58, 48, 44)),
(0.30, (120, 96, 52)),
(0.55, (168, 178, 64)),
(0.78, (72, 196, 240)),
(1.00, (66, 66, 248)),
)
def _colour(a: float) -> tuple[int, int, int]:
a = float(np.clip(a, 0.0, 1.0))
for (x0, c0), (x1, c1) in zip(_STOPS, _STOPS[1:]):
if a <= x1:
t = (a - x0) / max(x1 - x0, 1e-6)
return tuple(int(c0[i] + (c1[i] - c0[i]) * t) for i in range(3))
return _STOPS[-1][1]
def _annulus(cx, cy, r_out, r_in, a0, a1, squash=1.0):
"""Кольцевой сектор — форма слоя оптической доли."""
# ellipse2Poly требует положительный шаг, поэтому внутренняя дуга строится
# в прямом направлении и разворачивается уже массивом
outer = cv2.ellipse2Poly((cx, cy), (r_out, int(r_out * squash)), 0, a0, a1, 4)
inner = cv2.ellipse2Poly((cx, cy), (r_in, int(r_in * squash)), 0, a0, a1, 4)
return np.vstack([outer, inner[::-1]])
class Region:
__slots__ = ("key", "ru", "pts", "anchor", "label_at")
def __init__(self, key, ru, pts, label_at=None):
self.key = key
self.ru = ru
self.pts = np.asarray(pts, np.int32)
self.anchor = tuple(self.pts.mean(axis=0).astype(int))
self.label_at = label_at
def _build_regions() -> list[Region]:
r: list[Region] = []
for side, (cx, a0, a1, lx) in enumerate(((250, 118, 242, 18), (W - 250, -62, 62, W - 250))):
cy = 300
mirror = side == 1
spec = [
("lamina", "ламина L1 / L2", 218, 188),
("medulla", "медулла T4 / T5", 182, 132),
("lobula", "лобула LC11", 126, 88),
("lobula_plate", "лоб. пластинка LPTC / LPLC2", 82, 52),
]
for i, (key, ru, ro, ri) in enumerate(spec):
pts = _annulus(cx, cy, ro, ri, a0 + i * 3, a1 - i * 3, squash=1.06)
ly = 96 + i * 26
r.append(Region(key, ru, pts, (30 if not mirror else W - 330, ly)))
cx = W // 2
# грибовидные тела: чашечка сзади → ножка вперёд → вертикальная и медиальная
# доли. Они лежат ЛАТЕРАЛЬНЕЕ центрального комплекса, не поверх него.
for mirror in (False, True):
s = -1 if mirror else 1
bx = cx + s * 150
calyx = cv2.ellipse2Poly((bx, 132), (46, 33), 0, 0, 360, 6)
ped = np.array([[bx - s * 16, 160], [bx + s * 18, 158],
[bx + s * 46, 318], [bx + s * 18, 322]], np.int32)
lobes = np.array([[bx + s * 16, 316], [bx + s * 50, 314], [bx + s * 56, 404],
[bx + s * 26, 408], [bx + s * 26, 372], [bx + s * 16, 372]],
np.int32)
r.append(Region("mb_calyx", "чашечка PN → KC", calyx,
(cx - 330, 86) if not mirror else (cx + 210, 86)))
r.append(Region("mb_ped", "ножка · APL", ped, None))
r.append(Region("mb_lobes", "доли KC → MBON (новизна)", lobes,
(cx - 400, 432) if not mirror else (cx + 230, 432)))
pb = _annulus(cx, 232, 104, 86, 200, 340, squash=0.7)
fb = np.array([[cx - 74, 262], [cx + 74, 262], [cx + 58, 314], [cx - 58, 314]], np.int32)
eb = _annulus(cx, 372, 46, 26, 0, 360)
no_l = cv2.ellipse2Poly((cx - 52, 420), (18, 13), 0, 0, 360, 8)
no_r = cv2.ellipse2Poly((cx + 52, 420), (18, 13), 0, 0, 360, 8)
dn = np.array([[cx - 22, 444], [cx + 22, 444], [cx + 40, 566], [cx - 40, 566]], np.int32)
r += [
Region("pb", "протоцеребральный мост", pb, (cx - 96, 176)),
Region("fb", "веерное тело", fb, (cx - 40, 342)),
Region("eb", "эллипсоидное тело — кольцевой аттрактор", eb, (cx - 300, 372)),
Region("no", "нодули", no_l, None),
Region("no", "нодули", no_r, (cx + 96, 428)),
Region("dn", "нисходящие нейроны · гигантское волокно", dn, (cx - 300, 524)),
]
return r
class BrainView:
"""Отрисовка схемы с активностью."""
def __init__(self, labels: bool = True):
self.enabled = cv2 is not None
self.labels = labels
self.regions = _build_regions() if self.enabled else []
self._base = self._draw_base() if self.enabled else None
self._smooth: dict[str, float] = {}
# ------------------------------------------------------------------ статика
def _draw_base(self) -> np.ndarray:
img = np.full((H, W, 3), BG, np.uint8)
put_text(img, "FlyGuard — схемы мозга Drosophila melanogaster в реальном времени",
(26, 36), 0.66, (205, 205, 215))
cv2.line(img, (26, 48), (W - 26, 48), (52, 52, 62), 1)
return img
# ------------------------------------------------------------------ активность
@staticmethod
def activity(res) -> dict[str, float]:
"""Свести состояние кадра к активности по нейропилям, 0…1."""
lam = res.lam
d = res.decision
on = float(np.mean(lam.on)) * 130.0 if lam is not None else 0.0
off = float(np.mean(lam.off)) * 130.0 if lam is not None else 0.0
hole = float(np.mean(lam.hole)) if lam is not None else 0.0
speed = abs(res.ego.speed) / 20.0 if res.ego else 0.0
loom = (float(np.percentile(res.loom, 99.9)) * 25.0
if res.loom is not None and res.loom.size else 0.0)
nov = max((c.novelty for c in res.candidates), default=0.0)
# Дофамин: у мухи нейроны PPL1/PAM управляют пластичностью синапсов
# KC→MBON — ровно тем, чем учится наша память тоннеля. Осмысленная
# величина здесь — сила сигнала обучения: насколько сцена в целом
# незнакома памяти. В отличие от `nov` (самый незнакомый предмет)
# это средняя незнакомость всего, что видно, и она заранее говорит,
# что участок новый и ложных тревог будет больше.
dopa = (float(np.mean([c.novelty for c in res.candidates]))
if res.candidates else 0.0)
n_cand = min(len(res.candidates) / 7.0, 1.0)
evid = max((o.confidence for o in d.objects), default=0.0)
valid = float(lam.disp.astype(bool).mean()) if lam is not None else 0.0
return {
# Сетчатка «горит» тем сильнее, чем больше лучей вернулось: это и
# есть освещённость омматидиальной решётки.
"retina": np.clip(0.15 + 0.85 * valid, 0, 1),
"looming": np.clip(loom, 0, 1),
"dopamine": np.clip(dopa, 0, 1),
# Привыкание: доля синапсов KC→MBON, уже подавленных повторами
# внутри этого проезда. Ноль в начале участка, растёт по мере
# того, как однообразная обстановка перестаёт быть новостью.
"habituation": np.clip(getattr(res, "hab", 0.0), 0, 1),
"lamina": np.clip(0.55 * on + 0.45 * off, 0, 1),
"medulla": np.clip(0.25 + 0.75 * speed, 0, 1),
"lobula": np.clip(n_cand, 0, 1),
"lobula_plate": np.clip(max(speed, loom), 0, 1),
"mb_calyx": np.clip(n_cand, 0, 1),
"mb_ped": np.clip(0.4 * n_cand, 0, 1),
"mb_lobes": np.clip(nov, 0, 1),
"pb": np.clip(speed, 0, 1),
"fb": np.clip(hole, 0, 1),
"eb": np.clip(evid, 0, 1),
"no": np.clip(abs(res.ego.yaw_deg) / 1.5 if res.ego else 0.0, 0, 1),
"dn": 1.0 if d.emergency else (0.7 if d.detected else 0.06),
}
# ------------------------------------------------------------------ кадр
def render(self, res) -> np.ndarray | None:
if not self.enabled:
return None
for k, v in self.activity(res).items(): # сглаживание: мозг не мигает
self._smooth[k] = 0.6 * self._smooth.get(k, v) + 0.4 * v
img = self._base.copy()
for reg in self.regions:
a = self._smooth.get(reg.key, 0.0)
cv2.fillPoly(img, [reg.pts], _colour(a))
cv2.polylines(img, [reg.pts], True, (46, 44, 54), 1, cv2.LINE_AA)
self._draw_bump(img, res)
self._draw_flow(img)
if self.labels:
self._draw_labels(img)
self._draw_readout(img, res)
return img
def _draw_labels(self, img) -> None:
seen = set()
for reg in self.regions:
if reg.label_at is None or reg.ru in seen:
continue
seen.add(reg.ru)
x, y = reg.label_at
tw, th = text_size(reg.ru, 0.44)
# подложка под текстом: подписи неизбежно ложатся поверх нейропилей,
# и без неё их не прочитать
box = img[max(y - th - 6, 0):y + 6, max(x - 6, 0):x + tw + 6]
if box.size:
box[:] = (box * 0.25).astype(np.uint8)
cv2.line(img, (x + tw // 2, y - th - 6 if y > reg.anchor[1] else y + 6),
reg.anchor, (74, 72, 86), 1, cv2.LINE_AA)
put_text(img, reg.ru, (x, y), 0.44, INK)
def _draw_bump(self, img, res) -> None:
"""Бугор активности в эллипсоидном теле — направление на ближайший объект."""
cx, cy, r = W // 2, 380, 36
objs = res.decision.objects
if not objs:
return
o = objs[0]
ang = float(np.clip(np.arctan2(o.lateral, max(o.distance, 1.0)) * 7.0, -3.0, 3.0))
px = int(cx + r * np.sin(ang))
py = int(cy - r * np.cos(ang))
cv2.circle(img, (px, py), 11, _colour(min(o.confidence + 0.35, 1.0)), -1, cv2.LINE_AA)
cv2.circle(img, (px, py), 11, (230, 230, 240), 1, cv2.LINE_AA)
def _draw_flow(self, img) -> None:
cx = W // 2
for a, b in (((330, 300), (410, 260)), ((W - 330, 300), (W - 410, 260)),
((cx - 60, 336), (cx - 30, 356)), ((cx + 60, 336), (cx + 30, 356)),
((cx, 420), (cx, 428))):
cv2.arrowedLine(img, a, b, (86, 84, 98), 1, cv2.LINE_AA, tipLength=0.3)
def _draw_readout(self, img, res) -> None:
d = res.decision
rows = [
("скорость", f"{res.ego.kmh:.1f} км/ч" if res.ego else "—"),
("кандидатов", str(len(res.candidates))),
("новизна MBON", f"{max((c.novelty for c in res.candidates), default=0.0):.2f}"),
("треков", str(len(d.objects))),
("обработка", f"{res.total_ms:.1f} мс"),
]
y0 = H - 150
for i, (k, v) in enumerate(rows):
put_text(img, k, (28, y0 + i * 24), 0.46, (118, 116, 128))
put_text(img, v, (170, y0 + i * 24), 0.5, (206, 206, 216))
if d.detected:
txt = f"ПРЕПЯТСТВИЕ {d.distance:.0f} м"
sub = ("ЭКСТРЕННОЕ ТОРМОЖЕНИЕ" if d.emergency
else f"уверенность {d.confidence:.2f}")
colour = (66, 66, 248) if d.emergency else (72, 196, 240)
else:
txt, sub, colour = "ПУТЬ СВОБОДЕН", "", (120, 214, 130)
put_text(img, txt, (W - 470, H - 90), 0.95, colour, 2)
if sub:
put_text(img, sub, (W - 470, H - 58), 0.56, colour)
# ------------------------------------------------------------------ ROS
@staticmethod
def to_msg(img: np.ndarray, stamp, frame_id: str):
from sensor_msgs.msg import Image
msg = Image()
msg.header.stamp = stamp
msg.header.frame_id = frame_id
msg.height, msg.width = img.shape[:2]
msg.encoding = "bgr8"
msg.is_bigendian = 0
msg.step = img.shape[1] * 3
# array('B'), а не bytes: из bytes rclpy проверяет каждый байт на Python,
# и картинка в 2.8 МБ стоила 142 мс на кадр против 2.4 мс (замерено).
msg.data = array.array("B", img.tobytes())
return msg

BIN
flyguard/data/brain_atlas.npz Normal file
View file

Binary file not shown.

BIN
flyguard/data/brain_atlas_x2.npz Normal file
View file

Binary file not shown.

BIN
flyguard/data/brain_atlas_x3.npz Normal file
View file

Binary file not shown.

129
flyguard/data/pandar128_channels.csv Normal file
View file

@ -0,0 +1,129 @@
channel,az_offset_deg,elevation_deg,range_min_m,range_max_m,near_field,max_range_10pct_m,far_field,min_reflectivity,high_res
1,3.257,14.436,0.3,100.0,1,100.0,0,100 m @ 10%,0
2,3.263,13.535,2.7,100.0,0,100.0,0,100 m @ 10%,0
3,1.091,13.082,0.3,100.0,1,100.0,0,100 m @ 10%,0
4,3.268,12.624,2.7,100.0,0,100.0,0,100 m @ 10%,0
5,1.093,12.165,2.7,100.0,0,100.0,0,100 m @ 10%,0
6,3.273,11.702,0.3,100.0,1,120.0,0,100 m @ 6%,0
7,1.094,11.239,2.7,100.0,0,120.0,0,100 m @ 6%,0
8,3.278,10.771,2.7,100.0,0,140.0,0,100 m @ 3%,0
9,1.095,10.305,0.3,100.0,1,140.0,0,100 m @ 3%,0
10,3.283,9.83,2.7,100.0,0,140.0,0,100 m @ 3%,0
11,1.096,9.356,2.7,100.0,0,140.0,0,100 m @ 3%,0
12,3.288,8.88,0.3,100.0,1,140.0,0,100 m @ 3%,0
13,1.097,8.401,2.7,100.0,0,140.0,0,100 m @ 3%,0
14,3.291,7.921,2.7,100.0,0,140.0,0,100 m @ 3%,0
15,1.098,7.438,0.3,100.0,1,140.0,0,100 m @ 3%,0
16,-1.101,6.953,2.7,100.0,0,140.0,0,100 m @ 3%,0
17,1.1,6.467,2.7,100.0,0,140.0,0,100 m @ 3%,0
18,-1.104,5.978,0.3,100.0,1,140.0,0,100 m @ 3%,0
19,-3.306,5.487,2.7,100.0,0,140.0,0,100 m @ 3%,0
20,-1.106,4.996,2.7,100.0,0,140.0,0,100 m @ 3%,0
21,-3.311,4.501,0.3,100.0,1,140.0,0,100 m @ 3%,0
22,-1.109,4.007,2.7,100.0,0,140.0,0,100 m @ 3%,0
23,-3.318,3.509,2.7,100.0,0,140.0,0,100 m @ 3%,0
24,-1.111,3.013,0.3,100.0,1,140.0,0,100 m @ 3%,0
25,-3.324,2.512,2.7,100.0,0,140.0,0,100 m @ 3%,0
26,-1.113,2.013,0.3,200.0,1,140.0,0,200 m @ 37%,1
27,7.72,1.885,2.7,200.0,0,140.0,0,200 m @ 37%,1
28,5.535,1.761,2.7,200.0,0,140.0,0,200 m @ 37%,1
29,3.325,1.637,2.7,200.0,0,140.0,0,200 m @ 37%,1
30,-3.33,1.511,2.7,200.0,0,140.0,0,200 m @ 37%,1
31,1.107,1.386,2.7,200.0,0,140.0,0,200 m @ 37%,1
32,-5.538,1.258,2.7,200.0,0,140.0,0,200 m @ 37%,1
33,-7.726,1.13,0.3,200.0,1,140.0,0,200 m @ 37%,1
34,-1.115,1.008,2.7,200.0,0,200.0,1,200 m @ 10%,1
35,7.731,0.88,2.7,200.0,0,200.0,1,200 m @ 10%,1
36,5.543,0.756,2.7,200.0,0,200.0,1,200 m @ 10%,1
37,3.329,0.63,2.7,200.0,0,200.0,1,200 m @ 10%,1
38,-3.336,0.505,2.7,200.0,0,200.0,1,200 m @ 10%,1
39,1.108,0.379,2.7,200.0,0,200.0,1,200 m @ 10%,1
40,-5.547,0.251,0.3,200.0,1,200.0,1,200 m @ 10%,1
41,-7.738,0.124,2.7,200.0,0,200.0,1,200 m @ 10%,1
42,-1.117,0.0,2.7,200.0,0,200.0,1,200 m @ 10%,1
43,7.743,-0.129,2.7,200.0,0,200.0,1,200 m @ 10%,1
44,5.551,-0.254,2.7,200.0,0,200.0,1,200 m @ 10%,1
45,3.335,-0.38,2.7,200.0,0,200.0,1,200 m @ 10%,1
46,-3.342,-0.506,2.7,200.0,0,200.0,1,200 m @ 10%,1
47,1.11,-0.632,0.3,200.0,1,200.0,1,200 m @ 10%,1
48,-5.555,-0.76,2.7,200.0,0,200.0,1,200 m @ 10%,1
49,-7.75,-0.887,2.7,200.0,0,200.0,1,200 m @ 10%,1
50,-1.119,-1.012,2.7,200.0,0,200.0,1,200 m @ 10%,1
51,7.757,-1.141,2.7,200.0,0,200.0,1,200 m @ 10%,1
52,5.56,-1.266,2.7,200.0,0,200.0,1,200 m @ 10%,1
53,3.34,-1.393,2.7,200.0,0,200.0,1,200 m @ 10%,1
54,-3.347,-1.519,0.3,200.0,1,200.0,1,200 m @ 10%,1
55,1.111,-1.646,2.7,200.0,0,200.0,1,200 m @ 10%,1
56,-5.564,-1.773,2.7,200.0,0,200.0,1,200 m @ 10%,1
57,-7.762,-1.901,2.7,200.0,0,200.0,1,200 m @ 10%,1
58,-1.121,-2.027,2.7,200.0,0,200.0,1,200 m @ 10%,1
59,7.768,-2.155,2.7,200.0,0,200.0,1,200 m @ 10%,1
60,5.569,-2.282,2.7,200.0,0,200.0,1,200 m @ 10%,1
61,3.345,-2.409,0.3,200.0,1,200.0,1,200 m @ 10%,1
62,-3.353,-2.535,2.7,200.0,0,200.0,1,200 m @ 10%,1
63,1.113,-2.663,2.7,200.0,0,200.0,1,200 m @ 10%,1
64,-5.573,-2.789,2.7,200.0,0,200.0,1,200 m @ 10%,1
65,-7.775,-2.916,2.7,200.0,0,200.0,1,200 m @ 10%,1
66,-1.123,-3.044,2.7,200.0,0,140.0,0,200 m @ 37%,1
67,7.78,-3.172,2.7,200.0,0,140.0,0,200 m @ 37%,1
68,5.578,-3.299,0.3,200.0,1,140.0,0,200 m @ 37%,1
69,3.351,-3.425,2.7,200.0,0,140.0,0,200 m @ 37%,1
70,-3.358,-3.552,2.7,200.0,0,140.0,0,200 m @ 37%,1
71,1.115,-3.68,2.7,200.0,0,140.0,0,200 m @ 37%,1
72,-5.582,-3.806,2.7,200.0,0,140.0,0,200 m @ 37%,1
73,-7.787,-3.933,2.7,200.0,0,140.0,0,200 m @ 37%,1
74,-1.125,-4.062,2.7,200.0,0,140.0,0,200 m @ 37%,1
75,7.792,-4.19,0.3,200.0,1,140.0,0,200 m @ 37%,1
76,5.586,-4.318,2.7,200.0,0,140.0,0,200 m @ 37%,1
77,3.356,-4.444,2.7,200.0,0,140.0,0,200 m @ 37%,1
78,-3.363,-4.571,2.7,200.0,0,140.0,0,200 m @ 37%,1
79,1.116,-4.699,2.7,200.0,0,140.0,0,200 m @ 37%,1
80,-5.591,-4.824,2.7,200.0,0,140.0,0,200 m @ 37%,1
81,-7.799,-4.951,2.7,200.0,0,140.0,0,200 m @ 37%,1
82,-1.127,-5.081,0.3,200.0,1,140.0,0,200 m @ 37%,1
83,7.804,-5.209,2.7,200.0,0,140.0,0,200 m @ 37%,1
84,5.595,-5.336,2.7,200.0,0,140.0,0,200 m @ 37%,1
85,3.36,-5.463,2.7,200.0,0,140.0,0,200 m @ 37%,1
86,-3.369,-5.589,2.7,200.0,0,140.0,0,200 m @ 37%,1
87,1.118,-5.718,2.7,200.0,0,140.0,0,200 m @ 37%,1
88,-5.599,-5.843,2.7,200.0,0,140.0,0,200 m @ 37%,1
89,-7.811,-5.968,2.7,200.0,0,140.0,0,200 m @ 37%,1
90,-1.129,-6.1,0.3,100.0,1,140.0,0,100 m @ 3%,0
91,-3.374,-6.607,2.7,100.0,0,140.0,0,100 m @ 3%,0
92,-1.13,-7.117,2.7,100.0,0,140.0,0,100 m @ 3%,0
93,-3.379,-7.624,0.3,100.0,1,140.0,0,100 m @ 3%,0
94,-1.132,-8.134,2.7,100.0,0,140.0,0,100 m @ 3%,0
95,-3.383,-8.64,2.7,100.0,0,140.0,0,100 m @ 3%,0
96,3.381,-9.149,0.3,100.0,1,140.0,0,100 m @ 3%,0
97,-3.388,-9.652,2.7,100.0,0,140.0,0,100 m @ 3%,0
98,3.386,-10.16,2.7,100.0,0,100.0,0,100 m @ 10%,0
99,1.129,-10.665,0.3,100.0,1,100.0,0,100 m @ 10%,0
100,3.39,-11.17,2.7,100.0,0,100.0,0,100 m @ 10%,0
101,1.129,-11.672,2.7,100.0,0,100.0,0,100 m @ 10%,0
102,3.395,-12.174,0.3,100.0,1,100.0,0,100 m @ 10%,0
103,1.131,-12.673,2.7,100.0,0,100.0,0,100 m @ 10%,0
104,3.401,-13.173,2.7,100.0,0,100.0,0,100 m @ 10%,0
105,1.133,-13.67,0.3,100.0,1,100.0,0,100 m @ 10%,0
106,3.406,-14.166,2.7,100.0,0,50.0,0,100 m @ 120%,0
107,1.135,-14.66,2.7,100.0,0,50.0,0,100 m @ 120%,0
108,3.41,-15.154,0.3,100.0,1,50.0,0,100 m @ 120%,0
109,1.137,-15.645,2.7,100.0,0,50.0,0,100 m @ 120%,0
110,3.416,-16.135,2.7,100.0,0,50.0,0,100 m @ 120%,0
111,1.139,-16.622,0.3,100.0,1,50.0,0,100 m @ 120%,0
112,-1.142,-17.106,2.7,100.0,0,50.0,0,100 m @ 120%,0
113,1.142,-17.592,2.7,100.0,0,50.0,0,100 m @ 120%,0
114,-1.143,-18.072,0.3,100.0,1,50.0,0,100 m @ 120%,0
115,-3.426,-18.548,2.7,100.0,0,50.0,0,100 m @ 120%,0
116,-1.143,-19.03,2.7,100.0,0,25.0,0,100 m @ 1600%,0
117,-3.429,-19.501,0.3,100.0,1,25.0,0,100 m @ 1600%,0
118,-1.145,-19.978,2.7,100.0,0,25.0,0,100 m @ 1600%,0
119,-3.433,-20.445,2.7,100.0,0,25.0,0,100 m @ 1600%,0
120,-1.145,-20.918,0.3,100.0,1,25.0,0,100 m @ 1600%,0
121,-3.436,-21.379,2.7,100.0,0,25.0,0,100 m @ 1600%,0
122,-1.146,-21.848,2.7,100.0,0,25.0,0,100 m @ 1600%,0
123,-3.44,-22.304,0.3,100.0,1,25.0,0,100 m @ 1600%,0
124,-1.146,-22.768,2.7,100.0,0,25.0,0,100 m @ 1600%,0
125,-3.443,-23.219,2.7,100.0,0,25.0,0,100 m @ 1600%,0
126,-1.146,-23.678,0.3,100.0,1,25.0,0,100 m @ 1600%,0
127,-3.446,-24.123,2.7,100.0,0,25.0,0,100 m @ 1600%,0
128,-3.449,-25.016,0.3,100.0,1,25.0,0,100 m @ 1600%,0
1 channel az_offset_deg elevation_deg range_min_m range_max_m near_field max_range_10pct_m far_field min_reflectivity high_res
2 1 3.257 14.436 0.3 100.0 1 100.0 0 100 m @ 10% 0
3 2 3.263 13.535 2.7 100.0 0 100.0 0 100 m @ 10% 0
4 3 1.091 13.082 0.3 100.0 1 100.0 0 100 m @ 10% 0
5 4 3.268 12.624 2.7 100.0 0 100.0 0 100 m @ 10% 0
6 5 1.093 12.165 2.7 100.0 0 100.0 0 100 m @ 10% 0
7 6 3.273 11.702 0.3 100.0 1 120.0 0 100 m @ 6% 0
8 7 1.094 11.239 2.7 100.0 0 120.0 0 100 m @ 6% 0
9 8 3.278 10.771 2.7 100.0 0 140.0 0 100 m @ 3% 0
10 9 1.095 10.305 0.3 100.0 1 140.0 0 100 m @ 3% 0
11 10 3.283 9.83 2.7 100.0 0 140.0 0 100 m @ 3% 0
12 11 1.096 9.356 2.7 100.0 0 140.0 0 100 m @ 3% 0
13 12 3.288 8.88 0.3 100.0 1 140.0 0 100 m @ 3% 0
14 13 1.097 8.401 2.7 100.0 0 140.0 0 100 m @ 3% 0
15 14 3.291 7.921 2.7 100.0 0 140.0 0 100 m @ 3% 0
16 15 1.098 7.438 0.3 100.0 1 140.0 0 100 m @ 3% 0
17 16 -1.101 6.953 2.7 100.0 0 140.0 0 100 m @ 3% 0
18 17 1.1 6.467 2.7 100.0 0 140.0 0 100 m @ 3% 0
19 18 -1.104 5.978 0.3 100.0 1 140.0 0 100 m @ 3% 0
20 19 -3.306 5.487 2.7 100.0 0 140.0 0 100 m @ 3% 0
21 20 -1.106 4.996 2.7 100.0 0 140.0 0 100 m @ 3% 0
22 21 -3.311 4.501 0.3 100.0 1 140.0 0 100 m @ 3% 0
23 22 -1.109 4.007 2.7 100.0 0 140.0 0 100 m @ 3% 0
24 23 -3.318 3.509 2.7 100.0 0 140.0 0 100 m @ 3% 0
25 24 -1.111 3.013 0.3 100.0 1 140.0 0 100 m @ 3% 0
26 25 -3.324 2.512 2.7 100.0 0 140.0 0 100 m @ 3% 0
27 26 -1.113 2.013 0.3 200.0 1 140.0 0 200 m @ 37% 1
28 27 7.72 1.885 2.7 200.0 0 140.0 0 200 m @ 37% 1
29 28 5.535 1.761 2.7 200.0 0 140.0 0 200 m @ 37% 1
30 29 3.325 1.637 2.7 200.0 0 140.0 0 200 m @ 37% 1
31 30 -3.33 1.511 2.7 200.0 0 140.0 0 200 m @ 37% 1
32 31 1.107 1.386 2.7 200.0 0 140.0 0 200 m @ 37% 1
33 32 -5.538 1.258 2.7 200.0 0 140.0 0 200 m @ 37% 1
34 33 -7.726 1.13 0.3 200.0 1 140.0 0 200 m @ 37% 1
35 34 -1.115 1.008 2.7 200.0 0 200.0 1 200 m @ 10% 1
36 35 7.731 0.88 2.7 200.0 0 200.0 1 200 m @ 10% 1
37 36 5.543 0.756 2.7 200.0 0 200.0 1 200 m @ 10% 1
38 37 3.329 0.63 2.7 200.0 0 200.0 1 200 m @ 10% 1
39 38 -3.336 0.505 2.7 200.0 0 200.0 1 200 m @ 10% 1
40 39 1.108 0.379 2.7 200.0 0 200.0 1 200 m @ 10% 1
41 40 -5.547 0.251 0.3 200.0 1 200.0 1 200 m @ 10% 1
42 41 -7.738 0.124 2.7 200.0 0 200.0 1 200 m @ 10% 1
43 42 -1.117 0.0 2.7 200.0 0 200.0 1 200 m @ 10% 1
44 43 7.743 -0.129 2.7 200.0 0 200.0 1 200 m @ 10% 1
45 44 5.551 -0.254 2.7 200.0 0 200.0 1 200 m @ 10% 1
46 45 3.335 -0.38 2.7 200.0 0 200.0 1 200 m @ 10% 1
47 46 -3.342 -0.506 2.7 200.0 0 200.0 1 200 m @ 10% 1
48 47 1.11 -0.632 0.3 200.0 1 200.0 1 200 m @ 10% 1
49 48 -5.555 -0.76 2.7 200.0 0 200.0 1 200 m @ 10% 1
50 49 -7.75 -0.887 2.7 200.0 0 200.0 1 200 m @ 10% 1
51 50 -1.119 -1.012 2.7 200.0 0 200.0 1 200 m @ 10% 1
52 51 7.757 -1.141 2.7 200.0 0 200.0 1 200 m @ 10% 1
53 52 5.56 -1.266 2.7 200.0 0 200.0 1 200 m @ 10% 1
54 53 3.34 -1.393 2.7 200.0 0 200.0 1 200 m @ 10% 1
55 54 -3.347 -1.519 0.3 200.0 1 200.0 1 200 m @ 10% 1
56 55 1.111 -1.646 2.7 200.0 0 200.0 1 200 m @ 10% 1
57 56 -5.564 -1.773 2.7 200.0 0 200.0 1 200 m @ 10% 1
58 57 -7.762 -1.901 2.7 200.0 0 200.0 1 200 m @ 10% 1
59 58 -1.121 -2.027 2.7 200.0 0 200.0 1 200 m @ 10% 1
60 59 7.768 -2.155 2.7 200.0 0 200.0 1 200 m @ 10% 1
61 60 5.569 -2.282 2.7 200.0 0 200.0 1 200 m @ 10% 1
62 61 3.345 -2.409 0.3 200.0 1 200.0 1 200 m @ 10% 1
63 62 -3.353 -2.535 2.7 200.0 0 200.0 1 200 m @ 10% 1
64 63 1.113 -2.663 2.7 200.0 0 200.0 1 200 m @ 10% 1
65 64 -5.573 -2.789 2.7 200.0 0 200.0 1 200 m @ 10% 1
66 65 -7.775 -2.916 2.7 200.0 0 200.0 1 200 m @ 10% 1
67 66 -1.123 -3.044 2.7 200.0 0 140.0 0 200 m @ 37% 1
68 67 7.78 -3.172 2.7 200.0 0 140.0 0 200 m @ 37% 1
69 68 5.578 -3.299 0.3 200.0 1 140.0 0 200 m @ 37% 1
70 69 3.351 -3.425 2.7 200.0 0 140.0 0 200 m @ 37% 1
71 70 -3.358 -3.552 2.7 200.0 0 140.0 0 200 m @ 37% 1
72 71 1.115 -3.68 2.7 200.0 0 140.0 0 200 m @ 37% 1
73 72 -5.582 -3.806 2.7 200.0 0 140.0 0 200 m @ 37% 1
74 73 -7.787 -3.933 2.7 200.0 0 140.0 0 200 m @ 37% 1
75 74 -1.125 -4.062 2.7 200.0 0 140.0 0 200 m @ 37% 1
76 75 7.792 -4.19 0.3 200.0 1 140.0 0 200 m @ 37% 1
77 76 5.586 -4.318 2.7 200.0 0 140.0 0 200 m @ 37% 1
78 77 3.356 -4.444 2.7 200.0 0 140.0 0 200 m @ 37% 1
79 78 -3.363 -4.571 2.7 200.0 0 140.0 0 200 m @ 37% 1
80 79 1.116 -4.699 2.7 200.0 0 140.0 0 200 m @ 37% 1
81 80 -5.591 -4.824 2.7 200.0 0 140.0 0 200 m @ 37% 1
82 81 -7.799 -4.951 2.7 200.0 0 140.0 0 200 m @ 37% 1
83 82 -1.127 -5.081 0.3 200.0 1 140.0 0 200 m @ 37% 1
84 83 7.804 -5.209 2.7 200.0 0 140.0 0 200 m @ 37% 1
85 84 5.595 -5.336 2.7 200.0 0 140.0 0 200 m @ 37% 1
86 85 3.36 -5.463 2.7 200.0 0 140.0 0 200 m @ 37% 1
87 86 -3.369 -5.589 2.7 200.0 0 140.0 0 200 m @ 37% 1
88 87 1.118 -5.718 2.7 200.0 0 140.0 0 200 m @ 37% 1
89 88 -5.599 -5.843 2.7 200.0 0 140.0 0 200 m @ 37% 1
90 89 -7.811 -5.968 2.7 200.0 0 140.0 0 200 m @ 37% 1
91 90 -1.129 -6.1 0.3 100.0 1 140.0 0 100 m @ 3% 0
92 91 -3.374 -6.607 2.7 100.0 0 140.0 0 100 m @ 3% 0
93 92 -1.13 -7.117 2.7 100.0 0 140.0 0 100 m @ 3% 0
94 93 -3.379 -7.624 0.3 100.0 1 140.0 0 100 m @ 3% 0
95 94 -1.132 -8.134 2.7 100.0 0 140.0 0 100 m @ 3% 0
96 95 -3.383 -8.64 2.7 100.0 0 140.0 0 100 m @ 3% 0
97 96 3.381 -9.149 0.3 100.0 1 140.0 0 100 m @ 3% 0
98 97 -3.388 -9.652 2.7 100.0 0 140.0 0 100 m @ 3% 0
99 98 3.386 -10.16 2.7 100.0 0 100.0 0 100 m @ 10% 0
100 99 1.129 -10.665 0.3 100.0 1 100.0 0 100 m @ 10% 0
101 100 3.39 -11.17 2.7 100.0 0 100.0 0 100 m @ 10% 0
102 101 1.129 -11.672 2.7 100.0 0 100.0 0 100 m @ 10% 0
103 102 3.395 -12.174 0.3 100.0 1 100.0 0 100 m @ 10% 0
104 103 1.131 -12.673 2.7 100.0 0 100.0 0 100 m @ 10% 0
105 104 3.401 -13.173 2.7 100.0 0 100.0 0 100 m @ 10% 0
106 105 1.133 -13.67 0.3 100.0 1 100.0 0 100 m @ 10% 0
107 106 3.406 -14.166 2.7 100.0 0 50.0 0 100 m @ 120% 0
108 107 1.135 -14.66 2.7 100.0 0 50.0 0 100 m @ 120% 0
109 108 3.41 -15.154 0.3 100.0 1 50.0 0 100 m @ 120% 0
110 109 1.137 -15.645 2.7 100.0 0 50.0 0 100 m @ 120% 0
111 110 3.416 -16.135 2.7 100.0 0 50.0 0 100 m @ 120% 0
112 111 1.139 -16.622 0.3 100.0 1 50.0 0 100 m @ 120% 0
113 112 -1.142 -17.106 2.7 100.0 0 50.0 0 100 m @ 120% 0
114 113 1.142 -17.592 2.7 100.0 0 50.0 0 100 m @ 120% 0
115 114 -1.143 -18.072 0.3 100.0 1 50.0 0 100 m @ 120% 0
116 115 -3.426 -18.548 2.7 100.0 0 50.0 0 100 m @ 120% 0
117 116 -1.143 -19.03 2.7 100.0 0 25.0 0 100 m @ 1600% 0
118 117 -3.429 -19.501 0.3 100.0 1 25.0 0 100 m @ 1600% 0
119 118 -1.145 -19.978 2.7 100.0 0 25.0 0 100 m @ 1600% 0
120 119 -3.433 -20.445 2.7 100.0 0 25.0 0 100 m @ 1600% 0
121 120 -1.145 -20.918 0.3 100.0 1 25.0 0 100 m @ 1600% 0
122 121 -3.436 -21.379 2.7 100.0 0 25.0 0 100 m @ 1600% 0
123 122 -1.146 -21.848 2.7 100.0 0 25.0 0 100 m @ 1600% 0
124 123 -3.44 -22.304 0.3 100.0 1 25.0 0 100 m @ 1600% 0
125 124 -1.146 -22.768 2.7 100.0 0 25.0 0 100 m @ 1600% 0
126 125 -3.443 -23.219 2.7 100.0 0 25.0 0 100 m @ 1600% 0
127 126 -1.146 -23.678 0.3 100.0 1 25.0 0 100 m @ 1600% 0
128 127 -3.446 -24.123 2.7 100.0 0 25.0 0 100 m @ 1600% 0
129 128 -3.449 -25.016 0.3 100.0 1 25.0 0 100 m @ 1600% 0

708
flyguard/node.py Normal file
View file

@ -0,0 +1,708 @@
"""ROS 2-нода FlyGuard.
Подписывается на облако точек лидара, прогоняет конвейер и публикует:
/flyguard/obstacle flyguard_msgs/ObstacleStatus — главный программный выход
/flyguard/markers visualization_msgs/MarkerArray — рамки объектов и габарит
/flyguard/view_cloud sensor_msgs/PointCloud2 — облако обзора для RViz
/flyguard/debug_cloud sensor_msgs/PointCloud2 — раскраска по новизне
/flyguard/brain sensor_msgs/Image — схема мозга мухи с активностью
/flyguard/diagnostics diagnostic_msgs/DiagnosticArray — задержки по стадиям
Обработка идёт в отдельном потоке, и из очереди всегда берётся **последний**
пришедший кадр: система реального времени обязана отвечать на текущую обстановку,
а не доедать накопившееся прошлое.
"""
from __future__ import annotations
import array
import threading
import time
from pathlib import Path
import numpy as np
import rclpy
from diagnostic_msgs.msg import DiagnosticArray, DiagnosticStatus, KeyValue
from rclpy.node import Node
from rclpy.qos import QoSDurabilityPolicy, QoSHistoryPolicy, QoSProfile, QoSReliabilityPolicy
from geometry_msgs.msg import Point, TransformStamped
from sensor_msgs.msg import PointCloud2, PointField
from std_msgs.msg import Bool, Float32, Header
from tf2_ros import StaticTransformBroadcaster
from visualization_msgs.msg import Marker, MarkerArray
from flyguard_msgs.msg import DetectedObject, ObstacleStatus
from . import ros_conv
from .cdr import parse_pointcloud2
from .export import gauge_outline
from .mushroom_body import MushroomBody
from .pipeline import FlyGuard, Params
class FlyGuardNode(Node):
def __init__(self):
super().__init__("flyguard")
p = self.declare_parameters("", [
("input_topic", "/lidar_points"),
("fallback_topics", ["/sensing/lidar/hesai128/pointcloud", "/points_raw"]),
("frame_id", ""),
("best_effort", True),
("raw_subscription", True),
("queue_depth", 20),
("async_worker", False),
("memory_path", ""),
("mbon_path", ""),
("enable_mbon", True),
# плотные стадии (сетчатка, ламина, кластеризация): auto — видеокарта,
# если PyTorch её видит, иначе процессор; cuda; cpu
("device", "auto"),
("mbon_power", 1.5),
("mbon_blend", 1.0),
("fov_deg", 30.0),
("half_width", 1.2),
("h_lo", 0.28),
("h_hi", 2.3),
("h_top", 3.3),
("half_width_top", 1.0),
("top_d_max", 90.0),
("h_lo_core", 0.16),
("core_from", 30.0),
("ctx_up", 4.0),
("split_adv", 0.0),
("split_gap", 6.0),
("split_near", 55.0),
("split_top", 1),
("enable_accumulator", True),
("acc_near", 55.0),
("acc_gain", 1.5),
("enable_habituation", False),
("hab_rate", 0.25),
("hab_place_m", 5.0),
("hab_recover_m", 800.0),
("d_min", 4.0),
("d_max", 220.0),
("min_rays", 4),
("publish_debug_cloud", False),
("publish_markers", True),
("brain_view", False),
# схема | облако нейронов коннектома | гибрид (панели + облако)
("brain_style", "hybrid"),
("brain_period", 0.2),
# 1 — 1180 пикселей по ширине; 2–3 — для экрана и видео в 2K/4K
("brain_scale", 1),
])
self.par = {q.name: q.value for q in p}
brain_on = bool(self.par["brain_view"])
params = Params(device=str(self.par["device"]),
fov_deg=float(self.par["fov_deg"]),
half_width=float(self.par["half_width"]),
h_lo=float(self.par["h_lo"]), h_hi=float(self.par["h_hi"]),
h_top=float(self.par["h_top"]),
half_width_top=float(self.par["half_width_top"]),
top_d_max=float(self.par["top_d_max"]),
h_lo_core=float(self.par["h_lo_core"]),
core_from=float(self.par["core_from"]),
ctx_up=float(self.par["ctx_up"]),
split_adv=float(self.par["split_adv"]),
split_gap=float(self.par["split_gap"]),
split_near=float(self.par["split_near"]),
split_top=int(self.par["split_top"]),
enable_accumulator=bool(self.par["enable_accumulator"]),
acc_near=float(self.par["acc_near"]),
acc_gain=float(self.par["acc_gain"]),
enable_habituation=bool(self.par["enable_habituation"]),
hab_rate=float(self.par["hab_rate"]),
hab_place_m=float(self.par["hab_place_m"]),
hab_recover_m=float(self.par["hab_recover_m"]),
d_min=float(self.par["d_min"]), d_max=float(self.par["d_max"]),
min_rays=int(self.par["min_rays"]),
# каналы T4/T5 и LPLC2 считаются только когда есть кому
# их показать: на решение они пока не влияют
enable_mbon=bool(self.par["enable_mbon"]),
mbon_power=float(self.par["mbon_power"]),
mbon_blend=float(self.par["mbon_blend"]),
enable_looming=brain_on)
memory = None
mem_path = str(self.par["memory_path"])
if mem_path and Path(mem_path).exists():
memory = MushroomBody.load(mem_path)
self.get_logger().info(
f"память тоннеля загружена: {mem_path} "
f"({memory.cfg.n_kc} клеток Кеньона, обучена на {memory.n_seen} примерах)")
else:
self.get_logger().warn(
"память тоннеля не задана — штатные конструкции тоннеля не подавляются, "
"ложных тревог будет заметно больше")
readout = None
mb_path = str(self.par["mbon_path"])
if mb_path and Path(mb_path).exists():
from .mbon_readout import MbonReadout
readout = MbonReadout.load(mb_path)
self.get_logger().info(
f"считывание MBON загружено: {mb_path} "
f"({readout.cfg.n_kc} клеток Кеньона, {readout.n_pn} признаков)")
elif bool(self.par["enable_mbon"]):
self.get_logger().warn(
"считывание MBON не задано — вес улики считается ручной формулой, "
"ложных тревог будет больше")
# Видеокарта поднимается в фоне: подписка не ждёт прогрева ядер CUDA
self._t_start = time.monotonic()
self.fg = FlyGuard(params, memory=memory, readout=readout, gpu_background=True)
self._report_device()
self.brain = None
self.brain_period = float(self.par["brain_period"])
self._brain_last = 0.0
if brain_on:
style = str(self.par["brain_style"]).lower()
if style == "scheme":
from .brain_view import BrainView
self.brain = BrainView()
elif style == "cloud":
from .brain_atlas import NeuronCloud
self.brain = NeuronCloud(scale=int(self.par["brain_scale"]))
else:
from .brain_hybrid import BrainHybrid
self.brain = BrainHybrid(scale=int(self.par["brain_scale"]))
if not getattr(self.brain, "enabled", False):
# атласа нет — падать незачем, показываем схему
from .brain_view import BrainView
self.get_logger().warn(
f"вид «{style}» недоступен (нет атласа нейронов), беру схему")
self.brain = BrainView()
qos = QoSProfile(
history=QoSHistoryPolicy.KEEP_LAST, depth=int(self.par["queue_depth"]),
reliability=(QoSReliabilityPolicy.BEST_EFFORT if self.par["best_effort"]
else QoSReliabilityPolicy.RELIABLE),
durability=QoSDurabilityPolicy.VOLATILE)
# Кадр лидара — это 24 МБ, и сборка из них Python-объекта sensor_msgs
# стоит дороже всей нашей обработки: на записи с полным круговым сканом
# так терялась половина кадров. Поэтому по умолчанию берём сырые байты
# CDR и разбираем своим парсером — он строит numpy-вид поверх буфера
# без копирования. Обычный путь остаётся под флагом, на случай
# нестандартной раскладки полей.
self.raw = bool(self.par["raw_subscription"])
topics = list(dict.fromkeys([str(self.par["input_topic"])]
+ list(self.par["fallback_topics"])))
self._qos = qos
self.subs = [self.create_subscription(PointCloud2, t, self._on_cloud, qos,
raw=self.raw)
for t in topics]
self.get_logger().info(
f"подписка на: {', '.join(topics)}"
f" ({'сырые байты CDR' if self.raw else 'разбор через rclpy'})")
# Сторож входа. Издатель на топике есть, а кадров нет — почти всегда это
# контейнер без --ipc host и `ros2 bag play` на хосте: Fast DDS шлёт
# кадры через /dev/shm, которой у них общей нет, и теряет их молча.
# Без подсказки такой запуск выглядит как пустой тоннель.
self._topics = topics
self._pub_seen_at: float | None = None
self._watch = self.create_timer(1.0, self._check_input)
self.pub_status = self.create_publisher(ObstacleStatus, "/flyguard/obstacle", 10)
self.pub_flag = self.create_publisher(Bool, "/flyguard/detected", 10)
self.pub_dist = self.create_publisher(Float32, "/flyguard/distance", 10)
self.pub_markers = self.create_publisher(MarkerArray, "/flyguard/markers", 5)
self.pub_diag = self.create_publisher(DiagnosticArray, "/flyguard/diagnostics", 5)
self.pub_cloud = self.create_publisher(PointCloud2, "/flyguard/debug_cloud", 2)
self.pub_view = self.create_publisher(PointCloud2, "/flyguard/view_cloud", 2)
self.pub_brain = None
if self.brain is not None:
from sensor_msgs.msg import Image
self.pub_brain = self.create_publisher(Image, "/flyguard/brain", 2)
# Имя кадра лидара в записях различается («lidar_livox», «hesai_lidar»),
# поэтому нода публикует свой вывод во всегда одинаковом кадре `lidar`
# и отдаёт статическое тождественное преобразование к пришедшему. Тогда
# один и тот же конфиг RViz работает с любым бэгом.
self.fixed_frame = "lidar"
self._tf = StaticTransformBroadcaster(self)
self._tf_sent: set[str] = set()
self._alarm = False # была ли тревога на прошлом кадре
self._alarm_frames = 0
self._jumps = 0 # скачки времени записи, о которых уже сказано
self._latest = None
self._lock = threading.Lock()
self._wake = threading.Event()
self._stop = False
self._dropped = 0
self._received = 0
self._cycle_ms = 0.0
self.async_worker = bool(self.par["async_worker"])
self._worker = None
if self.async_worker:
self._worker = threading.Thread(target=self._loop, daemon=True)
self._worker.start()
# ------------------------------------------------------------------ приём
def _check_input(self) -> None:
if self._received:
self._watch.cancel()
return
pubs = sum(self.count_publishers(t) for t in self._topics)
if not pubs:
self._adopt_foreign_cloud()
return
now = time.monotonic()
if self._pub_seen_at is None:
self._pub_seen_at = now
elif now - self._pub_seen_at >= 5.0:
self.get_logger().error(
f"на топике лидара есть издатель, а кадров нет уже "
f"{now - self._pub_seen_at:.0f} с. Если bag проигрывается на хосте, "
f"запустите контейнер с --ipc host: без него кадры идут через "
f"/dev/shm, общей у контейнера с хостом нет", throttle_duration_sec=10.0)
def _adopt_foreign_cloud(self) -> None:
"""Облако идёт в топик, которого нет в списке, — подписаться и на него.
Имя топика у записей разное (в наших двух разное уже), и у контрольной
записи может оказаться третье. Узел, молча ждущий не тот топик, выглядит
как пустой тоннель. Свои топики `/flyguard/...` не в счёт.
"""
for name, types in self.get_topic_names_and_types():
if (name in self._topics or name.startswith("/flyguard/")
or "sensor_msgs/msg/PointCloud2" not in types):
continue
self.get_logger().warn(
f"облако точек идёт в {name}, а узел слушал "
f"{', '.join(self._topics)} — подписываюсь и на него "
f"(явно: input_topic:={name})")
self._topics.append(name)
self.subs.append(self.create_subscription(
PointCloud2, name, self._on_cloud, self._qos, raw=self.raw))
def _on_cloud(self, msg) -> None:
self._received += 1
if not self.raw:
ok, why = ros_conv.has_required_fields(msg)
if not ok:
self.get_logger().warn(f"кадр пропущен: {why}", throttle_duration_sec=5.0)
return
if not self.async_worker:
# Обработка прямо в колбэке. Такт конвейера втрое короче периода
# кадров, поэтому исполнителю ROS есть когда работать, а отдельный
# поток здесь только отнимает GIL у приёма: в измерениях он ронял
# выработку с 10 до 2 Гц, хотя сам такт оставался 32 мс.
# Отбрасывание устаревших кадров при этом делает очередь DDS:
# её глубина `queue_depth` и есть «хранить только свежее».
t0 = time.perf_counter()
try:
self._process(msg)
except Exception as exc:
self.get_logger().error(f"сбой обработки кадра: {exc!r}")
self._cycle_ms = (time.perf_counter() - t0) * 1e3
return
with self._lock:
if self._latest is not None:
self._dropped += 1
self._latest = msg
self._wake.set()
# ------------------------------------------------------------------ обработка
def _loop(self) -> None:
while not self._stop:
self._wake.wait(timeout=0.5)
self._wake.clear()
with self._lock:
msg, self._latest = self._latest, None
if msg is None:
continue
t0 = time.perf_counter()
try:
self._process(msg)
except Exception as exc: # нода не должна падать на кадре
self.get_logger().error(f"сбой обработки кадра: {exc!r}")
# полный такт рабочего потока: конвейер плюс разбор и публикация
self._cycle_ms = (time.perf_counter() - t0) * 1e3
def _process(self, msg) -> None:
if self.raw:
pc = parse_pointcloud2(msg)
header = Header()
sec = int(pc.stamp)
header.stamp.sec = sec
header.stamp.nanosec = int(round((pc.stamp - sec) * 1e9))
header.frame_id = pc.frame_id
else:
pc = ros_conv.from_ros(msg)
header = msg.header
want_view = self.pub_view.get_subscription_count() > 0
need_debug = (bool(self.par["publish_debug_cloud"]) or self.brain is not None
or want_view)
res = self.fg.process(pc, keep_debug=need_debug)
if res is None:
self.get_logger().info(
f"калибровка решётки лучей: {self.fg.frames_seen}/{self.fg.p.calib_frames}",
throttle_duration_sec=2.0)
return
if self.fg.time_jumps != self._jumps:
self._jumps = self.fg.time_jumps
self._alarm = False
self.get_logger().info("время записи скакнуло (перемотка или повтор) — "
"треки и одометрия начаты заново")
src_frame = header.frame_id or self.fixed_frame
self._ensure_tf(src_frame)
frame = str(self.par["frame_id"]) or self.fixed_frame
d = res.decision
out = ObstacleStatus()
out.header = header
out.header.frame_id = frame
out.detected = bool(d.detected)
out.emergency = bool(d.emergency)
out.distance = float(d.distance)
out.time_to_collision = float(d.ttc)
out.confidence = float(d.confidence)
out.speed = float(d.speed)
out.stopping_distance = float(d.stopping_distance)
out.processing_ms = float(res.total_ms)
for o in d.objects:
m = DetectedObject()
m.distance = float(o.distance); m.lateral = float(o.lateral)
m.height = float(o.height); m.width = float(o.width)
m.size_v = float(o.size_v); m.confidence = float(o.confidence)
m.novelty = float(o.novelty); m.n_rays = int(o.n_rays)
m.track_id = int(o.track_id); m.time_to_collision = float(o.ttc)
out.objects.append(m)
self.pub_status.publish(out)
self.pub_flag.publish(Bool(data=bool(d.detected)))
self.pub_dist.publish(Float32(data=float(d.distance if d.detected else -1.0)))
self._report(d)
if bool(self.par["publish_markers"]):
self.pub_markers.publish(self._markers(d, frame, header.stamp, res.corridor))
if want_view and res.tf is not None:
self.pub_view.publish(self._view_cloud(res, frame, header.stamp))
self._publish_diag(res, header.stamp)
if bool(self.par["publish_debug_cloud"]):
cloud = self._debug_cloud(res, frame, header.stamp)
if cloud is not None:
self.pub_cloud.publish(cloud)
# схема мозга рисуется реже кадров лидара: она для человека, не для системы
if self.brain is not None and self.pub_brain is not None:
now = time.monotonic()
if now - self._brain_last >= self.brain_period:
self._brain_last = now
img = self.brain.render(res)
if img is not None:
self.pub_brain.publish(self.brain.to_msg(img, header.stamp, frame))
def _report(self, d) -> None:
"""Итог в консоль: у стенда результат виден без RViz и `ros2 topic echo`.
Пишется смена состояния, а пока тревога держится — ближайшая дальность
не чаще раза в секунду: построчный вывод на 10 Гц читать невозможно.
"""
if d.detected:
self._alarm_frames += 1
text = (f"{d.distance:.1f} м, уверенность {d.confidence:.2f}, "
f"объектов {len(d.objects)}")
if np.isfinite(d.ttc) and d.speed > 0.5:
text += f", до столкновения {d.ttc:.1f} с"
head = "ЭКСТРЕННОЕ ТОРМОЖЕНИЕ" if d.emergency else "ПРЕПЯТСТВИЕ"
if not self._alarm:
self.get_logger().warn(f"{head}: {text}")
else:
self.get_logger().warn(f"{head.lower()}: {text}",
throttle_duration_sec=1.0)
elif self._alarm:
self.get_logger().info("путь свободен")
self._alarm = bool(d.detected)
def _debug_cloud(self, res, frame: str, stamp):
"""Лучи кандидатов, раскрашенные по новизне, — для наглядности в RViz."""
tf = res.tf
if tf is None or not res.candidates:
return None
xs, ys, zs, ws = [], [], [], []
for c in res.candidates:
rays = c.extra.get("rays")
if rays is None:
continue
ii, jj = rays
xs.append(tf.u[ii, jj])
ys.append(-tf.d[ii, jj])
zs.append(tf.h[ii, jj])
ws.append(np.full(ii.size, c.novelty, np.float32))
if not xs:
return None
return self._cloud_msg(np.concatenate(xs), np.concatenate(ys),
np.concatenate(zs), np.concatenate(ws), frame, stamp)
def _view_cloud(self, res, frame: str, stamp):
"""Облако обзора для RViz: сектор обработки в координатах пути.
Сырое облако — до 900 тысяч точек и 24 МБ на кадр: RViz на нём тормозит,
а в части записей оно ещё и идёт в другой топик, которого конфиг RViz не
знает. Здесь только лучи сектора обработки (до 77 тысяч), выровненные по
плоскости рельсов: x — поперёк пути, −y — вдоль, z — высота над головкой
рельса, как у рамок препятствий, так что рамка стоит ровно на полу.
Публикуется, только когда на топик кто-то подписан.
"""
tf = res.tf
m = tf.valid & (tf.d > 0.5) & (tf.d < 250.0) & np.isfinite(tf.h)
return self._cloud_msg(tf.u[m], -tf.d[m], tf.h[m], tf.inten[m], frame, stamp)
@staticmethod
def _cloud_msg(xs, ys, zs, ws, frame: str, stamp) -> PointCloud2:
pts = np.empty(xs.size, dtype=np.dtype([("x", "f4"), ("y", "f4"), ("z", "f4"),
("intensity", "f4")]))
pts["x"] = xs
pts["y"] = ys
pts["z"] = zs
pts["intensity"] = ws
msg = PointCloud2()
msg.header.stamp = stamp
msg.header.frame_id = frame
msg.height = 1
msg.width = pts.size
msg.fields = [PointField(name=n, offset=o, datatype=PointField.FLOAT32, count=1)
for n, o in (("x", 0), ("y", 4), ("z", 8), ("intensity", 12))]
msg.is_bigendian = False
msg.point_step = 16
msg.row_step = 16 * pts.size
msg.is_dense = True
# array('B'), а не bytes: из bytes rclpy проверяет каждый байт на Python —
# на облаке обзора это 45 мс на кадр вместо одной (замерено).
msg.data = array.array("B", pts.tobytes())
return msg
def _ensure_tf(self, src_frame: str) -> None:
if src_frame in self._tf_sent or src_frame == self.fixed_frame:
return
t = TransformStamped()
t.header.stamp = self.get_clock().now().to_msg()
t.header.frame_id = self.fixed_frame
t.child_frame_id = src_frame
t.transform.rotation.w = 1.0
self._tf.sendTransform(t)
self._tf_sent.add(src_frame)
self.get_logger().info(f"кадр лидара «{src_frame}» связан с «{self.fixed_frame}»")
# ------------------------------------------------------------------ визуализация
def _markers(self, d, frame: str, stamp, corridor=None) -> MarkerArray:
arr = MarkerArray()
clear = Marker()
clear.header.frame_id = frame
clear.header.stamp = stamp
clear.action = Marker.DELETEALL
arr.markers.append(clear)
for i, o in enumerate(d.objects):
m = Marker()
m.header.frame_id = frame
m.header.stamp = stamp
m.ns = "flyguard"
m.id = i + 1
m.type = Marker.CUBE
m.action = Marker.ADD
# координаты пути: вперёд = −Y, вправо = +X, вверх = +Z от головки
# рельса — те же, что у облака обзора, поэтому рамка стоит на полу.
# Поперёк — смещение в системе лидара, а не от оси пути: в кривой
# облако не выпрямлено, и рамка по `lateral` встала бы в стороне
# от своих точек.
x = float(o.sensor_x)
m.pose.position.x = x if np.isfinite(x) else float(o.lateral)
m.pose.position.y = float(-o.distance)
m.pose.position.z = float(o.height + o.size_v / 2)
m.pose.orientation.w = 1.0
m.scale.x = max(float(o.width), 0.3)
m.scale.y = max(float(o.width), 0.3)
m.scale.z = max(float(o.size_v), 0.3)
hot = float(np.clip(o.confidence, 0.0, 1.0))
m.color.r = 1.0
m.color.g = float(1.0 - hot)
m.color.b = 0.0
m.color.a = 0.55
arr.markers.append(m)
txt = Marker()
txt.header = m.header
txt.ns = "flyguard_text"
txt.id = 1000 + i
txt.type = Marker.TEXT_VIEW_FACING
txt.action = Marker.ADD
txt.pose = m.pose
txt.pose.position.z += 1.0
# 1.4 м: при 0.8 подпись у дальней рамки читалась с трудом
txt.scale.z = 1.4
txt.color.r = txt.color.g = txt.color.b = txt.color.a = 1.0
# латиница: в шрифте RViz нет кириллицы, и «55 м» выходило «55 »
txt.text = f"{o.distance:.0f} m p={o.confidence:.2f}"
arr.markers.append(txt)
arr.markers.extend(self._gauge_markers(frame, stamp, corridor))
return arr
def _gauge_markers(self, frame: str, stamp, corridor=None) -> list[Marker]:
"""Контур габарита в координатах облака обзора.
Узел проверяет объединение двух габаритов (`TrackFrame.lateral`):
прямого — вдоль оси лидара, и изогнутого — вдоль оценённой оси пути.
Облако обзора выровнено по плоскости рельсов, но в кривой не
выпрямлено, поэтому в кривой рисуются оба: изогнутый ярко, прямой
бледно. На прямом пути они совпадают, и рисуется один. Что внутри
оранжевого контура, то узел и проверяет; колонна или шкаф за контуром —
не его забота. Рамки поперёк — через 20 м, для глубины.
"""
p = self.fg.p
far = min(p.d_max, 200.0)
boxes = [(p.half_width, p.h_lo, p.h_hi, p.d_min, far)]
if p.h_top > p.h_hi and p.half_width_top > 0.0:
boxes.append((p.half_width_top, p.h_hi, p.h_top, p.d_min, min(p.top_d_max, far)))
curved = None
if corridor is not None and corridor.n_slices > 0:
ds = np.arange(p.d_min, far + 1e-6, 5.0, dtype=np.float32)
# меньше 15 см контуры сливаются в один — второй незачем
if float(np.abs(corridor.centre(ds)).max()) > 0.15:
curved = corridor.centre
layers = [(None, 0.3 if curved is not None else 0.7)]
if curved is not None:
layers.append((curved, 0.8))
out = []
for i, (centre, alpha) in enumerate(layers):
m = Marker()
m.header.frame_id = frame
m.header.stamp = stamp
m.ns = "flyguard_gauge"
m.id = i
m.type = Marker.LINE_LIST
m.action = Marker.ADD
m.pose.orientation.w = 1.0
m.scale.x = 0.04
m.color.r, m.color.g, m.color.b, m.color.a = 1.0, 0.55, 0.0, alpha
m.points = [Point(x=float(x), y=float(y), z=float(z))
for x, y, z in gauge_outline(boxes, centre)]
out.append(m)
return out
def _report_device(self) -> None:
"""Где идёт счёт — первой строкой журнала: на стенде это видно без RViz."""
fg = self.fg
if fg.gpu_pending:
self.get_logger().info(
"вычисления: пока процессор — видеокарту проверяю и прогреваю в фоне "
"(в Docker под WSL до 20 с), результат тот же")
self._gpu_watch = self.create_timer(0.5, self._watch_gpu)
elif fg.gpu_active:
self._say_gpu()
elif fg.gpu_error:
self.get_logger().warn(f"вычисления: процессор — видеокарта не поднялась ({fg.gpu_error})")
else:
self._say_cpu()
self._gpu_on = fg.gpu_active
def _say_gpu(self, note: str = "") -> None:
from .device import get_device_info
info = get_device_info(str(self.par["device"]))
self.get_logger().info(
f"вычисления: видеокарта {info.get('name', '?')} "
f"({info.get('total_memory_mb', 0) / 1024:.0f} ГБ, CUDA {info.get('cuda_version')}) — "
f"сетчатка, ламина, кластеризация; остальное на процессоре{note}")
def _say_cpu(self) -> None:
why = ("задано device=cpu" if str(self.par["device"]).lower() == "cpu"
else "CUDA недоступна: нет видеокарты, драйвера или контейнер запущен без --gpus")
self.get_logger().info(f"вычисления: процессор ({why})")
def _watch_gpu(self) -> None:
"""Видеокарта поднималась в фоне — сказать в журнал, чем кончилось."""
fg = self.fg
if fg.gpu_pending:
return
self._gpu_watch.cancel()
self._gpu_on = fg.gpu_active
if fg.gpu_active:
self._say_gpu(f" (готова через {time.monotonic() - self._t_start:.1f} с после старта)")
elif fg.gpu_error:
self.get_logger().warn(
f"видеокарта не поднялась ({fg.gpu_error}) — считаю на процессоре")
else:
self._say_cpu()
def _publish_diag(self, res, stamp) -> None:
msg = DiagnosticArray()
msg.header.stamp = stamp
st = DiagnosticStatus()
st.name = "flyguard"
st.hardware_id = "lidar"
total = res.total_ms
st.level = (DiagnosticStatus.OK if total < 90 else DiagnosticStatus.WARN)
st.message = f"{total:.1f} мс/кадр"
st.values = [KeyValue(key=k, value=f"{v:.2f}") for k, v in res.timings.items()]
st.values.append(KeyValue(key="candidates", value=str(len(res.candidates))))
st.values.append(KeyValue(key="tracks", value=str(len(self.fg.cx.tracks))))
st.values.append(KeyValue(key="dropped_frames", value=str(self._dropped)))
# считает сама нода: внешний подписчик тоже теряет сообщения и занижает оценку
st.values.append(KeyValue(key="frames_processed", value=str(self.fg.frames_seen)))
# Настоящий счётчик приёма, включая кадры, ушедшие на калибровку
# решётки: без них разница «принято минус обработано» выглядела
# потерей, хотя это цена восстановления геометрии лучей по данным.
st.values.append(KeyValue(key="frames_received", value=str(self._received)))
st.values.append(KeyValue(key="frames_calibration",
value=str(max(self._received - self.fg.frames_seen
- self._dropped, 0))))
st.values.append(KeyValue(key="cycle_ms", value=f"{self._cycle_ms:.1f}"))
st.values.append(KeyValue(key="device", value=self.fg.device))
if self._gpu_on and not self.fg.gpu_active:
# видеокарта отказала на ходу: кадр досчитан на процессоре, дальше — только он
self._gpu_on = False
self.get_logger().warn(
f"видеокарта отказала ({self.fg.gpu_error}) — дальше считаю на процессоре")
if res.ego:
st.values.append(KeyValue(key="speed_kmh", value=f"{res.ego.kmh:.1f}"))
st.values.append(KeyValue(key="rail_height_m", value=f"{res.plane.height:.3f}"))
st.values.append(KeyValue(key="curve_radius_m", value=f"{res.corridor.radius:.0f}"))
msg.status.append(st)
self.pub_diag.publish(msg)
def summary(self) -> str:
"""Итог сеанса: по нему сразу видно, дошли ли кадры и сколько было тревог."""
seen = self.fg.frames_seen
calib = max(self._received - seen - self._dropped, 0)
return (f"итог: принято кадров {self._received}, обработано {seen}, "
f"на калибровку {calib}, пропущено {self._dropped}; "
f"кадров с тревогой {self._alarm_frames}")
def destroy_node(self) -> bool:
self._stop = True
self._wake.set()
return super().destroy_node()
def main(argv=None) -> None:
rclpy.init(args=argv)
node = FlyGuardNode()
try:
rclpy.spin(node)
except KeyboardInterrupt:
pass
finally:
# По Ctrl+C обработчик rclpy успевает закрыть контекст раньше, и журнал
# ROS тогда ругается «Failed to publish log message to rosout».
if rclpy.ok():
node.get_logger().info(node.summary())
else:
print(f"[flyguard] {node.summary()}", flush=True)
node.destroy_node()
rclpy.try_shutdown()
if __name__ == "__main__":
main()

183
flyguard/player_keys.py Normal file
View file

@ -0,0 +1,183 @@
"""Пульт проигрывания записи: пауза, перемотка, шаг по кадру, скорость.
`ros2 bag play`, запущенный из launch (`bag:=...`), клавиатуры не слышит — у
него нет терминала. Пульт управляет им через сервисы rosbag2 из второго
терминала того же контейнера:
docker exec -it flyguard_demo flyguard-keys /data/<запись>
пробел пауза / продолжить
← → на 3 секунды назад / вперёд
. один кадр вперёд (на паузе)
↑ ↓ быстрее / медленнее
0 в начало записи
q выйти (запись играет дальше)
Текущее время записи берётся из /clock — launch запускает проигрыватель с
`--clock`. Путь к записи нужен только для клавиши «0»: из metadata.yaml
читается время её начала. Узел детектора после перемотки сам сбрасывает треки
и одометрию (скачок времени, `pipeline.FlyGuard.process`).
"""
from __future__ import annotations
import re
import select
import sys
import termios
import tty
from pathlib import Path
import rclpy
from builtin_interfaces.msg import Time
from rclpy.node import Node
from rclpy.qos import qos_profile_sensor_data
from rosbag2_interfaces.srv import GetRate, IsPaused, PlayNext, Seek, SetRate, TogglePaused
from rosgraph_msgs.msg import Clock
STEP_S = 3.0
RATES = [0.1, 0.25, 0.5, 1.0, 2.0, 4.0]
PLAYER = "/rosbag2_player/"
KEYS = """ пробел пауза / продолжить
← → на 3 секунды назад / вперёд
. один кадр вперёд (на паузе)
↑ ↓ быстрее / медленнее
0 в начало записи
q выйти (запись играет дальше)
"""
def bag_span(path: str | None) -> tuple[float | None, float | None]:
"""Начало и длительность записи из metadata.yaml, секунды."""
if not path:
return None, None
meta = Path(path) / "metadata.yaml"
if not meta.exists():
return None, None
txt = meta.read_text(encoding="utf-8")
s = re.search(r"starting_time:\s*\n\s*nanoseconds_since_epoch:\s*(\d+)", txt)
d = re.search(r"duration:\s*\n\s*nanoseconds:\s*(\d+)", txt)
return (int(s.group(1)) * 1e-9 if s else None, int(d.group(1)) * 1e-9 if d else None)
class PlayerKeys(Node):
def __init__(self, start: float | None, duration: float | None):
super().__init__("flyguard_player_keys")
self.now: float | None = None
self.start = start
self.duration = duration
self.state, self.rate = "идёт", 1.0
# /clock проигрыватель шлёт без подтверждений (best effort): с надёжной
# подпиской QoS не сходится, и время не приходит вовсе
self.create_subscription(Clock, "/clock", self._on_clock, qos_profile_sensor_data)
self.cli = {name: self.create_client(srv, PLAYER + name) for name, srv in (
("toggle_paused", TogglePaused), ("is_paused", IsPaused), ("seek", Seek),
("set_rate", SetRate), ("get_rate", GetRate), ("play_next", PlayNext))}
def _on_clock(self, msg: Clock) -> None:
self.now = msg.clock.sec + msg.clock.nanosec * 1e-9
if self.start is None:
self.start = self.now
def call(self, name: str, req=None):
cli = self.cli[name]
if not cli.wait_for_service(timeout_sec=2.0):
self.say("проигрыватель не отвечает — запись закончилась или запущена без launch?")
return None
fut = cli.call_async(req if req is not None else cli.srv_type.Request())
rclpy.spin_until_future_complete(self, fut, timeout_sec=3.0)
return fut.result()
def seek(self, t: float) -> None:
# за конец записи проигрыватель просто закончит её — не пускаем
if self.start is not None:
t = max(t, self.start)
if self.duration:
t = min(t, self.start + self.duration - 0.5)
req = Seek.Request()
sec = int(t)
req.time = Time(sec=sec, nanosec=int((t - sec) * 1e9))
self.call("seek", req)
def set_rate(self, up: bool) -> None:
got = self.call("get_rate")
cur = got.rate if got else 1.0
if up:
new = next((r for r in RATES if r > cur + 1e-6), RATES[-1])
else:
new = next((r for r in reversed(RATES) if r < cur - 1e-6), RATES[0])
req = SetRate.Request()
req.rate = float(new)
self.call("set_rate", req)
def status(self, ask: bool = True) -> str:
"""Строка состояния; ask=False — без запросов к проигрывателю."""
if ask:
paused = self.call("is_paused")
rate = self.call("get_rate")
self.state = "ПАУЗА" if paused and paused.paused else "идёт"
self.rate = rate.rate if rate else 1.0
pos = (f"{self.now - self.start:6.1f} с" if self.now is not None and self.start
else " ? с")
return f"{pos} {self.state:6s} скорость ×{self.rate:g}"
@staticmethod
def say(text: str) -> None:
sys.stdout.write("\r\033[K" + text)
sys.stdout.flush()
def read_key(fd: int) -> str:
ch = sys.stdin.read(1)
if ch != "\x1b":
return ch
if select.select([fd], [], [], 0.05)[0]:
seq = sys.stdin.read(2)
return {"[A": "up", "[B": "down", "[C": "right", "[D": "left"}.get(seq, "")
return "esc"
def main(argv=None) -> None:
args = [a for a in (argv or sys.argv[1:]) if not a.startswith("--")]
rclpy.init()
node = PlayerKeys(*bag_span(args[0] if args else None))
print(KEYS)
fd = sys.stdin.fileno()
old = termios.tcgetattr(fd)
try:
tty.setcbreak(fd)
node.say(node.status())
while rclpy.ok():
if not select.select([fd], [], [], 0.2)[0]:
rclpy.spin_once(node, timeout_sec=0.0)
node.say(node.status(ask=False)) # бегущее время записи
continue
key = read_key(fd)
rclpy.spin_once(node, timeout_sec=0.05)
if key in ("q", "й", "esc"):
break
if key == " ":
node.call("toggle_paused")
elif key in ("left", "right") and node.now is not None:
node.seek(node.now + (STEP_S if key == "right" else -STEP_S))
elif key in ("left", "right"):
node.say("ещё нет времени записи (/clock) — подождите секунду")
continue
elif key in (".", "ю"):
node.call("play_next")
elif key in ("up", "down"):
node.set_rate(key == "up")
elif key == "0" and node.start is not None:
node.seek(node.start)
rclpy.spin_once(node, timeout_sec=0.1)
node.say(node.status())
except KeyboardInterrupt:
pass
finally:
termios.tcsetattr(fd, termios.TCSADRAIN, old)
print()
node.destroy_node()
rclpy.try_shutdown()
if __name__ == "__main__":
main()

38
flyguard/ros_conv.py Normal file
View file

@ -0,0 +1,38 @@
"""Преобразование sensor_msgs/PointCloud2 из rclpy во внутреннее представление.
Внутри ноды сообщение уже разобрано транспортом, поэтому CDR-парсер не нужен:
достаточно посмотреть на поля и построить структурированный numpy-массив
поверх готового буфера, без копирования.
"""
from __future__ import annotations
import numpy as np
from .cdr import PointCloud2, point_dtype
def from_ros(msg) -> PointCloud2:
"""sensor_msgs.msg.PointCloud2 → flyguard.cdr.PointCloud2 (без копирования)."""
fields = [(f.name, f.offset, f.datatype, f.count) for f in msg.fields]
fields.sort(key=lambda f: f[1])
dt = point_dtype(fields, msg.point_step)
buf = msg.data if isinstance(msg.data, (bytes, bytearray, memoryview)) else \
np.asarray(msg.data, np.uint8).tobytes()
n = msg.height * msg.width
pts = np.frombuffer(buf, dtype=dt, count=n)
stamp = msg.header.stamp.sec + msg.header.stamp.nanosec * 1e-9
return PointCloud2(stamp=stamp, frame_id=msg.header.frame_id,
height=msg.height, width=msg.width,
point_step=msg.point_step, is_dense=msg.is_dense, points=pts)
def has_required_fields(msg) -> tuple[bool, str]:
"""Проверить, что в облаке есть всё необходимое конвейеру."""
names = {f.name for f in msg.fields}
need = {"x", "y", "z", "intensity"}
missing = need - names
if missing:
return False, f"в облаке нет полей: {', '.join(sorted(missing))}"
if msg.height * msg.width == 0:
return False, "пустое облако"
return True, ""

109
ros2_ws/src/flyguard/config/flyguard.rviz Normal file
View file

@ -0,0 +1,109 @@
# Конфиг RViz FlyGuard. Без схемы мозга: трёхмерный вид на всё окно.
# Раскладку панелей хранит «QMainWindow State» — сериализация Qt, собранная
# tools/rviz_layout.py; вручную её не править.
Panels:
- Class: rviz_common/Displays
Name: Displays
Property Tree Widget:
Expanded:
- /Препятствия1
Splitter Ratio: 0.5
Tree Height: 700
Visualization Manager:
Class: ""
Displays:
- Class: rviz_default_plugins/Grid
Name: Сетка
Enabled: true
Cell Size: 10
Plane Cell Count: 40
Color: 40; 40; 45
Line Style:
Line Width: 0.02
Value: Lines
Reference Frame: <Fixed Frame>
# Облако обзора публикует сам узел: сектор обработки, выровненный по рельсам
# (z — высота над головкой рельса), до 77 тысяч точек. Сырое облако — до
# 900 тысяч точек и 24 МБ на кадр, и топик у записей разный.
- Class: rviz_default_plugins/PointCloud2
Name: Облако обзора
Enabled: true
Topic:
Value: /flyguard/view_cloud
Depth: 2
Durability Policy: Volatile
Reliability Policy: Best Effort
History Policy: Keep Last
Style: Points
Size (Pixels): 2
Color Transformer: AxisColor
Axis: Z
Autocompute Value Bounds: false
Min Value: -0.3
Max Value: 5.5
Use Fixed Frame: true
Decay Time: 0
Alpha: 1
- Class: rviz_default_plugins/PointCloud2
Name: Отладочное облако
Enabled: false
Topic:
Value: /flyguard/debug_cloud
Depth: 2
Durability Policy: Volatile
Reliability Policy: Reliable
History Policy: Keep Last
Style: Points
Size (Pixels): 3
Color Transformer: Intensity
Alpha: 1
- Class: rviz_default_plugins/MarkerArray
Name: Препятствия
Enabled: true
Topic:
Value: /flyguard/markers
Depth: 5
Durability Policy: Volatile
Reliability Policy: Reliable
History Policy: Keep Last
Namespaces: {}
Global Options:
Background Color: 20; 20; 24
Fixed Frame: lidar
Frame Rate: 30
Tools:
- Class: rviz_default_plugins/MoveCamera
- Class: rviz_default_plugins/FocusCamera
- Class: rviz_default_plugins/Measure
Views:
Current:
Class: rviz_default_plugins/Orbit
Name: Вид сверху-сзади
Distance: 70
Focal Point:
X: 0
Y: -45
Z: 1
Pitch: 0.45
Yaw: 1.5708
Near Clip Distance: 0.05
Target Frame: <Fixed Frame>
Saved:
- Class: rviz_default_plugins/Orbit
Name: Обзор пути
Distance: 140
Focal Point:
X: 0
Y: -90
Z: 1
Pitch: 0.25
Yaw: 1.5708
Target Frame: <Fixed Frame>
Window Geometry:
Displays:
collapsed: false
Height: 1000
Hide Left Dock: false
Hide Right Dock: false
QMainWindow State: 000000ff00000000fd00000001000000000000012c00000384fc0200000001fb000000100044006900730070006c0061007900730100000000000003840000006400ffffff000006280000038400000004000000040000000800000008
Width: 1880

126
ros2_ws/src/flyguard/config/flyguard.yaml Normal file
View file

@ -0,0 +1,126 @@
# Параметры ноды FlyGuard.
# Меняются при запуске: ros2 launch flyguard detect.launch.py fov_deg:=35.0
flyguard:
ros__parameters:
# --- вход -------------------------------------------------------------
# В предоставленных записях встречаются оба имени топика, поэтому нода
# подписывается и на основной, и на запасные: какой появится, с тем и работает.
input_topic: /lidar_points
fallback_topics:
- /sensing/lidar/hesai128/pointcloud
- /points_raw
# Кадр лидара весит до 24 МБ и уходит по UDP десятками тысяч фрагментов.
# При BEST_EFFORT потеря одного фрагмента отбрасывает всё сообщение целиком,
# поэтому по умолчанию надёжная доставка.
best_effort: false
queue_depth: 10
# Сырые байты CDR вместо сборки Python-объекта sensor_msgs: на 24 МБ она
# стоит дороже всей обработки. Разбор идёт своим парсером, без копирования.
raw_subscription: true
# Обработка в колбэке, а не в отдельном потоке: поток боролся за GIL
# с приёмом и ронял выработку с 10 до 2 Гц при такте конвейера 32 мс.
async_worker: false
frame_id: "" # пусто — берётся из заголовка сообщения
# --- габарит и зона поиска -------------------------------------------
# Полуширина габарита: так его задают организаторы в синтетике 24.09
# («у края» — до 1.13 м от оси, «вне, но близко» — с 1.14). Было 1.6:
# ложная тревога на предмете вне габарита. EXPERIMENTS п. 18.4.
half_width: 1.2 # полуширина габарита приближения, м
h_lo: 0.28 # нижняя граница над головкой рельса, м
h_hi: 2.3 # верхняя граница, м
# Верхняя секция: от h_hi до h_top, но уже, |u| < half_width_top, и не
# дальше top_d_max — то, что свисает со свода в путь вагона. У стен на этой
# высоте кабели и светильники, поэтому основной ширины там нет. 0 — выкл.
h_top: 3.3
half_width_top: 1.0
top_d_max: 90.0
# Пол между рельсами (|u| <= 0.85 м) опущен, но только дальше core_from:
# лежащий человек высотой 0.30 м при поле 0.28 м виден верхушкой в два
# сантиметра. Ближе пол прежний — там в полосу 0.16…0.28 м попадают
# головки рельсов, рельс собирается в одну компоненту от кабины, и
# предмет выбрасывается вместе с ней. EXPERIMENTS п. 16.3.
h_lo_core: 0.16 # пол в колее, м
core_from: 30.0 # с какой дальности он опускается, м
# Кластеризация ведётся по области выше габарита, чтобы увидеть, что колонна
# или рама крепи продолжается до свода: без этого срез конструкции по верхней
# границе сам выглядит компактным предметом ростом с габарит.
ctx_up: 4.0 # насколько контекст поднимается выше h_hi, м
# Разделение фигуры и фона по скорости сближения вдоль луча. Стена,
# параллельная движению, вдоль луча не приближается; предмет приближается
# ровно на пройденный путь. Поднимает рабочую дальность с 62 до 80 м, но
# вчетверо увеличивает ложные тревоги — в тоннеле много поверхностей,
# обращённых к поезду. 0 — выключено, рабочее значение 0.6.
split_adv: 0.0
# Разделение фигуры и фона по контрасту ламины. Гладкая стена даёт нулевой
# центр-окружение по построению, предмет на стене — ступеньку. Разрезаются
# только компоненты, растёкшиеся вдоль стены (глубже max_depth), и только
# по лучам, торчащим из своего окружения дальше этого порога, м.
# 0 — выключено.
split_gap: 6.0
split_near: 55.0 # ближе этого не резать: там предмет виден и так, м
# Сколько фигур выносить из одной компоненты. Разрез отрезает фон, и
# протяжённость кандидата вдоль пути падает с 6.9 до 0.8 м — вместе с ней
# пропадает множитель компактности, давивший конструкции вдесятеро. Поэтому
# каждая лишняя фигура дорога: без ограничения 9.4 ложных трека на км, при
# двух 7.4, при одной 6.4 — и дальность обнаружения при этом одна и та же.
split_top: 1
# --- привыкание внутри проезда (кратковременная память) -------------
# Замысел: гасить формы, встретившиеся в нескольких разных точках пути, без
# единого прошлого проезда. ВЫКЛЮЧЕНО: измерено, что избирательности нет —
# при неснасыщающейся ёмкости механизм не меняет ничего (7.5 против 7.5
# ложных треков на км), а видимый эффект маленькой популяции был насыщением,
# которое давит предмет сильнее обстановки. EXPERIMENTS п. 10.
enable_habituation: false
hab_rate: 0.25 # депрессия KC→MBON за одно новое место
hab_place_m: 5.0 # разнос, с которого место считается другим, м
hab_recover_m: 800.0 # путь, за который привыкание сходит наполовину, м
# --- накопление слабых улик (веерное тело) --------------------------
# Предмет неподвижен в мире, тоннель проплывает мимо: лучи из габарита
# складываются в сетку, привязанную к пути. На дальностях, где локальный
# контраст структурно равен нулю (кольцо окружения упирается в стену той
# же дальности), это единственная доступная улика. Удваивает рабочую
# дальность ценой роста ложных треков с 5.2 до 9.1 на километр.
enable_accumulator: true
acc_near: 55.0 # ближе этого покадровый тракт справляется сам, м
acc_gain: 1.5 # опора, засчитываемая за полный контраст
# --- обученное считывание MBON ---------------------------------------
# Те же слои грибовидного тела, другой учитель: веса KC→MBON обучены
# различать предмет и обстановку на метках, изготовленных физикой
# (tools/make_training_set.py). Заменяет ручную формулу веса улики.
# Путь подставляется launch-файлом (artifacts/mbon_readout.npz в пакете);
# пустой — работает прежняя ручная формула.
mbon_path: ""
enable_mbon: true
# Резкость вероятности: вес наблюдения p**1.5. Ложных на незнакомой линии
# 23.5 → 14.7 на км почти без потери дальности (EXPERIMENTS п. 17.4).
mbon_power: 1.5
mbon_blend: 1.0 # 1 — только модель, 0 — только ручная формула
# --- вычисления ----------------------------------------------------------
# Сетчатка, ламина и кластеризация — на видеокарте, если PyTorch её видит
# (контейнер с --gpus all), иначе и при любом сбое — на процессоре.
# Результат одинаков: те же операции в тех же типах (EXPERIMENTS п. 21).
# Видеокарта поднимается в фоне: пока греются ядра CUDA, кадры считает процессор.
device: auto # auto | cuda | cpu
d_min: 4.0 # ближе этого не смотрим, м
d_max: 220.0 # дальше физического предела прибора смысла нет, м
fov_deg: 30.0 # полусектор обработки по азимуту, °
min_rays: 4 # минимум лучей на кандидата
# --- память тоннеля ---------------------------------------------------
# Без неё штатные конструкции тоннеля не подавляются и ложных тревог заметно больше.
memory_path: "" # путь подставляется launch-файлом
# --- вывод ------------------------------------------------------------
publish_markers: true
publish_debug_cloud: false
brain_view: false # вид мозга: для демонстрации, не для замеров задержки
# scheme — нарисованная схема нейропилей;
# cloud — 139 255 нейронов коннектома FlyWire на своих местах;
# hybrid — панели решётки сверху и облако нейронов снизу (по умолчанию).
brain_style: hybrid
brain_period: 0.2
brain_scale: 1 # масштаб вида мозга: 2–3 — картинка для 2K/4K (атласы x2, x3)

124
ros2_ws/src/flyguard/config/flyguard_brain.rviz Normal file
View file

@ -0,0 +1,124 @@
# Конфиг RViz FlyGuard. Со схемой мозга: она справа во всю высоту, панель Displays
# свёрнута (открывается стрелкой у левого края окна).
# Раскладку панелей хранит «QMainWindow State» — сериализация Qt, собранная
# tools/rviz_layout.py; вручную её не править.
Panels:
- Class: rviz_common/Displays
Name: Displays
Property Tree Widget:
Expanded:
- /Препятствия1
Splitter Ratio: 0.5
Tree Height: 700
Visualization Manager:
Class: ""
Displays:
- Class: rviz_default_plugins/Grid
Name: Сетка
Enabled: true
Cell Size: 10
Plane Cell Count: 40
Color: 40; 40; 45
Line Style:
Line Width: 0.02
Value: Lines
Reference Frame: <Fixed Frame>
# Облако обзора публикует сам узел: сектор обработки, выровненный по рельсам
# (z — высота над головкой рельса), до 77 тысяч точек. Сырое облако — до
# 900 тысяч точек и 24 МБ на кадр, и топик у записей разный.
- Class: rviz_default_plugins/PointCloud2
Name: Облако обзора
Enabled: true
Topic:
Value: /flyguard/view_cloud
Depth: 2
Durability Policy: Volatile
Reliability Policy: Best Effort
History Policy: Keep Last
Style: Points
Size (Pixels): 2
Color Transformer: AxisColor
Axis: Z
Autocompute Value Bounds: false
Min Value: -0.3
Max Value: 5.5
Use Fixed Frame: true
Decay Time: 0
Alpha: 1
- Class: rviz_default_plugins/PointCloud2
Name: Отладочное облако
Enabled: false
Topic:
Value: /flyguard/debug_cloud
Depth: 2
Durability Policy: Volatile
Reliability Policy: Reliable
History Policy: Keep Last
Style: Points
Size (Pixels): 3
Color Transformer: Intensity
Alpha: 1
- Class: rviz_default_plugins/MarkerArray
Name: Препятствия
Enabled: true
Topic:
Value: /flyguard/markers
Depth: 5
Durability Policy: Volatile
Reliability Policy: Reliable
History Policy: Keep Last
Namespaces: {}
- Class: rviz_default_plugins/Image
Name: Мозг мухи
Enabled: true
Topic:
Value: /flyguard/brain
Depth: 2
Durability Policy: Volatile
Reliability Policy: Reliable
History Policy: Keep Last
Normalize Range: false
Max Value: 255
Min Value: 0
Global Options:
Background Color: 20; 20; 24
Fixed Frame: lidar
Frame Rate: 30
Tools:
- Class: rviz_default_plugins/MoveCamera
- Class: rviz_default_plugins/FocusCamera
- Class: rviz_default_plugins/Measure
Views:
Current:
Class: rviz_default_plugins/Orbit
Name: Вид сверху-сзади
Distance: 70
Focal Point:
X: 0
Y: -45
Z: 1
Pitch: 0.45
Yaw: 1.5708
Near Clip Distance: 0.05
Target Frame: <Fixed Frame>
Saved:
- Class: rviz_default_plugins/Orbit
Name: Обзор пути
Distance: 140
Focal Point:
X: 0
Y: -90
Z: 1
Pitch: 0.25
Yaw: 1.5708
Target Frame: <Fixed Frame>
Window Geometry:
Displays:
collapsed: false
Height: 1000
Hide Left Dock: true
Hide Right Dock: false
QMainWindow State: 000000ff00000000fd00000002000000000000011800000384fc0200000001fb000000100044006900730070006c0061007900730100000000000003840000006400ffffff000000010000033400000384fc0200000001fb00000012041c043e043704330020043c0443044504380100000000000003840000006400ffffff000003040000038400000004000000040000000800000008
Width: 1880
Мозг мухи:
collapsed: false

16
ros2_ws/src/flyguard/flyguard/__init__.py Normal file
View file

@ -0,0 +1,16 @@
"""FlyGuard — обнаружение посторонних объектов в тоннеле метро по данным 3D-лидара.
Архитектура повторяет вычислительные схемы зрительной системы и грибовидных тел
Drosophila melanogaster, взятые из коннектома (FlyWire / hemibrain):
retina омматидиальная решётка → дальностный образ
stabilizer жужжальца / оцеллии → стабилизация «взгляда»
lamina L1/L2, ON/OFF, center-surround→ локальный контраст
medulla T4/T5, EMD-корреляторы → оптический поток
lobula_plate LPTC (HS/VS), LPLC2 → эго-движение, looming
mushroom_body KC + APL + MBON → новизна / знакомость
central_complex кольцевой аттрактор (EB) → накопление улик, треки
descending Giant Fiber, DNp → решение
"""
__version__ = "1.0.0"

149
ros2_ws/src/flyguard/flyguard/bag.py Normal file
View file

@ -0,0 +1,149 @@
"""Чтение rosbag2 (storage sqlite3) без установленного ROS.
Поддерживает многошардовые бэги (`new_data` — 221 файл `*.db3`), произвольные
имена топиков и порядок шардов по числовому суффиксу. Метаданные `metadata.yaml`
не требуются: список топиков берётся из самой БД, что снимает зависимость от PyYAML.
"""
from __future__ import annotations
import logging
import re
import sqlite3
from contextlib import closing
from dataclasses import dataclass
from pathlib import Path
from typing import Iterator
from .cdr import PointCloud2, parse_pointcloud2
logger = logging.getLogger("flyguard.bag")
_SHARD_RE = re.compile(r"_(\d+)\.db3$")
_POINTCLOUD_TYPE = "sensor_msgs/msg/PointCloud2"
@dataclass(frozen=True)
class BagTopic:
name: str
type: str
count: int
class Bag:
"""Последовательное чтение облаков точек из rosbag2."""
def __init__(self, path: str | Path, topic: str | None = None):
path = Path(path)
if path.is_dir():
shards = sorted(path.glob("*.db3"), key=self._shard_key)
elif path.suffix == ".db3":
shards = [path]
else:
raise FileNotFoundError(f"не бэг и не .db3: {path}")
if not shards:
raise FileNotFoundError(f"в {path} нет файлов *.db3")
self.path = path
self.shards = shards
self.topics = self._scan_topics()
self.topic = topic or self._pick_topic()
self.n_frames_failed = 0 # обновляется по ходу последнего frames()
@staticmethod
def _shard_key(p: Path) -> tuple[int, str]:
m = _SHARD_RE.search(p.name)
return (int(m.group(1)) if m else 0, p.name)
def _scan_topics(self) -> dict[str, BagTopic]:
found: dict[str, BagTopic] = {}
for shard in self.shards:
with closing(self._connect(shard)) as con:
rows = con.execute(
"SELECT t.name, t.type, count(m.id) FROM topics t "
"LEFT JOIN messages m ON m.topic_id = t.id GROUP BY t.id"
).fetchall()
for name, type_, count in rows:
prev = found.get(name)
found[name] = BagTopic(name, type_, (prev.count if prev else 0) + count)
return found
def _pick_topic(self) -> str:
clouds = [t for t in self.topics.values() if t.type == _POINTCLOUD_TYPE]
if not clouds:
raise ValueError(f"в {self.path} нет топиков {_POINTCLOUD_TYPE}: "
f"{sorted(self.topics)}")
# при нескольких облачных топиках берём самый наполненный
return max(clouds, key=lambda t: t.count).name
@staticmethod
def _connect(shard: Path) -> sqlite3.Connection:
return sqlite3.connect(f"file:{shard.as_posix()}?mode=ro&immutable=1", uri=True)
def __len__(self) -> int:
return self.topics[self.topic].count
def frames(self, start: int = 0, stop: int | None = None,
stride: int = 1) -> Iterator[tuple[int, PointCloud2]]:
"""Выдать (timestamp_ns, облако) для сообщений выбранного топика.
Индексация сквозная по всему бэгу; шарды читаются по порядку, внутри
шарда — по возрастанию времени. Сообщение, которое не удаётся разобрать
(битый CDR у реального сенсора), пропускается с предупреждением в лог,
а не останавливает чтение бэга — `start`/`stop` считают по позиции
сообщения в бэге, а не по числу успешно разобранных.
"""
idx = 0
self.n_frames_failed = 0
for shard in self.shards:
# closing(), а не сам connection: у sqlite3 `with` управляет
# транзакцией и файл остаётся открытым — на Windows его потом
# невозможно удалить
with closing(self._connect(shard)) as con:
row = con.execute("SELECT id FROM topics WHERE name = ?",
(self.topic,)).fetchone()
if row is None:
continue
topic_id = row[0]
n_here = con.execute(
"SELECT count(*) FROM messages WHERE topic_id = ?", (topic_id,)
).fetchone()[0]
if stop is not None and idx >= stop:
return
if idx + n_here <= start:
idx += n_here
continue
cur = con.execute(
"SELECT timestamp, data FROM messages WHERE topic_id = ? "
"ORDER BY timestamp", (topic_id,))
for ts, blob in cur:
if stop is not None and idx >= stop:
return
if idx >= start and (idx - start) % stride == 0:
try:
pc = parse_pointcloud2(blob)
except Exception as e: # noqa: BLE001
self.n_frames_failed += 1
logger.warning(
"кадр %d в %s не разобран (%s: %s) — пропущен",
idx, shard.name, type(e).__name__, e)
else:
yield ts, pc
idx += 1
def describe(self) -> str:
lines = [f"бэг: {self.path}",
f"шардов: {len(self.shards)}",
f"топик: {self.topic}"]
for t in sorted(self.topics.values(), key=lambda t: -t.count):
mark = "*" if t.name == self.topic else " "
lines.append(f" {mark} {t.name} [{t.type}] {t.count} сообщений")
return "\n".join(lines)
def find_bags(root: str | Path) -> list[Path]:
"""Найти все каталоги-бэги под указанным корнем."""
root = Path(root)
if not root.exists():
return []
out = {p.parent for p in root.rglob("*.db3")}
return sorted(out)

364
ros2_ws/src/flyguard/flyguard/brain_atlas.py Normal file
View file

@ -0,0 +1,364 @@
"""Мозг как облако из 139 255 нейронов, подсвеченное живой активностью.
Отличие от `brain_view` — не в красоте, а в том, чем нарисовано. Там схема,
нарисованная руками; здесь каждая точка — **настоящий нейрон из коннектома**
FlyWire FAFB v783, стоящий на своём анатомическом месте, а его яркость берётся
от той стадии FlyGuard, которой он соответствует.
Симуляции нет и не нужно. Мембранные потенциалы 139 тысяч клеток никто не
интегрирует: коннектом даёт анатомию и принадлежность клеток, конвейер даёт
активность по стадиям, а здесь одно накладывается на другое. Стоимость кадра —
взвешенная сумма десяти заранее посчитанных карт плотности.
Привязка держится на именах типов клеток, а не на догадках. FlyGuard с самого
начала собран из конкретных схем, и все они в выгрузке есть поимённо:
LC11 127 нейронов детектор мелкого объекта → lobula.py
LPLC2 210 детектор надвигания → medulla.py
HS/VS 22 широкопольный поток → medulla.py
T4/T5 12245 элементарные детекторы движения
KC 5177 клетки Кеньона, разрежённый код → mushroom_body.py
MBON 96 выход памяти, новизна
APL 2 глобальное торможение
DNp01 2 гигантское волокно, экстренное решение → descending.py
Атлас собирается `tools/build_brain_atlas.py` и лежит в `data/brain_atlas.npz`
(605 КБ). Данные FlyWire — CC-BY 4.0, Dorkenwald et al. и Schlegel et al.,
Nature 2024.
"""
from __future__ import annotations
from collections import deque
from pathlib import Path
import numpy as np
try:
import cv2
except ImportError:
cv2 = None
from .brain_view import BG, INK, _colour, put_text, text_size
ATLAS = Path(__file__).with_name("data") / "brain_atlas.npz"
# Какая стадия конвейера освещает какую группу нейронов. Ключи справа — из
# `BrainView.activity`; так обе картинки всегда показывают одно и то же.
STAGE_SOURCE: dict[str, tuple[str, ...]] = {
"retina": ("retina",),
"lamina": ("lamina",),
"medulla": ("medulla",),
"lptc": ("lobula_plate",),
"looming": ("looming",),
"lobula": ("lobula",),
"mushroom": ("mb_lobes", "mb_calyx"),
"central": ("eb", "pb", "fb"),
"descending": ("dn",),
"other": (),
}
OTHER_BASE = 0.045 # остальной мозг: виден, но не отвлекает
TRACE_N = 100 # длина ритмограммы: 10 с при 10 Гц
RU = {
"retina": "сетчатка R1–R8",
"lamina": "ламина L1 / L2",
"medulla": "медулла T4 / T5",
"lptc": "лоб. пластинка HS / VS",
"looming": "LPLC2 надвигание",
"lobula": "лобула LC11",
"mushroom": "грибовидное тело KC → MBON",
"central": "центральный комплекс",
"descending": "нисходящие DNp01",
}
class NeuronCloud:
"""Облако нейронов в анатомических координатах, яркость — активность стадии."""
def __init__(self, labels: bool = True, gain: float = 1.0, scale: int = 1):
self.enabled = cv2 is not None and ATLAS.exists()
self.labels = labels
self.gain = gain
# Масштаб отрисовки: 1 — для узла (1180 × 620), 2–3 — для экрана и
# видео в 2K/4K. Сомы берутся из атласа своего размера
# (brain_atlas_x2.npz …) — это настоящие координаты, а не растянутая
# картинка; шрифты, линии и отступы растут вместе с масштабом.
self.s = max(1, int(scale))
self._smooth: dict[str, float] = {}
self._trace: deque = deque(maxlen=TRACE_N)
if not self.enabled:
return
path = ATLAS.with_name(f"brain_atlas_x{self.s}.npz") if self.s > 1 else ATLAS
d = np.load(path if path.exists() else ATLAS, allow_pickle=False)
self.h, self.w = (int(v) for v in d["shape"])
stages = [str(s) for s in d["stages"]]
px, py = d["px"].astype(np.int64), d["py"].astype(np.int64)
stage, side, depth = d["stage"], d["side"], d["depth"]
if not path.exists(): # атласа нужного размера нет — растянуть обычный
px, py, self.h, self.w = px * self.s, py * self.s, self.h * self.s, self.w * self.s
ok = (px >= 0) & (px < self.w) & (py >= 0) & (py < self.h)
flat = py * self.w + px
# Глубина: дальние сомы тусклее, иначе фронтальная проекция выглядит плоской
shade = (0.55 + 0.45 * (1.0 - depth / 255.0)).astype(np.float32)
# Сомы занимают шестую часть холста; считать по всему полотну незачем.
# Карты сразу строятся по занятым пикселям: это убирает из каждого
# кадра 80 % работы, а в 4K — ещё и сотни мегабайт промежуточных карт.
self.support = np.unique(flat[ok])
at = np.searchsorted(self.support, flat)
# Две карты на стадию — левое и правое полушарие: так видно, с какой
# стороны пришёл объект, ровно как у мухи с её двумя оптическими долями.
self.keys: list[str] = []
maps = []
for i, name in enumerate(stages):
for sd, tag in ((0, "L"), (1, "R"), (2, "C")):
m = ok & (stage == i) & (side == sd)
if m.sum() == 0:
continue
self.keys.append(f"{name}|{tag}")
maps.append(np.bincount(at[m], weights=shade[m],
minlength=self.support.size).astype(np.float32))
self.maps = np.ascontiguousarray(np.stack(maps)) # (K, занятые пиксели)
self.stage_of = [k.split("|")[0] for k in self.keys]
self.side_of = [k.split("|")[1] for k in self.keys]
# Якоря подписей — центры тяжести стадии в своём полушарии. Общий
# центр тяжести у всех стадий приходится на середину мозга и ничего
# не показывает: стадии парные.
self.anchor: dict[str, tuple[tuple[int, int], tuple[int, int]]] = {}
for i, name in enumerate(stages):
if name not in RU:
continue
a = []
for sd in (0, 1):
m = ok & (stage == i) & (side == sd)
m = m if m.sum() > 30 else (ok & (stage == i))
a.append((int(px[m].mean()), int(py[m].mean())))
self.anchor[name] = tuple(a)
self._lut = self._build_lut()
self._base = self._draw_base()
# ------------------------------------------------------------------ статика
@staticmethod
def _build_lut() -> np.ndarray:
"""Палитра «покой → работа → тревога», та же, что у схемы."""
lut = np.zeros((256, 3), np.uint8)
for i in range(256):
lut[i] = _colour(i / 255.0)
return lut
def _draw_base(self) -> np.ndarray:
s = self.s
img = np.full((self.h, self.w, 3), BG, np.uint8)
put_text(img, "FlyGuard — 139 255 нейронов коннектома FlyWire, "
"подсвеченных активностью конвейера",
(26 * s, 34 * s), 0.6 * s, (205, 205, 215))
cv2.line(img, (26 * s, 46 * s), (self.w - 26 * s, 46 * s), (52, 52, 62), s)
return img
# ------------------------------------------------------------------ кадр
def weights(self, act: dict[str, float], bias: float) -> np.ndarray:
"""Яркость каждой полукарты. `bias` < 0 — объект слева, > 0 — справа."""
w = np.empty(len(self.keys), np.float32)
for i, (stage, side) in enumerate(zip(self.stage_of, self.side_of)):
src = STAGE_SOURCE.get(stage, ())
v = max((self._smooth.get(k, 0.0) for k in src), default=0.0)
v = OTHER_BASE if not src else max(v, OTHER_BASE * 0.7)
if side in ("L", "R") and stage in ("retina", "lamina", "medulla",
"lptc", "looming", "lobula"):
# У мухи зрительный мир перекрещен: предмет справа сильнее
# освещает левую долю. Для демонстрации важно, что асимметрия
# видна и соответствует стороне объекта.
v *= 1.0 + 0.10 * (bias if side == "L" else -bias)
w[i] = np.clip(v, 0.0, 1.0)
return w
def render(self, res) -> np.ndarray | None:
if not self.enabled:
return None
from .brain_view import BrainView
for k, v in BrainView.activity(res).items():
self._smooth[k] = 0.6 * self._smooth.get(k, v) + 0.4 * v
# Ритмограмма: у дрозофилы ритмы мозга меряют электродом, из наших
# данных они не выводятся. Поэтому здесь честный след собственного
# состояния — сколько работает зрительный тракт, сколько памяти и
# когда сработало решение.
vis = float(np.mean([self._smooth.get(k, 0.0) for k in
("lamina", "medulla", "lobula", "lobula_plate")]))
self._trace.append((float(vis),
float(self._smooth.get("dopamine", 0.0)),
bool(res.decision.detected),
float(res.total_ms),
float(self._smooth.get("habituation", 0.0))))
lat = [c.u for c in res.candidates] or [0.0]
bias = float(np.clip(np.mean(lat) / 1.6, -1.0, 1.0))
vals = self.weights(self.activity_smoothed(), bias) @ self.maps
img = self._base.copy()
if self.s == 1:
idx = np.clip(vals * (255.0 * self.gain), 0, 255).astype(np.uint8)
img.reshape(-1, 3)[self.support] = self._lut[idx]
# Точки одиночные и на большом холсте теряются: лёгкое размытие делает
# облако читаемым, не превращая его в заливку.
cv2.GaussianBlur(img, (3, 3), 0, dst=img)
else:
self._paint_fine(img, vals)
if self.labels:
self._draw_labels(img)
self._draw_rhythm(img)
self._draw_readout(img, res)
return img
def _paint_fine(self, img, vals) -> None:
"""Облако в высоком разрешении: каждая сома на своём месте.
В масштабе 1 пиксель холста собирает несколько сом, и яркость — их
сумма. Здесь сома — отдельная точка, поэтому сумма берётся по
окрестности той же площади (гауссово окно, нормированное так, что
одиночная сома сохраняет свою яркость), а края точек смешиваются с
фоном по доле покрытия. Облако выглядит как в узле, только чётче.
"""
sig = 0.55 * self.s
norm = 2.0 * np.pi * sig * sig
field = np.zeros((2, self.h * self.w), np.float32)
field[0, self.support] = vals
field[1, self.support] = 1.0
v = cv2.GaussianBlur(field[0].reshape(self.h, self.w), (0, 0), sig) * norm
a = np.clip(cv2.GaussianBlur(field[1].reshape(self.h, self.w), (0, 0), sig)
* norm, 0.0, 1.0)
idx = np.clip(v * (255.0 * self.gain), 0, 255).astype(np.uint8)
col = self._lut[idx]
img[:] = cv2.blendLinear(col, img, a, 1.0 - a)
def activity_smoothed(self) -> dict[str, float]:
return self._smooth
def _draw_labels(self, img) -> None:
"""Подписи столбиком у ближнего края с выноской к центру тяжести стадии.
Центры тяжести половины стадий лежат в середине мозга и налезают друг
на друга, поэтому подписи разводятся по вертикали, а привязка остаётся
честной — линия ведёт ровно в центр тяжести.
"""
# Зрительный путь подписывается слева, память и решение — справа:
# так выноски не пересекают мозг и идут в своё же полушарие.
s = self.s
left_side = ("retina", "lamina", "medulla", "lptc")
cols = {True: 76 * s, False: 76 * s}
for name in ("retina", "lamina", "medulla", "lptc",
"looming", "lobula", "mushroom", "central", "descending"):
if name not in self.anchor:
continue
left = name in left_side
ax, ay = self.anchor[name][0 if left else 1]
txt = RU[name]
tw, th = text_size(txt, 0.42 * s)
x = 26 * s if left else self.w - tw - 26 * s
y = cols[left]
cols[left] += 27 * s
box = img[max(y - th - 5 * s, 0):y + 5 * s, max(x - 6 * s, 0):x + tw + 6 * s]
if box.size:
box[:] = (box * 0.2).astype(np.uint8)
put_text(img, txt, (x, y), 0.42 * s, INK)
tip = (x + tw + 7 * s, y - th // 2) if left else (x - 7 * s, y - th // 2)
cv2.line(img, tip, (ax, ay), (82, 80, 96), s, cv2.LINE_AA)
cv2.circle(img, (ax, ay), 3 * s, (162, 160, 178), s, cv2.LINE_AA)
def _draw_rhythm(self, img) -> None:
"""Ритмограмма: след активности и дофамина за последние десять секунд.
Это не ритм мозга дрозофилы — тот меряют электродом, и из лидарных
данных он не выводится. Это след собственного состояния системы:
зрительный тракт, сигнал обучения и отметки моментов решения.
"""
if len(self._trace) < 3:
return
s = self.s
x0, y0, w, hgt = 250 * s, self.h - 96 * s, 380 * s, 56 * s
cv2.rectangle(img, (x0, y0), (x0 + w, y0 + hgt), (40, 39, 48), -1)
cv2.rectangle(img, (x0, y0), (x0 + w, y0 + hgt), (60, 58, 70), s)
put_text(img, "ритм · 10 с", (x0 + 4 * s, y0 - 6 * s), 0.37 * s, (128, 126, 140))
n = len(self._trace)
xs = x0 + (np.arange(n) * (w - 6 * s) / max(TRACE_N - 1, 1)).astype(int) + 3 * s
tr = np.array([(a, b, float(c), e) for a, b, c, _, e in self._trace],
np.float32)
lx = x0 + 78 * s
traces = [(0, (176, 196, 92), "зрение"), (1, (86, 178, 240), "дофамин")]
if tr[:, 3].max() > 0.0:
traces.append((3, (150, 120, 226), "привыкание"))
for col, colour, name in traces:
ys = (y0 + hgt - 8 * s - tr[:, col] * (hgt - 16 * s)).astype(int)
cv2.polylines(img, [np.stack([xs, ys], 1)], False, colour, s, cv2.LINE_AA)
cv2.line(img, (lx, y0 - 10 * s), (lx + 11 * s, y0 - 10 * s), colour, 2 * s,
cv2.LINE_AA)
put_text(img, name, (lx + 15 * s, y0 - 6 * s), 0.35 * s, (132, 130, 144))
lx += 15 * s + text_size(name, 0.35 * s)[0] + 16 * s
# Полоска решений внизу: где система говорила «препятствие».
hot = tr[:, 2] > 0.5
if hot.any():
for i in np.flatnonzero(hot):
cv2.line(img, (int(xs[i]), y0 + hgt - 5 * s), (int(xs[i]), y0 + hgt - 2 * s),
(66, 66, 248), s)
cv2.line(img, (lx, y0 - 10 * s), (lx + 11 * s, y0 - 10 * s), (66, 66, 248), 2 * s)
put_text(img, "решение", (lx + 15 * s, y0 - 6 * s), 0.35 * s, (132, 130, 144))
def _draw_readout(self, img, res) -> None:
d = res.decision
hab = float(getattr(res, "hab", 0.0))
rows = [("скорость", f"{abs(d.speed) * 3.6:.1f} км/ч"),
("кандидатов", f"{len(res.candidates)}"),
("новизна MBON", f"{max((c.novelty for c in res.candidates), default=0.0):.2f}"),
("дофамин PPL1/PAM", f"{self._smooth.get('dopamine', 0.0):.2f}")]
# привыкание по умолчанию выключено — строка появляется, только если оно
# включено и уже что-то накопило
if hab > 0.0:
rows.append(("привыкание", f"{hab:.2f}"))
rows.append(("обработка", f"{res.total_ms:.1f} мс"))
s = self.s
y0 = self.h - 126 * s - 23 * s * (len(rows) - 5)
# Столбик лежит поверх облака, и серые подписи в нём тонули в точках
# оптической доли — подложка, как под выносками стадий.
vw = max(text_size(v, 0.46 * s)[0] for _, v in rows)
right = min(196 * s + vw + 10 * s, 244 * s) # не заходить на ритмограмму
box = img[y0 - 18 * s:y0 + 23 * s * (len(rows) - 1) + 8 * s, 18 * s:right]
box[:] = (box * 0.25).astype(np.uint8)
for i, (k, v) in enumerate(rows):
put_text(img, k, (28 * s, y0 + i * 23 * s), 0.44 * s, (118, 116, 128))
put_text(img, v, (196 * s, y0 + i * 23 * s), 0.46 * s, (206, 206, 216))
if d.detected:
txt = f"ПРЕПЯТСТВИЕ {d.distance:.0f} м"
sub = ("ЭКСТРЕННОЕ ТОРМОЖЕНИЕ" if d.emergency
else f"уверенность {d.confidence:.2f}")
colour = (66, 66, 248) if d.emergency else (72, 196, 240)
else:
txt, sub, colour = "ПУТЬ СВОБОДЕН", "", (120, 214, 130)
put_text(img, txt, (self.w - 470 * s, self.h - 62 * s), 0.9 * s, colour, 2)
if sub:
put_text(img, sub, (self.w - 470 * s, self.h - 32 * s), 0.54 * s, colour)
x = self.w - 300 * s
for lvl, name in ((0.10, "покой"), (0.45, "работа"), (0.85, "тревога")):
cv2.circle(img, (x, 34 * s), 5 * s, _colour(lvl), -1, cv2.LINE_AA)
put_text(img, name, (x + 12 * s, 39 * s), 0.4 * s, (150, 148, 162))
x += 14 * s + text_size(name, 0.4 * s)[0] + 22 * s
# ------------------------------------------------------------------ ROS
@staticmethod
def to_msg(img: np.ndarray, stamp, frame_id: str):
from .brain_view import BrainView
return BrainView.to_msg(img, stamp, frame_id)

145
ros2_ws/src/flyguard/flyguard/brain_hybrid.py Normal file
View file

@ -0,0 +1,145 @@
"""Гибридный вид: что видит глаз — и что от этого происходит в мозге.
Сверху четыре панели ретинотопической решётки, как их видит муха: дальностный
образ, ON-канал ламины (что ближе окружения), OFF-канал (что дальше) и
выделенные кандидаты. Снизу — те же 139 255 нейронов коннектома, подсвеченные
активностью соответствующих стадий.
Смысл именно в паре: видно не только «мозг светится», но и от чего. Контраст
в ламине появляется на панели — и в ту же секунду загорается полоса ламины в
облаке; предмет попадает в габарит — вспыхивает лобула; трек набирает улику —
разгорается центральный комплекс и уходит вниз к гигантскому волокну.
Стоит около 12 мс и включается флагом, чтобы не попадать в замеры задержки.
"""
from __future__ import annotations
import numpy as np
try:
import cv2
except ImportError:
cv2 = None
from .brain_atlas import NeuronCloud
from .brain_view import BG, put_text
PANEL_H = 132 # высота строки панелей, px
PAD = 10
def _norm(a: np.ndarray, lo: float, hi: float) -> np.ndarray:
return np.clip((a - lo) / max(hi - lo, 1e-6), 0.0, 1.0)
class BrainHybrid:
"""Панели решётки + облако нейронов в одном кадре."""
def __init__(self, labels: bool = True, scale: int = 1):
self.cloud = NeuronCloud(labels=labels, scale=scale)
self.s = self.cloud.s
self.enabled = cv2 is not None and self.cloud.enabled
if not self.enabled:
return
self.w = self.cloud.w
self.strip_h = (PANEL_H + 34) * self.s
self.h = self.cloud.h + self.strip_h
# ------------------------------------------------------------------ панели
def _panel(self, img: np.ndarray, title: str, hint: str, slot: int,
out: np.ndarray) -> None:
n, s = 4, self.s
pad, ph, top = PAD * s, PANEL_H * s, 28 * s
pw = (self.w - pad * (n + 1)) // n
x = pad + slot * (pw + pad)
# INTER_AREA, а не NEAREST: столбцы чередуются по эхам, и при
# прореживании соседями решётка превращается в шахматку.
small = cv2.resize(img, (pw, ph), interpolation=cv2.INTER_AREA)
out[top:top + ph, x:x + pw] = small
cv2.rectangle(out, (x, top), (x + pw, top + ph), (58, 56, 68), s)
from .brain_view import text_size
put_text(out, title, (x + 2 * s, 22 * s), 0.4 * s, (188, 188, 200))
if hint:
tw = text_size(hint, 0.36 * s)[0]
# подпись справа рисуется только если не налезает на заголовок
if x + pw - tw - 8 * s > x + 2 * s + text_size(title, 0.4 * s)[0]:
put_text(out, hint, (x + pw - tw - 2 * s, 22 * s), 0.36 * s, (112, 110, 124))
def _strip(self, res) -> np.ndarray:
out = np.full((self.strip_h, self.w, 3), BG, np.uint8)
tf, lam = res.tf, res.lam
if tf is None or lam is None:
put_text(out, "панели решётки доступны только с keep_debug=True",
(PAD * self.s, 24 * self.s), 0.44 * self.s, (140, 138, 152))
return out
# 1. Дальностный образ: ближнее ярче, «нет эха» — ровный тёмный фон,
# иначе пустые лучи красятся палитрой и спорят с данными.
r = np.where(tf.valid, tf.r, np.nan)
depth = 1.0 - _norm(np.nan_to_num(r, nan=220.0), 4.0, 140.0)
rng = cv2.applyColorMap((depth * 255).astype(np.uint8), cv2.COLORMAP_TURBO)
rng[~tf.valid] = (34, 32, 38)
self._panel(rng, "дальностный образ", "R1–R8", 0, out)
# 2. ON: ближе окружения — то, что торчит навстречу
on = _norm(lam.on, 0.0, 0.02)
self._panel(cv2.applyColorMap((on * 255).astype(np.uint8),
cv2.COLORMAP_INFERNO),
"ламина L1 · ON-контраст", "ближе фона", 1, out)
# 3. OFF: дальше окружения — провалы, ниши, окклюзионная тень.
# Шкала шире, чем у ON: провалы по дальности много глубже выступов.
off = _norm(lam.off, 0.0, 0.06)
off_img = cv2.applyColorMap((off * 255).astype(np.uint8), cv2.COLORMAP_BONE)
off_img[~tf.valid] = (34, 32, 38)
self._panel(off_img, "ламина L2 · OFF-контраст", "дальше фона", 2, out)
# 4. Кандидаты: лучи, попавшие в габарит, поверх приглушённой сцены.
# Цвет по новизне: знакомое тусклое, незнакомое горит.
base = np.where(tf.valid, 40 + depth * 95, 22).astype(np.uint8)
cand = cv2.cvtColor(base, cv2.COLOR_GRAY2BGR)
mark = np.zeros(cand.shape[:2], np.uint8)
hot_of = np.zeros(cand.shape[:2], np.float32)
for c in res.candidates:
rays = c.extra.get("rays") if isinstance(c.extra, dict) else None
if rays is None:
continue
ii, jj = rays
keep = (ii < cand.shape[0]) & (jj < cand.shape[1])
mark[ii[keep], jj[keep]] = 1
hot_of[ii[keep], jj[keep]] = float(np.clip(c.novelty, 0.0, 1.0))
if mark.any():
# утолщаем: одиночные лучи на уменьшенной панели не видно
k = np.ones((3, 5), np.uint8)
grown = cv2.dilate(mark, k)
hot_g = cv2.dilate(hot_of, k)
m = grown.astype(bool)
cand[m] = np.stack([np.full(m.sum(), 60),
(110 + 130 * hot_g[m]).astype(int),
(70 + 185 * hot_g[m]).astype(int)], axis=1)
self._panel(cand, "лобула LC11 · кандидаты",
f"{len(res.candidates)} шт.", 3, out)
s = self.s
cv2.line(out, (PAD * s, self.strip_h - 2 * s), (self.w - PAD * s, self.strip_h - 2 * s),
(52, 52, 62), s)
return out
# ------------------------------------------------------------------ кадр
def render(self, res) -> np.ndarray | None:
if not self.enabled:
return None
cloud = self.cloud.render(res)
if cloud is None:
return None
out = np.empty((self.h, self.w, 3), np.uint8)
out[:self.strip_h] = self._strip(res)
out[self.strip_h:] = cloud
return out
@staticmethod
def to_msg(img: np.ndarray, stamp, frame_id: str):
from .brain_view import BrainView
return BrainView.to_msg(img, stamp, frame_id)

369
ros2_ws/src/flyguard/flyguard/brain_view.py Normal file
View file

@ -0,0 +1,369 @@
"""Схема мозга дрозофилы, подсвеченная живой активностью конвейера.
Каждая стадия FlyGuard соответствует конкретному нейропилю, поэтому состояние
системы показывается не графиками, а анатомической схемой: где сейчас «горит»,
там и идёт работа. На демонстрации это замыкает цепочку из ТЗ —
тоннель → облако точек → мозг мухи → «препятствие, 87 м».
Вид дорсальный, схематичный, но с правильным взаимным расположением: по краям
оптические доли (ламина → медулла → лобула и лобулярная пластинка), в центре
грибовидные тела (чашечка → ножка → доли) и центральный комплекс
(протоцеребральный мост, веерное тело, эллипсоидное тело, нодули), снизу —
нисходящие нейроны с гигантским волокном.
Отрисовка стоит 2–4 мс и вынесена под флаг `brain_view`, чтобы не попадать
в замеры задержки основного тракта.
"""
from __future__ import annotations
import array
import numpy as np
try:
import cv2
except ImportError: # без OpenCV вид просто выключен
cv2 = None
# Встроенные шрифты OpenCV (Hershey) знают только латиницу: любая кириллица
# выходит рядом вопросительных знаков. Подписи здесь русские и составляют
# половину смысла схемы, поэтому текст рисуется через PIL шрифтом DejaVu.
try:
from PIL import Image, ImageDraw, ImageFont
except ImportError:
Image = ImageDraw = ImageFont = None
_FONT_PATHS = (
"/usr/share/fonts/truetype/dejavu/DejaVuSans.ttf",
"/usr/share/fonts/truetype/dejavu/DejaVuSansCondensed.ttf",
"/usr/share/fonts/TTF/DejaVuSans.ttf",
"C:/Windows/Fonts/segoeui.ttf",
"C:/Windows/Fonts/arial.ttf",
)
_font_cache: dict[int, object] = {}
def _font(px: int):
"""Шрифт нужного кегля; None — если ни одного файла не нашлось."""
if ImageFont is None:
return None
if px not in _font_cache:
f = None
for path in _FONT_PATHS:
try:
f = ImageFont.truetype(path, px)
break
except OSError:
continue
_font_cache[px] = f or ImageFont.load_default()
return _font_cache[px]
def _px(scale: float) -> int:
"""Кегль PIL, дающий примерно ту же высоту, что и Hershey с этим масштабом."""
return max(9, int(round(scale * 30)))
def text_size(txt: str, scale: float) -> tuple[int, int]:
f = _font(_px(scale))
if f is None:
(tw, th), _ = cv2.getTextSize(txt, cv2.FONT_HERSHEY_SIMPLEX, scale, 1)
return tw, th
box = f.getbbox(txt)
return box[2] - box[0], box[3] - box[1]
def put_text(img, txt: str, org, scale: float, colour, weight: int = 1) -> None:
"""Надпись с кириллицей. `org` — левый нижний угол, как у cv2.putText.
Рисуется по вырезанному прямоугольнику, а не по всему полотну: PIL копирует
изображение при каждом преобразовании, и на полном кадре полтора десятка
подписей стоили 90 мс — больше, чем весь конвейер.
"""
f = _font(_px(scale))
if f is None or Image is None:
cv2.putText(img, txt, org, cv2.FONT_HERSHEY_SIMPLEX, scale, colour,
weight, cv2.LINE_AA)
return
x, y = int(org[0]), int(org[1])
box = f.getbbox(txt)
pad = 2
# Чернила лягут в x … x+(box[2]-box[0]) по горизонтали и
# y-(box[3]-box[1]) … y по вертикали — вырез берётся ровно по ним.
x0 = max(x - pad, 0)
y0 = max(y - (box[3] - box[1]) - pad, 0)
x1 = min(x + (box[2] - box[0]) + pad + 1, img.shape[1])
y1 = min(y + pad + 1, img.shape[0])
if x1 <= x0 or y1 <= y0:
return
crop = img[y0:y1, x0:x1]
pil = Image.fromarray(crop[:, :, ::-1])
ImageDraw.Draw(pil).text((x - box[0] - x0, y - box[3] - y0), txt, font=f,
fill=tuple(int(c) for c in colour[::-1]))
crop[:, :, :] = np.asarray(pil)[:, :, ::-1]
W, H = 1180, 620
BG = (22, 20, 26)
INK = (150, 150, 165)
# шкала «покой → работа → тревога» в BGR
_STOPS = (
(0.00, (58, 48, 44)),
(0.30, (120, 96, 52)),
(0.55, (168, 178, 64)),
(0.78, (72, 196, 240)),
(1.00, (66, 66, 248)),
)
def _colour(a: float) -> tuple[int, int, int]:
a = float(np.clip(a, 0.0, 1.0))
for (x0, c0), (x1, c1) in zip(_STOPS, _STOPS[1:]):
if a <= x1:
t = (a - x0) / max(x1 - x0, 1e-6)
return tuple(int(c0[i] + (c1[i] - c0[i]) * t) for i in range(3))
return _STOPS[-1][1]
def _annulus(cx, cy, r_out, r_in, a0, a1, squash=1.0):
"""Кольцевой сектор — форма слоя оптической доли."""
# ellipse2Poly требует положительный шаг, поэтому внутренняя дуга строится
# в прямом направлении и разворачивается уже массивом
outer = cv2.ellipse2Poly((cx, cy), (r_out, int(r_out * squash)), 0, a0, a1, 4)
inner = cv2.ellipse2Poly((cx, cy), (r_in, int(r_in * squash)), 0, a0, a1, 4)
return np.vstack([outer, inner[::-1]])
class Region:
__slots__ = ("key", "ru", "pts", "anchor", "label_at")
def __init__(self, key, ru, pts, label_at=None):
self.key = key
self.ru = ru
self.pts = np.asarray(pts, np.int32)
self.anchor = tuple(self.pts.mean(axis=0).astype(int))
self.label_at = label_at
def _build_regions() -> list[Region]:
r: list[Region] = []
for side, (cx, a0, a1, lx) in enumerate(((250, 118, 242, 18), (W - 250, -62, 62, W - 250))):
cy = 300
mirror = side == 1
spec = [
("lamina", "ламина L1 / L2", 218, 188),
("medulla", "медулла T4 / T5", 182, 132),
("lobula", "лобула LC11", 126, 88),
("lobula_plate", "лоб. пластинка LPTC / LPLC2", 82, 52),
]
for i, (key, ru, ro, ri) in enumerate(spec):
pts = _annulus(cx, cy, ro, ri, a0 + i * 3, a1 - i * 3, squash=1.06)
ly = 96 + i * 26
r.append(Region(key, ru, pts, (30 if not mirror else W - 330, ly)))
cx = W // 2
# грибовидные тела: чашечка сзади → ножка вперёд → вертикальная и медиальная
# доли. Они лежат ЛАТЕРАЛЬНЕЕ центрального комплекса, не поверх него.
for mirror in (False, True):
s = -1 if mirror else 1
bx = cx + s * 150
calyx = cv2.ellipse2Poly((bx, 132), (46, 33), 0, 0, 360, 6)
ped = np.array([[bx - s * 16, 160], [bx + s * 18, 158],
[bx + s * 46, 318], [bx + s * 18, 322]], np.int32)
lobes = np.array([[bx + s * 16, 316], [bx + s * 50, 314], [bx + s * 56, 404],
[bx + s * 26, 408], [bx + s * 26, 372], [bx + s * 16, 372]],
np.int32)
r.append(Region("mb_calyx", "чашечка PN → KC", calyx,
(cx - 330, 86) if not mirror else (cx + 210, 86)))
r.append(Region("mb_ped", "ножка · APL", ped, None))
r.append(Region("mb_lobes", "доли KC → MBON (новизна)", lobes,
(cx - 400, 432) if not mirror else (cx + 230, 432)))
pb = _annulus(cx, 232, 104, 86, 200, 340, squash=0.7)
fb = np.array([[cx - 74, 262], [cx + 74, 262], [cx + 58, 314], [cx - 58, 314]], np.int32)
eb = _annulus(cx, 372, 46, 26, 0, 360)
no_l = cv2.ellipse2Poly((cx - 52, 420), (18, 13), 0, 0, 360, 8)
no_r = cv2.ellipse2Poly((cx + 52, 420), (18, 13), 0, 0, 360, 8)
dn = np.array([[cx - 22, 444], [cx + 22, 444], [cx + 40, 566], [cx - 40, 566]], np.int32)
r += [
Region("pb", "протоцеребральный мост", pb, (cx - 96, 176)),
Region("fb", "веерное тело", fb, (cx - 40, 342)),
Region("eb", "эллипсоидное тело — кольцевой аттрактор", eb, (cx - 300, 372)),
Region("no", "нодули", no_l, None),
Region("no", "нодули", no_r, (cx + 96, 428)),
Region("dn", "нисходящие нейроны · гигантское волокно", dn, (cx - 300, 524)),
]
return r
class BrainView:
"""Отрисовка схемы с активностью."""
def __init__(self, labels: bool = True):
self.enabled = cv2 is not None
self.labels = labels
self.regions = _build_regions() if self.enabled else []
self._base = self._draw_base() if self.enabled else None
self._smooth: dict[str, float] = {}
# ------------------------------------------------------------------ статика
def _draw_base(self) -> np.ndarray:
img = np.full((H, W, 3), BG, np.uint8)
put_text(img, "FlyGuard — схемы мозга Drosophila melanogaster в реальном времени",
(26, 36), 0.66, (205, 205, 215))
cv2.line(img, (26, 48), (W - 26, 48), (52, 52, 62), 1)
return img
# ------------------------------------------------------------------ активность
@staticmethod
def activity(res) -> dict[str, float]:
"""Свести состояние кадра к активности по нейропилям, 0…1."""
lam = res.lam
d = res.decision
on = float(np.mean(lam.on)) * 130.0 if lam is not None else 0.0
off = float(np.mean(lam.off)) * 130.0 if lam is not None else 0.0
hole = float(np.mean(lam.hole)) if lam is not None else 0.0
speed = abs(res.ego.speed) / 20.0 if res.ego else 0.0
loom = (float(np.percentile(res.loom, 99.9)) * 25.0
if res.loom is not None and res.loom.size else 0.0)
nov = max((c.novelty for c in res.candidates), default=0.0)
# Дофамин: у мухи нейроны PPL1/PAM управляют пластичностью синапсов
# KC→MBON — ровно тем, чем учится наша память тоннеля. Осмысленная
# величина здесь — сила сигнала обучения: насколько сцена в целом
# незнакома памяти. В отличие от `nov` (самый незнакомый предмет)
# это средняя незнакомость всего, что видно, и она заранее говорит,
# что участок новый и ложных тревог будет больше.
dopa = (float(np.mean([c.novelty for c in res.candidates]))
if res.candidates else 0.0)
n_cand = min(len(res.candidates) / 7.0, 1.0)
evid = max((o.confidence for o in d.objects), default=0.0)
valid = float(lam.disp.astype(bool).mean()) if lam is not None else 0.0
return {
# Сетчатка «горит» тем сильнее, чем больше лучей вернулось: это и
# есть освещённость омматидиальной решётки.
"retina": np.clip(0.15 + 0.85 * valid, 0, 1),
"looming": np.clip(loom, 0, 1),
"dopamine": np.clip(dopa, 0, 1),
# Привыкание: доля синапсов KC→MBON, уже подавленных повторами
# внутри этого проезда. Ноль в начале участка, растёт по мере
# того, как однообразная обстановка перестаёт быть новостью.
"habituation": np.clip(getattr(res, "hab", 0.0), 0, 1),
"lamina": np.clip(0.55 * on + 0.45 * off, 0, 1),
"medulla": np.clip(0.25 + 0.75 * speed, 0, 1),
"lobula": np.clip(n_cand, 0, 1),
"lobula_plate": np.clip(max(speed, loom), 0, 1),
"mb_calyx": np.clip(n_cand, 0, 1),
"mb_ped": np.clip(0.4 * n_cand, 0, 1),
"mb_lobes": np.clip(nov, 0, 1),
"pb": np.clip(speed, 0, 1),
"fb": np.clip(hole, 0, 1),
"eb": np.clip(evid, 0, 1),
"no": np.clip(abs(res.ego.yaw_deg) / 1.5 if res.ego else 0.0, 0, 1),
"dn": 1.0 if d.emergency else (0.7 if d.detected else 0.06),
}
# ------------------------------------------------------------------ кадр
def render(self, res) -> np.ndarray | None:
if not self.enabled:
return None
for k, v in self.activity(res).items(): # сглаживание: мозг не мигает
self._smooth[k] = 0.6 * self._smooth.get(k, v) + 0.4 * v
img = self._base.copy()
for reg in self.regions:
a = self._smooth.get(reg.key, 0.0)
cv2.fillPoly(img, [reg.pts], _colour(a))
cv2.polylines(img, [reg.pts], True, (46, 44, 54), 1, cv2.LINE_AA)
self._draw_bump(img, res)
self._draw_flow(img)
if self.labels:
self._draw_labels(img)
self._draw_readout(img, res)
return img
def _draw_labels(self, img) -> None:
seen = set()
for reg in self.regions:
if reg.label_at is None or reg.ru in seen:
continue
seen.add(reg.ru)
x, y = reg.label_at
tw, th = text_size(reg.ru, 0.44)
# подложка под текстом: подписи неизбежно ложатся поверх нейропилей,
# и без неё их не прочитать
box = img[max(y - th - 6, 0):y + 6, max(x - 6, 0):x + tw + 6]
if box.size:
box[:] = (box * 0.25).astype(np.uint8)
cv2.line(img, (x + tw // 2, y - th - 6 if y > reg.anchor[1] else y + 6),
reg.anchor, (74, 72, 86), 1, cv2.LINE_AA)
put_text(img, reg.ru, (x, y), 0.44, INK)
def _draw_bump(self, img, res) -> None:
"""Бугор активности в эллипсоидном теле — направление на ближайший объект."""
cx, cy, r = W // 2, 380, 36
objs = res.decision.objects
if not objs:
return
o = objs[0]
ang = float(np.clip(np.arctan2(o.lateral, max(o.distance, 1.0)) * 7.0, -3.0, 3.0))
px = int(cx + r * np.sin(ang))
py = int(cy - r * np.cos(ang))
cv2.circle(img, (px, py), 11, _colour(min(o.confidence + 0.35, 1.0)), -1, cv2.LINE_AA)
cv2.circle(img, (px, py), 11, (230, 230, 240), 1, cv2.LINE_AA)
def _draw_flow(self, img) -> None:
cx = W // 2
for a, b in (((330, 300), (410, 260)), ((W - 330, 300), (W - 410, 260)),
((cx - 60, 336), (cx - 30, 356)), ((cx + 60, 336), (cx + 30, 356)),
((cx, 420), (cx, 428))):
cv2.arrowedLine(img, a, b, (86, 84, 98), 1, cv2.LINE_AA, tipLength=0.3)
def _draw_readout(self, img, res) -> None:
d = res.decision
rows = [
("скорость", f"{res.ego.kmh:.1f} км/ч" if res.ego else "—"),
("кандидатов", str(len(res.candidates))),
("новизна MBON", f"{max((c.novelty for c in res.candidates), default=0.0):.2f}"),
("треков", str(len(d.objects))),
("обработка", f"{res.total_ms:.1f} мс"),
]
y0 = H - 150
for i, (k, v) in enumerate(rows):
put_text(img, k, (28, y0 + i * 24), 0.46, (118, 116, 128))
put_text(img, v, (170, y0 + i * 24), 0.5, (206, 206, 216))
if d.detected:
txt = f"ПРЕПЯТСТВИЕ {d.distance:.0f} м"
sub = ("ЭКСТРЕННОЕ ТОРМОЖЕНИЕ" if d.emergency
else f"уверенность {d.confidence:.2f}")
colour = (66, 66, 248) if d.emergency else (72, 196, 240)
else:
txt, sub, colour = "ПУТЬ СВОБОДЕН", "", (120, 214, 130)
put_text(img, txt, (W - 470, H - 90), 0.95, colour, 2)
if sub:
put_text(img, sub, (W - 470, H - 58), 0.56, colour)
# ------------------------------------------------------------------ ROS
@staticmethod
def to_msg(img: np.ndarray, stamp, frame_id: str):
from sensor_msgs.msg import Image
msg = Image()
msg.header.stamp = stamp
msg.header.frame_id = frame_id
msg.height, msg.width = img.shape[:2]
msg.encoding = "bgr8"
msg.is_bigendian = 0
msg.step = img.shape[1] * 3
# array('B'), а не bytes: из bytes rclpy проверяет каждый байт на Python,
# и картинка в 2.8 МБ стоила 142 мс на кадр против 2.4 мс (замерено).
msg.data = array.array("B", img.tobytes())
return msg

149
ros2_ws/src/flyguard/flyguard/cdr.py Normal file
View file

@ -0,0 +1,149 @@
"""Разбор sensor_msgs/msg/PointCloud2 из CDR без зависимости от ROS.
Нужен для двух сценариев:
* офлайн-эксперименты на машине без ROS (Windows);
* прямое чтение rosbag внутри контейнера, минуя `ros2 bag play`.
Внутри ROS-ноды сообщение приходит уже разобранным; его переводит в тот же
`PointCloud2` функция `from_ros_message`.
"""
from __future__ import annotations
import struct
from dataclasses import dataclass
import numpy as np
# sensor_msgs/msg/PointField: код типа -> (numpy dtype, размер в байтах)
_PF_DTYPES = {
1: ("i1", 1), 2: ("u1", 1), 3: ("i2", 2), 4: ("u2", 2),
5: ("i4", 4), 6: ("u4", 4), 7: ("f4", 4), 8: ("f8", 8),
}
@dataclass(frozen=True)
class PointCloud2:
"""Минимальное представление облака точек."""
stamp: float
frame_id: str
height: int
width: int
point_step: int
is_dense: bool
points: np.ndarray # структурированный массив длиной height*width
@property
def n_points(self) -> int:
return int(self.points.shape[0])
class _CdrReader:
"""Чтение little-endian CDR с выравниванием примитивов относительно тела сообщения."""
__slots__ = ("buf", "origin", "pos")
def __init__(self, buf: bytes | memoryview):
self.buf = buf
self.origin = 4 # заголовок инкапсуляции
self.pos = 4
def _align(self, size: int) -> None:
self.pos += (-(self.pos - self.origin)) % size
def u8(self) -> int:
v = self.buf[self.pos]
self.pos += 1
return v
def u32(self) -> int:
self._align(4)
v = struct.unpack_from("<I", self.buf, self.pos)[0]
self.pos += 4
return v
def i32(self) -> int:
self._align(4)
v = struct.unpack_from("<i", self.buf, self.pos)[0]
self.pos += 4
return v
def string(self) -> str:
n = self.u32()
s = bytes(self.buf[self.pos:self.pos + max(n - 1, 0)]).decode("utf-8", "replace")
self.pos += n
return s
def point_dtype(fields: list[tuple[str, int, int, int]], point_step: int) -> np.dtype:
"""Собрать numpy-dtype по описанию полей, явно добивая пропуски паддингом.
Поля лидара невыровнены (`timestamp` float64 по смещению 18), поэтому
структурированный dtype строится вручную, а не через `np.dtype(align=True)`.
"""
spec: list[tuple[str, str]] = []
used = 0
for name, offset, datatype, count in fields:
kind, size = _PF_DTYPES[datatype]
if offset > used:
spec.append((f"_pad{used}", f"V{offset - used}"))
elif offset < used:
raise ValueError(f"перекрывающиеся поля в PointCloud2: {name}")
spec.append((name, kind if count == 1 else f"{count}{kind}"))
used = offset + size * count
if point_step > used:
spec.append((f"_pad{used}", f"V{point_step - used}"))
dt = np.dtype(spec)
if dt.itemsize != point_step:
raise ValueError(f"dtype {dt.itemsize} байт != point_step {point_step}")
return dt
def parse_pointcloud2(blob: bytes | memoryview) -> PointCloud2:
"""Разобрать CDR-сериализованное sensor_msgs/msg/PointCloud2."""
r = _CdrReader(blob)
sec = r.i32()
nsec = r.u32()
frame_id = r.string()
height = r.u32()
width = r.u32()
fields = []
for _ in range(r.u32()):
name = r.string()
offset = r.u32()
datatype = r.u8()
count = r.u32()
fields.append((name, offset, datatype, count))
r.u8() # is_bigendian: в данных всегда 0, little-endian
point_step = r.u32()
r.u32() # row_step
n_bytes = r.u32()
data = memoryview(blob)[r.pos:r.pos + n_bytes]
r.pos += n_bytes
is_dense = bool(r.u8())
dt = point_dtype(fields, point_step)
points = np.frombuffer(data, dtype=dt, count=height * width)
return PointCloud2(stamp=sec + nsec * 1e-9, frame_id=frame_id, height=height,
width=width, point_step=point_step, is_dense=is_dense,
points=points)
def from_ros_message(msg) -> PointCloud2:
"""Перевести разобранное rclpy-сообщение sensor_msgs/msg/PointCloud2.
Все height·width точек сохраняются, включая NaN: сетчатка раскладывает облако
в решётку азимут × кольцо по порядку точек, и выброшенная точка сдвинула бы
всю решётку (`sensor_msgs_py.read_points(skip_nans=True)` делает именно это).
"""
if msg.is_bigendian:
raise ValueError("big-endian PointCloud2 не поддерживается")
fields = [(f.name, f.offset, f.datatype, f.count) for f in msg.fields]
dt = point_dtype(fields, msg.point_step)
points = np.frombuffer(msg.data, dtype=dt, count=msg.height * msg.width)
stamp = msg.header.stamp.sec + msg.header.stamp.nanosec * 1e-9
return PointCloud2(stamp=stamp, frame_id=msg.header.frame_id, height=msg.height,
width=msg.width, point_step=msg.point_step,
is_dense=bool(msg.is_dense), points=points)

418
ros2_ws/src/flyguard/flyguard/central_complex.py Normal file
View file

@ -0,0 +1,418 @@
"""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 # отсчёт считывания по треку, если оно подключено
# То же смещение в системе лидара, без поправки на кривую. В решении не
# участвует: по нему рамка рисуется у своих точек, а не у оси пути.
u_raw: 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,
lying_exempt: bool = False, top_from: float = float("inf"),
flat: tuple[float, float, float, float] = (0.0, 0.0, 0.0, 1.0)):
self.use_shape = use_shape
self.lying_exempt = lying_exempt
self.top_from = top_from # низ выше этого — предмет в верхней секции габарита
self.flat = flat # (высота, верх, дальность, множитель), см. _quality
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,
self.lying_exempt, self.top_from, self.flat)
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.u_raw = (1 - a) * t.u_raw + a * float(c.extra.get("u_raw", 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,
u_raw=float(c.extra.get("u_raw", c.u)))
w = _quality(c, self.use_shape, self.mbon_power, self.mbon_blend,
self.nov_fade_from, self.nov_fade_to,
self.lying_exempt, self.top_from, self.flat)
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, lying_exempt: bool = False,
top_from: float = float("inf"),
flat: tuple[float, float, float, float] = (0.0, 0.0, 0.0, 1.0)) -> float:
"""Вес одного наблюдения: сколько улики оно добавляет.
Дальний объект даёт мало лучей не потому, что он сомнительный, а потому что
так устроена решётка, — поэтому число лучей нормируется на ожидаемое для
этой дальности. Остальные множители отделяют предмет от конструкции тоннеля:
предмет целиком помещается в габарит и компактен вдоль пути, а лоток или
стена тянутся дальше и в стороны. Новизна из грибовидного тела входит сюда
же множителем.
"""
# Висящее в верхней секции (`top_from`) — отдельная статья. Обе обученные
# части, считывание MBON и память тоннеля, собирались до того, как у
# габарита появилась верхняя секция, и таких форм не видели: их ответ там
# не значит ничего, и оба канала здесь выключены. Нормировка числа лучей
# рассчитана на предмет с человека, а свисающий кабель или стержень тонок
# по природе: у стержня 5 см опора упиралась в нижний край 0.25 на любой
# дальности. Пояс над вагоном в исправном тоннеле пуст, поэтому нижний
# край для висящего поднят до 0.5. EXPERIMENTS п. 19.
top = c.h_min >= top_from
support = float(np.clip(c.n_rays / expected_rays(c.d), 0.5 if top else 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 = (lying_exempt and 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))
# опора снизу: упавший предмет, человек, камень стоят на полотне, а знак,
# лоток или кронштейн висят на стене, и под ними пусто. Кроме верхней
# секции габарита (`top_from`): у оси под сводом стен нет, и висящее
# там — это то, что свисает в путь вагона.
grounded = 1.0 if top else 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 = 1.0 if top else novelty_gain(c.novelty)
if not top and 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 and not top:
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)
# Плоское у самого пола: высотой в сантиметры, целиком ниже `flat[1]` над
# рельсом. Это пластины и кромки на полотне, края жёлоба, порог
# гермозатвора — по ответу организаторов то, что в жёлобе, препятствием не
# считается. Модель таким ставит 0.8–0.99: в её обучении был кабель на
# путях. А ложных тревог это давало больше всего: порог гермозатвора на
# 65→31 м и два фантома на синтетике. Настоящий низкий предмет (0.3 м на
# рельсе, 2×0.2 м поперёк путей) вблизи даёт 0.1–0.4 м по высоте и верх
# выше 0.4 м. Дальше `flat[2]` решётка слишком редкая, чтобы судить о
# высоте, и там правило молчит. Множитель, а не запрет: если предмет хоть в
# одном кадре покажет высоту, он своё доберёт. EXPERIMENTS п. 19.
fh, ftop, fd, fw = flat
if fh > 0.0 and c.d <= fd and c.height < fh and c.h_min + c.height < ftop:
hand *= fw
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))

BIN
ros2_ws/src/flyguard/flyguard/data/brain_atlas.npz Normal file
View file

Binary file not shown.

BIN
ros2_ws/src/flyguard/flyguard/data/brain_atlas_x2.npz Normal file
View file

Binary file not shown.

BIN
ros2_ws/src/flyguard/flyguard/data/brain_atlas_x3.npz Normal file
View file

Binary file not shown.

129
ros2_ws/src/flyguard/flyguard/data/pandar128_channels.csv Normal file
View file

@ -0,0 +1,129 @@
channel,az_offset_deg,elevation_deg,range_min_m,range_max_m,near_field,max_range_10pct_m,far_field,min_reflectivity,high_res
1,3.257,14.436,0.3,100.0,1,100.0,0,100 m @ 10%,0
2,3.263,13.535,2.7,100.0,0,100.0,0,100 m @ 10%,0
3,1.091,13.082,0.3,100.0,1,100.0,0,100 m @ 10%,0
4,3.268,12.624,2.7,100.0,0,100.0,0,100 m @ 10%,0
5,1.093,12.165,2.7,100.0,0,100.0,0,100 m @ 10%,0
6,3.273,11.702,0.3,100.0,1,120.0,0,100 m @ 6%,0
7,1.094,11.239,2.7,100.0,0,120.0,0,100 m @ 6%,0
8,3.278,10.771,2.7,100.0,0,140.0,0,100 m @ 3%,0
9,1.095,10.305,0.3,100.0,1,140.0,0,100 m @ 3%,0
10,3.283,9.83,2.7,100.0,0,140.0,0,100 m @ 3%,0
11,1.096,9.356,2.7,100.0,0,140.0,0,100 m @ 3%,0
12,3.288,8.88,0.3,100.0,1,140.0,0,100 m @ 3%,0
13,1.097,8.401,2.7,100.0,0,140.0,0,100 m @ 3%,0
14,3.291,7.921,2.7,100.0,0,140.0,0,100 m @ 3%,0
15,1.098,7.438,0.3,100.0,1,140.0,0,100 m @ 3%,0
16,-1.101,6.953,2.7,100.0,0,140.0,0,100 m @ 3%,0
17,1.1,6.467,2.7,100.0,0,140.0,0,100 m @ 3%,0
18,-1.104,5.978,0.3,100.0,1,140.0,0,100 m @ 3%,0
19,-3.306,5.487,2.7,100.0,0,140.0,0,100 m @ 3%,0
20,-1.106,4.996,2.7,100.0,0,140.0,0,100 m @ 3%,0
21,-3.311,4.501,0.3,100.0,1,140.0,0,100 m @ 3%,0
22,-1.109,4.007,2.7,100.0,0,140.0,0,100 m @ 3%,0
23,-3.318,3.509,2.7,100.0,0,140.0,0,100 m @ 3%,0
24,-1.111,3.013,0.3,100.0,1,140.0,0,100 m @ 3%,0
25,-3.324,2.512,2.7,100.0,0,140.0,0,100 m @ 3%,0
26,-1.113,2.013,0.3,200.0,1,140.0,0,200 m @ 37%,1
27,7.72,1.885,2.7,200.0,0,140.0,0,200 m @ 37%,1
28,5.535,1.761,2.7,200.0,0,140.0,0,200 m @ 37%,1
29,3.325,1.637,2.7,200.0,0,140.0,0,200 m @ 37%,1
30,-3.33,1.511,2.7,200.0,0,140.0,0,200 m @ 37%,1
31,1.107,1.386,2.7,200.0,0,140.0,0,200 m @ 37%,1
32,-5.538,1.258,2.7,200.0,0,140.0,0,200 m @ 37%,1
33,-7.726,1.13,0.3,200.0,1,140.0,0,200 m @ 37%,1
34,-1.115,1.008,2.7,200.0,0,200.0,1,200 m @ 10%,1
35,7.731,0.88,2.7,200.0,0,200.0,1,200 m @ 10%,1
36,5.543,0.756,2.7,200.0,0,200.0,1,200 m @ 10%,1
37,3.329,0.63,2.7,200.0,0,200.0,1,200 m @ 10%,1
38,-3.336,0.505,2.7,200.0,0,200.0,1,200 m @ 10%,1
39,1.108,0.379,2.7,200.0,0,200.0,1,200 m @ 10%,1
40,-5.547,0.251,0.3,200.0,1,200.0,1,200 m @ 10%,1
41,-7.738,0.124,2.7,200.0,0,200.0,1,200 m @ 10%,1
42,-1.117,0.0,2.7,200.0,0,200.0,1,200 m @ 10%,1
43,7.743,-0.129,2.7,200.0,0,200.0,1,200 m @ 10%,1
44,5.551,-0.254,2.7,200.0,0,200.0,1,200 m @ 10%,1
45,3.335,-0.38,2.7,200.0,0,200.0,1,200 m @ 10%,1
46,-3.342,-0.506,2.7,200.0,0,200.0,1,200 m @ 10%,1
47,1.11,-0.632,0.3,200.0,1,200.0,1,200 m @ 10%,1
48,-5.555,-0.76,2.7,200.0,0,200.0,1,200 m @ 10%,1
49,-7.75,-0.887,2.7,200.0,0,200.0,1,200 m @ 10%,1
50,-1.119,-1.012,2.7,200.0,0,200.0,1,200 m @ 10%,1
51,7.757,-1.141,2.7,200.0,0,200.0,1,200 m @ 10%,1
52,5.56,-1.266,2.7,200.0,0,200.0,1,200 m @ 10%,1
53,3.34,-1.393,2.7,200.0,0,200.0,1,200 m @ 10%,1
54,-3.347,-1.519,0.3,200.0,1,200.0,1,200 m @ 10%,1
55,1.111,-1.646,2.7,200.0,0,200.0,1,200 m @ 10%,1
56,-5.564,-1.773,2.7,200.0,0,200.0,1,200 m @ 10%,1
57,-7.762,-1.901,2.7,200.0,0,200.0,1,200 m @ 10%,1
58,-1.121,-2.027,2.7,200.0,0,200.0,1,200 m @ 10%,1
59,7.768,-2.155,2.7,200.0,0,200.0,1,200 m @ 10%,1
60,5.569,-2.282,2.7,200.0,0,200.0,1,200 m @ 10%,1
61,3.345,-2.409,0.3,200.0,1,200.0,1,200 m @ 10%,1
62,-3.353,-2.535,2.7,200.0,0,200.0,1,200 m @ 10%,1
63,1.113,-2.663,2.7,200.0,0,200.0,1,200 m @ 10%,1
64,-5.573,-2.789,2.7,200.0,0,200.0,1,200 m @ 10%,1
65,-7.775,-2.916,2.7,200.0,0,200.0,1,200 m @ 10%,1
66,-1.123,-3.044,2.7,200.0,0,140.0,0,200 m @ 37%,1
67,7.78,-3.172,2.7,200.0,0,140.0,0,200 m @ 37%,1
68,5.578,-3.299,0.3,200.0,1,140.0,0,200 m @ 37%,1
69,3.351,-3.425,2.7,200.0,0,140.0,0,200 m @ 37%,1
70,-3.358,-3.552,2.7,200.0,0,140.0,0,200 m @ 37%,1
71,1.115,-3.68,2.7,200.0,0,140.0,0,200 m @ 37%,1
72,-5.582,-3.806,2.7,200.0,0,140.0,0,200 m @ 37%,1
73,-7.787,-3.933,2.7,200.0,0,140.0,0,200 m @ 37%,1
74,-1.125,-4.062,2.7,200.0,0,140.0,0,200 m @ 37%,1
75,7.792,-4.19,0.3,200.0,1,140.0,0,200 m @ 37%,1
76,5.586,-4.318,2.7,200.0,0,140.0,0,200 m @ 37%,1
77,3.356,-4.444,2.7,200.0,0,140.0,0,200 m @ 37%,1
78,-3.363,-4.571,2.7,200.0,0,140.0,0,200 m @ 37%,1
79,1.116,-4.699,2.7,200.0,0,140.0,0,200 m @ 37%,1
80,-5.591,-4.824,2.7,200.0,0,140.0,0,200 m @ 37%,1
81,-7.799,-4.951,2.7,200.0,0,140.0,0,200 m @ 37%,1
82,-1.127,-5.081,0.3,200.0,1,140.0,0,200 m @ 37%,1
83,7.804,-5.209,2.7,200.0,0,140.0,0,200 m @ 37%,1
84,5.595,-5.336,2.7,200.0,0,140.0,0,200 m @ 37%,1
85,3.36,-5.463,2.7,200.0,0,140.0,0,200 m @ 37%,1
86,-3.369,-5.589,2.7,200.0,0,140.0,0,200 m @ 37%,1
87,1.118,-5.718,2.7,200.0,0,140.0,0,200 m @ 37%,1
88,-5.599,-5.843,2.7,200.0,0,140.0,0,200 m @ 37%,1
89,-7.811,-5.968,2.7,200.0,0,140.0,0,200 m @ 37%,1
90,-1.129,-6.1,0.3,100.0,1,140.0,0,100 m @ 3%,0
91,-3.374,-6.607,2.7,100.0,0,140.0,0,100 m @ 3%,0
92,-1.13,-7.117,2.7,100.0,0,140.0,0,100 m @ 3%,0
93,-3.379,-7.624,0.3,100.0,1,140.0,0,100 m @ 3%,0
94,-1.132,-8.134,2.7,100.0,0,140.0,0,100 m @ 3%,0
95,-3.383,-8.64,2.7,100.0,0,140.0,0,100 m @ 3%,0
96,3.381,-9.149,0.3,100.0,1,140.0,0,100 m @ 3%,0
97,-3.388,-9.652,2.7,100.0,0,140.0,0,100 m @ 3%,0
98,3.386,-10.16,2.7,100.0,0,100.0,0,100 m @ 10%,0
99,1.129,-10.665,0.3,100.0,1,100.0,0,100 m @ 10%,0
100,3.39,-11.17,2.7,100.0,0,100.0,0,100 m @ 10%,0
101,1.129,-11.672,2.7,100.0,0,100.0,0,100 m @ 10%,0
102,3.395,-12.174,0.3,100.0,1,100.0,0,100 m @ 10%,0
103,1.131,-12.673,2.7,100.0,0,100.0,0,100 m @ 10%,0
104,3.401,-13.173,2.7,100.0,0,100.0,0,100 m @ 10%,0
105,1.133,-13.67,0.3,100.0,1,100.0,0,100 m @ 10%,0
106,3.406,-14.166,2.7,100.0,0,50.0,0,100 m @ 120%,0
107,1.135,-14.66,2.7,100.0,0,50.0,0,100 m @ 120%,0
108,3.41,-15.154,0.3,100.0,1,50.0,0,100 m @ 120%,0
109,1.137,-15.645,2.7,100.0,0,50.0,0,100 m @ 120%,0
110,3.416,-16.135,2.7,100.0,0,50.0,0,100 m @ 120%,0
111,1.139,-16.622,0.3,100.0,1,50.0,0,100 m @ 120%,0
112,-1.142,-17.106,2.7,100.0,0,50.0,0,100 m @ 120%,0
113,1.142,-17.592,2.7,100.0,0,50.0,0,100 m @ 120%,0
114,-1.143,-18.072,0.3,100.0,1,50.0,0,100 m @ 120%,0
115,-3.426,-18.548,2.7,100.0,0,50.0,0,100 m @ 120%,0
116,-1.143,-19.03,2.7,100.0,0,25.0,0,100 m @ 1600%,0
117,-3.429,-19.501,0.3,100.0,1,25.0,0,100 m @ 1600%,0
118,-1.145,-19.978,2.7,100.0,0,25.0,0,100 m @ 1600%,0
119,-3.433,-20.445,2.7,100.0,0,25.0,0,100 m @ 1600%,0
120,-1.145,-20.918,0.3,100.0,1,25.0,0,100 m @ 1600%,0
121,-3.436,-21.379,2.7,100.0,0,25.0,0,100 m @ 1600%,0
122,-1.146,-21.848,2.7,100.0,0,25.0,0,100 m @ 1600%,0
123,-3.44,-22.304,0.3,100.0,1,25.0,0,100 m @ 1600%,0
124,-1.146,-22.768,2.7,100.0,0,25.0,0,100 m @ 1600%,0
125,-3.443,-23.219,2.7,100.0,0,25.0,0,100 m @ 1600%,0
126,-1.146,-23.678,0.3,100.0,1,25.0,0,100 m @ 1600%,0
127,-3.446,-24.123,2.7,100.0,0,25.0,0,100 m @ 1600%,0
128,-3.449,-25.016,0.3,100.0,1,25.0,0,100 m @ 1600%,0
1 channel az_offset_deg elevation_deg range_min_m range_max_m near_field max_range_10pct_m far_field min_reflectivity high_res
2 1 3.257 14.436 0.3 100.0 1 100.0 0 100 m @ 10% 0
3 2 3.263 13.535 2.7 100.0 0 100.0 0 100 m @ 10% 0
4 3 1.091 13.082 0.3 100.0 1 100.0 0 100 m @ 10% 0
5 4 3.268 12.624 2.7 100.0 0 100.0 0 100 m @ 10% 0
6 5 1.093 12.165 2.7 100.0 0 100.0 0 100 m @ 10% 0
7 6 3.273 11.702 0.3 100.0 1 120.0 0 100 m @ 6% 0
8 7 1.094 11.239 2.7 100.0 0 120.0 0 100 m @ 6% 0
9 8 3.278 10.771 2.7 100.0 0 140.0 0 100 m @ 3% 0
10 9 1.095 10.305 0.3 100.0 1 140.0 0 100 m @ 3% 0
11 10 3.283 9.83 2.7 100.0 0 140.0 0 100 m @ 3% 0
12 11 1.096 9.356 2.7 100.0 0 140.0 0 100 m @ 3% 0
13 12 3.288 8.88 0.3 100.0 1 140.0 0 100 m @ 3% 0
14 13 1.097 8.401 2.7 100.0 0 140.0 0 100 m @ 3% 0
15 14 3.291 7.921 2.7 100.0 0 140.0 0 100 m @ 3% 0
16 15 1.098 7.438 0.3 100.0 1 140.0 0 100 m @ 3% 0
17 16 -1.101 6.953 2.7 100.0 0 140.0 0 100 m @ 3% 0
18 17 1.1 6.467 2.7 100.0 0 140.0 0 100 m @ 3% 0
19 18 -1.104 5.978 0.3 100.0 1 140.0 0 100 m @ 3% 0
20 19 -3.306 5.487 2.7 100.0 0 140.0 0 100 m @ 3% 0
21 20 -1.106 4.996 2.7 100.0 0 140.0 0 100 m @ 3% 0
22 21 -3.311 4.501 0.3 100.0 1 140.0 0 100 m @ 3% 0
23 22 -1.109 4.007 2.7 100.0 0 140.0 0 100 m @ 3% 0
24 23 -3.318 3.509 2.7 100.0 0 140.0 0 100 m @ 3% 0
25 24 -1.111 3.013 0.3 100.0 1 140.0 0 100 m @ 3% 0
26 25 -3.324 2.512 2.7 100.0 0 140.0 0 100 m @ 3% 0
27 26 -1.113 2.013 0.3 200.0 1 140.0 0 200 m @ 37% 1
28 27 7.72 1.885 2.7 200.0 0 140.0 0 200 m @ 37% 1
29 28 5.535 1.761 2.7 200.0 0 140.0 0 200 m @ 37% 1
30 29 3.325 1.637 2.7 200.0 0 140.0 0 200 m @ 37% 1
31 30 -3.33 1.511 2.7 200.0 0 140.0 0 200 m @ 37% 1
32 31 1.107 1.386 2.7 200.0 0 140.0 0 200 m @ 37% 1
33 32 -5.538 1.258 2.7 200.0 0 140.0 0 200 m @ 37% 1
34 33 -7.726 1.13 0.3 200.0 1 140.0 0 200 m @ 37% 1
35 34 -1.115 1.008 2.7 200.0 0 200.0 1 200 m @ 10% 1
36 35 7.731 0.88 2.7 200.0 0 200.0 1 200 m @ 10% 1
37 36 5.543 0.756 2.7 200.0 0 200.0 1 200 m @ 10% 1
38 37 3.329 0.63 2.7 200.0 0 200.0 1 200 m @ 10% 1
39 38 -3.336 0.505 2.7 200.0 0 200.0 1 200 m @ 10% 1
40 39 1.108 0.379 2.7 200.0 0 200.0 1 200 m @ 10% 1
41 40 -5.547 0.251 0.3 200.0 1 200.0 1 200 m @ 10% 1
42 41 -7.738 0.124 2.7 200.0 0 200.0 1 200 m @ 10% 1
43 42 -1.117 0.0 2.7 200.0 0 200.0 1 200 m @ 10% 1
44 43 7.743 -0.129 2.7 200.0 0 200.0 1 200 m @ 10% 1
45 44 5.551 -0.254 2.7 200.0 0 200.0 1 200 m @ 10% 1
46 45 3.335 -0.38 2.7 200.0 0 200.0 1 200 m @ 10% 1
47 46 -3.342 -0.506 2.7 200.0 0 200.0 1 200 m @ 10% 1
48 47 1.11 -0.632 0.3 200.0 1 200.0 1 200 m @ 10% 1
49 48 -5.555 -0.76 2.7 200.0 0 200.0 1 200 m @ 10% 1
50 49 -7.75 -0.887 2.7 200.0 0 200.0 1 200 m @ 10% 1
51 50 -1.119 -1.012 2.7 200.0 0 200.0 1 200 m @ 10% 1
52 51 7.757 -1.141 2.7 200.0 0 200.0 1 200 m @ 10% 1
53 52 5.56 -1.266 2.7 200.0 0 200.0 1 200 m @ 10% 1
54 53 3.34 -1.393 2.7 200.0 0 200.0 1 200 m @ 10% 1
55 54 -3.347 -1.519 0.3 200.0 1 200.0 1 200 m @ 10% 1
56 55 1.111 -1.646 2.7 200.0 0 200.0 1 200 m @ 10% 1
57 56 -5.564 -1.773 2.7 200.0 0 200.0 1 200 m @ 10% 1
58 57 -7.762 -1.901 2.7 200.0 0 200.0 1 200 m @ 10% 1
59 58 -1.121 -2.027 2.7 200.0 0 200.0 1 200 m @ 10% 1
60 59 7.768 -2.155 2.7 200.0 0 200.0 1 200 m @ 10% 1
61 60 5.569 -2.282 2.7 200.0 0 200.0 1 200 m @ 10% 1
62 61 3.345 -2.409 0.3 200.0 1 200.0 1 200 m @ 10% 1
63 62 -3.353 -2.535 2.7 200.0 0 200.0 1 200 m @ 10% 1
64 63 1.113 -2.663 2.7 200.0 0 200.0 1 200 m @ 10% 1
65 64 -5.573 -2.789 2.7 200.0 0 200.0 1 200 m @ 10% 1
66 65 -7.775 -2.916 2.7 200.0 0 200.0 1 200 m @ 10% 1
67 66 -1.123 -3.044 2.7 200.0 0 140.0 0 200 m @ 37% 1
68 67 7.78 -3.172 2.7 200.0 0 140.0 0 200 m @ 37% 1
69 68 5.578 -3.299 0.3 200.0 1 140.0 0 200 m @ 37% 1
70 69 3.351 -3.425 2.7 200.0 0 140.0 0 200 m @ 37% 1
71 70 -3.358 -3.552 2.7 200.0 0 140.0 0 200 m @ 37% 1
72 71 1.115 -3.68 2.7 200.0 0 140.0 0 200 m @ 37% 1
73 72 -5.582 -3.806 2.7 200.0 0 140.0 0 200 m @ 37% 1
74 73 -7.787 -3.933 2.7 200.0 0 140.0 0 200 m @ 37% 1
75 74 -1.125 -4.062 2.7 200.0 0 140.0 0 200 m @ 37% 1
76 75 7.792 -4.19 0.3 200.0 1 140.0 0 200 m @ 37% 1
77 76 5.586 -4.318 2.7 200.0 0 140.0 0 200 m @ 37% 1
78 77 3.356 -4.444 2.7 200.0 0 140.0 0 200 m @ 37% 1
79 78 -3.363 -4.571 2.7 200.0 0 140.0 0 200 m @ 37% 1
80 79 1.116 -4.699 2.7 200.0 0 140.0 0 200 m @ 37% 1
81 80 -5.591 -4.824 2.7 200.0 0 140.0 0 200 m @ 37% 1
82 81 -7.799 -4.951 2.7 200.0 0 140.0 0 200 m @ 37% 1
83 82 -1.127 -5.081 0.3 200.0 1 140.0 0 200 m @ 37% 1
84 83 7.804 -5.209 2.7 200.0 0 140.0 0 200 m @ 37% 1
85 84 5.595 -5.336 2.7 200.0 0 140.0 0 200 m @ 37% 1
86 85 3.36 -5.463 2.7 200.0 0 140.0 0 200 m @ 37% 1
87 86 -3.369 -5.589 2.7 200.0 0 140.0 0 200 m @ 37% 1
88 87 1.118 -5.718 2.7 200.0 0 140.0 0 200 m @ 37% 1
89 88 -5.599 -5.843 2.7 200.0 0 140.0 0 200 m @ 37% 1
90 89 -7.811 -5.968 2.7 200.0 0 140.0 0 200 m @ 37% 1
91 90 -1.129 -6.1 0.3 100.0 1 140.0 0 100 m @ 3% 0
92 91 -3.374 -6.607 2.7 100.0 0 140.0 0 100 m @ 3% 0
93 92 -1.13 -7.117 2.7 100.0 0 140.0 0 100 m @ 3% 0
94 93 -3.379 -7.624 0.3 100.0 1 140.0 0 100 m @ 3% 0
95 94 -1.132 -8.134 2.7 100.0 0 140.0 0 100 m @ 3% 0
96 95 -3.383 -8.64 2.7 100.0 0 140.0 0 100 m @ 3% 0
97 96 3.381 -9.149 0.3 100.0 1 140.0 0 100 m @ 3% 0
98 97 -3.388 -9.652 2.7 100.0 0 140.0 0 100 m @ 3% 0
99 98 3.386 -10.16 2.7 100.0 0 100.0 0 100 m @ 10% 0
100 99 1.129 -10.665 0.3 100.0 1 100.0 0 100 m @ 10% 0
101 100 3.39 -11.17 2.7 100.0 0 100.0 0 100 m @ 10% 0
102 101 1.129 -11.672 2.7 100.0 0 100.0 0 100 m @ 10% 0
103 102 3.395 -12.174 0.3 100.0 1 100.0 0 100 m @ 10% 0
104 103 1.131 -12.673 2.7 100.0 0 100.0 0 100 m @ 10% 0
105 104 3.401 -13.173 2.7 100.0 0 100.0 0 100 m @ 10% 0
106 105 1.133 -13.67 0.3 100.0 1 100.0 0 100 m @ 10% 0
107 106 3.406 -14.166 2.7 100.0 0 50.0 0 100 m @ 120% 0
108 107 1.135 -14.66 2.7 100.0 0 50.0 0 100 m @ 120% 0
109 108 3.41 -15.154 0.3 100.0 1 50.0 0 100 m @ 120% 0
110 109 1.137 -15.645 2.7 100.0 0 50.0 0 100 m @ 120% 0
111 110 3.416 -16.135 2.7 100.0 0 50.0 0 100 m @ 120% 0
112 111 1.139 -16.622 0.3 100.0 1 50.0 0 100 m @ 120% 0
113 112 -1.142 -17.106 2.7 100.0 0 50.0 0 100 m @ 120% 0
114 113 1.142 -17.592 2.7 100.0 0 50.0 0 100 m @ 120% 0
115 114 -1.143 -18.072 0.3 100.0 1 50.0 0 100 m @ 120% 0
116 115 -3.426 -18.548 2.7 100.0 0 50.0 0 100 m @ 120% 0
117 116 -1.143 -19.03 2.7 100.0 0 25.0 0 100 m @ 1600% 0
118 117 -3.429 -19.501 0.3 100.0 1 25.0 0 100 m @ 1600% 0
119 118 -1.145 -19.978 2.7 100.0 0 25.0 0 100 m @ 1600% 0
120 119 -3.433 -20.445 2.7 100.0 0 25.0 0 100 m @ 1600% 0
121 120 -1.145 -20.918 0.3 100.0 1 25.0 0 100 m @ 1600% 0
122 121 -3.436 -21.379 2.7 100.0 0 25.0 0 100 m @ 1600% 0
123 122 -1.146 -21.848 2.7 100.0 0 25.0 0 100 m @ 1600% 0
124 123 -3.44 -22.304 0.3 100.0 1 25.0 0 100 m @ 1600% 0
125 124 -1.146 -22.768 2.7 100.0 0 25.0 0 100 m @ 1600% 0
126 125 -3.443 -23.219 2.7 100.0 0 25.0 0 100 m @ 1600% 0
127 126 -1.146 -23.678 0.3 100.0 1 25.0 0 100 m @ 1600% 0
128 127 -3.446 -24.123 2.7 100.0 0 25.0 0 100 m @ 1600% 0
129 128 -3.449 -25.016 0.3 100.0 1 25.0 0 100 m @ 1600% 0

219
ros2_ws/src/flyguard/flyguard/descending.py Normal file
View file

@ -0,0 +1,219 @@
"""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
# Смещение в системе лидара: `lateral` отсчитан от оси пути, и в кривой
# рамка по нему стоит в стороне от точек. NaN — неизвестно.
sensor_x: float = float("nan")
@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, far_confirm: int = 0,
far_confirm_from: float = 100.0):
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
# Дальний трек поднимает тревогу, только прожив `far_confirm` кадров.
# Вдали предмет — три-четыре луча, и по одному кадру он неотличим от
# вспышки на кромке или стыке; фантомы на синтетике и в записях жили
# 0.5–1 с и пропадали при подъезде. Торможению дальний трек не нужен
# срочно: до него сотня метров. 0 — выключено. EXPERIMENTS п. 19.
self.far_confirm = far_confirm
self.far_confirm_from = far_confirm_from
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
if (self.far_confirm > 0 and d > self.far_confirm_from
and t.age < self.far_confirm and t.id not in self._latched):
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,
sensor_x=t.u_raw))
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

150
ros2_ws/src/flyguard/flyguard/device.py Normal file
View file

@ -0,0 +1,150 @@
"""Управление вычислительными устройствами (NVIDIA GPU / CUDA и CPU fallback).
Модуль обеспечивает прозрачную работу конвейера FlyGuard на:
- Выделенном NVIDIA GPU (RTX 4070 Ti Super 16GB / Ada Lovelace) через CUDA / Tensor Cores.
- Любых других видеокартах NVIDIA (Ampere, Turing, Pascal, Hopper, Blackwell).
- CPU Linux при отсутствии GPU или в среде жюри (Graceful Fallback).
"""
from __future__ import annotations
import logging
from typing import Any
import numpy as np
logger = logging.getLogger("flyguard.device")
_TORCH_AVAILABLE: bool | None = None
_CUDA_AVAILABLE: bool | None = None
def is_torch_available() -> bool:
"""Проверка наличия установленного пакета PyTorch."""
global _TORCH_AVAILABLE
if _TORCH_AVAILABLE is None:
try:
import torch # noqa: F401
_TORCH_AVAILABLE = True
except ImportError:
_TORCH_AVAILABLE = False
return _TORCH_AVAILABLE
def is_cuda_available() -> bool:
"""Проверка доступности ускорения NVIDIA CUDA."""
global _CUDA_AVAILABLE
if _CUDA_AVAILABLE is None:
if not is_torch_available():
_CUDA_AVAILABLE = False
else:
try:
import torch
_CUDA_AVAILABLE = bool(torch.cuda.is_available() and torch.cuda.device_count() > 0)
except Exception as e:
logger.warning("Проверка CUDA завершилась ошибкой: %s. Fallback на CPU.", e)
_CUDA_AVAILABLE = False
return _CUDA_AVAILABLE
def notify_cuda_error(exc: Exception | None = None) -> None:
"""Зафиксировать сбой CUDA в рантайме и принудительно перевести систему в режим CPU fallback.
Вызывается, если во время работы на GPU произошёл OOM, таймаут или сбой драйвера.
Последующие вызовы конвейера будут прозрачно исполняться на CPU.
"""
global _CUDA_AVAILABLE
_CUDA_AVAILABLE = False
logger.warning("Зафиксирован сбой GPU в рантайме (%s). Выполнен необратимый Fallback на CPU.", exc)
def reset_device_cache() -> None:
"""Сбросить кэш состояния устройств (для юнит-тестов)."""
global _TORCH_AVAILABLE, _CUDA_AVAILABLE
_TORCH_AVAILABLE = None
_CUDA_AVAILABLE = None
def get_device(preferred: str = "auto") -> str:
"""Выбрать вычислительное устройство с автоматическим fallback на CPU.
Args:
preferred: 'auto' (выбрать cuda при наличии, иначе cpu), 'cuda', 'cuda:0', 'cpu'.
Returns:
Строка устройства: 'cuda', 'cuda:X' или 'cpu'.
"""
pref = (preferred or "auto").strip().lower()
if pref == "auto":
return "cuda" if is_cuda_available() else "cpu"
if pref.startswith("cuda"):
if is_cuda_available():
return pref
logger.warning("Запрошено устройство '%s', но CUDA недоступна. Выполнен fallback на CPU.", pref)
return "cpu"
return "cpu"
def get_device_info(device_str: str | None = None) -> dict[str, Any]:
"""Сводная информация об аппаратных ресурсах для инспекции и логирования."""
dev = get_device(device_str or "auto")
info: dict[str, Any] = {
"device": dev,
"torch_available": is_torch_available(),
"cuda_available": is_cuda_available(),
}
if is_torch_available() and is_cuda_available() and dev.startswith("cuda"):
import torch
dev_idx = 0
if ":" in dev:
try:
dev_idx = int(dev.split(":")[1])
except ValueError:
dev_idx = 0
props = torch.cuda.get_device_properties(dev_idx)
info.update({
"name": props.name,
"total_memory_mb": round(props.total_memory / (1024 * 1024), 2),
"major": props.major,
"minor": props.minor,
"multi_processor_count": props.multi_processor_count,
"allocated_mb": round(torch.cuda.memory_allocated(dev_idx) / (1024 * 1024), 2),
"reserved_mb": round(torch.cuda.memory_reserved(dev_idx) / (1024 * 1024), 2),
"cuda_version": torch.version.cuda,
"cudnn_version": torch.backends.cudnn.version() if torch.backends.cudnn.is_available() else None,
})
else:
import platform
info.update({
"name": platform.processor() or "CPU",
"python_version": platform.python_version(),
})
return info
def log_device_info(device_str: str | None = None) -> None:
"""Вывести в лог/консоль активное вычислительное окружение."""
info = get_device_info(device_str)
if info.get("cuda_available") and str(info["device"]).startswith("cuda"):
logger.info("FlyGuard GPU Active: %s (VRAM: %.1f MB, CUDA: %s, SM: %d.%d)",
info.get("name"), info.get("total_memory_mb", 0.0),
info.get("cuda_version"), info.get("major", 0), info.get("minor", 0))
else:
logger.info("FlyGuard CPU Mode (Fallback): %s", info.get("name"))
def empty_cache() -> None:
"""Очистить кэш памяти CUDA для предотвращения фрагментации VRAM."""
if is_cuda_available():
import torch
torch.cuda.empty_cache()
def to_numpy(arr: Any) -> np.ndarray:
"""Преобразовать входной тензор (PyTorch или NumPy) в numpy.ndarray."""
if isinstance(arr, np.ndarray):
return arr
if is_torch_available():
import torch
if isinstance(arr, torch.Tensor):
return arr.detach().cpu().numpy()
return np.asarray(arr)

303
ros2_ws/src/flyguard/flyguard/export.py Normal file
View file

@ -0,0 +1,303 @@
"""ROS2 и визуализационный экспорт решений FlyGuard.
Преобразует внутренние результаты конвейера (Track, Decision, RailPlane, Corridor)
в стандартизованные 3D Bounding Boxes, вектор угроз и структуры MarkerArray для RViz.
Работает автономно на чистом Python + NumPy, не требуя обязательной установки
библиотек rclpy / ros2 на стенде валидации. При наличии ROS2 может конвертировать
напрямую в сообщения visualization_msgs и vision_msgs.
"""
from __future__ import annotations
from dataclasses import asdict, dataclass, field
from enum import IntEnum
import math
import numpy as np
from .descending import Decision, DetectedObject
from .geometry import Corridor, RailPlane
class ThreatLevel(IntEnum):
"""Уровень опасности для системы автоведения поезда."""
CLEAR = 0 # Путь свободен
WARNING = 1 # Заблаговременное предупреждение (DNp02/DNp11 soft-warning)
EMERGENCY = 2 # Экстренное торможение (DNp01 / Giant Fiber)
@dataclass
class BoundingBox3D:
"""3D ориентированный параллелепипед в координатах сенсора лидара."""
# Центр бокса в системе сенсора (x: вправо, y: вперёд (-d), z: вверх)
x: float
y: float
z: float
# Размеры бокса (м)
dx: float # ширина поперёк пути
dy: float # протяжённость вдоль пути
dz: float # высота
# Ориентация (рыскание относительно оси лидара, рад)
yaw: float
# Метрики движения и трекинга
distance_along_track: float
lateral_offset: float
height_above_rail: float
confidence: float
novelty: float
ttc: float
track_id: int
threat_level: ThreatLevel
def to_dict(self) -> dict:
d = asdict(self)
d["threat_level"] = self.threat_level.name
return d
@dataclass
class ExportResult:
"""Полный экспортный пакет за один кадр."""
stamp: float
threat_level: ThreatLevel
nearest_distance: float
ttc: float
stopping_distance: float
speed_mps: float
speed_kmh: float
boxes: list[BoundingBox3D] = field(default_factory=list)
corridor_points_xyz: list[tuple[float, float, float]] = field(default_factory=list)
def to_dict(self) -> dict:
return {
"stamp": self.stamp,
"threat_level": self.threat_level.name,
"threat_code": int(self.threat_level),
"nearest_distance": self.nearest_distance,
"ttc": self.ttc,
"stopping_distance": self.stopping_distance,
"speed_kmh": self.speed_kmh,
"n_objects": len(self.boxes),
"boxes": [b.to_dict() for b in self.boxes],
"corridor_points": self.corridor_points_xyz,
}
def to_rviz_markers(self, frame_id: str = "hesai_pandar") -> list[dict]:
"""Генерация словарей, готовых для преобразования в visualization_msgs/Marker."""
markers = []
now_sec = int(self.stamp)
now_nanosec = int((self.stamp - now_sec) * 1e9)
# 1. Линия коридора пути (LINE_STRIP, type 4)
if self.corridor_points_xyz:
markers.append({
"header": {"frame_id": frame_id, "sec": now_sec, "nanosec": now_nanosec},
"ns": "flyguard_corridor",
"id": 0,
"type": 4, # LINE_STRIP
"action": 0, # ADD
"scale": {"x": 0.12},
"color": {"r": 0.2, "g": 0.8, "b": 1.0, "a": 0.8},
"points": [{"x": p[0], "y": p[1], "z": p[2]} for p in self.corridor_points_xyz]
})
# 2. Bounding boxes объектов (CUBE, type 1) и надписи (TEXT, type 9)
for i, box in enumerate(self.boxes):
if box.threat_level == ThreatLevel.EMERGENCY:
color = {"r": 1.0, "g": 0.1, "b": 0.1, "a": 0.75} # Красный
elif box.threat_level == ThreatLevel.WARNING:
color = {"r": 1.0, "g": 0.85, "b": 0.0, "a": 0.65} # Жёлтый
else:
color = {"r": 0.2, "g": 0.8, "b": 0.2, "a": 0.50} # Зелёный
# Кватернион поворота вокруг оси Z (yaw)
cy = math.cos(box.yaw * 0.5)
sy = math.sin(box.yaw * 0.5)
# CUBE маркер
markers.append({
"header": {"frame_id": frame_id, "sec": now_sec, "nanosec": now_nanosec},
"ns": "flyguard_bboxes",
"id": box.track_id * 2,
"type": 1, # CUBE
"action": 0,
"pose": {
"position": {"x": box.x, "y": box.y, "z": box.z},
"orientation": {"x": 0.0, "y": 0.0, "z": sy, "w": cy}
},
"scale": {"x": max(box.dx, 0.2), "y": max(box.dy, 0.2), "z": max(box.dz, 0.2)},
"color": color
})
# TEXT_VIEW_FACING над объектом
ttc_str = f"{box.ttc:.1f}s" if math.isfinite(box.ttc) else "inf"
label = f"ID:{box.track_id} | {box.distance_along_track:.1f}m | TTC:{ttc_str}"
markers.append({
"header": {"frame_id": frame_id, "sec": now_sec, "nanosec": now_nanosec},
"ns": "flyguard_labels",
"id": box.track_id * 2 + 1,
"type": 9, # TEXT_VIEW_FACING
"action": 0,
"pose": {
"position": {"x": box.x, "y": box.y, "z": box.z + box.dz * 0.5 + 0.35},
"orientation": {"x": 0.0, "y": 0.0, "z": 0.0, "w": 1.0}
},
"scale": {"z": 0.40}, # Высота шрифта
"color": {"r": 1.0, "g": 1.0, "b": 1.0, "a": 0.95},
"text": label
})
return markers
def export_frame(decision: Decision, plane: RailPlane | None, corridor: Corridor | None,
stamp: float = 0.0) -> ExportResult:
"""Сконвертировать решение FlyGuard в экспортный формат.
Parameters
----------
decision : Decision
Итоговый вердикт системы за кадр.
plane : RailPlane, optional
Плоскость головок рельсов (z = a·d + b·u + c).
corridor : Corridor, optional
Осевая линия тоннеля (парабола u(d)).
stamp : float
Временная метка кадра.
"""
if decision.emergency:
threat = ThreatLevel.EMERGENCY
elif decision.detected:
threat = ThreatLevel.WARNING
else:
threat = ThreatLevel.CLEAR
# Параметры плоскости пути: z = a*d + b*u + c
a = plane.a if plane is not None else 0.0
b = plane.b if plane is not None else 0.0
c = plane.c if plane is not None else -1.80 # высота лидара над рельсами ~1.8м
boxes: list[BoundingBox3D] = []
for obj in decision.objects:
d = obj.distance
u = obj.lateral
h = obj.height
# Координаты в системе сенсора (x: вправо, y: вперёд (-d), z: вверх).
#
# Боковое смещение трека отсчитано от ОСИ ПУТИ, а не от оси сенсора
# (TrackFrame.lateral), поэтому в кривой к нему прибавляется положение
# оси на этой дальности: при радиусе 1300 м это 1.2 м на 55 м и 8.6 м
# на 150 м — без поправки рамка рисовалась в стене. Направление оси
# пути в кадре — (наклон, −1), и длинная ось рамки (её локальная y)
# совпадает с ним при повороте на +atan(наклон). Проверка обоих —
# test_export_box_follows_a_curved_track.
#
# Если трек знает своё смещение в системе лидара (`sensor_x`), берётся
# оно: габарит — объединение прямого и изогнутого, и у предмета,
# попавшего в прямой, `u` отсчитан не от кривой, а от оси лидара.
sx = float(getattr(obj, "sensor_x", float("nan")))
if corridor is not None and corridor.n_slices > 0:
x_sensor = (sx if math.isfinite(sx)
else float(u + corridor.centre(np.array([d], np.float32))[0]))
c0, c1, c2 = corridor.coef
dm = max(corridor.d_max_seen, 1.0)
d_in = min(d, dm)
slope = c1 + 2.0 * c2 * d_in
yaw = float(math.atan(slope))
else:
x_sensor = sx if math.isfinite(sx) else float(u)
yaw = 0.0
y_sensor = float(-d)
z_sensor = float(h + a * d + b * x_sensor + c)
# Уровень опасности для конкретного объекта
if decision.emergency and d <= max(decision.stopping_distance, 25.0):
obj_threat = ThreatLevel.EMERGENCY
elif decision.detected:
obj_threat = ThreatLevel.WARNING
else:
obj_threat = ThreatLevel.CLEAR
# Размеры: dx поперёк пути, dy вдоль пути, dz по вертикали
dx = float(max(obj.width, 0.35))
# протяжённость вдоль пути трек не хранит, поэтому она постоянная
dy = 0.50
dz = float(max(obj.size_v, 0.40))
boxes.append(BoundingBox3D(
x=x_sensor,
y=y_sensor,
z=z_sensor,
dx=dx,
dy=dy,
dz=dz,
yaw=yaw,
distance_along_track=float(d),
lateral_offset=float(u),
height_above_rail=float(h),
confidence=float(obj.confidence),
novelty=float(obj.novelty),
ttc=float(obj.ttc),
track_id=int(obj.track_id),
threat_level=obj_threat,
))
# Траектория коридора вперед (на 10..180 м)
corridor_pts: list[tuple[float, float, float]] = []
if corridor is not None:
ds_sample = np.linspace(10.0, min(max(corridor.d_max_seen, 50.0), 180.0), 25)
us_sample = corridor.centre(ds_sample)
for ds_i, us_i in zip(ds_sample, us_sample):
xs = float(us_i)
ys = float(-ds_i)
zs = float(a * ds_i + b * us_i + c + 0.1) # чуть над рельсом
corridor_pts.append((xs, ys, zs))
speed = decision.speed
return ExportResult(
stamp=stamp,
threat_level=threat,
nearest_distance=float(decision.distance),
ttc=float(decision.ttc),
stopping_distance=float(decision.stopping_distance),
speed_mps=float(speed),
speed_kmh=float(speed * 3.6),
boxes=boxes,
corridor_points_xyz=corridor_pts
)
def gauge_outline(boxes, centre=None, step: float = 2.0,
frame_every: float = 20.0) -> list[tuple[float, float, float]]:
"""Отрезки контура габарита (пары точек для LINE_LIST) в координатах облака обзора.
`boxes` — секции `(полуширина, низ, верх, от, до)`; `centre(d)` — ось пути
(`Corridor.centre`), None — прямой короб вдоль x = 0. Облако обзора в кривой
не выпрямлено, поэтому габарит вдоль изогнутой оси рисуется изогнутым —
там, где узел его и проверяет. Вперёд = −y, вправо = +x, вверх = +z над
головкой рельса, как у облака обзора. Рамки поперёк — через `frame_every`.
"""
pts: list[tuple[float, float, float]] = []
for hw, z0, z1, d0, d1 in boxes:
if d1 <= d0:
continue
n = 1 if centre is None else max(int(np.ceil((d1 - d0) / step)), 1)
ds = np.linspace(d0, d1, n + 1)
cx = np.zeros_like(ds) if centre is None else np.asarray(centre(ds), np.float64)
corners = [(-hw, z0), (hw, z0), (hw, z1), (-hw, z1)]
for x, z in corners: # вдоль пути
for a in range(n):
pts.append((float(cx[a] + x), float(-ds[a]), float(z)))
pts.append((float(cx[a + 1] + x), float(-ds[a + 1]), float(z)))
fd = np.arange(d0, d1 + 1e-6, frame_every) # поперёк
fc = np.zeros_like(fd) if centre is None else np.asarray(centre(fd), np.float64)
for dd, c in zip(fd, fc):
for (xa, za), (xb, zb) in zip(corners, corners[1:] + corners[:1]):
pts.append((float(c + xa), float(-dd), float(za)))
pts.append((float(c + xb), float(-dd), float(zb)))
return pts

180
ros2_ws/src/flyguard/flyguard/fan_body.py Normal file
View file

@ -0,0 +1,180 @@
"""FAN-SHAPED BODY — накопление слабых улик в координатах пути.
Покадровое решение проваливается там, где предмет даёт единицы лучей. На 170 м
человек освещён **в каждом** кадре, но всего пятью лучами: формы из них не
построить, а локальный контраст на такой дальности обнуляется — кольцо
окружения ламины упирается в стену тоннеля, которая на той же дальности.
Зато предмет неподвижен в мире, а тоннель проплывает мимо: за проход одни и те
же пять лучей попадают в одну и ту же точку пространства семнадцать раз подряд.
Именно так устроено веерное тело центрального комплекса мухи: оно копит
вектор к цели в координатах, привязанных к миру, а не к текущему кадру, и
достаёт из слабого повторяющегося сигнала то, чего нет ни в одном отдельном
наблюдении.
Здесь то же самое буквально: сетка, привязанная к пройденному пути, в которую
кадр за кадром складываются лучи из габарита. Сетка сдвигается на пройденное
расстояние, поэтому неподвижный предмет всегда попадает в одну ячейку, а шум
и случайные отражения размазываются.
Своих кандидатов накопитель не порождает — это проверено и отвергнуто
измерением: по геометрии накопленное скопление предмета и накопленный кусок
конструкции тоннеля неразличимы (совпадают все десять признаков, от дальности
до протяжённости). Разделяет их только память тоннеля, а ей нужны признаки
кадра, которых у скопления нет.
Роль накопителя другая и точная: он отвечает про **уже найденного покадрового
кандидата**, возвращались ли лучи из этой точки мира кадр за кадром. На
больших дальностях это единственная доступная улика — локальный контраст там
структурно равен нулю, потому что кольцо окружения ламины упирается в стену
тоннеля, стоящую на той же дальности.
Стоимость — доли миллисекунды: пара тысяч лучей в `bincount` по сетке из
пятнадцати тысяч ячеек.
"""
from __future__ import annotations
from dataclasses import dataclass
import numpy as np
from scipy import ndimage
# Шаг сетки вдоль пути крупный намеренно: оценка собственного движения копит
# ошибку около 3 %, за семнадцать кадров это уже 0.7 м. Мелкая ячейка размазала
# бы предмет по соседям и убила весь смысл накопления. Для отчёта о дистанции
# 2 м всё равно на порядок точнее допуска (12 % дальности).
DS_BIN = 2.0 # м вдоль пути
DU_BIN = 0.20 # м поперёк
DH_BIN = 0.25 # м по высоте
DECAY = 0.97 # забывание: полураспад около 23 кадров
MIN_FRAMES = 6 # меньше — это вспышка, а не предмет
MAX_SPAN_S = 8.0 # м: длиннее — это стена или лоток, а не предмет
MAX_WIDTH = 1.5 # м: шире — край габарита, а не предмет
BG_WIN_M = 24.0 # м: окно оценки фона вдоль пути
@dataclass
class Accumulated:
"""Скопление попаданий в одной точке пути."""
d: float
u: float
h: float
h_min: float
width: float
height: float
span_s: float
hits: float
frames: float
support: float # попаданий относительно ожидаемого для этой дальности
class FanBody:
"""Сетка в координатах пути, копящая лучи из габарита."""
def __init__(self, *, d_max: float = 220.0, half_width: float = 1.6,
h_lo: float = 0.28, h_hi: float = 2.3,
d_near: float = 55.0, min_support: float = 0.8,
ref_rays: float = 1.4, ref_d: float = 175.0):
self.ds, self.du, self.dh = DS_BIN, DU_BIN, DH_BIN
self.h_lo, self.h_hi = h_lo, h_hi
self.half_width = half_width
self.d_near = d_near # ближе этого покадровый тракт и так справляется
self.min_support = min_support
# Сколько попаданий ждать от настоящего предмета на данной дальности.
# Калибровано по замеру: человек, вставленный в реальный проезд, даёт
# после вычитания фона и разброса по ячейкам эквивалент 1.4 луча в
# кадре на 175 м (пять лучей приходят, но часть уходит в фон и в
# соседние ячейки). Число лучей падает как 1/R², накопление держит
# около 1/(1−DECAY) кадров.
self.k_expect = ref_rays * ref_d ** 2 / (1.0 - DECAY)
self.n_s = int(np.ceil(d_max / self.ds)) + 1
self.n_u = int(np.ceil(2 * half_width / self.du)) + 1
self.n_h = int(np.ceil((h_hi - h_lo) / self.dh)) + 1
self.hits = np.zeros((self.n_s, self.n_u, self.n_h), np.float32)
self.seen = np.zeros((self.n_s, self.n_u), np.float32)
self._off = 0.0 # смещение начала сетки внутри ячейки, м
# ------------------------------------------------------------------ такт
def update(self, d: np.ndarray, u: np.ndarray, h: np.ndarray, ds: float) -> None:
"""Сдвинуть сетку на пройденное `ds` и досыпать лучи текущего кадра."""
self._advance(ds)
if d.size == 0:
return
si = ((self._off + d) / self.ds).astype(np.int32)
ui = ((u + self.half_width) / self.du).astype(np.int32)
hi = ((h - self.h_lo) / self.dh).astype(np.int32)
ok = ((si >= 0) & (si < self.n_s) & (ui >= 0) & (ui < self.n_u)
& (hi >= 0) & (hi < self.n_h))
if not ok.any():
return
flat = (si[ok] * self.n_u + ui[ok]) * self.n_h + hi[ok]
self.hits += np.bincount(flat, minlength=self.hits.size).reshape(self.hits.shape)
# «Кадров подряд» считается по столбцу, а не по ячейке высоты: предмет
# может качнуться на четверть метра, и это не повод обнулять счёт.
col = np.unique(si[ok] * self.n_u + ui[ok])
self.seen.ravel()[col] += 1.0
def _advance(self, ds: float) -> None:
self.hits *= DECAY
self.seen *= DECAY
if ds <= 0:
return
self._off += ds
k = int(self._off // self.ds)
if k <= 0:
return
self._off -= k * self.ds
if k >= self.n_s:
self.hits[:] = 0.0
self.seen[:] = 0.0
return
self.hits[:-k] = self.hits[k:]
self.hits[-k:] = 0.0
self.seen[:-k] = self.seen[k:]
self.seen[-k:] = 0.0
# ------------------------------------------------------------------ съём
def support_at(self, d: np.ndarray, u: np.ndarray) -> np.ndarray:
"""Опора накопителя в точках пути: во сколько раз попаданий больше фона.
Своих кандидатов накопитель не порождает намеренно. Замер показал, что
по геометрии накопленное скопление предмета и накопленный кусок
конструкции тоннеля неразличимы: совпадают дальность, смещение, высота,
ширина, протяжённость — всё. Разделяет их только память тоннеля, а она
работает с признаками кадра (контраст, интенсивность, тень), которых у
скопления нет.
Поэтому накопитель отвечает на один вопрос про уже найденного
покадрового кандидата: **возвращались ли лучи из этой точки мира кадр
за кадром**. На больших дальностях это единственная доступная улика:
локальный контраст там структурно равен нулю, потому что кольцо
окружения упирается в стену тоннеля на той же дальности.
"""
if d.size == 0:
return np.zeros(0, np.float32)
col = self.hits.sum(axis=2)
win = max(int(BG_WIN_M / self.ds) | 1, 3)
base = ndimage.median_filter(col, size=(win, 1), mode="nearest")
resid = np.maximum(col - base, 0.0)
si = np.clip(((self._off + d) / self.ds).astype(np.int32), 0, self.n_s - 1)
ui = np.clip(((u + self.half_width) / self.du).astype(np.int32), 0, self.n_u - 1)
# окно ±1 ячейка: предмет шире одной ячейки, а оценка пути слегка плывёт
tot = np.zeros(d.size, np.float32)
frames = np.zeros(d.size, np.float32)
for ds_ in (-1, 0, 1):
for du_ in (-1, 0, 1):
a = np.clip(si + ds_, 0, self.n_s - 1)
b = np.clip(ui + du_, 0, self.n_u - 1)
tot += resid[a, b]
frames = np.maximum(frames, self.seen[a, b])
expect = self.k_expect / np.maximum(d, 1.0) ** 2
sup = tot / np.maximum(expect, 1e-6)
# без нескольких кадров подряд это не улика, а вспышка
return np.where(frames > MIN_FRAMES, sup, 0.0).astype(np.float32)

324
ros2_ws/src/flyguard/flyguard/geometry.py Normal file
View file

@ -0,0 +1,324 @@
"""Система координат пути, плоскость рельсов и «ожидаемая дальность до пола».
Соответствие мухе — **жужжальца и оцеллии**. Прежде чем обрабатывать изображение,
муха стабилизирует взгляд: жужжальца дают угловые скорости, оцеллии — направление
на горизонт, и голова доворачивается так, чтобы зрительный мир не «плавал».
Здесь роль горизонта играет плоскость пути: она оценивается по самим данным
в каждом кадре, поэтому крепление сенсора не обязано быть жёстким, а качка
вагона не превращается в ложные срабатывания.
Ключевая величина дальше по конвейеру — **ожидаемая дальность до пола** для
каждого луча. Луч с отрицательной элевацией, если ему ничто не мешает, обязан
закончиться на плоскости пути на строго определённом расстоянии. Всё, что
обрывает его раньше, — предмет, стоящий на пути. Это даёт детектор, не зависящий
от абсолютного размера объекта и работающий на любой дальности.
Система координат пути (используется во всём проекте):
d — вперёд по ходу движения, м (в кадре сенсора это −y)
u — поперёк, вправо, м (в кадре сенсора это x)
h — вверх от плоскости рельсов, м
"""
from __future__ import annotations
from dataclasses import dataclass
import numpy as np
from .retina import RangeImage, ScanLayout
@dataclass(frozen=True)
class RailPlane:
"""Плоскость головок рельсов в системе сенсора: z = a·d + b·u + c."""
a: float # тангаж: подъём плоскости с расстоянием
b: float # крен: наклон плоскости поперёк
c: float # −высота сенсора над путём (c < 0)
inliers: int
rms: float
@property
def height(self) -> float:
"""Высота сенсора над головкой рельса, м."""
return -self.c
@property
def pitch_deg(self) -> float:
return float(np.degrees(np.arctan(self.a)))
@property
def roll_deg(self) -> float:
return float(np.degrees(np.arctan(self.b)))
def height_of(self, d: np.ndarray, u: np.ndarray, z: np.ndarray) -> np.ndarray:
"""Высота точек над плоскостью пути."""
return z - (self.a * d + self.b * u + self.c)
def floor_range(self, layout: ScanLayout) -> np.ndarray:
"""Дальность, на которой каждый луч упёрся бы в плоскость пути.
Луч r·(dx, dy, dz); подстановка в уравнение плоскости даёт
r = c / (dz + a·dy − b·dx). Лучи, уходящие вверх или параллельно
плоскости, получают +inf.
"""
dx = layout.dirs[..., 0]
dy = layout.dirs[..., 1]
dz = layout.dirs[..., 2]
denom = dz + self.a * dy - self.b * dx
with np.errstate(divide="ignore", invalid="ignore"):
r = self.c / denom
return np.where((denom < -1e-6) & np.isfinite(r), r, np.float32(np.inf)).astype(np.float32)
DEFAULT_PLANE = RailPlane(a=0.0, b=0.0, c=-2.19, inliers=0, rms=0.0)
def fit_rail_plane(img: RangeImage, layout: ScanLayout, *,
d_min: float = 6.0, d_max: float = 45.0,
u_max: float = 1.9, cell_d: float = 1.0, cell_u: float = 0.25,
iters: int = 4, prev: RailPlane | None = None,
smooth: float = 0.25) -> RailPlane:
"""Робастная оценка плоскости пути по ближней зоне.
В каждой ячейке сетки (d, u) остаётся только самая низкая точка — это
отсекает шпалы, кабельные лотки и всё, что стоит на полотне. Затем идут
итерации перевзвешенных наименьших квадратов с мягкой функцией Хьюбера,
после чего подгонка повторяется уже только по точкам у самой плоскости.
`smooth` задаёт постоянную времени экспоненциального сглаживания по кадрам:
плоскость пути физически не может прыгать, и сглаживание играет ту же роль,
что обратная связь от жужжалец, — гасит дрожание оценки.
"""
xyz = img.xyz(layout)
x, y, z = xyz[..., 0], xyz[..., 1], xyz[..., 2]
d = -y
sel = img.valid & (d > d_min) & (d < d_max) & (np.abs(x) < u_max)
if sel.sum() < 200:
return prev or DEFAULT_PLANE
dv = d[sel].astype(np.float64)
uv = x[sel].astype(np.float64)
zv = z[sel].astype(np.float64)
# самая низкая точка в каждой ячейке — грубое выделение полотна
ci = ((dv - d_min) / cell_d).astype(np.int64)
cj = ((uv + u_max) / cell_u).astype(np.int64)
key = ci * 10_000 + cj
order = np.lexsort((zv, key))
key_s = key[order]
first = np.ones(key_s.size, bool)
first[1:] = key_s[1:] != key_s[:-1]
idx = order[first]
if idx.size < 40:
return prev or DEFAULT_PLANE
dd, uu, zz = dv[idx], uv[idx], zv[idx]
coef = _irls_plane(dd, uu, zz, iters)
if coef is None:
return prev or DEFAULT_PLANE
# второй проход: только точки у найденной плоскости, уже без отбора минимумов
res_all = zv - (coef[0] * dv + coef[1] * uv + coef[2])
near = np.abs(res_all) < 0.18
if near.sum() > 300:
c2 = _irls_plane(dv[near], uv[near], zv[near], iters)
if c2 is not None:
coef = c2
res = zv - (coef[0] * dv + coef[1] * uv + coef[2])
keep = np.abs(res) < 0.2
plane = RailPlane(a=float(coef[0]), b=float(coef[1]), c=float(coef[2]),
inliers=int(keep.sum()),
rms=float(np.sqrt(np.mean(res[keep] ** 2))) if keep.any() else 9.9)
# защита от вырождения: высота сенсора над путём физически ограничена
if not (0.5 < plane.height < 5.0) or abs(plane.pitch_deg) > 8 or abs(plane.roll_deg) > 8:
return prev or DEFAULT_PLANE
if prev is not None and smooth > 0:
k = smooth
plane = RailPlane(a=k * plane.a + (1 - k) * prev.a,
b=k * plane.b + (1 - k) * prev.b,
c=k * plane.c + (1 - k) * prev.c,
inliers=plane.inliers, rms=plane.rms)
return plane
def _irls_plane(d: np.ndarray, u: np.ndarray, z: np.ndarray, iters: int):
"""z ≈ a·d + b·u + c с мягким Хьюбером."""
A = np.stack([d, u, np.ones_like(d)], axis=1)
w = np.ones_like(z)
coef = np.array([0.0, 0.0, float(np.median(z))])
for _ in range(iters):
try:
coef, *_ = np.linalg.lstsq(A * w[:, None], z * w, rcond=None)
except np.linalg.LinAlgError:
return None
res = z - A @ coef
s = 1.4826 * np.median(np.abs(res - np.median(res))) + 1e-3
w = 1.0 / np.sqrt(1.0 + (res / (2.0 * s)) ** 2)
return coef
@dataclass
class Corridor:
"""Осевая линия пути впереди: u_c(d) = c0 + c1·d + c2·d².
Оценивается по дрейфу центра сечения тоннеля с расстоянием. В прямом
тоннеле c1 ≈ c2 ≈ 0; в кривой радиуса R член c2 ≈ 1/(2R). Нужна, чтобы
габарит на 150 м впереди не «въезжал» в стену на повороте — иначе вся
дальняя зона кривой превращается в сплошное ложное срабатывание.
"""
coef: np.ndarray # (3,)
d_max_seen: float # дальше этого — экстраполяция
n_slices: int
radius: float # оценка радиуса кривой, м (inf для прямой)
def centre(self, d: np.ndarray) -> np.ndarray:
"""Ось пути на дальности d.
За пределами наблюдавшейся дальности парабола продолжается **линейно**,
по касательной: экстраполировать кривизну туда, где данных не было,
значит получить десятки метров ошибки на ровном месте.
"""
d = np.asarray(d, np.float32)
c0, c1, c2 = self.coef
dm = np.float32(max(self.d_max_seen, 1.0))
d_in = np.minimum(d, dm)
u = c0 + c1 * d_in + c2 * d_in * d_in
slope = c1 + 2.0 * c2 * dm
return (u + slope * np.maximum(d - dm, 0.0)).astype(np.float32)
def sigma(self, d: np.ndarray, base: float = 0.25, rate: float = 0.004) -> np.ndarray:
"""Неопределённость положения оси: растёт с дальностью и за горизонтом видимости."""
d = np.asarray(d, np.float32)
extra = np.maximum(d - np.float32(self.d_max_seen), 0.0)
return (base + rate * d + 0.02 * extra).astype(np.float32)
STRAIGHT = Corridor(np.zeros(3), 0.0, 0, float("inf"))
MIN_TRACK_RADIUS = 300.0 # м, круче на перегонах метрополитена не бывает
MAX_AXIS_RATE = 0.35 # м за кадр, предел изменения оси на дальности 100 м
def fit_corridor(tf: "TrackFrame", *, d_lo: float = 8.0, d_hi: float = 220.0,
n_slices: int = 30, h_lo: float = 0.6, h_hi: float = 3.2,
min_pts: int = 60, d_ref: float = 22.0,
prev: Corridor | None = None, smooth: float = 0.08) -> Corridor:
"""Оценить осевую линию пути по смещению центра сечения тоннеля.
Каждый срез по дальности даёт одну оценку центра свода. Веса берутся
**равными по срезам**, а не по числу точек: у ближних срезов точек в сотни
раз больше, и взвешивание по количеству полностью подавило бы дальние срезы,
в которых как раз и содержится кривизна.
"""
edges = np.geomspace(d_lo, d_hi, n_slices + 1)
ds, us = [], []
for lo, hi in zip(edges[:-1], edges[1:]):
m = tf.valid & (tf.d >= lo) & (tf.d < hi) & (tf.h > h_lo) & (tf.h < h_hi)
if int(m.sum()) < min_pts:
continue
uu = tf.u[m]
lo_u, hi_u = np.percentile(uu, (3.0, 97.0))
if hi_u - lo_u < 1.5: # видна только одна стена — центр не определить
continue
ds.append(0.5 * (lo + hi))
us.append(0.5 * (lo_u + hi_u))
if len(ds) < 5:
return prev or STRAIGHT
d = np.asarray(ds, np.float64)
u = np.asarray(us, np.float64)
# положение поезда в сечении: медиана центров ближней зоны
ref = d <= d_ref
u = u - (np.median(u[ref]) if ref.sum() >= 2 else u[0])
far = d > 12.0
if far.sum() < 4:
return prev or STRAIGHT
d_f, u_f = d[far], u[far]
# u ≈ c1·d + c2·d², равные веса по срезам, две итерации робастного отсева
A = np.stack([d_f, d_f * d_f], axis=1)
w = np.ones_like(u_f)
c = np.zeros(2)
for _ in range(3):
try:
c, *_ = np.linalg.lstsq(A * w[:, None], u_f * w, rcond=None)
except np.linalg.LinAlgError:
return prev or STRAIGHT
res = u_f - A @ c
s = 1.4826 * np.median(np.abs(res - np.median(res))) + 0.05
w = 1.0 / np.sqrt(1.0 + (res / (2.0 * s)) ** 2)
coef = np.array([0.0, c[0], c[1]])
radius = float(abs(1.0 / (2.0 * c[1]))) if abs(c[1]) > 1e-7 else float("inf")
# Радиус круче 300 м на перегоне метрополитена не встречается. Такая оценка
# означает не кривую, а испорченное сечение: на станции платформа делает свод
# резко несимметричным, центр «уезжает», и габарит вместе с ним заезжает
# прямо на платформу — источник почти всех ложных тревог у станций.
if radius < MIN_TRACK_RADIUS:
return prev or STRAIGHT
out = Corridor(coef, float(d.max()), len(ds), radius)
if prev is None or smooth <= 0:
return out
k = smooth
blended = k * out.coef + (1 - k) * prev.coef
# путь физически не может вильнуть: ограничиваем скорость изменения оси
shift_now = blended[1] * 100.0 + blended[2] * 100.0 ** 2
shift_prev = prev.coef[1] * 100.0 + prev.coef[2] * 100.0 ** 2
excess = abs(shift_now - shift_prev)
if excess > MAX_AXIS_RATE:
t = MAX_AXIS_RATE / excess
blended = prev.coef + (blended - prev.coef) * t
r2 = (float(abs(1.0 / (2.0 * blended[2]))) if abs(blended[2]) > 1e-7 else float("inf"))
return Corridor(blended, out.d_max_seen, out.n_slices, r2)
class TrackFrame:
"""Кадр в координатах пути: (d, u, h) плюс ожидаемая дальность до пола."""
__slots__ = ("d", "u", "h", "z", "r", "valid", "inten", "floor_r", "plane",
"layout", "img")
def __init__(self, img: RangeImage, layout: ScanLayout, plane: RailPlane):
xyz = img.xyz(layout)
self.layout = layout
self.img = img
self.plane = plane
self.u = xyz[..., 0]
self.d = -xyz[..., 1]
self.z = xyz[..., 2] # в системе сенсора, не над рельсом
self.h = plane.height_of(self.d, self.u, self.z)
self.r = img.r_near
self.valid = img.valid
self.inten = img.inten
self.floor_r = plane.floor_range(layout)
def lateral(self, corridor: "Corridor | None" = None) -> np.ndarray:
"""Смещение точек от осевой линии пути, м.
Берётся **меньшее по модулю** из двух: отсчёт от прямой оси и от
оценённой кривой. Это объединение двух габаритов, а не замена одного
другим, и сделано осознанно: оценка оси неизбежно неточна, а система
безопасности не имеет права **сужать** зону поиска по неуверенной
оценке. Измерено: замена (а не объединение) поднимала пропуски
реального объекта с 1 % до 25 %, экономя при этом лишь 2.5 % кадров
с ложной тревогой — размен в неверную сторону.
"""
if corridor is None or corridor.n_slices == 0:
return self.u
curved = self.u - corridor.centre(self.d)
return np.where(np.abs(curved) < np.abs(self.u), curved, self.u)
def in_gauge(self, half_width: float = 1.7, h_lo: float = 0.05,
h_hi: float = 2.2, d_min: float = 3.0, d_max: float = 260.0,
corridor: "Corridor | None" = None) -> np.ndarray:
"""Маска лучей, чьи точки лежат внутри габарита приближения."""
return (self.valid & (self.d > d_min) & (self.d < d_max)
& (np.abs(self.lateral(corridor)) < half_width)
& (self.h > h_lo) & (self.h < h_hi))

500
ros2_ws/src/flyguard/flyguard/gpu.py Normal file
View file

@ -0,0 +1,500 @@
"""Видеокарта для плотных стадий конвейера: сетчатка, ламина, кластеризация.
Три стадии работают с целым образом 128 × 600 лучей и занимают больше
половины кадра: раскладка точек по решётке, ламина (центр минус окружение на
трёх масштабах) и кластеризация лучей с учётом глубины. Они и перенесены.
Остальное — плоскость пути, ось, оценка движения, треки, решение — остаётся
на процессоре: там мелкие массивы и ветвистая логика, и копирование на
видеокарту стоило бы дороже самого расчёта.
Каждая функция повторяет процессорную версию операция в операцию, в тех же
типах. Раскладка по порядку точек, ламина и кластеризация дают результат,
совпадающий с процессорным бит в бит (тесты и сверка на записях,
EXPERIMENTS п. 21):
* сетчатка — только выборки, умножения и корень, всё в float32 без слияния
операций;
* ламина — равномерные окна считаются через накопленные суммы в float64 и
округляются в float32 после каждой оси, ровно как `scipy.ndimage.
uniform_filter`: сумма чисел float32 такого диапазона в float64 точна, и
результат не зависит от порядка сложения;
* кластеризация — целочисленный граф; компоненты нумеруются по наименьшему
лучу, как у `scipy.sparse.csgraph.connected_components`.
Исключение — раскладка по углам точек (кадры с нарушенным порядком, как в
синтетике организаторов): арктангенс видеокарты и процессора может разойтись
в последнем знаке, а numpy сортирует ячейки неустойчиво. Расходятся считанные
лучи на кадр.
Любой сбой видеокарты — нет драйвера, не хватило памяти, ошибка ядра — ловит
конвейер: до конца работы он считает на процессоре (`device.
notify_cuda_error`), а кадр досчитывается там же, решение не теряется.
"""
from __future__ import annotations
import warnings
import numpy as np
from .lamina import SCALES, LaminaOutput
from .retina import DEG, ORDER_TOL_DEG, RangeImage, ScanLayout
class GpuStages:
"""Плотные стадии на видеокарте; держит таблицы решётки и образ кадра."""
def __init__(self, device: str = "cuda"):
import torch
self.torch = torch
self.dev = torch.device(device)
torch.zeros(1, device=self.dev) # поднять контекст сразу, а не на первом кадре
self._tab_key = None
self._tab: dict = {}
self._win: dict = {}
self._off: dict = {}
self._box: dict = {}
self._graphs = True # графы CUDA для ламины; при отказе — по одной операции
self._lam_key = None
# образ текущего кадра: ламина и кластеризация берут его отсюда, не копируя заново
self.r = None
self.valid = None
self._r_host = None
def warmup(self, shape: tuple[int, int] = (128, 600)) -> None:
"""Прогнать все три стадии на выдуманном кадре.
Первый запуск каждого ядра CUDA подгружает его код: без прогрева первый
кадр записи шёл 1–1.7 с, очередь подписки переполнялась, и терялись
первые 6–8 кадров (замерено в контейнере). Раскладка прогревается на
маленькой решётке обоими путями — по порядку точек и по углам.
"""
from .cdr import PointCloud2
t = self.torch
rng = np.random.default_rng(0)
n, w, e = 8, 64, 2
lay = ScanLayout(np.linspace(10.0, -20.0, n), -0.1, 3.2, np.zeros(n, np.int64),
np.zeros(n), w, e)
xyz = lay.dirs.transpose(1, 0, 2)[:, None, :, :] * np.float32(20.0) # столбец·эхо·кольцо
xyz = np.broadcast_to(xyz, (w, e, n, 3)).reshape(-1, 3)
pts = np.zeros(xyz.shape[0], dtype=[("x", "<f4"), ("y", "<f4"), ("z", "<f4"),
("intensity", "<f4")])
pts["x"], pts["y"], pts["z"] = xyz[:, 0], xyz[:, 1], xyz[:, 2]
pts["intensity"] = 10.0
cloud = PointCloud2(0.0, "", 1, pts.size, pts.dtype.itemsize, True, pts)
self.project(lay, cloud, slice(8, w - 8))
mixed = PointCloud2(0.0, "", 1, pts.size, pts.dtype.itemsize, True,
pts[rng.permutation(pts.size)])
self.project(lay, mixed, slice(8, w - 8))
r = (4.0 + 150.0 * rng.random(shape)).astype(np.float32)
self.r = t.as_tensor(r, device=self.dev)
self.valid = t.ones(shape, dtype=t.bool, device=self.dev)
self._r_host = r
self.lamina()
self.cluster_by_depth(rng.random(shape) < 0.3, r)
self.r = self.valid = self._r_host = None
t.cuda.synchronize(self.dev)
# ------------------------------------------------------------------ таблицы
def _tables(self, L: ScanLayout) -> dict:
key = (id(L), L.n_points, L.n_az)
if key != self._tab_key:
t, d = self.torch, self.dev
def up(a, dtype):
return t.as_tensor(np.ascontiguousarray(a), dtype=dtype, device=d)
self._tab = {
"sin_el": up(L._sin_el, t.float32),
"el_asc": up(L._el_asc, t.float64),
"el_order": up(L._el_order, t.int64),
"el_deg": up(L.el_deg, t.float64),
"el_step": up(L.el_step_deg, t.float64),
"resid": up(L.az_resid_deg, t.float64),
}
self._win = {}
self._tab_key = key
return self._tab
def _window(self, L: ScanLayout, start: int, stop: int) -> tuple:
"""Сырые столбцы, нужные сектору, и карта выпрямления внутри них."""
key = (start, stop)
if key not in self._win:
w = L.n_az
lo = start + int(L.col_shift.min())
hi = stop + int(L.col_shift.max())
g = L.gather[:, start:stop]
ok = L.gather_ok[:, start:stop]
if L.wrap:
raw = np.arange(lo, hi) % w
g = (g - lo) % w
else:
lo, hi = max(lo, 0), min(hi, w)
raw = np.arange(lo, hi)
g = g - lo
ok = ok & (g >= 0) & (g < (hi - lo))
g = np.clip(g, 0, hi - lo - 1)
# столбцы окна идут подряд, кроме стыка кругового скана
cut = np.flatnonzero(np.diff(raw) != 1) + 1
runs = [(int(a[0]), int(a[-1]) + 1) for a in np.split(raw, cut) if a.size]
t = self.torch
self._win[key] = (runs, t.as_tensor(g, dtype=t.int64, device=self.dev),
t.as_tensor(ok, device=self.dev))
return self._win[key]
# ------------------------------------------------------------------ загрузка точек
def _upload(self, pts: np.ndarray, rows: list[tuple[int, int]]):
"""Строки структурированного массива → байты на видеокарте, (N, шаг точки)."""
t = self.torch
step = pts.dtype.itemsize
parts = []
for a, b in rows:
raw = np.ascontiguousarray(pts[a:b]).view(np.uint8)
with warnings.catch_warnings():
# буфер сообщения только для чтения, а тензор мы не пишем
warnings.simplefilter("ignore", UserWarning)
parts.append(t.from_numpy(raw).to(self.dev, non_blocking=False))
buf = parts[0] if len(parts) == 1 else t.cat(parts)
return buf.view(-1, step)
def _field(self, buf, pts: np.ndarray, name: str):
dt, off = pts.dtype.fields[name][:2]
if dt.kind != "f" or dt.itemsize != 4 or dt.byteorder == ">":
raise TypeError(f"поле {name}: ожидался float32, а не {dt}")
return buf[:, off:off + 4].contiguous().view(self.torch.float32).reshape(-1)
# ------------------------------------------------------------------ сетчатка
def project(self, L: ScanLayout, pc, cols: slice | None) -> RangeImage:
"""То же, что `ScanLayout.project`, на видеокарте."""
w = L.n_az
start = 0 if cols is None else (cols.start or 0)
stop = w if cols is None else (cols.stop if cols.stop is not None else w)
self._tables(L)
img = None
if L.indexed and pc.n_points == L.n_points:
img = self._project_indexed(L, pc, start, stop)
if img is None:
L.n_geometric += 1
img = self._project_geometric(L, pc, start, stop)
self._r_host = img.r_near
return img
def _project_indexed(self, L: ScanLayout, pc, start: int, stop: int):
t = self.torch
tab = self._tab
n, e = L.n_rings, L.n_echo
runs, g, ok = self._window(L, start, stop)
pts = pc.points
per_col = e * n
buf = self._upload(pts, [(a * per_col, b * per_col) for a, b in runs])
ncol = sum(b - a for a, b in runs)
def cube(name: str):
return self._field(buf, pts, name).reshape(ncol, e, n).permute(2, 0, 1)
x, y, z = cube("x"), cube("y"), cube("z")
good = (x != 0) | (y != 0) | (z != 0)
r = t.sqrt(x * x + y * y + z * z)
good &= t.isfinite(r)
r = t.where(good, r, t.zeros((), dtype=r.dtype, device=self.dev))
dev = t.abs(z - r * tab["sin_el"][:, None, None])
tol = r * np.float32(ORDER_TOL_DEG * DEG) + np.float32(1e-3)
if bool(t.any(dev > tol)):
return None
if e == 1:
r_near = r[..., 0]
r_far = r[..., 0]
it = cube("intensity")[..., 0]
valid = good[..., 0]
else:
inten = cube("intensity")
inf = t.full((), float("inf"), dtype=r.dtype, device=self.dev)
near_i = t.argmin(t.where(good, r, inf), dim=-1, keepdim=True)
far_i = t.argmax(r, dim=-1, keepdim=True)
r_near = t.gather(r, -1, near_i)[..., 0]
r_far = t.gather(r, -1, far_i)[..., 0]
it = t.gather(inten, -1, near_i)[..., 0]
valid = good.any(dim=-1)
r_near = t.gather(r_near, 1, g)
r_far = t.gather(r_far, 1, g)
it = t.gather(it, 1, g)
valid = t.gather(valid, 1, g) & ok
zero = t.zeros((), dtype=r_near.dtype, device=self.dev)
r_near = t.where(valid, r_near, zero)
r_far = t.where(valid, r_far, zero)
return self._finish(pc.stamp, r_near, r_far, it, valid)
def _project_geometric(self, L: ScanLayout, pc, start: int, stop: int):
t = self.torch
tab = self._tab
n, wid = L.n_rings, stop - start
pts = pc.points
buf = self._upload(pts, [(0, pts.shape[0])])
x, y, z = (self._field(buf, pts, k) for k in ("x", "y", "z"))
good = (((x != 0) | (y != 0) | (z != 0)) & t.isfinite(x) & t.isfinite(y)
& t.isfinite(z))
idx = t.nonzero(good, as_tuple=True)[0]
x, y, z = x[idx], y[idx], z[idx]
step = L.az_step_deg
az = t.rad2deg(t.atan2(x, -y))
jf = (az - np.float32(L.az0_deg)) / np.float32(step)
if L.wrap:
jf = t.remainder(jf, L.n_az)
margin = float(np.abs(L.az_resid_deg).max()) / abs(step) + 1.0
sel = t.nonzero((jf > start - margin) & (jf < stop - 1 + margin), as_tuple=True)[0]
idx, x, y, z, az = idx[sel], x[sel], y[sel], z[sel], az[sel]
r = t.sqrt(x * x + y * y + z * z)
el = t.rad2deg(t.asin(t.clamp(z / t.clamp_min(r, np.float32(1e-6)), -1.0, 1.0)))
el64 = el.double()
asc = tab["el_asc"]
k = t.clamp(t.searchsorted(asc, el64), 1, n - 1)
k = k - ((el64 - asc[k - 1]) < (asc[k] - el64)).long()
h = tab["el_order"][k]
ok = t.abs(el64 - tab["el_deg"][h]) <= t.clamp_min(tab["el_step"][h], 0.2)
j = t.round(((az.double() - tab["resid"][h]) - L.az0_deg) / step).long()
if L.wrap:
j = t.remainder(j, L.n_az)
ok &= (j >= start) & (j < stop)
sel = t.nonzero(ok, as_tuple=True)[0]
r_near = t.zeros(n * wid, dtype=t.float32, device=self.dev)
r_far = t.zeros_like(r_near)
it = t.zeros_like(r_near)
valid = t.zeros(n * wid, dtype=t.bool, device=self.dev)
if sel.numel():
cell = h[sel] * wid + (j[sel] - start)
rr = r[sel]
mm = t.clamp_max(rr * 1000.0, float((1 << 20) - 1)).long()
order = t.argsort(cell * (1 << 20) + mm, stable=True)
cs, rs = cell[order], rr[order]
new = t.ones(cs.numel(), dtype=t.bool, device=self.dev)
new[1:] = cs[1:] != cs[:-1]
first = t.nonzero(new, as_tuple=True)[0]
last = t.cat([first[1:] - 1, first.new_tensor([cs.numel() - 1])])
r_near[cs[first]] = rs[first]
r_far[cs[last]] = rs[last]
valid[cs[first]] = True
if "intensity" in pts.dtype.names:
src = self._field(buf, pts, "intensity")[idx[sel]][order]
it[cs[first]] = src[first]
return self._finish(pc.stamp, r_near.reshape(n, wid), r_far.reshape(n, wid),
it.reshape(n, wid), valid.reshape(n, wid))
def _finish(self, stamp: float, r_near, r_far, it, valid) -> RangeImage:
self.r, self.valid = r_near, valid
host = [a.cpu().numpy() for a in (r_near, r_far, it, valid)]
return RangeImage(stamp, *host)
# ------------------------------------------------------------------ ламина
def _box_axis(self, X, sizes: list[int], dim: int):
"""Равномерное окно по одной оси для пачки размеров сразу.
`X` — (K, B, H, W) float32, k-й срез фильтруется окном `sizes[k]`
вдоль `dim` (2 или 3) с продолжением краевым значением. Суммы окон
берутся из накопленной суммы в float64: все частичные суммы чисел
float32 такого диапазона в float64 точны, поэтому сумма окна точна при
любом порядке сложения. Деление на размер и округление в float32 —
как в `scipy.ndimage.uniform_filter1d`.
"""
t = self.torch
Xd = X.double()
n = Xd.shape[dim]
lo, hi, n_lo, n_hi, size = self._box_tables(tuple(sizes), n, dim)
cs = t.cumsum(Xd, dim=dim)
cs = t.cat([t.zeros_like(cs.narrow(dim, 0, 1)), cs], dim=dim)
def along(a):
return a.expand(*Xd.shape[:dim], n, *Xd.shape[dim + 1:])
s = (t.gather(cs, dim, along(hi)) - t.gather(cs, dim, along(lo))
+ along(n_lo) * Xd.narrow(dim, 0, 1) + along(n_hi) * Xd.narrow(dim, n - 1, 1))
return (s / size).float()
def _box_tables(self, sizes: tuple, n: int, dim: int):
"""Границы окон по оси: считаются один раз (и до записи графа CUDA)."""
key = (sizes, n, dim)
if key not in self._box:
t = self.torch
p = t.as_tensor([s // 2 for s in sizes], dtype=t.int64, device=self.dev)
i = t.arange(n, device=self.dev)
shape = [len(sizes), 1, 1, 1]
shape[dim] = n
lo = (i[None, :] - p[:, None]).clamp(min=0).reshape(shape)
hi = ((i[None, :] + p[:, None]).clamp(max=n - 1) + 1).reshape(shape)
n_lo = (p[:, None] - i[None, :]).clamp(min=0).double().reshape(shape)
n_hi = (i[None, :] + p[:, None] - (n - 1)).clamp(min=0).double().reshape(shape)
size = t.as_tensor(sizes, dtype=t.float64, device=self.dev).reshape(-1, 1, 1, 1)
self._box[key] = (lo, hi, n_lo, n_hi, size)
return self._box[key]
def _boxes(self, X, sizes: list[tuple[int, int]]):
"""`scipy.ndimage.uniform_filter(x, size, mode="nearest")` для пачки.
`X` — (B, H, W) float32; выход (len(sizes), B, H, W): по осям по
очереди, с округлением в float32 после каждой, как в scipy.
"""
K = len(sizes)
Y = self._box_axis(X[None].expand(K, *X.shape), [s[0] for s in sizes], 2)
return self._box_axis(Y, [s[1] for s in sizes], 3)
def lamina(self, r_max: float = 300.0) -> LaminaOutput:
"""То же, что `lamina._process_cpu`, по образу текущего кадра.
Сама видеокарта считает ламину за доли миллисекунды, а запуск сотни
мелких операций из Python стоит в несколько раз дороже. Поэтому ламина
записывается в граф CUDA один раз на размер образа и дальше
запускается одной командой. Не поддерживает граф драйвер — считаем
теми же операциями по одной.
"""
t = self.torch
key = (tuple(self.r.shape), float(r_max))
out = None
if self._graphs:
try:
if self._lam_key != key:
self._capture_lamina(key, r_max)
self._lam_in[0].copy_(self.r)
self._lam_in[1].copy_(self.valid)
self._lam_graph.replay()
out = self._lam_out
except Exception:
self._graphs = False
self._lam_key = None
if out is None:
out = self._lamina_core(self.r, self.valid, r_max)
f = out[0].cpu().numpy()
return LaminaOutput(disp=f[0], on=f[1], off=f[2], on_scale=out[1].cpu().numpy(),
surround=f[3], hole=f[4])
def _capture_lamina(self, key: tuple, r_max: float) -> None:
t = self.torch
self._lam_in = (t.empty_like(self.r), t.empty_like(self.valid))
self._lam_in[0].copy_(self.r)
self._lam_in[1].copy_(self.valid)
side = t.cuda.Stream(self.dev)
side.wait_stream(t.cuda.current_stream(self.dev))
with t.cuda.stream(side):
for _ in range(2): # прогрев: таблицы окон и кэш памяти
self._lamina_core(*self._lam_in, r_max)
t.cuda.current_stream(self.dev).wait_stream(side)
g = t.cuda.CUDAGraph()
with t.cuda.graph(g):
self._lam_out = self._lamina_core(*self._lam_in, r_max)
self._lam_graph, self._lam_key = g, key
def _lamina_core(self, r, valid, r_max: float):
"""Ламина целиком на видеокарте: (disp, on, off, surround, hole) и on_scale."""
t = self.torch
v = valid.float()
zero = t.zeros((), dtype=t.float32, device=self.dev)
disp = t.where(valid & (r > 0.05), 1.0 / r, zero) * v
# все окна кадра одной пачкой: на каждом масштабе центр и окружение,
# для диспаритета и для маски эха, плюс окно «дыры»
sizes = []
for r_in, r_out in SCALES:
sizes += [(2 * r_in + 1, 2 * r_in + 1), (2 * r_out + 1, 4 * r_out + 1)]
sizes.append((5, 15))
box = self._boxes(t.stack([disp, v]), sizes)
on = t.zeros_like(disp)
off = t.zeros_like(disp)
on_scale = t.zeros(disp.shape, dtype=t.int8, device=self.dev)
surround_mid = None
for k, (r_in, r_out) in enumerate(SCALES):
s_in, s_out = sizes[2 * k], sizes[2 * k + 1]
n_in = s_in[0] * s_in[1]
n_out = s_out[0] * s_out[1]
num = box[2 * k + 1, 0] * n_out - box[2 * k, 0] * n_in
den = box[2 * k + 1, 1] * n_out - box[2 * k, 1] * n_in
sur = t.where(den > 0.5, num / den, zero)
enough = den > 8.0
c = t.where(enough, disp - sur, zero)
pos = t.clamp_min(c, 0.0) * v
neg = t.clamp_min(-(disp - sur), 0.0) * enough
better = pos > on
on = t.where(better, pos, on)
on_scale = t.where(better, t.full((), k, dtype=t.int8, device=self.dev), on_scale)
off = t.maximum(off, neg)
if k == 1:
surround_mid = sur
hole = 1.0 - box[-1, 1]
on = t.clamp(on, 0.0, 1.0 / max(r_max, 1.0) * 1e4)
return t.stack([disp, on, off, surround_mid, hole]), on_scale
# ------------------------------------------------------------------ кластеризация
def _offsets(self, col_reach: int, row_reach: int):
key = (col_reach, row_reach)
if key not in self._off:
pairs = [(dr, dc) for dr in range(0, row_reach + 1)
for dc in range(-col_reach, col_reach + 1) if not (dr == 0 and dc <= 0)]
p = self.torch.tensor(pairs, dtype=self.torch.int64, device=self.dev)
self._off[key] = (p[:, 0:1], p[:, 1:2])
return self._off[key]
def cluster_by_depth(self, mask: np.ndarray, r: np.ndarray, *,
rel_tol: float = 0.06, abs_tol: float = 0.35,
col_reach: int = 3, row_reach: int = 2):
"""То же, что `lobula.cluster_by_depth`, с теми же номерами компонент.
Рёбра строятся сразу для всех соседей, компоненты — подвешиванием к
меньшему номеру со сжатием путей: у каждой компоненты корнем остаётся
её наименьший луч, и нумерация по корням совпадает со scipy.
"""
t = self.torch
h, w = mask.shape
if r is self._r_host and self.r is not None:
rt = self.r.reshape(-1)
else:
rt = t.as_tensor(np.ascontiguousarray(r), dtype=t.float32,
device=self.dev).reshape(-1)
m = t.as_tensor(np.ascontiguousarray(mask), device=self.dev).reshape(-1)
idx = t.nonzero(m, as_tuple=True)[0]
n = idx.numel()
if n == 0:
return np.zeros(mask.shape, np.int32), 0
lut = t.full((h * w,), -1, dtype=t.int64, device=self.dev)
lut[idx] = t.arange(n, device=self.dev)
rows = t.div(idx, w, rounding_mode="floor")
cols = idx - rows * w
ri = rt[idx]
dr, dc = self._offsets(col_reach, row_reach)
nr = rows[None, :] + dr
nc = cols[None, :] + dc
ok = (nr < h) & (nc >= 0) & (nc < w)
nb = lut[t.where(ok, nr * w + nc, t.zeros_like(nr))]
ok &= nb >= 0
rb = ri[nb.clamp(min=0)]
ra = ri[None, :].expand_as(rb)
ok &= t.abs(ra - rb) <= (abs_tol + rel_tol * t.minimum(ra, rb))
s = t.arange(n, device=self.dev)[None, :].expand_as(nb)[ok]
d = nb[ok]
parent = t.arange(n, device=self.dev)
while True:
for _ in range(4):
ps, pd = parent[s], parent[d]
parent.scatter_reduce_(0, t.maximum(ps, pd), t.minimum(ps, pd), reduce="amin")
parent = parent[parent]
parent = parent[parent]
if not bool((parent[s] != parent[d]).any()):
break
while True:
nxt = parent[parent]
if bool(t.equal(nxt, parent)):
break
parent = nxt
roots, comp = t.unique(parent, return_inverse=True)
labels = t.zeros(h * w, dtype=t.int32, device=self.dev)
labels[idx] = (comp + 1).to(t.int32)
return labels.reshape(h, w).cpu().numpy(), int(roots.numel())

216
ros2_ws/src/flyguard/flyguard/lamina.py Normal file
View file

@ -0,0 +1,216 @@
"""LAMINA — локальный контраст, разделение ON/OFF.
Первый нейропиль за фоторецепторами. Клетки L1 и L2 получают один и тот же вход
от R1–R6 и расходятся на два канала: L1 → ON (стало ярче), L2 → OFF (стало
темнее). Оба канала предварительно проходят **латеральное торможение** от
амакриновых клеток и Dm9 — классическое «центр минус окружение», которое
подавляет ровный фон и оставляет только локальные отклонения.
Перенос на лидар:
* «Яркость» — это **диспаритет** δ = 1/R, а не сама дальность. Так правильно
по двум причинам: угловой размер предмета пропорционален 1/R, и шум лидара
в диспаритете почти однороден, тогда как в дальности растёт квадратично.
* **ON** = объект ближе своего окружения — выступ, то есть препятствие.
* **OFF** = дальше окружения или эха нет вовсе — провал, то есть окклюзионная
тень **за** препятствием. Тень часто во много раз крупнее самого предмета,
и именно она даёт шанс увидеть мелкий объект на большой дальности.
* Размер предмета в лучах меняется с дальностью на два порядка, поэтому
окружение берётся **на нескольких масштабах** сразу — как у колонковых
нейронов лобулы с разными размерами рецептивных полей, сходящихся на один
нисходящий нейрон.
"""
from __future__ import annotations
from dataclasses import dataclass
import numpy as np
from scipy.ndimage import uniform_filter
# (радиус центра, радиус окружения) в лучах: кольца × столбцы
SCALES: tuple[tuple[int, int], ...] = ((1, 6), (3, 14), (7, 30))
@dataclass
class LaminaOutput:
"""Каналы ламины для одного кадра."""
disp: np.ndarray # (H, W) диспаритет 1/R, 0 там, где эха нет
on: np.ndarray # (H, W) ON-контраст, максимум по масштабам, 1/м
off: np.ndarray # (H, W) OFF-контраст, 1/м
on_scale: np.ndarray # (H, W) int8 — на каком масштабе отклик максимален
surround: np.ndarray # (H, W) диспаритет окружения на среднем масштабе
hole: np.ndarray # (H, W) доля «нет эха» в окрестности
def _masked_mean(v: np.ndarray, m: np.ndarray, size: tuple[int, int]) -> np.ndarray:
"""Среднее по прямоугольному окну только по валидным отсчётам."""
num = uniform_filter(v, size=size, mode="nearest")
den = uniform_filter(m, size=size, mode="nearest")
return num, den
def _annulus_mean(v: np.ndarray, m: np.ndarray, r_in: int, r_out: int):
"""Среднее по кольцу: большое окно минус вырезанный центр.
Реализовано через два равномерных фильтра (каждый разделим и работает за
O(N)), поэтому стоимость не зависит от размера окна.
"""
s_in = (2 * r_in + 1, 2 * r_in + 1)
s_out = (2 * r_out + 1, 4 * r_out + 1) # шире по азимуту: решётка анизотропна
n_in = s_in[0] * s_in[1]
n_out = s_out[0] * s_out[1]
num_i, den_i = _masked_mean(v, m, s_in)
num_o, den_o = _masked_mean(v, m, s_out)
num = num_o * n_out - num_i * n_in
den = den_o * n_out - den_i * n_in
out = np.divide(num, den, out=np.zeros_like(num), where=den > 0.5)
return out, den
def _process_cpu(r: np.ndarray, valid: np.ndarray, *, r_max: float = 300.0) -> LaminaOutput:
"""CPU-реализация через SciPy uniform_filter."""
v = valid.astype(np.float32)
disp = np.zeros_like(r, dtype=np.float32)
np.divide(1.0, r, out=disp, where=valid & (r > 0.05))
disp *= v
on = np.zeros_like(disp)
off = np.zeros_like(disp)
on_scale = np.zeros(disp.shape, np.int8)
surround_mid = None
for k, (r_in, r_out) in enumerate(SCALES):
sur, cnt = _annulus_mean(disp, v, r_in, r_out)
enough = cnt > 8.0
c = np.where(enough, disp - sur, 0.0)
pos = np.maximum(c, 0.0) * v # ближе окружения
# провал считается и там, где эха нет: 1/∞ = 0 — это тоже сигнал
neg = np.maximum(-(disp - sur), 0.0) * enough
better = pos > on
on = np.where(better, pos, on)
on_scale = np.where(better, np.int8(k), on_scale)
off = np.maximum(off, neg)
if k == 1:
surround_mid = sur
# доля лучей без эха в окрестности — мера «дыры» в поверхности
hole = 1.0 - uniform_filter(v, size=(5, 15), mode="nearest")
# диспаритет физически ограничен снизу дальностью прибора
np.clip(on, 0.0, 1.0 / max(r_max, 1.0) * 1e4, out=on)
return LaminaOutput(disp=disp, on=on, off=off, on_scale=on_scale,
surround=surround_mid if surround_mid is not None else np.zeros_like(disp),
hole=hole.astype(np.float32))
def _process_gpu(r: np.ndarray, valid: np.ndarray, *, r_max: float = 300.0, device: str = "cuda") -> LaminaOutput:
"""Ускоренная GPU-реализация 2D-фильтрации DoG через PyTorch CUDA тензоры.
На NVIDIA RTX 4070 Ti Super сокращает время расчета кадра с 8 мс до 0.25 мс.
"""
import torch
import torch.nn.functional as F
with torch.no_grad():
dev = torch.device(device)
r_t = torch.as_tensor(r, dtype=torch.float32, device=dev)
v_t = torch.as_tensor(valid, dtype=torch.float32, device=dev)
mask_valid = (v_t > 0.5) & (r_t > 0.05)
disp_t = torch.where(mask_valid, 1.0 / r_t, torch.zeros_like(r_t)) * v_t
disp_4d = disp_t.unsqueeze(0).unsqueeze(0) # (1, 1, H, W)
v_4d = v_t.unsqueeze(0).unsqueeze(0)
on_t = torch.zeros_like(disp_t)
off_t = torch.zeros_like(disp_t)
on_scale_t = torch.zeros_like(disp_t, dtype=torch.int8)
surround_mid_t = None
for k, (r_in, r_out) in enumerate(SCALES):
pad_i = (r_in, r_in, r_in, r_in)
pad_o = (2 * r_out, 2 * r_out, r_out, r_out)
k_in = (2 * r_in + 1, 2 * r_in + 1)
k_out = (2 * r_out + 1, 4 * r_out + 1)
disp_pad_i = F.pad(disp_4d, pad_i, mode='replicate')
disp_pad_o = F.pad(disp_4d, pad_o, mode='replicate')
v_pad_i = F.pad(v_4d, pad_i, mode='replicate')
v_pad_o = F.pad(v_4d, pad_o, mode='replicate')
n_in = float(k_in[0] * k_in[1])
n_out = float(k_out[0] * k_out[1])
sum_disp_i = F.avg_pool2d(disp_pad_i, k_in, stride=1) * n_in
sum_disp_o = F.avg_pool2d(disp_pad_o, k_out, stride=1) * n_out
sum_v_i = F.avg_pool2d(v_pad_i, k_in, stride=1) * n_in
sum_v_o = F.avg_pool2d(v_pad_o, k_out, stride=1) * n_out
num_t = (sum_disp_o - sum_disp_i).squeeze(0).squeeze(0)
den_t = (sum_v_o - sum_v_i).squeeze(0).squeeze(0)
sur_t = torch.where(den_t > 0.5, num_t / den_t, torch.zeros_like(num_t))
enough_t = den_t > 8.0
c_t = torch.where(enough_t, disp_t - sur_t, torch.zeros_like(disp_t))
pos_t = torch.clamp_min(c_t, 0.0) * v_t
neg_t = torch.clamp_min(-(disp_t - sur_t), 0.0) * enough_t.float()
better_t = pos_t > on_t
on_t = torch.where(better_t, pos_t, on_t)
on_scale_t = torch.where(better_t, torch.tensor(k, dtype=torch.int8, device=dev), on_scale_t)
off_t = torch.maximum(off_t, neg_t)
if k == 1:
surround_mid_t = sur_t
pad_hole = (7, 7, 2, 2)
v_pad_h = F.pad(v_4d, pad_hole, mode='replicate')
hole_mean = F.avg_pool2d(v_pad_h, (5, 15), stride=1).squeeze(0).squeeze(0)
hole_t = 1.0 - hole_mean
on_t = torch.clamp(on_t, 0.0, 1.0 / max(r_max, 1.0) * 1e4)
return LaminaOutput(
disp=disp_t.cpu().numpy(),
on=on_t.cpu().numpy(),
off=off_t.cpu().numpy(),
on_scale=on_scale_t.cpu().numpy(),
surround=surround_mid_t.cpu().numpy() if surround_mid_t is not None else np.zeros_like(r, dtype=np.float32),
hole=hole_t.cpu().numpy().astype(np.float32)
)
def process(r: np.ndarray, valid: np.ndarray, *, r_max: float = 300.0,
device: str | None = None) -> LaminaOutput:
"""Посчитать ON/OFF-каналы ламины по дальностному образу (автовыбор GPU / CPU)."""
target_dev = device or "cpu" # без явной просьбы — процессор, как раньше
if target_dev == "auto":
from .device import get_device
target_dev = get_device("auto")
if target_dev.startswith("cuda"):
try:
return _process_gpu(r, valid, r_max=r_max, device=target_dev)
except Exception as e:
from .device import notify_cuda_error
notify_cuda_error(e)
return _process_cpu(r, valid, r_max=r_max)
return _process_cpu(r, valid, r_max=r_max)
def contrast_to_depth_gap(on: np.ndarray, r: np.ndarray) -> np.ndarray:
"""Перевести ON-контраст диспаритета в «насколько ближе окружения», м.
δ − δ_sur = 1/R − 1/R_sur ⇒ R_sur − R = on · R · R_sur. Для оценки берётся
R_sur = R/(1 − on·R), что даёт разрыв по глубине в метрах.
"""
x = np.clip(on * r, 0.0, 0.999)
with np.errstate(divide="ignore", invalid="ignore"):
gap = r * x / (1.0 - x)
return np.nan_to_num(gap, nan=0.0, posinf=1e4).astype(np.float32)

533
ros2_ws/src/flyguard/flyguard/lobula.py Normal file
View file

@ -0,0 +1,533 @@
"""LOBULA — колонковые детекторы объектов (аналог LC11) и сборка кандидатов.
В лобуле мухи сидят колонковые нейроны LC: каждый смотрит в свой участок поля
зрения и реагирует на **признак**, а не на картинку целиком. LC11 — детектор
мелкого объекта: он возбуждается компактным пятном, выделяющимся из фона, и
подавляется широкопольным движением, то есть собственным перемещением мухи.
Выход LC-популяции — не изображение, а короткий список «где что-то есть».
Здесь тем же занимается `find_candidates`: из ретинотопических масок улик
собираются связные пятна, для каждого считаются геометрические признаки, и
дальше по конвейеру идут уже десятки кандидатов вместо сотен тысяч лучей.
Решение о том, препятствие это или штатная конструкция тоннеля, здесь НЕ
принимается — этим занимаются грибовидное тело (знакомость) и центральный
комплекс (накопление улик по кадрам).
"""
from __future__ import annotations
from dataclasses import dataclass, field
import numpy as np
from scipy import ndimage
from .geometry import Corridor, TrackFrame
from .lamina import LaminaOutput, contrast_to_depth_gap
_NEIGHBOURS = np.ones((3, 3), bool)
@dataclass
class Candidate:
"""Кандидат в препятствия — компактная группа лучей."""
d: float # расстояние вперёд, м
u: float # смещение от оси пути, м
h: float # высота над головкой рельса, м
d_min: float # ближняя граница, м
h_min: float # нижняя точка над рельсом, м — есть ли опора снизу
width: float # ширина, м
height: float # высота, м
depth: float # протяжённость вдоль пути, м
containment: float # доля лучей пятна, оставшихся внутри габарита, 0…1
n_rays: int # число лучей
n_rings: int
n_cols: int
gap: float # насколько ближе фона, м
on: float # ON-контраст, 1/м
floor_deficit: float # насколько раньше оборвался луч до полотна, м
shadow: float # доля тени (нет эха) сразу за объектом
inten: float # медианная интенсивность
az_deg: float
el_deg: float
bbox: tuple[int, int, int, int]
score: float = 0.0
novelty: float = 1.0
extra: dict = field(default_factory=dict)
@property
def area(self) -> float:
return self.width * self.height
def cluster_by_depth(mask: np.ndarray, r: np.ndarray, *,
rel_tol: float = 0.06, abs_tol: float = 0.35,
col_reach: int = 3, row_reach: int = 2):
"""Связные компоненты с учётом разрыва по глубине.
Обычная связность по соседству склеивает предмет со стеной, которая
оказалась в том же месте изображения, но на сто метров дальше. Поэтому два
соседних луча объединяются только если их дальности близки — допуск растёт
с расстоянием, потому что и разрешение, и шум растут так же.
Соседство берётся с запасом по азимуту (`col_reach`), чтобы пропуски эха
внутри предмета не дробили его на части.
Возвращает (labels, n_labels); метка 0 — фон.
"""
idx = np.flatnonzero(mask.ravel())
if idx.size == 0:
return np.zeros(mask.shape, np.int32), 0
h, w = mask.shape
lut = np.full(mask.size, -1, np.int64)
lut[idx] = np.arange(idx.size)
ri = r.ravel()[idx]
rows, cols = np.divmod(idx, w)
src, dst = [], []
for dr in range(0, row_reach + 1):
for dc in range(-col_reach, col_reach + 1):
if dr == 0 and dc <= 0:
continue
nr, nc = rows + dr, cols + dc
ok = (nr < h) & (nc >= 0) & (nc < w)
nb = lut[np.where(ok, nr * w + nc, 0)]
ok &= nb >= 0
if not ok.any():
continue
a = np.flatnonzero(ok)
b = nb[ok]
close = np.abs(ri[a] - ri[b]) <= (abs_tol + rel_tol * np.minimum(ri[a], ri[b]))
if close.any():
src.append(a[close])
dst.append(b[close])
n_nodes = idx.size
if src:
from scipy.sparse import coo_matrix
from scipy.sparse.csgraph import connected_components
s = np.concatenate(src)
t = np.concatenate(dst)
g = coo_matrix((np.ones(s.size, np.int8), (s, t)), shape=(n_nodes, n_nodes))
n_comp, comp = connected_components(g, directed=False)
else:
n_comp = n_nodes
comp = np.arange(n_nodes)
labels = np.zeros(mask.size, np.int32)
labels[idx] = comp + 1
return labels.reshape(mask.shape), n_comp
def split_by_figure(labels: np.ndarray, n: int, r: np.ndarray,
figure: np.ndarray, *, max_depth: float, thr: float,
col_reach: int = 3, row_reach: int = 2, top: int = 0):
"""Вырезать фигуру из компоненты, растёкшейся по фону.
Гладкая стена тоннеля связна по глубине от ближнего поля до горизонта:
соседние лучи отличаются на сантиметры, и допуск сшивает их в одну
компоненту длиной сто метров. Предмет у такой стены попадает в неё же
и вместе с ней отбрасывается правилом `max_depth` — система слепнет
именно там, где смотрит вдоль стены, а это опаснее лишней тревоги.
Разорвать такую компоненту по дальности нельзя: предмет и стена рядом с
ним стоят на одной дальности. Зато они по-разному **приближаются**. Вдоль
фиксированного луча стена, параллельная движению, не приближается вовсе:
поезд едет, точка пересечения скользит по стене, дальность не меняется.
Предмет, обращённый к поезду, приближается ровно на пройденный путь.
Отсюда `advance` ≈ 0 у фона и ≈ 1 у фигуры.
Это и есть разделение фигуры и фона, которым у мухи занят канал
T4/T5 → LPTC: широкопольный поток задаёт ожидание, а то, что движется
иначе, становится фигурой.
Второй, более сильный источник того же признака — **контраст ламины**.
Гладкая стена по построению даёт нулевой центр-окружение: сколько бы ни
менялась дальность вдоль стены, она меняется плавно, и центр равен своему
окружению. Предмет на стене — это ступенька, и она видна. Замер на
вставленном человеке (`roundT_pressureGate_roundT` и `roundT_doubleT`):
при пороге 6 м порог проходят 85 % и 75 % лучей предмета против 0.13 % и
0.22 % лучей фона, а сам фон распадается всего на 5–9 компонент в кадре
вместо 1259 у разреза по допуску.
Поэтому `figure` — любой покадровый признак «это не фон»: скорость
сближения вдоль луча или контраст к окружению в метрах. Пересобираются
только переглубокие компоненты и только по лучам с `figure > thr`;
остальные метки не трогаются.
`top` ограничивает число фигур, выносимых из одной компоненты, оставляя
сильнейшие по признаку. Это не косметика, а необходимость: разрез отрезает
фон, и протяжённость кандидата вдоль пути падает с 6.9 до 0.8 м — вместе с
ней исчезает множитель компактности в весе улики, который до того давил
конструкции в десять раз. Каждая лишняя фигура поэтому стоит дорого.
Ограничение — тот же приём глобального торможения, которым APL оставляет
активными считанные проценты клеток Кеньона. 0 — без ограничения.
"""
if n == 0 or figure is None:
return labels, n
flat = labels.ravel()
rr = r.ravel()
adv = figure.ravel()
order = np.argsort(flat, kind="stable")
starts = np.searchsorted(flat[order], np.arange(n + 2))
out, next_id, copied = labels, n, False
for lbl in range(1, n + 1):
idx = order[starts[lbl]:starts[lbl + 1]]
if idx.size < 2:
continue
v = rr[idx]
v = v[np.isfinite(v)]
if v.size < 2 or float(v.max() - v.min()) <= max_depth:
continue
keep = idx[adv[idx] > thr]
if keep.size < 4 or keep.size == idx.size:
continue
sub = np.zeros(labels.shape, bool)
sub.ravel()[keep] = True
parts, n2 = cluster_by_depth(sub, r, col_reach=col_reach, row_reach=row_reach)
if n2 == 0:
continue
lbl_of = parts.ravel()[keep]
if top > 0 and n2 > top:
# сильнейшие по признаку: сумма превышения порога по всем лучам
w = np.bincount(lbl_of, weights=adv[keep] - thr, minlength=n2 + 1)
best = np.argsort(w[1:])[::-1][:top] + 1
alive = np.isin(lbl_of, best)
keep, lbl_of = keep[alive], lbl_of[alive]
if keep.size < 4:
continue
# перенумеровать оставшиеся подряд
_, lbl_of = np.unique(lbl_of, return_inverse=True)
lbl_of += 1
n2 = int(lbl_of.max())
if not copied:
out, copied = labels.copy(), True
# фон этой компоненты уходит из рассмотрения: он и был бы отброшен
out.ravel()[idx] = 0
out.ravel()[keep] = next_id + lbl_of
next_id += n2
return out, next_id
def _core_zone(abs_lat: np.ndarray, d: np.ndarray, core_width: float,
core_from: float) -> np.ndarray:
"""Где действует пониженный пол: в колее и не ближе core_from."""
zone = abs_lat <= core_width
if core_from > 0.0:
with np.errstate(invalid="ignore"):
zone = zone & (d >= core_from)
return zone
def gauge_mask(tf: TrackFrame, corridor: Corridor, *, half_width: float,
h_lo: float, h_hi: float, d_min: float, d_max: float,
k_sigma: float = 0.0, h_lo_core: float = 1e9,
core_width: float = 0.85, core_from: float = 0.0,
platform_filter: bool = False,
platform_u: float = 1.30, platform_h_lo: float = 1.05,
platform_h_hi: float = 1.25,
h_top: float = 0.0, half_width_top: float = 0.0,
top_d_max: float = 90.0):
"""Лучи, попавшие в габарит приближения. Считается один раз на кадр.
Нужны и для выделения кандидатов, и для накопителя веерного тела, поэтому
вынесены отдельно: пересчитывать один и тот же набор масок дважды незачем.
Верхняя секция (`h_top` > `h_hi`) — от `h_hi` до `h_top`, уже основной:
|u| < `half_width_top`. Она ловит то, что свисает со свода в путь вагона,
и не цепляет кабели и светильники на стенах, которые у круглого свода на
этой высоте подходят к оси ближе полуширины габарита. Дальше `top_d_max`
её нет: там ошибка наклона плоскости и оси пути поднимает и сдвигает к
оси конструкции свода, и на пустых записях они шли в кандидаты сотнями.
"""
lat = tf.lateral(corridor)
abs_lat = np.abs(lat)
in_range = tf.valid & (tf.d > d_min) & (tf.d < d_max)
if k_sigma > 0.0:
sig = corridor.sigma(tf.d)
w_eff = half_width + k_sigma * np.maximum(sig - 0.25, 0.0)
else:
w_eff = half_width
# Двухзонный пол: в межрельсовой колее (|u| <= core_width) порог h_lo_core (0.16 м),
# чтобы не терять лежащего человека или низкие предметы. Опускается он
# только дальше core_from: вблизи в полосу 0.16…0.28 м попадают головки
# рельсов, и предмет теряется вместе с ними (EXPERIMENTS п. 16).
floor_h = np.where(_core_zone(abs_lat, tf.d, core_width, core_from),
min(h_lo_core, h_lo), h_lo)
inside = in_range & (abs_lat < w_eff) & (tf.h > floor_h) & (tf.h < h_hi)
if h_top > h_hi and half_width_top > 0.0:
inside |= (in_range & (tf.d < top_d_max) & (abs_lat < half_width_top)
& (tf.h >= h_hi) & (tf.h < h_top))
if platform_filter:
platform = (abs_lat >= platform_u) & (tf.h >= platform_h_lo) & (tf.h <= platform_h_hi)
inside = inside & ~platform
return inside, lat, in_range
def find_candidates(tf: TrackFrame, lam: LaminaOutput, corridor: Corridor, *,
half_width: float = 1.7, h_lo: float = 0.28, h_hi: float = 2.3,
d_min: float = 4.0, d_max: float = 220.0,
min_rays: int = 4, min_rays_far: int = 0,
min_rays_far_from: float = 90.0,
max_candidates: int = 64,
dilate_cols: int = 2, max_depth: float = 15.0,
ctx_up: float = 4.0, split_adv: float = 0.0,
split_gap: float = 0.0, split_near: float = 0.0,
near_long: float = 0.0,
split_top: int = 0,
advance: np.ndarray | None = None,
masks: tuple | None = None,
k_sigma: float = 0.0, h_lo_core: float = 1e9,
core_width: float = 0.85, core_from: float = 0.0,
platform_filter: bool = False,
platform_u: float = 1.30, platform_h_lo: float = 1.05,
platform_h_hi: float = 1.25,
h_top: float = 0.0, half_width_top: float = 0.0,
top_d_max: float = 90.0, top_detect: bool = False,
top_min_rays: int = 3, cluster=None) -> list[Candidate]:
"""Выделить кандидатов: связные пятна лучей, попавших в габарит.
Кластеризация ведётся по **расширенной** области, а членство в габарите
проверяется уже для готового пятна. Без этого вертикальный лоскут стены,
срезанный границей коридора, неотличим от предмета: обрезка сама создаёт
компактное пятно нужного размера. Доля лучей, оставшихся внутри габарита
(`containment`), сразу показывает, предмет это целиком или край стены.
"""
if masks is None:
masks = gauge_mask(tf, corridor, half_width=half_width, h_lo=h_lo,
h_hi=h_hi, d_min=d_min, d_max=d_max,
k_sigma=k_sigma, h_lo_core=h_lo_core,
core_width=core_width, core_from=core_from,
platform_filter=platform_filter,
platform_u=platform_u, platform_h_lo=platform_h_lo,
platform_h_hi=platform_h_hi)
inside, lat, in_range = masks
if not inside.any():
return []
# Контекст расширяется в стороны и вверх, но НЕ вниз: полотно пути проходит
# прямо под каждым предметом и на большой дальности попадает в тот же допуск
# по глубине, так что расширение вниз склеило бы любой объект с полом.
# Вверх, наоборот, нужно доставать до свода: колонна и рама крепи идут от
# полотна до потолка, и если обрезать контекст на полпути, срез сам выглядит
# компактным предметом ростом с габарит.
abs_lat = np.abs(lat)
if k_sigma > 0.0:
sig = corridor.sigma(tf.d)
w_ctx = half_width + 1.6 + k_sigma * np.maximum(sig - 0.25, 0.0)
else:
w_ctx = half_width + 1.6
floor_h = np.where(_core_zone(abs_lat, tf.d, core_width, core_from),
min(h_lo_core, h_lo), h_lo)
context = (in_range & (abs_lat < w_ctx)
& (tf.h > floor_h) & (tf.h < h_hi + ctx_up))
if platform_filter:
platform = (abs_lat >= platform_u) & (tf.h >= platform_h_lo) & (tf.h <= platform_h_hi)
context = context & ~platform
gap_img = contrast_to_depth_gap(lam.on, tf.r)
# `cluster` — та же кластеризация на видеокарте (FlyGuard._cluster), с тем
# же результатом; здесь самая большая маска кадра, и выигрыш в ней
labels, n = (cluster or cluster_by_depth)(context, tf.r, col_reach=dilate_cols + 1)
if n == 0:
return []
if split_gap > 0:
# Контраст к окружению как признак фигуры. Если задан и порог движения,
# выполняются оба условия сразу: стена должна и приближаться как предмет,
# и торчать из своего окружения.
fig = gap_img if (split_adv <= 0 or advance is None) else np.where(advance > split_adv, gap_img, 0.0)
if split_near > 0:
# Ближе этой дальности разрез был выключен целиком, и причина
# стояла в комментарии неверная: считалось, что вблизи предмет со
# стеной не слипается. Замер показал обратное — на 6…30 м предмет,
# смещённый от оси на 0.9 м, теряется в 34 % наблюдений против 1 %
# на самой оси, и хуже всего там, где есть платформа. Он уходит
# вместе с её кромкой.
#
# Резать вблизи всё подряд дорого: обстановки, дающей контраст, в
# ближнем поле на порядок больше. Поэтому режем только компоненты,
# СЛИШКОМ РАСТЯНУТЫЕ по глубине, чтобы быть предметом: человек
# занимает по дальности десятки сантиметров, кромка платформы и
# стена — десятки метров. Мерой служит разброс дальности внутри
# компоненты (две свёртки по меткам, доли миллисекунды).
near = tf.d <= split_near
if near_long > 0.0 and n > 0:
lab = labels.ravel()
rr = np.where(np.isfinite(tf.r), tf.r, 0.0).ravel()
cnt = np.bincount(lab, minlength=n + 1).astype(np.float64)
s1 = np.bincount(lab, weights=rr, minlength=n + 1)
s2 = np.bincount(lab, weights=rr * rr, minlength=n + 1)
c = np.maximum(cnt, 1.0)
var = np.maximum(s2 / c - (s1 / c) ** 2, 0.0)
stretched = np.sqrt(var) > near_long
stretched[0] = False
near = near & ~stretched[labels]
fig = np.where(near, 0.0, fig)
labels, n = split_by_figure(labels, n, tf.r, fig, max_depth=max_depth,
thr=split_gap, col_reach=dilate_cols + 1,
top=split_top)
elif split_adv > 0 and advance is not None:
labels, n = split_by_figure(labels, n, tf.r, advance, max_depth=max_depth,
thr=split_adv, col_reach=dilate_cols + 1)
flat = labels.ravel()
counts = np.bincount(flat, minlength=n + 1)
counts[0] = 0
inner = np.bincount(flat, weights=inside.ravel().astype(np.float64), minlength=n + 1)
inner[0] = 0
# Сколько лучей достаточно, чтобы считать компоненту кандидатом. Вблизи
# обрывок из трёх лучей — это кусок чего-то большего, и он только
# дробит настоящий предмет: замер даёт P@50 у человека 0.70 → 0.64,
# если разрешить тройки везде. Вдали три луча — всё, что вообще есть
# (медиана на 150 м — шесть), и запрет отсекает предмет целиком.
# Поэтому порог зависит от дальности, как и нормировка числа лучей в
# `_quality`. 0 — не различать (EXPERIMENTS п. 12.5).
need = np.full(inner.shape, float(min_rays))
if min_rays_far > 0:
din = np.where(inside, np.nan_to_num(tf.d, nan=0.0, posinf=0.0), 0.0)
dsum = np.bincount(flat, weights=din.ravel().astype(np.float64),
minlength=n + 1)
dmean = dsum / np.maximum(inner, 1.0)
need = np.where(dmean >= min_rays_far_from, float(min_rays_far),
float(min_rays))
keep = np.flatnonzero(inner >= need)
if keep.size == 0:
return []
if keep.size > max_candidates:
keep = keep[np.argsort(inner[keep])[::-1][:max_candidates]]
with np.errstate(invalid="ignore"):
deficit = np.where(np.isfinite(tf.floor_r), tf.floor_r - tf.r, 0.0)
deficit = np.where(tf.valid & (deficit > 0), deficit, 0.0).astype(np.float32)
order = np.argsort(labels.ravel(), kind="stable")
sorted_lbl = labels.ravel()[order]
starts = np.searchsorted(sorted_lbl, np.arange(n + 2))
width_img = labels.shape[1]
def build(sel: np.ndarray, n_ctx: int) -> Candidate | None:
"""Кандидат по лучам ядра `sel` (плоские индексы) из компоненты в `n_ctx` лучей."""
ii, jj = np.divmod(sel, width_img)
d = tf.d[ii, jj]
u = lat[ii, jj]
h = tf.h[ii, jj]
nn = d.size
depth = float(d.max() - d.min()) if nn > 1 else 0.0
if depth > max_depth:
# ни один посторонний предмет не тянется на десятки метров вдоль пути:
# это полотно, стена или кабельный лоток, взятые под скользящим углом
return None
cand = Candidate(
d=float(np.median(d)), u=float(np.median(u)), h=float(np.median(h)),
d_min=float(d.min()), h_min=float(h.min()),
width=float(u.max() - u.min()) if nn > 1 else 0.0,
height=float(h.max() - h.min()) if nn > 1 else 0.0,
depth=depth,
containment=float(sel.size / max(n_ctx, 1)),
n_rays=int(nn), n_rings=int(np.unique(ii).size), n_cols=int(np.unique(jj).size),
gap=float(np.median(gap_img[ii, jj])),
on=float(np.median(lam.on[ii, jj])),
floor_deficit=float(np.median(deficit[ii, jj])),
shadow=float(np.median(lam.hole[ii, jj])),
inten=float(np.median(tf.inten[ii, jj])),
az_deg=float(np.mean(tf.layout.az_grid_deg[jj])),
el_deg=float(np.mean(tf.layout.el_deg[ii])),
bbox=(int(ii.min()), int(ii.max()), int(jj.min()), int(jj.max())),
)
# индексы лучей нужны только для отладочной визуализации; они небольшие
cand.extra["rays"] = (ii, jj)
# Смещение в системе лидара, без поправки на кривую: `u` отсчитан от
# оси пути, и в кривой рамка по нему встала бы в стороне от своих
# точек (на синтетике — в 1.2 м на 39 м). Нужно только для рисования.
cand.extra["u_raw"] = float(np.median(tf.u[ii, jj]))
return cand
use_top = top_detect and h_top > h_hi and half_width_top > 0.0
h_flat = tf.h.ravel()
inside_flat = inside.ravel()
out: list[Candidate] = []
for lbl in keep:
ctx_idx = order[starts[lbl]:starts[lbl + 1]]
sel = ctx_idx[inside_flat[ctx_idx]] # признаки считаем по ядру в габарите
if sel.size < need[lbl]:
continue
if use_top and np.all(h_flat[sel] >= h_hi):
# целиком в верхней секции: это работа отдельного прохода ниже —
# здесь такой кандидат склеен со сводом и только двоил бы трек
continue
cand = build(sel, ctx_idx.size)
if cand is not None:
out.append(cand)
if use_top:
out += _hanging(tf, abs_lat, in_range, build, h_hi=h_hi, h_top=h_top,
half_width_top=half_width_top, top_d_max=top_d_max,
min_rays=top_min_rays, col_reach=dilate_cols + 1)
out.sort(key=lambda c: c.d)
return out
def _hanging(tf: TrackFrame, abs_lat, in_range, build, *, h_hi: float, h_top: float,
half_width_top: float, top_d_max: float, min_rays: int,
col_reach: int, margin_u: float = 0.3, below: float = 0.4) -> list[Candidate]:
"""Свисающее со свода в путь вагона: отдельный проход по верхней секции.
Основной проход тянет контекст до свода, чтобы колонна не выглядела
предметом, — и поэтому всё, что свисает с потолка, склеивается со сводом:
доля лучей в габарите у стержня из синтетики организаторов была ноль.
Просто не пускать свод в контекст нельзя: тогда освобождаются и стоящие
конструкции, прицепленные к своду, — на `roundT_doubleT` так вышла тревога
на светофоре у стрелки. Поэтому здесь свой проход: контекст — только сама
верхняя секция с запасом `margin_u` по сторонам и `below` вниз. Компонента,
у которой есть лучи ниже `h_hi`, продолжается к полу: это стоящий предмет
(столб, светофор, человек высокого роста), и его ведёт основной проход.
Висящему не нужна опора снизу, и обученное считывание его не судит
(см. central_complex._quality, `top_from`). EXPERIMENTS п. 19.
"""
band = (in_range & (tf.d < top_d_max) & (abs_lat < half_width_top)
& (tf.h >= h_hi) & (tf.h < h_top))
if not band.any():
return []
ext = (in_range & (tf.d < top_d_max) & (abs_lat < half_width_top + margin_u)
& (tf.h > h_hi - below) & (tf.h < h_top))
labels, n = cluster_by_depth(ext, tf.r, col_reach=col_reach)
if n == 0:
return []
flat_lbl = labels.ravel()
order = np.argsort(flat_lbl, kind="stable")
starts = np.searchsorted(flat_lbl[order], np.arange(n + 2))
band_flat = band.ravel()
low_flat = (tf.h < h_hi).ravel()
out = []
for lbl in range(1, n + 1):
idx = order[starts[lbl]:starts[lbl + 1]]
if idx.size < min_rays or low_flat[idx].any():
continue
sel = idx[band_flat[idx]]
if sel.size < min_rays:
continue
cand = build(sel, idx.size)
if cand is None:
continue
if cand.height < 0.1 and cand.h_min > h_top - 0.2:
# Плоское у самой верхней границы — это свод: на 80–90 м ошибка
# наклона плоскости пути опускает его в секцию (на `squareT_…`
# так вышла тревога на 83 м, высота 3.66–3.69 м при границе 3.7).
# Свисающее начинается ниже и имеет высоту.
continue
cand.extra["top"] = True
out.append(cand)
return out

390
ros2_ws/src/flyguard/flyguard/mbon_readout.py Normal file
View file

@ -0,0 +1,390 @@
"""MBON с обучением с учителем — считывание, а не новая сеть.
Грибовидное тело в `mushroom_body.py` учится **без меток**: синапсы KC→MBON
депрессируются на всём, что тоннель показывает часто, и выход MBON означает
«незнакомо». Это ровно familiarity suppression MBON-α′3 и ровно то, что нужно,
когда меток нет.
Но у мухи та же схема умеет и другое. При обучении с подкреплением
дофаминергические нейроны PPL1/PAM депрессируют KC→MBON **избирательно** — те
клетки, что были активны вместе с наказанием, — и выход MBON начинает означать
не «незнакомо», а «это предвещает удар». Один и тот же нейропиль, один и тот же
разрежённый код, другой учитель.
Здесь сделано именно это. Слои не меняются:
* вход — те же признаки кандидата плюс опора веерного тела (23 «проекционных
нейрона»);
* KC — та же случайная разрежённая проекция по 6 «когтей» на клетку;
* APL — то же глобальное торможение «победитель забирает всё», но отклик
остаётся **градуальным** и делится на общую активность (дивизивная
нормировка), а не превращается в единицы и нули;
* MBON — один выход, веса которого обучены различать предмет и обстановку.
Метки берутся не из разметки (её нет), а из физики: `flyguard.synth` вставляет
предмет трассировкой лучей, и кандидат считается предметом, если его ядро
состоит из лучей, в которые предмет действительно записан
(`tools/make_training_set.py`).
Зачем это нужно поверх ручной формулы веса улики. В `central_complex._quality`
шесть множителей, придуманных руками, а в дескрипторе 23 признака: **тень и
интенсивность в вес улики не входят вообще**, хотя окклюзионная тень за
предметом на большой дальности во много раз крупнее самого предмета.
Стоимость в инференсе — одно умножение матрицы 23 × n_kc на кандидата, доли
миллисекунды на CPU. GPU нужен только на обучении, и то не обязателен.
"""
from __future__ import annotations
from dataclasses import dataclass
from pathlib import Path
import numpy as np
from .lobula import Candidate
from .mushroom_body import describe
@dataclass
class MbonConfig:
"""Параметры считывания."""
# Ёмкость выбирается замером (`tools/train_mbon.py --sweep-kc`), а не на
# глаз: отбор «победитель забирает всё» стоит дороже матмула, и в худшем
# кадре с 64 кандидатами 20 000 клеток это 7.3 мс, 8 000 — 2.8 мс, 4 000 —
# 1.3 мс. Умолчание взято средним: платить временем кадра за ёмкость имеет
# смысл, только если развёртка показала, что она что-то даёт.
n_kc: int = 8_000 # клеток Кеньона в этом контуре
claws: int = 6 # входов на клетку (из коннектома)
sparsity: float = 0.0125 # доля активных после торможения APL — 100 клеток
seed: int = 20260921
def describe_full(c: Candidate) -> np.ndarray:
"""Вектор кандидата для считывания: дескриптор памяти плюс опора накопителя."""
acc = float(c.extra.get("acc_support", 0.0)) if c.extra else 0.0
return np.append(describe(c), np.float32(acc)).astype(np.float32)
class MbonReadout:
"""Разрежённый код клеток Кеньона + обученный линейный выход MBON."""
def __init__(self, cfg: MbonConfig | None = None, n_pn: int = 23):
self.cfg = cfg or MbonConfig()
self.n_pn = int(n_pn)
self.n_active = max(1, int(round(self.cfg.n_kc * self.cfg.sparsity)))
rng = np.random.default_rng(self.cfg.seed)
idx = np.stack([rng.choice(self.n_pn, size=self.cfg.claws, replace=False)
for _ in range(self.cfg.n_kc)])
sign = rng.choice((-1.0, 1.0), size=idx.shape)
w = np.zeros((self.cfg.n_kc, self.n_pn), np.float32)
np.put_along_axis(w, idx, sign.astype(np.float32), axis=1)
self.W = w
# Опорный отсчёт по полосам дальности: медиана калиброванного
# логита среди ВСЕХ кандидатов полосы. Нужен, чтобы отделить
# «насколько это предметно» от «как часто предметы вообще
# встречаются на такой дальности» — см. _prior_corr.
self.ref_edges = np.array([0., 30., 55., 80., 110., 160., 230.],
np.float32)
self.ref_z: np.ndarray | None = None
self.mean = np.zeros(self.n_pn, np.float32)
self.scale = np.ones(self.n_pn, np.float32)
self.w_mbon = np.zeros(self.cfg.n_kc, np.float32)
self.bias = np.float32(0.0)
# калибровка выхода: sigmoid(gain·z + shift). Градиентный спуск по
# разрежённому коду хорошо упорядочивает кандидатов, но масштаб логита
# зависит от ёмкости и числа эпох, а нам нужна осмысленная вероятность —
# она идёт множителем в вес улики.
self.gain = np.float32(1.0)
self.shift = np.float32(0.0)
# ------------------------------------------------------------------ код
def fit_normalizer(self, X: np.ndarray) -> None:
X = np.atleast_2d(np.asarray(X, np.float32))
self.mean = X.mean(0).astype(np.float32)
s = X.std(0).astype(np.float32)
self.scale = np.where(s > 1e-6, s, 1.0).astype(np.float32)
def encode(self, X: np.ndarray, device: str | None = None):
"""Признаки → (индексы активных клеток, их нормированный отклик).
Отклик остаётся градуальным и делится на свою сумму: это дивизивная
нормировка APL. Двоичный код тут заметно хуже — он выбрасывает
«насколько» клетка возбуждена, а для решения это как раз важно.
Нормировка приводит средний отклик к единице, а не сумму: иначе каждая
из сотни активных клеток даёт вклад 0.01, логиты выходят микроскопические
и обучение сваливается в разумное ранжирование при бессмысленной
калибровке (замерено: AUC 0.97 при доле верных 0.37).
"""
X = np.atleast_2d(np.asarray(X, np.float32))
if X.shape[1] != self.n_pn:
raise ValueError(f"считывание обучено на {self.n_pn} признаках, "
f"а дескриптор даёт {X.shape[1]}")
k = self.n_active
chunk = max(1, int(2 ** 26 // max(self.cfg.n_kc, 1)))
if device == "auto":
from .device import get_device
device = get_device("auto")
if device and device != "cpu":
try:
import torch
out_i = np.empty((X.shape[0], k), np.int64)
out_v = np.empty((X.shape[0], k), np.float32)
with torch.no_grad():
m = torch.as_tensor(self.mean, device=device)
s = torch.as_tensor(self.scale, device=device)
w = torch.as_tensor(self.W, device=device).T.contiguous()
for i in range(0, X.shape[0], chunk):
t = torch.as_tensor(X[i:i + chunk], device=device)
y = torch.relu(((t - m) / s) @ w)
v, a = torch.topk(y, k, dim=1)
v = v * (k / v.sum(1, keepdim=True).clamp_min(1e-6))
out_i[i:i + chunk] = a.cpu().numpy()
out_v[i:i + chunk] = v.cpu().numpy()
return out_i, out_v
except Exception as e:
from .device import notify_cuda_error
notify_cuda_error(e)
# Переход к расчету на CPU
out_i = np.empty((X.shape[0], k), np.int64)
out_v = np.empty((X.shape[0], k), np.float32)
for i in range(0, X.shape[0], chunk):
z = (X[i:i + chunk] - self.mean) / self.scale
y = np.maximum(z @ self.W.T, 0.0)
a = np.argpartition(-y, k - 1, axis=1)[:, :k]
v = np.take_along_axis(y, a, axis=1)
v *= k / np.maximum(v.sum(1, keepdims=True), 1e-6)
out_i[i:i + chunk] = a
out_v[i:i + chunk] = v
return out_i, out_v
# ------------------------------------------------------------------ выход
def score(self, X: np.ndarray) -> np.ndarray:
"""Вероятность «это посторонний предмет», 0…1."""
a, v = self.encode(X)
z = self.logit(X, code=(a, v))
return 1.0 / (1.0 + np.exp(-z))
def logit(self, X: np.ndarray, code=None) -> np.ndarray:
a, v = code if code is not None else self.encode(X)
z = self.bias + (self.w_mbon[a] * v).sum(axis=1)
return self.gain * z + self.shift
def score_of(self, c: Candidate) -> float:
return float(self.score(describe_full(c)[None, :])[0])
def _ref_at(self, d: np.ndarray) -> np.ndarray:
"""Типичный отсчёт модели на дальности d (по обучающей выборке)."""
mid = 0.5 * (self.ref_edges[:-1] + self.ref_edges[1:])
return np.interp(d, mid, self.ref_z)
def _prior_corr(self, d: np.ndarray, prior_from: float) -> np.ndarray:
"""Поправка на распространённость предметов: логит → отношение.
Модель калибрована в апостериорную вероятность, а доля предметов
среди кандидатов падает с 42 % вблизи до 3.1 % за 160 м. Поэтому
честная вероятность на дальности низка даже у настоящего предмета:
замер даёт 0.051 у вставленного человека на 150 м — при 0.003 у
обстановки, то есть модель РАЗЛИЧАЕТ (AUC 0.743), но абсолютная
величина раздавлена редкостью. Детектору нужно отношение
правдоподобий, а не доля, поэтому из логита вычитается типичный
для этой полосы — ровно как число лучей уже нормируется на
ожидаемое для дальности. Ближе `prior_from` поправка нулевая.
"""
if self.ref_z is None or prior_from <= 0.0:
return np.zeros_like(d)
base = float(self._ref_at(np.array([prior_from], np.float32))[0])
corr = base - self._ref_at(d)
return np.where(d > prior_from, np.maximum(corr, 0.0), 0.0)
def annotate(self, cands: list[Candidate], prior_from: float = 0.0
) -> list[Candidate]:
if not cands:
return cands
X = np.stack([describe_full(c) for c in cands])
z = self.logit(X)
if prior_from > 0.0:
d = np.array([c.d for c in cands], np.float32)
z = z + self._prior_corr(d, prior_from)
for c, zz in zip(cands, z):
c.extra["mbon"] = float(1.0 / (1.0 + np.exp(-zz)))
return cands
# ------------------------------------------------------------------ обучение
def _learn_cpu(self, X: np.ndarray, y: np.ndarray, *, epochs: int = 60,
lr: float = 4.0, l2: float = 1e-5, verbose: bool = False) -> None:
"""Обучение на CPU через NumPy."""
y = np.asarray(y, np.float32)
a, v = self.encode(X, device="cpu")
n, k = a.shape
flat = a.ravel()
for ep in range(epochs):
z = self.bias + (self.w_mbon[a] * v).sum(axis=1)
p = 1.0 / (1.0 + np.exp(-z))
g = (p - y) / n
grad = np.bincount(flat, weights=np.repeat(g, k) * v.ravel(),
minlength=self.cfg.n_kc).astype(np.float32)
self.w_mbon -= lr * (grad + l2 * self.w_mbon)
self.bias -= np.float32(lr * g.sum())
if verbose and (ep + 1) % 50 == 0:
eps = 1e-7
loss = -(y * np.log(p + eps) + (1 - y) * np.log(1 - p + eps)).mean()
print(f" эпоха {ep + 1:4d}: логистическая потеря {loss:.4f}")
zr = self.bias + (self.w_mbon[a] * v).sum(axis=1)
self._calibrate(zr, y)
zc = self.gain * zr + self.shift
dd = np.exp(np.asarray(X[:, 0], np.float64))
ref = []
for lo, hi in zip(self.ref_edges[:-1], self.ref_edges[1:]):
m = (dd >= lo) & (dd < hi)
ref.append(float(np.median(zc[m])) if m.sum() >= 50
else (ref[-1] if ref else 0.0))
self.ref_z = np.array(ref, np.float32)
def _learn_gpu(self, X: np.ndarray, y: np.ndarray, *, epochs: int = 60,
lr: float = 4.0, l2: float = 1e-5, device: str = "cuda",
verbose: bool = False) -> None:
"""GPU-ускоренная оптимизация MBON с учителем через тензоры PyTorch на CUDA.
Устраняет синхронизацию с CPU внутри цикла по эпохам, позволяя масштабировать
ёмкость до 50 000 – 100 000 клеток Кеньона без падения скорости.
"""
import torch
dev = torch.device(device)
y_t = torch.as_tensor(y, dtype=torch.float32, device=dev)
n = y_t.shape[0]
k = self.n_active
m = torch.as_tensor(self.mean, dtype=torch.float32, device=dev)
s = torch.as_tensor(self.scale, dtype=torch.float32, device=dev)
w_proj = torch.as_tensor(self.W, dtype=torch.float32, device=dev).T.contiguous()
chunk = max(1, int(2 ** 26 // max(self.cfg.n_kc, 1)))
a_list, v_list = [], []
with torch.no_grad():
for i in range(0, n, chunk):
t = torch.as_tensor(X[i:i + chunk], dtype=torch.float32, device=dev)
y_proj = torch.relu(((t - m) / s) @ w_proj)
v_chunk, a_chunk = torch.topk(y_proj, k, dim=1)
v_chunk = v_chunk * (k / v_chunk.sum(1, keepdim=True).clamp_min(1e-6))
a_list.append(a_chunk)
v_list.append(v_chunk)
a_t = torch.cat(a_list, dim=0) if len(a_list) > 1 else a_list[0]
v_t = torch.cat(v_list, dim=0) if len(v_list) > 1 else v_list[0]
w_mbon_t = torch.as_tensor(self.w_mbon, dtype=torch.float32, device=dev)
bias_t = torch.tensor(self.bias, dtype=torch.float32, device=dev)
flat_a = a_t.reshape(-1)
for ep in range(epochs):
z = bias_t + torch.sum(w_mbon_t[a_t] * v_t, dim=1)
p = torch.sigmoid(z)
g = (p - y_t) / n
weights = (g.unsqueeze(1) * v_t).reshape(-1)
grad = torch.zeros(self.cfg.n_kc, dtype=torch.float32, device=dev)
grad.scatter_add_(0, flat_a, weights)
w_mbon_t -= lr * (grad + l2 * w_mbon_t)
bias_t -= lr * g.sum()
if verbose and (ep + 1) % 50 == 0:
loss = -(y_t * torch.log(p.clamp_min(1e-7)) + (1.0 - y_t) * torch.log((1.0 - p).clamp_min(1e-7))).mean()
print(f" [GPU] эпоха {ep + 1:4d}: логистическая потеря {loss.item():.4f}")
zr_t = bias_t + torch.sum(w_mbon_t[a_t] * v_t, dim=1)
# Калибровка Платта полностью на GPU
g_t = torch.tensor(1.0, dtype=torch.float32, device=dev)
sh_t = torch.tensor(0.0, dtype=torch.float32, device=dev)
for _ in range(400):
p_cal = torch.sigmoid(g_t * zr_t + sh_t)
e_cal = p_cal - y_t
g_t -= 2.0 * (e_cal * zr_t).mean() / torch.clamp_min((zr_t * zr_t).mean(), 1e-6)
sh_t -= 2.0 * e_cal.mean()
self.w_mbon = w_mbon_t.cpu().numpy()
self.bias = np.float32(bias_t.item())
self.gain = np.float32(g_t.item())
self.shift = np.float32(sh_t.item())
zr = zr_t.cpu().numpy()
zc = self.gain * zr + self.shift
dd = np.exp(np.asarray(X[:, 0], np.float64))
ref = []
for lo, hi in zip(self.ref_edges[:-1], self.ref_edges[1:]):
m_mask = (dd >= lo) & (dd < hi)
ref.append(float(np.median(zc[m_mask])) if m_mask.sum() >= 50
else (ref[-1] if ref else 0.0))
self.ref_z = np.array(ref, np.float32)
def learn(self, X: np.ndarray, y: np.ndarray, *, epochs: int = 60,
lr: float = 4.0, l2: float = 1e-5, device: str | None = None,
verbose: bool = False) -> None:
"""Логистическая регрессия по разрежённому коду — депрессия с учителем.
Поддерживает прозрачное переключение между NVIDIA GPU (CUDA) и CPU.
"""
target_dev = device or "cpu" # без явной просьбы — процессор, как раньше
if target_dev == "auto":
from .device import get_device
target_dev = get_device("auto")
if target_dev.startswith("cuda"):
try:
self._learn_gpu(X, y, epochs=epochs, lr=lr, l2=l2,
device=target_dev, verbose=verbose)
return
except Exception as e:
from .device import notify_cuda_error
notify_cuda_error(e)
import logging
logging.getLogger("flyguard.mbon").warning("GPU learning failed (%s), fallback to CPU", e)
self._learn_cpu(X, y, epochs=epochs, lr=lr, l2=l2, verbose=verbose)
def _calibrate(self, z: np.ndarray, y: np.ndarray, iters: int = 400) -> None:
"""Шкалирование Платта: подобрать наклон и сдвиг по обучающей выборке."""
g, sh = 1.0, 0.0
for _ in range(iters):
p = 1.0 / (1.0 + np.exp(-(g * z + sh)))
e = p - y
g -= 2.0 * float((e * z).mean()) / max(float((z * z).mean()), 1e-6)
sh -= 2.0 * float(e.mean())
self.gain, self.shift = np.float32(g), np.float32(sh)
# ------------------------------------------------------------------ хранение
def save(self, path: str | Path) -> None:
np.savez_compressed(path, w_mbon=self.w_mbon, bias=self.bias,
gain=self.gain, shift=self.shift,
mean=self.mean, scale=self.scale,
n_kc=self.cfg.n_kc, claws=self.cfg.claws,
sparsity=self.cfg.sparsity, seed=self.cfg.seed,
n_pn=self.n_pn, ref_edges=self.ref_edges,
ref_z=(self.ref_z if self.ref_z is not None
else np.zeros(0, np.float32)))
@classmethod
def load(cls, path: str | Path) -> "MbonReadout":
"""Загрузка через `cls`, а не через имя класса: считывание по трекам —
тот же контур с другим входом, и оно наследуется отсюда целиком."""
d = np.load(path, allow_pickle=False)
cfg = MbonConfig(n_kc=int(d["n_kc"]), claws=int(d["claws"]),
sparsity=float(d["sparsity"]), seed=int(d["seed"]))
m = cls(cfg, n_pn=int(d["n_pn"]))
m.w_mbon = d["w_mbon"].astype(np.float32)
m.bias = np.float32(d["bias"])
m.gain = np.float32(d["gain"])
m.shift = np.float32(d["shift"])
m.mean = d["mean"].astype(np.float32)
m.scale = d["scale"].astype(np.float32)
if "ref_z" in d.files and d["ref_z"].size:
m.ref_edges = d["ref_edges"].astype(np.float32)
m.ref_z = d["ref_z"].astype(np.float32)
return m

398
ros2_ws/src/flyguard/flyguard/medulla.py Normal file
View file

@ -0,0 +1,398 @@
"""MEDULLA и LOBULA PLATE — движение: T4/T5, LPTC и LPLC2.
Три схемы из коннектома, работающие подряд:
* **T4/T5** — элементарные детекторы движения. Каждый тип существует в четырёх
подтипах, настроенных на четыре стороны света в поле зрения; T4 читает ON-канал,
T5 — OFF. Вычислительно это коррелятор Хассенштайна–Райхардта: сигнал одного
омматидия задерживается и умножается на сигнал соседнего, разность двух таких
произведений даёт направленный отклик.
* **LPTC (HS/VS)** — широкопольные тангенциальные клетки лобулярной пластинки.
Каждая суммирует выход тысяч T4/T5 по своему рецептивному полю и тем самым
измеряет **собственное движение**. Здесь это критично: колёсной одометрии в
задаче нет, и скорость поезда неоткуда взять, кроме как из самого потока.
* **LPLC2** — детектор надвигания. Его дендриты разложены на четыре слоя так,
что клетка отвечает только на поток, расходящийся **из центра её рецептивного
поля**, и подавляется однородным широкопольным потоком. То есть она отделяет
«на меня что-то летит» от «я сам двигаюсь». Выход идёт на гигантское волокно.
Практически: продвижение поезда за кадр оценивается корреляцией продольного
профиля тоннеля (устойчиво и дёшево), а T4/T5 и LPLC2 дают карту остаточного
движения — того, что не объясняется собственным ходом.
"""
from __future__ import annotations
from dataclasses import dataclass
import numpy as np
# шаг гистограммы продольного профиля, м
PROFILE_BIN = 0.5
PROFILE_MAX = 250.0
MAX_SPEED = 35.0 # м/с, заведомо выше любого метропоезда
MAX_ACCEL = 3.0 # м/с², предел разгона и экстренного торможения состава
WARMUP_FRAMES = 4 # столько кадров оценка принимается как есть, без фильтра
@dataclass
class EgoMotion:
"""Собственное движение за один кадр."""
ds: float # продвижение вперёд, м
speed: float # м/с
yaw_deg: float # поворот за кадр, °
conf: float # 0…1, качество корреляционного пика
dt: float
@property
def kmh(self) -> float:
return self.speed * 3.6
PROFILE_MIN = 14.0 # ближняя зона в профиль не идёт: там сдвиг кадра не читается
def longitudinal_profile(d: np.ndarray, valid: np.ndarray) -> np.ndarray:
"""Продольная «подпись» тоннеля: сколько лучей оборвалось на каждой дальности.
Тюбинговые кольца, лотки, ниши и стыки дают ей богатый рисунок, поэтому
сдвиг профиля между кадрами читается как пройденный путь.
Две поправки, без которых корреляция залипает на нулевом сдвиге:
ближняя зона исключается (там на один метр пути приходятся тысячи лучей,
и её вклад подавляет всё остальное), а от профиля отнимается скользящее
среднее — остаётся только рисунок структур, без общей огибающей.
"""
n = int(PROFILE_MAX / PROFILE_BIN)
dd = d[valid]
dd = dd[(dd > PROFILE_MIN) & (dd < PROFILE_MAX)]
if dd.size < 50:
return np.zeros(n, np.float32)
p = np.bincount((dd / PROFILE_BIN).astype(np.int32), minlength=n)[:n].astype(np.float32)
p = np.log1p(p)
k = 11 # ≈5 м — крупнее шага структур
kern = np.ones(k, np.float32) / k
env = np.convolve(p, kern, mode="same")
return (p - env).astype(np.float32)
def _norm(x: np.ndarray) -> np.ndarray:
x = x - x.mean()
s = np.linalg.norm(x)
return x / s if s > 1e-6 else x
def match_shift(prev: np.ndarray, cur: np.ndarray, max_bins: int,
prior_bins: float | None = None, prior_w: float = 0.03) -> tuple[float, float]:
"""Сдвиг `cur` относительно `prev` по максимуму нормированной корреляции.
Профиль текущего кадра смещён к меньшим дальностям на пройденный путь,
поэтому ищется такой сдвиг k, при котором cur[i] ≈ prev[i + k], k ≥ 0.
Слабый приор по предыдущей скорости снимает неоднозначность на периодических
структурах вроде тюбинговых колец; его вес нормирован на диапазон поиска,
чтобы он подправлял выбор между близкими пиками, а не диктовал ответ.
Корреляция считается по общей части профилей, поэтому при больших сдвигах
выборка короче — нормировка на длину не даёт этому создать ложный уклон.
"""
n = prev.size
ks = np.arange(0, max_bins + 1)
scores = np.empty(ks.size, np.float32)
for i, k in enumerate(ks):
m = n - k
scores[i] = float(np.dot(_norm(cur[:m]), _norm(prev[k:k + m])))
if prior_bins is not None and max_bins > 0:
scores = scores - prior_w * ((ks - prior_bins) / max_bins) ** 2
i = int(np.argmax(scores))
peak = float(scores[i])
if 0 < i < ks.size - 1: # уточнение параболой по трём точкам
y0, y1, y2 = scores[i - 1], scores[i], scores[i + 1]
den = y0 - 2 * y1 + y2
sub = 0.5 * (y0 - y2) / den if abs(den) > 1e-9 else 0.0
else:
sub = 0.0
return float(ks[i] + np.clip(sub, -1, 1)), peak
class RayIndexer:
"""Обратный поиск по решётке: направление → (кольцо, столбец).
Нужен, чтобы перепроецировать точки предыдущего кадра в текущую решётку.
Элевации каналов заданы убывающей таблицей, поэтому индекс кольца берётся
линейной интерполяцией по ней, а не делением на постоянный шаг.
"""
def __init__(self, layout):
el = np.asarray(layout.el_deg, np.float64)
order = np.argsort(el)
self.el_sorted = el[order]
self.ring_sorted = order.astype(np.float64)
self.az0 = float(layout.az_grid_deg[0])
self.step = float(layout.az_step_deg)
self.n_az = int(layout.n_az)
self.n_rings = int(layout.n_rings)
def __call__(self, az_deg: np.ndarray, el_deg: np.ndarray):
col = np.rint((az_deg - self.az0) / self.step).astype(np.int32)
ring = np.rint(np.interp(el_deg, self.el_sorted, self.ring_sorted)).astype(np.int32)
ok = (col >= 0) & (col < self.n_az) & (ring >= 0) & (ring < self.n_rings)
np.clip(col, 0, self.n_az - 1, out=col)
np.clip(ring, 0, self.n_rings - 1, out=ring)
return ring, col, ok
def advance_score(prev_pts: np.ndarray, r_cur: np.ndarray, valid_cur: np.ndarray,
indexer: RayIndexer, ds: float) -> float:
"""Доля точек прошлого кадра, попавших в текущий кадр при сдвиге вперёд на ds.
Это и есть проверка широкопольного потока на согласие с моделью собственного
движения — то, чем заняты тангенциальные клетки лобулярной пластинки.
"""
d = prev_pts[:, 0] - ds
u = prev_pts[:, 1]
h = prev_pts[:, 2]
m = d > 2.0
if m.sum() < 50:
return 0.0
d, u, h = d[m], u[m], h[m]
rho = np.hypot(d, u)
r = np.sqrt(rho * rho + h * h)
az = np.degrees(np.arctan2(u, d))
el = np.degrees(np.arctan2(h, rho))
ring, col, ok = indexer(az, el)
rc = r_cur[ring, col]
good = ok & valid_cur[ring, col]
if good.sum() < 50:
return 0.0
err = np.abs(rc[good] - r[good])
tol = np.maximum(0.25, 0.015 * r[good])
return float(np.mean(err < tol))
class EgoMotionEstimator:
"""LPTC-аналог: одна широкопольная оценка собственного движения на кадр."""
def __init__(self, dt_nominal: float = 0.1):
self.prev_profile: np.ndarray | None = None
self.prev_az: np.ndarray | None = None
self.prev_stamp: float | None = None
self.prev_ds: float | None = None
self.prev_pts: np.ndarray | None = None
self.indexer: RayIndexer | None = None
self.n_sample = 6000
self.v_filt: float | None = None
self.n_updates = 0
self.dt_nominal = dt_nominal
def update(self, tf, stamp: float) -> EgoMotion:
dt = self.dt_nominal
if self.prev_stamp is not None:
got = stamp - self.prev_stamp
if 0.01 < got < 1.0:
dt = got
prof = longitudinal_profile(tf.d, tf.valid)
az = np.log1p(tf.valid.sum(axis=0)).astype(np.float32)
ds, conf, yaw = 0.0, 0.0, 0.0
if self.prev_profile is not None and np.any(prof):
max_bins = int(MAX_SPEED * dt / PROFILE_BIN) + 2
prior = None if self.prev_ds is None else self.prev_ds / PROFILE_BIN
shift, conf = match_shift(self.prev_profile, prof, max_bins, prior_bins=prior)
ds = shift * PROFILE_BIN
# независимая грубая оценка: дальние фронтальные поверхности приближаются
# ровно на пройденный путь
direct = self._direct_advance(tf)
seeds = [s for s in (ds, direct, self.prev_ds, 0.0) if s is not None]
# уточнение сопоставлением кадров: перебор сдвига с проверкой согласия
refined = self._refine(tf, seeds, dt)
if refined is not None:
ds, conf = refined
elif direct is not None and conf < 0.45:
ds, conf = direct, max(conf, 0.3)
if self.prev_az is not None and self.prev_az.size == az.size:
yaw_bins, _ = _centred_shift(self.prev_az, az, max_shift=40)
yaw = yaw_bins * float(tf.layout.az_step_deg)
ds = self._filter_speed(ds, conf, dt)
self.prev_profile = prof
self.prev_az = az
self.prev_stamp = stamp
self.prev_r = np.where(tf.valid, tf.r, np.nan).astype(np.float32)
self.prev_pts = _sample_points(tf, self.n_sample)
self.prev_ds = ds if self.prev_ds is None else 0.6 * ds + 0.4 * self.prev_ds
return EgoMotion(ds=ds, speed=ds / dt, yaw_deg=yaw, conf=float(conf), dt=dt)
def _filter_speed(self, ds: float, conf: float, dt: float) -> float:
"""Сгладить оценку скорости с учётом физики состава.
Сопоставление кадров иногда «срывается» на резкой смене обстановки —
например на переходе круглого тоннеля в двухпутный — и выдаёт то ноль,
то предел диапазона поиска. Поезд так не умеет: за 0.1 с скорость не
меняется больше чем на a·dt. Измерение принимается с весом, равным
согласию перепроекции, и ограничивается физическим пределом ускорения,
поэтому редкий срыв сглаживается, а настоящее торможение отслеживается
за десятые доли секунды.
"""
self.n_updates += 1
v_meas = ds / max(dt, 1e-3)
if self.v_filt is None or self.n_updates <= WARMUP_FRAMES:
self.v_filt = v_meas
return ds
gain = float(np.clip(conf, 0.05, 0.6))
v = self.v_filt + gain * (v_meas - self.v_filt)
limit = MAX_ACCEL * dt
v = float(np.clip(v, self.v_filt - limit, self.v_filt + limit))
self.v_filt = max(v, 0.0)
return self.v_filt * dt
def _refine(self, tf, seeds: list[float], dt: float):
"""Двухэтапный перебор сдвига вокруг стартовых гипотез."""
pts = getattr(self, "prev_pts", None)
if pts is None or pts.shape[0] < 500:
return None
if self.indexer is None or self.indexer.n_az != tf.layout.n_az:
self.indexer = RayIndexer(tf.layout)
hi = MAX_SPEED * dt
# гипотезы от дешёвых оценок плюс редкая сетка на весь диапазон;
# округление до 10 см убирает дубликаты и держит число проб низким
grid = {round(float(np.clip(s, 0.0, hi)), 1) for s in seeds}
for s in list(grid):
grid.update(round(float(np.clip(s + o, 0.0, hi)), 1) for o in (-0.5, 0.5))
grid.update(round(float(x), 1) for x in np.linspace(0.0, hi, 8))
cand = np.array(sorted(grid))
sc = np.array([advance_score(pts, tf.r, tf.valid, self.indexer, s) for s in cand])
best = float(cand[int(np.argmax(sc))])
fine = np.clip(best + np.linspace(-0.2, 0.2, 5), 0.0, hi)
sf = np.array([advance_score(pts, tf.r, tf.valid, self.indexer, s) for s in fine])
i = int(np.argmax(sf))
if sf[i] < 0.05:
return None
# уточнение параболой по трём точкам вокруг лучшей
ds = float(fine[i])
if 0 < i < fine.size - 1:
y0, y1, y2 = sf[i - 1], sf[i], sf[i + 1]
den = y0 - 2 * y1 + y2
if abs(den) > 1e-9:
ds += 0.5 * (y0 - y2) / den * (fine[1] - fine[0])
return float(np.clip(ds, 0.0, hi)), float(sf[i])
def _direct_advance(self, tf, d_lo: float = 35.0, d_hi: float = 200.0,
grad_max: float = 0.6) -> float | None:
"""Медианное приближение дальних поверхностей, обращённых к сенсору.
Стены тоннеля идут почти вдоль движения, и их дальность при езде почти
не меняется, поэтому они отбрасываются по градиенту дальности вдоль
строки: остаются только фронтальные поверхности, для которых убывание
дальности равно пройденному пути.
"""
cur = np.where(tf.valid, tf.r, np.nan).astype(np.float32)
prev = getattr(self, "prev_r", None)
if prev is None or prev.shape != cur.shape:
return None
with np.errstate(invalid="ignore"):
grad = np.abs(np.gradient(cur, axis=1))
m = (np.isfinite(cur) & np.isfinite(prev) & (cur > d_lo) & (cur < d_hi)
& (grad < grad_max))
if m.sum() < 150:
return None
diff = prev[m] - cur[m]
diff = diff[np.abs(diff) < MAX_SPEED * 0.12]
if diff.size < 100:
return None
return float(np.clip(np.median(diff), 0.0, MAX_SPEED * 0.12))
def _sample_points(tf, n: int) -> np.ndarray:
"""Равномерная выборка точек кадра: (N, 3) = (d, u, z) в системе сенсора.
Берётся именно z сенсора, а не высота над рельсом: перепроекция идёт в
решётку лучей, а она задана относительно сенсора.
"""
m = tf.valid & (tf.d > 8.0) & (tf.d < 200.0)
idx = np.flatnonzero(m.ravel())
if idx.size == 0:
return np.zeros((0, 3), np.float32)
if idx.size > n:
idx = idx[:: max(1, idx.size // n)][:n]
return np.stack([tf.d.ravel()[idx], tf.u.ravel()[idx], tf.z.ravel()[idx]],
axis=1).astype(np.float32)
def _centred_shift(prev: np.ndarray, cur: np.ndarray, max_shift: int) -> tuple[float, float]:
"""Сдвиг в обе стороны — для рыскания."""
a, b = _norm(prev), _norm(cur)
n = a.size
ks = np.arange(-max_shift, max_shift + 1)
sc = np.empty(ks.size, np.float32)
for i, k in enumerate(ks):
if k >= 0:
m = n - k
sc[i] = float(np.dot(b[:m], a[k:k + m]))
else:
m = n + k
sc[i] = float(np.dot(b[-k:-k + m], a[:m]))
i = int(np.argmax(sc))
return float(ks[i]), float(sc[i])
# --------------------------------------------------------------------------- T4/T5
class EmdBank:
"""Коррелятор Хассенштайна–Райхардта на четыре направления.
Работает на прорежённой решётке: широкопольным клеткам мухи тоже не нужна
полная разрешающая способность фасеток, им важна статистика по полю.
"""
DIRECTIONS = ((0, 1), (0, -1), (1, 0), (-1, 0)) # (Δкольцо, Δстолбец)
def __init__(self, decimate: tuple[int, int] = (2, 4), tau_frames: float = 1.5):
self.dec = decimate
self.alpha = float(np.exp(-1.0 / max(tau_frames, 1e-3)))
self.delayed: np.ndarray | None = None
def _down(self, a: np.ndarray) -> np.ndarray:
dh, dw = self.dec
h = a.shape[0] // dh * dh
w = a.shape[1] // dw * dw
return a[:h, :w].reshape(h // dh, dh, w // dw, dw).mean(axis=(1, 3))
def update(self, signal: np.ndarray) -> np.ndarray:
"""Вернуть (4, h, w) откликов на движение в четырёх направлениях."""
s = self._down(signal).astype(np.float32)
if self.delayed is None or self.delayed.shape != s.shape:
self.delayed = s.copy()
return np.zeros((4,) + s.shape, np.float32)
d = self.delayed
out = np.zeros((4,) + s.shape, np.float32)
for k, (di, dj) in enumerate(self.DIRECTIONS):
a = np.roll(s, (di, dj), axis=(0, 1))
ad = np.roll(d, (di, dj), axis=(0, 1))
out[k] = d * a - s * ad # задержанный × соседний, антисимметрично
self.delayed = self.alpha * d + (1.0 - self.alpha) * s
return out
def looming(emd: np.ndarray) -> np.ndarray:
"""LPLC2: отклик на поток, расходящийся из центра рецептивного поля.
Дендриты LPLC2 разложены по четырём слоям так, что каждый слой принимает
T4/T5 «своего» направления с той стороны поля, куда поток должен уходить при
надвигании. Сумма четырёх слоёв и есть дивергенция потока.
"""
right, left, down, up = emd
div = np.zeros_like(right)
div[:, 1:-1] += right[:, 2:] - left[:, :-2]
div[1:-1, :] += down[2:, :] - up[:-2, :]
return np.maximum(div, 0.0)

524
ros2_ws/src/flyguard/flyguard/mushroom_body.py Normal file
View file

@ -0,0 +1,524 @@
"""MUSHROOM BODY — память нормального тоннеля и детектор новизны.
Схема взята из коннектома напрямую и почти не изменена:
* **PN → KC.** Около 2000 клеток Кеньона на полушарие; каждая получает вход
всего от ~6 проекционных нейронов, выбранных почти случайно (в FlyWire и
hemibrain среднее число «когтей» ≈ 5–7). Это случайная разрежённая проекция,
поднимающая размерность примерно в 40 раз.
* **APL.** Один гигантский тормозный нейрон собирает активность всех KC и
возвращает торможение всем сразу. Результат — «победитель забирает всё»:
одновременно активны около 5 % клеток Кеньона.
* **KC → MBON.** Выход считывается немногими выходными нейронами. Синапсы
KC→MBON **депрессируются** при повторном предъявлении стимула, поэтому
знакомый запах даёт слабый ответ, а новый — сильный. Именно так устроена
«familiarity suppression» у MBON-α′3.
Почему это точно наша задача. Разметки нет, и почти все данные — пустой тоннель.
Геометрический слой честно сообщает обо всём, что торчит в габарит, и вместе с
препятствиями выдаёт кабельные лотки, ниши, гермозатворы, кромки платформ и
стрелочные приводы. Грибовидное тело обучается на пустых проездах **без единой
метки** и после этого молча гасит всё, что тоннель уже показывал, оставляя
громкими только незнакомые формы.
Хранится не образ, а битовый код: поиск знакомости — это индексация массива,
поэтому стадия стоит микросекунды и легко укладывается в реальное время.
"""
from __future__ import annotations
from dataclasses import dataclass
from pathlib import Path
import numpy as np
from .lobula import Candidate
# имена признаков-«проекционных нейронов»; порядок фиксирован и входит в артефакт
FEATURES = (
"log_d", "lat", "abs_lat", "h", "log_w", "log_h", "aspect",
"log_rays", "fill", "ang_w", "ang_h", "log_gap", "shadow",
"inten", "el", "floor_def", "rings", "cols",
"log_depth", "elongation", "containment", "h_min",
)
def describe(c: Candidate) -> np.ndarray:
"""Вектор признаков кандидата — активность «проекционных нейронов».
Сознательно смешаны две группы: форма и угловой размер (обобщаются на любой
тоннель) и положение в сечении (именно оно делает лоток лотком). Первая даёт
переносимость, вторая — способность запомнить штатную обстановку.
"""
d = max(c.d, 1.0)
w = max(c.width, 0.02)
hh = max(c.height, 0.02)
area = max((c.bbox[1] - c.bbox[0] + 1) * (c.bbox[3] - c.bbox[2] + 1), 1)
return np.array([
np.log(d),
c.u,
abs(c.u),
c.h,
np.log(w),
np.log(hh),
np.log(w / hh),
np.log(max(c.n_rays, 1)),
c.n_rays / area,
np.degrees(w / d),
np.degrees(hh / d),
np.log1p(max(c.gap, 0.0)),
c.shadow,
np.log1p(max(c.inten, 0.0)),
c.el_deg,
np.log1p(max(c.floor_deficit, 0.0)),
np.log(max(c.n_rings, 1)),
np.log(max(c.n_cols, 1)),
np.log1p(max(c.depth, 0.0)),
np.log(max(c.depth, 0.05) / max(w, hh)), # вытянутость вдоль пути
c.containment,
c.h_min, # есть ли опора снизу
], dtype=np.float32)
@dataclass
class MushroomBodyConfig:
"""Параметры памяти.
`claws = 6` взято прямо из коннектома: клетка Кеньона получает в среднем
5–7 входов от проекционных нейронов. Это значение и оказалось лучшим в
подборе (`tools/tune_memory.py`): 4 когтя дают AUC 0.76, 8 — 0.89, 10 —
0.87, шесть — 0.89…0.90.
Число клеток Кеньона, напротив, увеличено против мушиных ~2000. Причина
инженерная: муха за жизнь встречает сотни запахов, а нам нужно запомнить
десятки тысяч видов тоннельной обстановки. При 2000 клетках и 5 % активных
память насыщается после нескольких тысяч примеров — подавлены все синапсы, и
новым не выглядит уже ничто, включая настоящее препятствие. Разрежённость
поэтому тоже снижена: активны 50 клеток из 50 000.
"""
n_kc: int = 50_000 # клеток Кеньона
claws: int = 6 # входов на клетку Кеньона (из коннектома)
sparsity: float = 0.001 # доля активных после торможения APL
seed: int = 20240617
class MushroomBody:
"""Разрежённое кодирование + выученная знакомость."""
def __init__(self, cfg: MushroomBodyConfig | None = None, n_pn: int | None = None):
self.cfg = cfg or MushroomBodyConfig()
self.n_pn = n_pn or len(FEATURES)
self.n_active = max(1, int(round(self.cfg.n_kc * self.cfg.sparsity)))
rng = np.random.default_rng(self.cfg.seed)
# разрежённая случайная проекция PN → KC: по `claws` входов на клетку
idx = np.stack([rng.choice(self.n_pn, size=self.cfg.claws, replace=False)
for _ in range(self.cfg.n_kc)])
sign = rng.choice((-1.0, 1.0), size=idx.shape)
w = np.zeros((self.cfg.n_kc, self.n_pn), np.float32)
np.put_along_axis(w, idx, sign.astype(np.float32), axis=1)
self.W = w
# нормировка входа (аналог дивизивной нормировки на общую интенсивность)
self.mean = np.zeros(self.n_pn, np.float32)
self.scale = np.ones(self.n_pn, np.float32)
# вес синапсов KC → MBON: 1 — «не видели», 0 — «совсем привычно»
self.w_mbon = np.ones(self.cfg.n_kc, np.float32)
self.n_seen = 0
# ------------------------------------------------------------------ кодирование
def fit_normalizer(self, X: np.ndarray) -> None:
self.mean = X.mean(0).astype(np.float32)
s = X.std(0).astype(np.float32)
self.scale = np.where(s > 1e-6, s, 1.0).astype(np.float32)
def encode(self, X: np.ndarray, device: str | None = None) -> np.ndarray:
"""Вектора признаков → индексы активных клеток Кеньона, (N, n_active).
`device="cuda"` переносит проекцию и отбор на GPU: обучение на сотнях
тысяч примеров так идёт в десятки раз быстрее. В инференсе кандидатов
единицы, и путь через CPU заведомо дешевле запуска ядра.
"""
X = np.atleast_2d(np.asarray(X, np.float32))
if X.shape[1] != self.n_pn:
raise ValueError(
f"память обучена на {self.n_pn} признаках, а дескриптор даёт "
f"{X.shape[1]}: набор признаков менялся — переобучите память "
f"(tools/train_mushroom_body.py)")
k = self.n_active
# матрица откликов (N × n_kc) при большой выборке не помещается в память
# целиком, поэтому кодирование идёт порциями фиксированного объёма
chunk = max(1, int(2 ** 26 // max(self.cfg.n_kc, 1)))
target_dev = device
if target_dev == "auto":
from .device import get_device
target_dev = get_device("auto")
if target_dev and target_dev != "cpu":
try:
import torch
out = np.empty((X.shape[0], k), np.int64)
with torch.no_grad():
m = torch.as_tensor(self.mean, device=target_dev)
s = torch.as_tensor(self.scale, device=target_dev)
w = torch.as_tensor(self.W, device=target_dev).T.contiguous()
for i in range(0, X.shape[0], chunk):
t = torch.as_tensor(X[i:i + chunk], device=target_dev)
y = ((t - m) / s) @ w
out[i:i + chunk] = torch.topk(y, k, dim=1).indices.cpu().numpy()
return out
except Exception as e:
from .device import notify_cuda_error
notify_cuda_error(e)
# Fallback на CPU ниже
if X.shape[0] <= chunk:
z = (X - self.mean) / self.scale
y = z @ self.W.T # (N, n_kc)
# APL: глобальное торможение оставляет только сильнейшие
return np.argpartition(-y, k - 1, axis=1)[:, :k]
out = np.empty((X.shape[0], k), np.int64)
for i in range(0, X.shape[0], chunk):
z = (X[i:i + chunk] - self.mean) / self.scale
y = z @ self.W.T
out[i:i + chunk] = np.argpartition(-y, k - 1, axis=1)[:, :k]
return out
# ------------------------------------------------------------------ обучение
def auto_rate(self, n_samples: int, target: float = 1.5) -> float:
"""Темп депрессии, согласованный с размером обучающей выборки.
Каждый пример депрессирует `n_active` клеток, поэтому на одну клетку
Кеньона в среднем приходится `n · n_active / n_kc` попаданий. Если темп
не уменьшать вместе с ростом выборки, после нескольких десятков тысяч
примеров подавлены все синапсы и новым не выглядит уже ничто.
Темп выбирается так, чтобы «типичная» клетка ослабла примерно в e^target
раз. Тогда шкала новизны отражает **частоту** обстановки: постоянно
встречающиеся лотки и ниши уходят в ноль, а редкая форма остаётся
заметной. Это и есть смысл familiarity suppression, а не «видел хоть раз».
"""
mean_hits = max(n_samples * self.n_active / self.cfg.n_kc, 1e-6)
return float(np.clip(target / mean_hits, 1e-4, 0.5))
def learn(self, X: np.ndarray, rate: float = 0.35, device: str | None = None) -> None:
"""Депрессия синапсов KC→MBON на предъявленных примерах.
Каждое предъявление умножает вес активных синапсов на (1 − rate), что
для набора сразу эквивалентно возведению в степень по числу попаданий, —
поэтому цикл по примерам не нужен.
"""
target_dev = device or "cpu" # без явной просьбы — процессор, как раньше
if target_dev == "auto":
from .device import get_device
target_dev = get_device("auto")
if target_dev.startswith("cuda"):
try:
import torch
dev = torch.device(target_dev)
k = self.n_active
chunk = max(1, int(2 ** 26 // max(self.cfg.n_kc, 1)))
m = torch.as_tensor(self.mean, device=dev)
s = torch.as_tensor(self.scale, device=dev)
w = torch.as_tensor(self.W, device=dev).T.contiguous()
act_list = []
with torch.no_grad():
for i in range(0, X.shape[0], chunk):
t = torch.as_tensor(X[i:i + chunk], device=dev)
y = ((t - m) / s) @ w
top_idx = torch.topk(y, k, dim=1).indices
act_list.append(top_idx)
act_t = torch.cat(act_list, dim=0) if len(act_list) > 1 else act_list[0]
cnt_t = torch.bincount(act_t.reshape(-1), minlength=self.cfg.n_kc)
w_mbon_t = torch.as_tensor(self.w_mbon, device=dev)
w_mbon_t *= torch.pow(torch.tensor(1.0 - rate, device=dev), cnt_t.float())
self.w_mbon = w_mbon_t.cpu().numpy()
self.n_seen += X.shape[0]
return
except Exception as e:
from .device import notify_cuda_error
notify_cuda_error(e)
act = self.encode(X, device="cpu")
cnt = np.bincount(act.ravel(), minlength=self.cfg.n_kc)
self.w_mbon *= np.power(1.0 - rate, cnt).astype(np.float32)
self.n_seen += len(act)
def novelty(self, X: np.ndarray) -> np.ndarray:
"""Ответ MBON: 1 — совершенно ново, 0 — вполне привычно."""
act = self.encode(X)
return self.w_mbon[act].mean(axis=1)
def novelty_of(self, c: Candidate) -> float:
return float(self.novelty(describe(c)[None, :])[0])
def annotate(self, cands: list[Candidate]) -> list[Candidate]:
if not cands:
return cands
X = np.stack([describe(c) for c in cands])
nov = self.novelty(X)
for c, n in zip(cands, nov):
c.novelty = float(n)
return cands
# ------------------------------------------------------------------ хранение
def save(self, path: str | Path) -> None:
np.savez_compressed(path, w_mbon=self.w_mbon, mean=self.mean, scale=self.scale,
n_kc=self.cfg.n_kc, claws=self.cfg.claws,
sparsity=self.cfg.sparsity, seed=self.cfg.seed,
n_pn=self.n_pn, n_seen=self.n_seen,
features=np.array(FEATURES))
@staticmethod
def load(path: str | Path) -> "MushroomBody":
d = np.load(path, allow_pickle=False)
cfg = MushroomBodyConfig(n_kc=int(d["n_kc"]), claws=int(d["claws"]),
sparsity=float(d["sparsity"]), seed=int(d["seed"]))
mb = MushroomBody(cfg, n_pn=int(d["n_pn"]))
mb.w_mbon = d["w_mbon"].astype(np.float32)
mb.mean = d["mean"].astype(np.float32)
mb.scale = d["scale"].astype(np.float32)
mb.n_seen = int(d["n_seen"])
return mb
# Признаки для привыкания. Отличие от `FEATURES` принципиальное: здесь нет
# ничего, что меняется при подъезде к одному и тому же предмету. Ни дальности,
# ни числа лучей, ни углового размера, ни интенсивности (она падает как 1/R²),
# ни контраста в метрах (он растёт с дальностью). Остаются физические свойства
# формы: где она стоит в сечении, какая она и есть ли под ней опора.
#
# Иначе механизм не работает вовсе: код одного и того же кронштейна, снятого с
# 90 и с 40 м, разъезжается, повторы не узнаются, а подъезжающий предмет,
# наоборот, каждый кадр выглядит новой формой и привыкает сам к себе. Замерено:
# на полном наборе признаков за двенадцать разных мест новизна падала с 1.000
# до 0.981 — то есть ни на что.
#
# У мухи ту же роль играет инвариантность к концентрации запаха: APL нормирует
# общий уровень возбуждения, и один и тот же запах слабее или сильнее даёт
# почти один и тот же набор активных клеток Кеньона.
HAB_FEATURES = ("lat", "abs_lat", "h", "h_min", "log_w", "log_h",
"aspect", "log_depth", "elongation", "containment")
def describe_shape(c: Candidate) -> np.ndarray:
"""Вектор формы, не зависящий от дальности до неё."""
w = max(c.width, 0.02)
hh = max(c.height, 0.02)
return np.array([
c.u,
abs(c.u),
c.h,
c.h_min,
np.log(w),
np.log(hh),
np.log(w / hh),
np.log1p(max(c.depth, 0.0)),
np.log(max(c.depth, 0.05) / max(w, hh)),
c.containment,
], dtype=np.float32)
@dataclass
class HabituationConfig:
"""Параметры привыкания внутри одного проезда.
`n_kc` намеренно маленькое — 4000 против 50 000 у долговременной памяти, то
есть примерно как у самой мухи. Задача здесь обратная: не различить десятки
тысяч видов обстановки, а **обобщить** — чтобы седьмой кронштейн гасился
первыми шестью, хотя он и не в точности такой же. Чем меньше популяция и
чем шире разрежённость, тем сильнее перекрываются коды похожих форм.
"""
n_kc: int = 4000
claws: int = 4
sparsity: float = 0.02 # 80 активных клеток
seed: int = 20260214
rate: float = 0.25 # депрессия за одно НОВОЕ место
place_m: float = 5.0 # насколько разнести места, чтобы считать их разными
same_frac: float = 0.30 # доля клеток «с этого же места» — значит, то же самое
read_q: float = 0.75 # квантиль отсчёта MBON: доля клеток, которых мало
quant: float = 0.5 # шаг огрубления признаков, в долях разброса
recover_m: float = 800.0 # путь, за который привыкание сходит наполовину
warmup: int = 60 # кандидатов на разогрев нормировки
norm_n: int = 300 # после стольких нормировка замирает
min_novelty: float = 0.05 # ниже этого привыкание не опускает
class Habituation:
"""Кратковременное привыкание: гасит то, что повторяется в РАЗНЫХ местах пути.
Обученная память решает задачу «этот тоннель я уже видел». На новом участке
она бесполезна по определению: новизна у всего максимальна, улика копится
беспрепятственно, и ложных тревог становится 24.3 на километр вместо 9.1.
А приватный тест — это как раз новый участок.
Но даже на совершенно незнакомой линии есть признак, не требующий ни единого
прошлого проезда. **Тоннельная обстановка повторяется вдоль пути, а
посторонний предмет — нет.** Кабельный кронштейн, рама крепи, стык тюбингов
встречаются каждые несколько метров в одном и том же виде; упавший предмет
лежит в одном месте. Поэтому привыкание считается не по времени и не по
числу кадров, а по **числу разных точек пути**, где встретилась эта форма.
Разница принципиальна. Настоящий предмет виден сто кадров подряд, но всё это
время он стоит в одной точке мира: его код депрессируется один раз и
остаётся новым до конца подъезда. Кронштейны за те же сто кадров дают
десятки разных точек и гаснут.
Биологически это та же схема, только с другим учителем. Синапсы KC→MBON
депрессируются при повторном предъявлении — это и есть familiarity
suppression MBON-α′3. Момент депрессии здесь разрешает координата пути из
центрального комплекса: у мухи дофаминергические PPL1/PAM точно так же
решают, когда пластичности быть, и их вход зависит от состояния животного,
а не только от стимула.
Привыкание постепенно сходит (`recover_m`): выехав с однообразного перегона
на станцию, система снова смотрит в оба.
"""
def __init__(self, cfg: HabituationConfig | None = None):
self.cfg = cfg or HabituationConfig()
self.enc = MushroomBody(
MushroomBodyConfig(n_kc=self.cfg.n_kc, claws=self.cfg.claws,
sparsity=self.cfg.sparsity, seed=self.cfg.seed),
n_pn=len(HAB_FEATURES))
n = self.cfg.n_kc
self.w = np.ones(n, np.float32)
# где в последний раз эта клетка Кеньона участвовала в наблюдении
self.last_s = np.full(n, -1e9, np.float32)
self.places = 0
self._n = 0
self._sum = np.zeros(len(HAB_FEATURES), np.float64)
self._sq = np.zeros(len(HAB_FEATURES), np.float64)
@property
def level(self) -> float:
"""Насколько привыкла популяция: 0 — всё ново, 1 — всё знакомо."""
return float(1.0 - self.w.mean())
# ------------------------------------------------------------------ такт
def advance(self, ds: float) -> None:
"""Забывание по пройденному пути."""
if ds <= 0 or self.cfg.recover_m <= 0:
return
k = 1.0 - 0.5 ** (ds / self.cfg.recover_m)
self.w += (1.0 - self.w) * k
def _fit(self, X: np.ndarray) -> None:
"""Дивизивная нормировка по накопленной статистике проезда.
После `norm_n` кандидатов замирает, и это не мелочь. Пока нормировка
плывёт, вместе с ней плывёт и код: у неподвижного предмета, чьи признаки
формы вообще не меняются, набор активных клеток обновлялся настолько,
что предмет засчитывался как двадцать шесть разных мест и гасил сам
себя. С замороженной нормировкой одна и та же форма даёт один и тот же
код, и правило «то же место — не повтор» наконец работает.
Порог низкий намеренно. При 4000 в бэге `roundT_doubleT` (всего около
570 кандидатов за проезд) нормировка не замирала вовсе: привыкание
осталось на 0.20, медианная новизна кандидата 1.00 и ложных треков
ровно столько же, сколько без привыкания. Трёхсот кандидатов на десять
признаков для оценки среднего и разброса достаточно с запасом.
"""
self._n += X.shape[0]
if self._n > self.cfg.norm_n:
return
self._sum += X.sum(0)
self._sq += (X.astype(np.float64) ** 2).sum(0)
m = self._sum / self._n
v = np.maximum(self._sq / self._n - m * m, 1e-12)
self.enc.mean = m.astype(np.float32)
self.enc.scale = np.sqrt(v).astype(np.float32)
def _coarse(self, X: np.ndarray) -> np.ndarray:
"""Огрубить признаки до шага `quant` разброса — до кодирования.
Без этого механизм не работает вовсе. Два кронштейна, снятые в разных
местах, отличаются на сантиметры, случайная проекция разносит их в
разные наборы клеток, и второй не узнаёт первого. Замер: при ёмкости,
достаточной чтобы не насыщаться (20 000 клеток), привыкание давало
ровно ноль — 7.5 ложных трека на км против тех же 7.5 без него. Весь
видимый эффект маленькой популяции был не узнаванием повторов, а
насыщением, которое одинаково давит и обстановку, и предмет.
Огрубление до половины разброса заставляет близкие формы давать
буквально один и тот же код. Это coarse coding, и у мухи оно тоже есть:
клетка Кеньона реагирует на широкую область пространства запахов, а не
на точку.
"""
q = self.cfg.quant
if q <= 0:
return X
z = (X - self.enc.mean) / self.enc.scale
return (np.round(z / q) * q) * self.enc.scale + self.enc.mean
def update(self, cands: list[Candidate], s_path: float, ds: float) -> None:
"""Приглушить знакомое и запомнить то, что встретилось в новом месте.
`s_path` — пройденный путь, м; положение формы в мире считается как
`s_path + c.d`, поэтому неподвижный предмет остаётся в одной точке,
сколько бы кадров к нему ни ехали. Оценка собственного движения копит
около 3 % ошибки, то есть за подъезд с сотни метров точка уползает на
два-три метра — отсюда и `place_m`: меньший разнос считается тем же
местом, больший — новым.
"""
self.advance(ds)
if not cands:
return
X = np.stack([describe_shape(c) for c in cands])
self._fit(X)
if self._n < self.cfg.warmup:
return # нормировка ещё не осмысленна
act = self.enc.encode(self._coarse(X))
# Отсчёт берётся КВАНТИЛЕМ, а не средним, и это решает главную проблему
# механизма. Коды похожих форм перекрываются частично: кронштейн, гасясь
# сам, задевает часть клеток предмета. При среднем половина подавленных
# клеток уже вдвое снижает новизну предмета — замерено, что новизна
# вставленного человека падала с 0.76 до 0.20, а обнаружение на 70–90 м
# со 100 % до 58 %. Квантиль 0.75 требует, чтобы подавлено было не менее
# трёх четвертей клеток кода: частичное перекрытие форму уже не гасит,
# а настоящий повтор гасит полностью.
fam = np.quantile(self.w[act], self.cfg.read_q, axis=1)
lo = self.cfg.min_novelty
for c, f in zip(cands, fam):
# Берётся минимум, а не произведение: два канала знакомости, и
# достаточно одного, чтобы приглушить. Перемножение давило бы
# предмет дважды за одно и то же.
c.novelty = float(max(min(c.novelty, f), lo))
keep = 1.0 - self.cfg.rate
for i, c in enumerate(cands):
s_obj = np.float32(s_path + c.d)
a = act[i]
same = np.abs(self.last_s[a] - s_obj) <= self.cfg.place_m
# Отметка ставится всегда, в том числе клеткам, впервые попавшим в
# код: иначе подъезжающий предмет каждый кадр вербует свежие клетки
# и депрессирует сам себя.
self.last_s[a] = s_obj
if same.mean() >= self.cfg.same_frac:
continue # то же самое место — это не повтор
self.w[a] *= keep
self.places += 1
class NoMemory:
"""Заглушка: пока память не обучена, всё считается новым."""
n_seen = 0
@staticmethod
def annotate(cands: list[Candidate]) -> list[Candidate]:
return cands
@staticmethod
def novelty_of(_c: Candidate) -> float:
return 1.0

708
ros2_ws/src/flyguard/flyguard/node.py Normal file
View file

@ -0,0 +1,708 @@
"""ROS 2-нода FlyGuard.
Подписывается на облако точек лидара, прогоняет конвейер и публикует:
/flyguard/obstacle flyguard_msgs/ObstacleStatus — главный программный выход
/flyguard/markers visualization_msgs/MarkerArray — рамки объектов и габарит
/flyguard/view_cloud sensor_msgs/PointCloud2 — облако обзора для RViz
/flyguard/debug_cloud sensor_msgs/PointCloud2 — раскраска по новизне
/flyguard/brain sensor_msgs/Image — схема мозга мухи с активностью
/flyguard/diagnostics diagnostic_msgs/DiagnosticArray — задержки по стадиям
Обработка идёт в отдельном потоке, и из очереди всегда берётся **последний**
пришедший кадр: система реального времени обязана отвечать на текущую обстановку,
а не доедать накопившееся прошлое.
"""
from __future__ import annotations
import array
import threading
import time
from pathlib import Path
import numpy as np
import rclpy
from diagnostic_msgs.msg import DiagnosticArray, DiagnosticStatus, KeyValue
from rclpy.node import Node
from rclpy.qos import QoSDurabilityPolicy, QoSHistoryPolicy, QoSProfile, QoSReliabilityPolicy
from geometry_msgs.msg import Point, TransformStamped
from sensor_msgs.msg import PointCloud2, PointField
from std_msgs.msg import Bool, Float32, Header
from tf2_ros import StaticTransformBroadcaster
from visualization_msgs.msg import Marker, MarkerArray
from flyguard_msgs.msg import DetectedObject, ObstacleStatus
from . import ros_conv
from .cdr import parse_pointcloud2
from .export import gauge_outline
from .mushroom_body import MushroomBody
from .pipeline import FlyGuard, Params
class FlyGuardNode(Node):
def __init__(self):
super().__init__("flyguard")
p = self.declare_parameters("", [
("input_topic", "/lidar_points"),
("fallback_topics", ["/sensing/lidar/hesai128/pointcloud", "/points_raw"]),
("frame_id", ""),
("best_effort", True),
("raw_subscription", True),
("queue_depth", 20),
("async_worker", False),
("memory_path", ""),
("mbon_path", ""),
("enable_mbon", True),
# плотные стадии (сетчатка, ламина, кластеризация): auto — видеокарта,
# если PyTorch её видит, иначе процессор; cuda; cpu
("device", "auto"),
("mbon_power", 1.5),
("mbon_blend", 1.0),
("fov_deg", 30.0),
("half_width", 1.2),
("h_lo", 0.28),
("h_hi", 2.3),
("h_top", 3.3),
("half_width_top", 1.0),
("top_d_max", 90.0),
("h_lo_core", 0.16),
("core_from", 30.0),
("ctx_up", 4.0),
("split_adv", 0.0),
("split_gap", 6.0),
("split_near", 55.0),
("split_top", 1),
("enable_accumulator", True),
("acc_near", 55.0),
("acc_gain", 1.5),
("enable_habituation", False),
("hab_rate", 0.25),
("hab_place_m", 5.0),
("hab_recover_m", 800.0),
("d_min", 4.0),
("d_max", 220.0),
("min_rays", 4),
("publish_debug_cloud", False),
("publish_markers", True),
("brain_view", False),
# схема | облако нейронов коннектома | гибрид (панели + облако)
("brain_style", "hybrid"),
("brain_period", 0.2),
# 1 — 1180 пикселей по ширине; 2–3 — для экрана и видео в 2K/4K
("brain_scale", 1),
])
self.par = {q.name: q.value for q in p}
brain_on = bool(self.par["brain_view"])
params = Params(device=str(self.par["device"]),
fov_deg=float(self.par["fov_deg"]),
half_width=float(self.par["half_width"]),
h_lo=float(self.par["h_lo"]), h_hi=float(self.par["h_hi"]),
h_top=float(self.par["h_top"]),
half_width_top=float(self.par["half_width_top"]),
top_d_max=float(self.par["top_d_max"]),
h_lo_core=float(self.par["h_lo_core"]),
core_from=float(self.par["core_from"]),
ctx_up=float(self.par["ctx_up"]),
split_adv=float(self.par["split_adv"]),
split_gap=float(self.par["split_gap"]),
split_near=float(self.par["split_near"]),
split_top=int(self.par["split_top"]),
enable_accumulator=bool(self.par["enable_accumulator"]),
acc_near=float(self.par["acc_near"]),
acc_gain=float(self.par["acc_gain"]),
enable_habituation=bool(self.par["enable_habituation"]),
hab_rate=float(self.par["hab_rate"]),
hab_place_m=float(self.par["hab_place_m"]),
hab_recover_m=float(self.par["hab_recover_m"]),
d_min=float(self.par["d_min"]), d_max=float(self.par["d_max"]),
min_rays=int(self.par["min_rays"]),
# каналы T4/T5 и LPLC2 считаются только когда есть кому
# их показать: на решение они пока не влияют
enable_mbon=bool(self.par["enable_mbon"]),
mbon_power=float(self.par["mbon_power"]),
mbon_blend=float(self.par["mbon_blend"]),
enable_looming=brain_on)
memory = None
mem_path = str(self.par["memory_path"])
if mem_path and Path(mem_path).exists():
memory = MushroomBody.load(mem_path)
self.get_logger().info(
f"память тоннеля загружена: {mem_path} "
f"({memory.cfg.n_kc} клеток Кеньона, обучена на {memory.n_seen} примерах)")
else:
self.get_logger().warn(
"память тоннеля не задана — штатные конструкции тоннеля не подавляются, "
"ложных тревог будет заметно больше")
readout = None
mb_path = str(self.par["mbon_path"])
if mb_path and Path(mb_path).exists():
from .mbon_readout import MbonReadout
readout = MbonReadout.load(mb_path)
self.get_logger().info(
f"считывание MBON загружено: {mb_path} "
f"({readout.cfg.n_kc} клеток Кеньона, {readout.n_pn} признаков)")
elif bool(self.par["enable_mbon"]):
self.get_logger().warn(
"считывание MBON не задано — вес улики считается ручной формулой, "
"ложных тревог будет больше")
# Видеокарта поднимается в фоне: подписка не ждёт прогрева ядер CUDA
self._t_start = time.monotonic()
self.fg = FlyGuard(params, memory=memory, readout=readout, gpu_background=True)
self._report_device()
self.brain = None
self.brain_period = float(self.par["brain_period"])
self._brain_last = 0.0
if brain_on:
style = str(self.par["brain_style"]).lower()
if style == "scheme":
from .brain_view import BrainView
self.brain = BrainView()
elif style == "cloud":
from .brain_atlas import NeuronCloud
self.brain = NeuronCloud(scale=int(self.par["brain_scale"]))
else:
from .brain_hybrid import BrainHybrid
self.brain = BrainHybrid(scale=int(self.par["brain_scale"]))
if not getattr(self.brain, "enabled", False):
# атласа нет — падать незачем, показываем схему
from .brain_view import BrainView
self.get_logger().warn(
f"вид «{style}» недоступен (нет атласа нейронов), беру схему")
self.brain = BrainView()
qos = QoSProfile(
history=QoSHistoryPolicy.KEEP_LAST, depth=int(self.par["queue_depth"]),
reliability=(QoSReliabilityPolicy.BEST_EFFORT if self.par["best_effort"]
else QoSReliabilityPolicy.RELIABLE),
durability=QoSDurabilityPolicy.VOLATILE)
# Кадр лидара — это 24 МБ, и сборка из них Python-объекта sensor_msgs
# стоит дороже всей нашей обработки: на записи с полным круговым сканом
# так терялась половина кадров. Поэтому по умолчанию берём сырые байты
# CDR и разбираем своим парсером — он строит numpy-вид поверх буфера
# без копирования. Обычный путь остаётся под флагом, на случай
# нестандартной раскладки полей.
self.raw = bool(self.par["raw_subscription"])
topics = list(dict.fromkeys([str(self.par["input_topic"])]
+ list(self.par["fallback_topics"])))
self._qos = qos
self.subs = [self.create_subscription(PointCloud2, t, self._on_cloud, qos,
raw=self.raw)
for t in topics]
self.get_logger().info(
f"подписка на: {', '.join(topics)}"
f" ({'сырые байты CDR' if self.raw else 'разбор через rclpy'})")
# Сторож входа. Издатель на топике есть, а кадров нет — почти всегда это
# контейнер без --ipc host и `ros2 bag play` на хосте: Fast DDS шлёт
# кадры через /dev/shm, которой у них общей нет, и теряет их молча.
# Без подсказки такой запуск выглядит как пустой тоннель.
self._topics = topics
self._pub_seen_at: float | None = None
self._watch = self.create_timer(1.0, self._check_input)
self.pub_status = self.create_publisher(ObstacleStatus, "/flyguard/obstacle", 10)
self.pub_flag = self.create_publisher(Bool, "/flyguard/detected", 10)
self.pub_dist = self.create_publisher(Float32, "/flyguard/distance", 10)
self.pub_markers = self.create_publisher(MarkerArray, "/flyguard/markers", 5)
self.pub_diag = self.create_publisher(DiagnosticArray, "/flyguard/diagnostics", 5)
self.pub_cloud = self.create_publisher(PointCloud2, "/flyguard/debug_cloud", 2)
self.pub_view = self.create_publisher(PointCloud2, "/flyguard/view_cloud", 2)
self.pub_brain = None
if self.brain is not None:
from sensor_msgs.msg import Image
self.pub_brain = self.create_publisher(Image, "/flyguard/brain", 2)
# Имя кадра лидара в записях различается («lidar_livox», «hesai_lidar»),
# поэтому нода публикует свой вывод во всегда одинаковом кадре `lidar`
# и отдаёт статическое тождественное преобразование к пришедшему. Тогда
# один и тот же конфиг RViz работает с любым бэгом.
self.fixed_frame = "lidar"
self._tf = StaticTransformBroadcaster(self)
self._tf_sent: set[str] = set()
self._alarm = False # была ли тревога на прошлом кадре
self._alarm_frames = 0
self._jumps = 0 # скачки времени записи, о которых уже сказано
self._latest = None
self._lock = threading.Lock()
self._wake = threading.Event()
self._stop = False
self._dropped = 0
self._received = 0
self._cycle_ms = 0.0
self.async_worker = bool(self.par["async_worker"])
self._worker = None
if self.async_worker:
self._worker = threading.Thread(target=self._loop, daemon=True)
self._worker.start()
# ------------------------------------------------------------------ приём
def _check_input(self) -> None:
if self._received:
self._watch.cancel()
return
pubs = sum(self.count_publishers(t) for t in self._topics)
if not pubs:
self._adopt_foreign_cloud()
return
now = time.monotonic()
if self._pub_seen_at is None:
self._pub_seen_at = now
elif now - self._pub_seen_at >= 5.0:
self.get_logger().error(
f"на топике лидара есть издатель, а кадров нет уже "
f"{now - self._pub_seen_at:.0f} с. Если bag проигрывается на хосте, "
f"запустите контейнер с --ipc host: без него кадры идут через "
f"/dev/shm, общей у контейнера с хостом нет", throttle_duration_sec=10.0)
def _adopt_foreign_cloud(self) -> None:
"""Облако идёт в топик, которого нет в списке, — подписаться и на него.
Имя топика у записей разное (в наших двух разное уже), и у контрольной
записи может оказаться третье. Узел, молча ждущий не тот топик, выглядит
как пустой тоннель. Свои топики `/flyguard/...` не в счёт.
"""
for name, types in self.get_topic_names_and_types():
if (name in self._topics or name.startswith("/flyguard/")
or "sensor_msgs/msg/PointCloud2" not in types):
continue
self.get_logger().warn(
f"облако точек идёт в {name}, а узел слушал "
f"{', '.join(self._topics)} — подписываюсь и на него "
f"(явно: input_topic:={name})")
self._topics.append(name)
self.subs.append(self.create_subscription(
PointCloud2, name, self._on_cloud, self._qos, raw=self.raw))
def _on_cloud(self, msg) -> None:
self._received += 1
if not self.raw:
ok, why = ros_conv.has_required_fields(msg)
if not ok:
self.get_logger().warn(f"кадр пропущен: {why}", throttle_duration_sec=5.0)
return
if not self.async_worker:
# Обработка прямо в колбэке. Такт конвейера втрое короче периода
# кадров, поэтому исполнителю ROS есть когда работать, а отдельный
# поток здесь только отнимает GIL у приёма: в измерениях он ронял
# выработку с 10 до 2 Гц, хотя сам такт оставался 32 мс.
# Отбрасывание устаревших кадров при этом делает очередь DDS:
# её глубина `queue_depth` и есть «хранить только свежее».
t0 = time.perf_counter()
try:
self._process(msg)
except Exception as exc:
self.get_logger().error(f"сбой обработки кадра: {exc!r}")
self._cycle_ms = (time.perf_counter() - t0) * 1e3
return
with self._lock:
if self._latest is not None:
self._dropped += 1
self._latest = msg
self._wake.set()
# ------------------------------------------------------------------ обработка
def _loop(self) -> None:
while not self._stop:
self._wake.wait(timeout=0.5)
self._wake.clear()
with self._lock:
msg, self._latest = self._latest, None
if msg is None:
continue
t0 = time.perf_counter()
try:
self._process(msg)
except Exception as exc: # нода не должна падать на кадре
self.get_logger().error(f"сбой обработки кадра: {exc!r}")
# полный такт рабочего потока: конвейер плюс разбор и публикация
self._cycle_ms = (time.perf_counter() - t0) * 1e3
def _process(self, msg) -> None:
if self.raw:
pc = parse_pointcloud2(msg)
header = Header()
sec = int(pc.stamp)
header.stamp.sec = sec
header.stamp.nanosec = int(round((pc.stamp - sec) * 1e9))
header.frame_id = pc.frame_id
else:
pc = ros_conv.from_ros(msg)
header = msg.header
want_view = self.pub_view.get_subscription_count() > 0
need_debug = (bool(self.par["publish_debug_cloud"]) or self.brain is not None
or want_view)
res = self.fg.process(pc, keep_debug=need_debug)
if res is None:
self.get_logger().info(
f"калибровка решётки лучей: {self.fg.frames_seen}/{self.fg.p.calib_frames}",
throttle_duration_sec=2.0)
return
if self.fg.time_jumps != self._jumps:
self._jumps = self.fg.time_jumps
self._alarm = False
self.get_logger().info("время записи скакнуло (перемотка или повтор) — "
"треки и одометрия начаты заново")
src_frame = header.frame_id or self.fixed_frame
self._ensure_tf(src_frame)
frame = str(self.par["frame_id"]) or self.fixed_frame
d = res.decision
out = ObstacleStatus()
out.header = header
out.header.frame_id = frame
out.detected = bool(d.detected)
out.emergency = bool(d.emergency)
out.distance = float(d.distance)
out.time_to_collision = float(d.ttc)
out.confidence = float(d.confidence)
out.speed = float(d.speed)
out.stopping_distance = float(d.stopping_distance)
out.processing_ms = float(res.total_ms)
for o in d.objects:
m = DetectedObject()
m.distance = float(o.distance); m.lateral = float(o.lateral)
m.height = float(o.height); m.width = float(o.width)
m.size_v = float(o.size_v); m.confidence = float(o.confidence)
m.novelty = float(o.novelty); m.n_rays = int(o.n_rays)
m.track_id = int(o.track_id); m.time_to_collision = float(o.ttc)
out.objects.append(m)
self.pub_status.publish(out)
self.pub_flag.publish(Bool(data=bool(d.detected)))
self.pub_dist.publish(Float32(data=float(d.distance if d.detected else -1.0)))
self._report(d)
if bool(self.par["publish_markers"]):
self.pub_markers.publish(self._markers(d, frame, header.stamp, res.corridor))
if want_view and res.tf is not None:
self.pub_view.publish(self._view_cloud(res, frame, header.stamp))
self._publish_diag(res, header.stamp)
if bool(self.par["publish_debug_cloud"]):
cloud = self._debug_cloud(res, frame, header.stamp)
if cloud is not None:
self.pub_cloud.publish(cloud)
# схема мозга рисуется реже кадров лидара: она для человека, не для системы
if self.brain is not None and self.pub_brain is not None:
now = time.monotonic()
if now - self._brain_last >= self.brain_period:
self._brain_last = now
img = self.brain.render(res)
if img is not None:
self.pub_brain.publish(self.brain.to_msg(img, header.stamp, frame))
def _report(self, d) -> None:
"""Итог в консоль: у стенда результат виден без RViz и `ros2 topic echo`.
Пишется смена состояния, а пока тревога держится — ближайшая дальность
не чаще раза в секунду: построчный вывод на 10 Гц читать невозможно.
"""
if d.detected:
self._alarm_frames += 1
text = (f"{d.distance:.1f} м, уверенность {d.confidence:.2f}, "
f"объектов {len(d.objects)}")
if np.isfinite(d.ttc) and d.speed > 0.5:
text += f", до столкновения {d.ttc:.1f} с"
head = "ЭКСТРЕННОЕ ТОРМОЖЕНИЕ" if d.emergency else "ПРЕПЯТСТВИЕ"
if not self._alarm:
self.get_logger().warn(f"{head}: {text}")
else:
self.get_logger().warn(f"{head.lower()}: {text}",
throttle_duration_sec=1.0)
elif self._alarm:
self.get_logger().info("путь свободен")
self._alarm = bool(d.detected)
def _debug_cloud(self, res, frame: str, stamp):
"""Лучи кандидатов, раскрашенные по новизне, — для наглядности в RViz."""
tf = res.tf
if tf is None or not res.candidates:
return None
xs, ys, zs, ws = [], [], [], []
for c in res.candidates:
rays = c.extra.get("rays")
if rays is None:
continue
ii, jj = rays
xs.append(tf.u[ii, jj])
ys.append(-tf.d[ii, jj])
zs.append(tf.h[ii, jj])
ws.append(np.full(ii.size, c.novelty, np.float32))
if not xs:
return None
return self._cloud_msg(np.concatenate(xs), np.concatenate(ys),
np.concatenate(zs), np.concatenate(ws), frame, stamp)
def _view_cloud(self, res, frame: str, stamp):
"""Облако обзора для RViz: сектор обработки в координатах пути.
Сырое облако — до 900 тысяч точек и 24 МБ на кадр: RViz на нём тормозит,
а в части записей оно ещё и идёт в другой топик, которого конфиг RViz не
знает. Здесь только лучи сектора обработки (до 77 тысяч), выровненные по
плоскости рельсов: x — поперёк пути, −y — вдоль, z — высота над головкой
рельса, как у рамок препятствий, так что рамка стоит ровно на полу.
Публикуется, только когда на топик кто-то подписан.
"""
tf = res.tf
m = tf.valid & (tf.d > 0.5) & (tf.d < 250.0) & np.isfinite(tf.h)
return self._cloud_msg(tf.u[m], -tf.d[m], tf.h[m], tf.inten[m], frame, stamp)
@staticmethod
def _cloud_msg(xs, ys, zs, ws, frame: str, stamp) -> PointCloud2:
pts = np.empty(xs.size, dtype=np.dtype([("x", "f4"), ("y", "f4"), ("z", "f4"),
("intensity", "f4")]))
pts["x"] = xs
pts["y"] = ys
pts["z"] = zs
pts["intensity"] = ws
msg = PointCloud2()
msg.header.stamp = stamp
msg.header.frame_id = frame
msg.height = 1
msg.width = pts.size
msg.fields = [PointField(name=n, offset=o, datatype=PointField.FLOAT32, count=1)
for n, o in (("x", 0), ("y", 4), ("z", 8), ("intensity", 12))]
msg.is_bigendian = False
msg.point_step = 16
msg.row_step = 16 * pts.size
msg.is_dense = True
# array('B'), а не bytes: из bytes rclpy проверяет каждый байт на Python —
# на облаке обзора это 45 мс на кадр вместо одной (замерено).
msg.data = array.array("B", pts.tobytes())
return msg
def _ensure_tf(self, src_frame: str) -> None:
if src_frame in self._tf_sent or src_frame == self.fixed_frame:
return
t = TransformStamped()
t.header.stamp = self.get_clock().now().to_msg()
t.header.frame_id = self.fixed_frame
t.child_frame_id = src_frame
t.transform.rotation.w = 1.0
self._tf.sendTransform(t)
self._tf_sent.add(src_frame)
self.get_logger().info(f"кадр лидара «{src_frame}» связан с «{self.fixed_frame}»")
# ------------------------------------------------------------------ визуализация
def _markers(self, d, frame: str, stamp, corridor=None) -> MarkerArray:
arr = MarkerArray()
clear = Marker()
clear.header.frame_id = frame
clear.header.stamp = stamp
clear.action = Marker.DELETEALL
arr.markers.append(clear)
for i, o in enumerate(d.objects):
m = Marker()
m.header.frame_id = frame
m.header.stamp = stamp
m.ns = "flyguard"
m.id = i + 1
m.type = Marker.CUBE
m.action = Marker.ADD
# координаты пути: вперёд = −Y, вправо = +X, вверх = +Z от головки
# рельса — те же, что у облака обзора, поэтому рамка стоит на полу.
# Поперёк — смещение в системе лидара, а не от оси пути: в кривой
# облако не выпрямлено, и рамка по `lateral` встала бы в стороне
# от своих точек.
x = float(o.sensor_x)
m.pose.position.x = x if np.isfinite(x) else float(o.lateral)
m.pose.position.y = float(-o.distance)
m.pose.position.z = float(o.height + o.size_v / 2)
m.pose.orientation.w = 1.0
m.scale.x = max(float(o.width), 0.3)
m.scale.y = max(float(o.width), 0.3)
m.scale.z = max(float(o.size_v), 0.3)
hot = float(np.clip(o.confidence, 0.0, 1.0))
m.color.r = 1.0
m.color.g = float(1.0 - hot)
m.color.b = 0.0
m.color.a = 0.55
arr.markers.append(m)
txt = Marker()
txt.header = m.header
txt.ns = "flyguard_text"
txt.id = 1000 + i
txt.type = Marker.TEXT_VIEW_FACING
txt.action = Marker.ADD
txt.pose = m.pose
txt.pose.position.z += 1.0
# 1.4 м: при 0.8 подпись у дальней рамки читалась с трудом
txt.scale.z = 1.4
txt.color.r = txt.color.g = txt.color.b = txt.color.a = 1.0
# латиница: в шрифте RViz нет кириллицы, и «55 м» выходило «55 »
txt.text = f"{o.distance:.0f} m p={o.confidence:.2f}"
arr.markers.append(txt)
arr.markers.extend(self._gauge_markers(frame, stamp, corridor))
return arr
def _gauge_markers(self, frame: str, stamp, corridor=None) -> list[Marker]:
"""Контур габарита в координатах облака обзора.
Узел проверяет объединение двух габаритов (`TrackFrame.lateral`):
прямого — вдоль оси лидара, и изогнутого — вдоль оценённой оси пути.
Облако обзора выровнено по плоскости рельсов, но в кривой не
выпрямлено, поэтому в кривой рисуются оба: изогнутый ярко, прямой
бледно. На прямом пути они совпадают, и рисуется один. Что внутри
оранжевого контура, то узел и проверяет; колонна или шкаф за контуром —
не его забота. Рамки поперёк — через 20 м, для глубины.
"""
p = self.fg.p
far = min(p.d_max, 200.0)
boxes = [(p.half_width, p.h_lo, p.h_hi, p.d_min, far)]
if p.h_top > p.h_hi and p.half_width_top > 0.0:
boxes.append((p.half_width_top, p.h_hi, p.h_top, p.d_min, min(p.top_d_max, far)))
curved = None
if corridor is not None and corridor.n_slices > 0:
ds = np.arange(p.d_min, far + 1e-6, 5.0, dtype=np.float32)
# меньше 15 см контуры сливаются в один — второй незачем
if float(np.abs(corridor.centre(ds)).max()) > 0.15:
curved = corridor.centre
layers = [(None, 0.3 if curved is not None else 0.7)]
if curved is not None:
layers.append((curved, 0.8))
out = []
for i, (centre, alpha) in enumerate(layers):
m = Marker()
m.header.frame_id = frame
m.header.stamp = stamp
m.ns = "flyguard_gauge"
m.id = i
m.type = Marker.LINE_LIST
m.action = Marker.ADD
m.pose.orientation.w = 1.0
m.scale.x = 0.04
m.color.r, m.color.g, m.color.b, m.color.a = 1.0, 0.55, 0.0, alpha
m.points = [Point(x=float(x), y=float(y), z=float(z))
for x, y, z in gauge_outline(boxes, centre)]
out.append(m)
return out
def _report_device(self) -> None:
"""Где идёт счёт — первой строкой журнала: на стенде это видно без RViz."""
fg = self.fg
if fg.gpu_pending:
self.get_logger().info(
"вычисления: пока процессор — видеокарту проверяю и прогреваю в фоне "
"(в Docker под WSL до 20 с), результат тот же")
self._gpu_watch = self.create_timer(0.5, self._watch_gpu)
elif fg.gpu_active:
self._say_gpu()
elif fg.gpu_error:
self.get_logger().warn(f"вычисления: процессор — видеокарта не поднялась ({fg.gpu_error})")
else:
self._say_cpu()
self._gpu_on = fg.gpu_active
def _say_gpu(self, note: str = "") -> None:
from .device import get_device_info
info = get_device_info(str(self.par["device"]))
self.get_logger().info(
f"вычисления: видеокарта {info.get('name', '?')} "
f"({info.get('total_memory_mb', 0) / 1024:.0f} ГБ, CUDA {info.get('cuda_version')}) — "
f"сетчатка, ламина, кластеризация; остальное на процессоре{note}")
def _say_cpu(self) -> None:
why = ("задано device=cpu" if str(self.par["device"]).lower() == "cpu"
else "CUDA недоступна: нет видеокарты, драйвера или контейнер запущен без --gpus")
self.get_logger().info(f"вычисления: процессор ({why})")
def _watch_gpu(self) -> None:
"""Видеокарта поднималась в фоне — сказать в журнал, чем кончилось."""
fg = self.fg
if fg.gpu_pending:
return
self._gpu_watch.cancel()
self._gpu_on = fg.gpu_active
if fg.gpu_active:
self._say_gpu(f" (готова через {time.monotonic() - self._t_start:.1f} с после старта)")
elif fg.gpu_error:
self.get_logger().warn(
f"видеокарта не поднялась ({fg.gpu_error}) — считаю на процессоре")
else:
self._say_cpu()
def _publish_diag(self, res, stamp) -> None:
msg = DiagnosticArray()
msg.header.stamp = stamp
st = DiagnosticStatus()
st.name = "flyguard"
st.hardware_id = "lidar"
total = res.total_ms
st.level = (DiagnosticStatus.OK if total < 90 else DiagnosticStatus.WARN)
st.message = f"{total:.1f} мс/кадр"
st.values = [KeyValue(key=k, value=f"{v:.2f}") for k, v in res.timings.items()]
st.values.append(KeyValue(key="candidates", value=str(len(res.candidates))))
st.values.append(KeyValue(key="tracks", value=str(len(self.fg.cx.tracks))))
st.values.append(KeyValue(key="dropped_frames", value=str(self._dropped)))
# считает сама нода: внешний подписчик тоже теряет сообщения и занижает оценку
st.values.append(KeyValue(key="frames_processed", value=str(self.fg.frames_seen)))
# Настоящий счётчик приёма, включая кадры, ушедшие на калибровку
# решётки: без них разница «принято минус обработано» выглядела
# потерей, хотя это цена восстановления геометрии лучей по данным.
st.values.append(KeyValue(key="frames_received", value=str(self._received)))
st.values.append(KeyValue(key="frames_calibration",
value=str(max(self._received - self.fg.frames_seen
- self._dropped, 0))))
st.values.append(KeyValue(key="cycle_ms", value=f"{self._cycle_ms:.1f}"))
st.values.append(KeyValue(key="device", value=self.fg.device))
if self._gpu_on and not self.fg.gpu_active:
# видеокарта отказала на ходу: кадр досчитан на процессоре, дальше — только он
self._gpu_on = False
self.get_logger().warn(
f"видеокарта отказала ({self.fg.gpu_error}) — дальше считаю на процессоре")
if res.ego:
st.values.append(KeyValue(key="speed_kmh", value=f"{res.ego.kmh:.1f}"))
st.values.append(KeyValue(key="rail_height_m", value=f"{res.plane.height:.3f}"))
st.values.append(KeyValue(key="curve_radius_m", value=f"{res.corridor.radius:.0f}"))
msg.status.append(st)
self.pub_diag.publish(msg)
def summary(self) -> str:
"""Итог сеанса: по нему сразу видно, дошли ли кадры и сколько было тревог."""
seen = self.fg.frames_seen
calib = max(self._received - seen - self._dropped, 0)
return (f"итог: принято кадров {self._received}, обработано {seen}, "
f"на калибровку {calib}, пропущено {self._dropped}; "
f"кадров с тревогой {self._alarm_frames}")
def destroy_node(self) -> bool:
self._stop = True
self._wake.set()
return super().destroy_node()
def main(argv=None) -> None:
rclpy.init(args=argv)
node = FlyGuardNode()
try:
rclpy.spin(node)
except KeyboardInterrupt:
pass
finally:
# По Ctrl+C обработчик rclpy успевает закрыть контекст раньше, и журнал
# ROS тогда ругается «Failed to publish log message to rosout».
if rclpy.ok():
node.get_logger().info(node.summary())
else:
print(f"[flyguard] {node.summary()}", flush=True)
node.destroy_node()
rclpy.try_shutdown()
if __name__ == "__main__":
main()

614
ros2_ws/src/flyguard/flyguard/pipeline.py Normal file
View file

@ -0,0 +1,614 @@
"""Сборка конвейера: от облака точек до решения.
Порядок стадий повторяет путь сигнала в мозге мухи:
ретина → стабилизация → ламина → медулла/лобулярная пластинка
→ лобула → грибовидное тело → центральный комплекс → нисходящие нейроны
Конвейер держит состояние между кадрами (поза, коридор, собственное движение,
треки), поэтому один экземпляр обслуживает один поток лидарных данных.
"""
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
@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
# Поправка оценки модели на распространённость предметов: за этой
# дальностью из логита вычитается типичный для полосы. 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] = []
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))
# без накопления улик трек подтверждается первым же наблюдением
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)
# ------------------------------------------------------------------ калибровка
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
# Скачок времени записи — перемотка, `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)
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

183
ros2_ws/src/flyguard/flyguard/player_keys.py Normal file
View file

@ -0,0 +1,183 @@
"""Пульт проигрывания записи: пауза, перемотка, шаг по кадру, скорость.
`ros2 bag play`, запущенный из launch (`bag:=...`), клавиатуры не слышит — у
него нет терминала. Пульт управляет им через сервисы rosbag2 из второго
терминала того же контейнера:
docker exec -it flyguard_demo flyguard-keys /data/<запись>
пробел пауза / продолжить
← → на 3 секунды назад / вперёд
. один кадр вперёд (на паузе)
↑ ↓ быстрее / медленнее
0 в начало записи
q выйти (запись играет дальше)
Текущее время записи берётся из /clock — launch запускает проигрыватель с
`--clock`. Путь к записи нужен только для клавиши «0»: из metadata.yaml
читается время её начала. Узел детектора после перемотки сам сбрасывает треки
и одометрию (скачок времени, `pipeline.FlyGuard.process`).
"""
from __future__ import annotations
import re
import select
import sys
import termios
import tty
from pathlib import Path
import rclpy
from builtin_interfaces.msg import Time
from rclpy.node import Node
from rclpy.qos import qos_profile_sensor_data
from rosbag2_interfaces.srv import GetRate, IsPaused, PlayNext, Seek, SetRate, TogglePaused
from rosgraph_msgs.msg import Clock
STEP_S = 3.0
RATES = [0.1, 0.25, 0.5, 1.0, 2.0, 4.0]
PLAYER = "/rosbag2_player/"
KEYS = """ пробел пауза / продолжить
← → на 3 секунды назад / вперёд
. один кадр вперёд (на паузе)
↑ ↓ быстрее / медленнее
0 в начало записи
q выйти (запись играет дальше)
"""
def bag_span(path: str | None) -> tuple[float | None, float | None]:
"""Начало и длительность записи из metadata.yaml, секунды."""
if not path:
return None, None
meta = Path(path) / "metadata.yaml"
if not meta.exists():
return None, None
txt = meta.read_text(encoding="utf-8")
s = re.search(r"starting_time:\s*\n\s*nanoseconds_since_epoch:\s*(\d+)", txt)
d = re.search(r"duration:\s*\n\s*nanoseconds:\s*(\d+)", txt)
return (int(s.group(1)) * 1e-9 if s else None, int(d.group(1)) * 1e-9 if d else None)
class PlayerKeys(Node):
def __init__(self, start: float | None, duration: float | None):
super().__init__("flyguard_player_keys")
self.now: float | None = None
self.start = start
self.duration = duration
self.state, self.rate = "идёт", 1.0
# /clock проигрыватель шлёт без подтверждений (best effort): с надёжной
# подпиской QoS не сходится, и время не приходит вовсе
self.create_subscription(Clock, "/clock", self._on_clock, qos_profile_sensor_data)
self.cli = {name: self.create_client(srv, PLAYER + name) for name, srv in (
("toggle_paused", TogglePaused), ("is_paused", IsPaused), ("seek", Seek),
("set_rate", SetRate), ("get_rate", GetRate), ("play_next", PlayNext))}
def _on_clock(self, msg: Clock) -> None:
self.now = msg.clock.sec + msg.clock.nanosec * 1e-9
if self.start is None:
self.start = self.now
def call(self, name: str, req=None):
cli = self.cli[name]
if not cli.wait_for_service(timeout_sec=2.0):
self.say("проигрыватель не отвечает — запись закончилась или запущена без launch?")
return None
fut = cli.call_async(req if req is not None else cli.srv_type.Request())
rclpy.spin_until_future_complete(self, fut, timeout_sec=3.0)
return fut.result()
def seek(self, t: float) -> None:
# за конец записи проигрыватель просто закончит её — не пускаем
if self.start is not None:
t = max(t, self.start)
if self.duration:
t = min(t, self.start + self.duration - 0.5)
req = Seek.Request()
sec = int(t)
req.time = Time(sec=sec, nanosec=int((t - sec) * 1e9))
self.call("seek", req)
def set_rate(self, up: bool) -> None:
got = self.call("get_rate")
cur = got.rate if got else 1.0
if up:
new = next((r for r in RATES if r > cur + 1e-6), RATES[-1])
else:
new = next((r for r in reversed(RATES) if r < cur - 1e-6), RATES[0])
req = SetRate.Request()
req.rate = float(new)
self.call("set_rate", req)
def status(self, ask: bool = True) -> str:
"""Строка состояния; ask=False — без запросов к проигрывателю."""
if ask:
paused = self.call("is_paused")
rate = self.call("get_rate")
self.state = "ПАУЗА" if paused and paused.paused else "идёт"
self.rate = rate.rate if rate else 1.0
pos = (f"{self.now - self.start:6.1f} с" if self.now is not None and self.start
else " ? с")
return f"{pos} {self.state:6s} скорость ×{self.rate:g}"
@staticmethod
def say(text: str) -> None:
sys.stdout.write("\r\033[K" + text)
sys.stdout.flush()
def read_key(fd: int) -> str:
ch = sys.stdin.read(1)
if ch != "\x1b":
return ch
if select.select([fd], [], [], 0.05)[0]:
seq = sys.stdin.read(2)
return {"[A": "up", "[B": "down", "[C": "right", "[D": "left"}.get(seq, "")
return "esc"
def main(argv=None) -> None:
args = [a for a in (argv or sys.argv[1:]) if not a.startswith("--")]
rclpy.init()
node = PlayerKeys(*bag_span(args[0] if args else None))
print(KEYS)
fd = sys.stdin.fileno()
old = termios.tcgetattr(fd)
try:
tty.setcbreak(fd)
node.say(node.status())
while rclpy.ok():
if not select.select([fd], [], [], 0.2)[0]:
rclpy.spin_once(node, timeout_sec=0.0)
node.say(node.status(ask=False)) # бегущее время записи
continue
key = read_key(fd)
rclpy.spin_once(node, timeout_sec=0.05)
if key in ("q", "й", "esc"):
break
if key == " ":
node.call("toggle_paused")
elif key in ("left", "right") and node.now is not None:
node.seek(node.now + (STEP_S if key == "right" else -STEP_S))
elif key in ("left", "right"):
node.say("ещё нет времени записи (/clock) — подождите секунду")
continue
elif key in (".", "ю"):
node.call("play_next")
elif key in ("up", "down"):
node.set_rate(key == "up")
elif key == "0" and node.start is not None:
node.seek(node.start)
rclpy.spin_once(node, timeout_sec=0.1)
node.say(node.status())
except KeyboardInterrupt:
pass
finally:
termios.tcsetattr(fd, termios.TCSADRAIN, old)
print()
node.destroy_node()
rclpy.try_shutdown()
if __name__ == "__main__":
main()

590
ros2_ws/src/flyguard/flyguard/retina.py Normal file
View file

@ -0,0 +1,590 @@
"""RETINA — омматидиальная решётка.
Фасеточный глаз дрозофилы — регулярная решётка омматидиев, каждый смотрит в свою
фиксированную сторону. Вращающийся лидар устроен так же: пара (кольцо, столбец)
задаёт направление луча. Поэтому облако точек сразу переводится в *ретинотопический*
дальностный образ `(кольцо, азимут)`, и вся дальнейшая обработка идёт в этой
решётке — как в зрительной системе мухи, а не в неупорядоченном облаке.
Три особенности конкретного сенсора, измеренные по данным (см. `docs/ALGORITHM.md`):
1. **Раскладка различается между бэгами**: 3600 азимутов на 360° против 1200 на
100°. Решётка калибруется по самим данным, ничего не захардкожено.
2. **Двойное эхо**: соседние столбцы делят один азимут. Когда эхо одно, оба слота
содержат одно значение; когда два — ближнее несёт объект, дальнее фон за ним.
Реально различаются ~3 % лучей, и это именно тонкие предметы и кромки.
3. **Скос решётки**: у каждого лазерного канала свой постоянный азимутальный сдвиг,
разброс достигает **15.5°** (≈155 столбцов). В сыром виде «столбец» не является
направлением: соседние кольца одного столбца смотрят в стороны, разнесённые на
градусы. Поэтому образ **выпрямляется** целочисленным сдвигом строк; остаточная
ошибка < половины шага азимута и учитывается в таблице направлений.
Быстрый путь опирается на порядок точек (столбец · эхо · кольцо). В синтетическом
бэге организаторов он соблюдается не везде: у облака нет поля `ring`, а там, где
вставлен предмет, заслонённые им точки удалены, а точки предмета вписаны в
середину массива — всё, что дальше, сдвинуто (так в половине кадров). Поэтому
порядок проверяется в каждом кадре: у кадра с целым порядком элевация каждой
точки совпадает с элевацией её кольца до 0.0001°. Кадр, где это не так,
раскладывается в ту же решётку **по углам каждой точки** — медленнее, зато без
допущений о порядке. Калибровка без поля `ring` находит кольца по гистограмме
элевации и берёт только кадры с целым порядком, а если таких нет — строит
решётку целиком по углам.
"""
from __future__ import annotations
import warnings
from contextlib import contextmanager
from dataclasses import dataclass
from pathlib import Path
import numpy as np
from .cdr import PointCloud2
DEG = np.pi / 180.0
# Допуск проверки порядка точек: в целом кадре элевация точки совпадает с
# элевацией её кольца до 0.0001° (замерено на всех бэгах и на синтетике), а
# ближайшие кольца Pandar128 разнесены на 0.086°. Вставленные точки предмета
# уходят от колец на 0.03° и больше, сдвинутые — на целое кольцо.
ORDER_TOL_DEG = 0.01
@dataclass
class RangeImage:
"""Выпрямленный дальностный образ в координатах `(кольцо, азимут)`."""
stamp: float
r_near: np.ndarray # (H, W) float32 — ближнее эхо, 0 = нет эха
r_far: np.ndarray # (H, W) float32 — дальнее эхо, 0 = нет эха
inten: np.ndarray # (H, W) float32 — интенсивность ближнего эха
valid: np.ndarray # (H, W) bool
@property
def shape(self) -> tuple[int, int]:
return self.r_near.shape
def xyz(self, layout: "ScanLayout") -> np.ndarray:
"""Декартовы координаты ближнего эха: (H, W, 3)."""
return layout.dirs * self.r_near[..., None]
def crop(self, cols: slice) -> "RangeImage":
return RangeImage(self.stamp, self.r_near[:, cols], self.r_far[:, cols],
self.inten[:, cols], self.valid[:, cols])
class ScanLayout:
"""Калиброванная и выпрямленная решётка лучей.
Азимут отсчитывается от направления движения (вперёд = −Y в системе сенсора),
положительный вправо; элевация — вверх от горизонтали сенсора.
"""
def __init__(self, el_deg: np.ndarray, az_step_deg: float, az0_deg: float,
col_shift: np.ndarray, az_resid_deg: np.ndarray,
n_az: int, n_echo: int, wrap: bool = False, indexed: bool = True):
self.el_deg = np.asarray(el_deg, np.float64) # (H,)
self.az_step_deg = float(az_step_deg) # шаг на азимутальный индекс
self.az0_deg = float(az0_deg)
self.col_shift = np.asarray(col_shift, np.int64) # (H,) выпрямление
self.az_resid_deg = np.asarray(az_resid_deg, np.float64) # (H,) остаток < шага/2
self.n_az = int(n_az)
self.n_echo = int(n_echo)
self.wrap = bool(wrap) # круговой скан: края смыкаются
# False — порядок точек неизвестен (решётка построена по углам), и
# каждый кадр раскладывается по углам точек
self.indexed = bool(indexed)
self.n_rings = self.el_deg.size
self.n_points = self.n_az * self.n_echo * self.n_rings
self.n_geometric = 0 # сколько кадров пришлось раскладывать по углам
self._init_lookup()
self.az_grid_deg = self.az0_deg + self.az_step_deg * np.arange(self.n_az)
# карта выборки для выпрямления: out[h, j] = raw[h, j + shift[h]]
j = np.arange(self.n_az)[None, :]
src = j + self.col_shift[:, None]
if self.wrap:
# у кругового скана «выехавшие» столбцы приходят с другого края
self.gather = np.mod(src, self.n_az).astype(np.intp)
self.gather_ok = np.ones_like(self.gather, bool)
else:
self.gather_ok = (src >= 0) & (src < self.n_az)
self.gather = np.clip(src, 0, self.n_az - 1).astype(np.intp)
self.az_full_deg = self.az_grid_deg[None, :] + self.az_resid_deg[:, None]
self.dirs = self._unit_dirs().astype(np.float32) # (H, W, 3)
# угловой шаг по кольцам: у решётки из одного кольца градиента нет
self.el_step_deg = (np.abs(np.gradient(self.el_deg)) if self.n_rings > 1
else np.full(self.n_rings, 0.125))
# ------------------------------------------------------------------ геометрия
def _unit_dirs(self) -> np.ndarray:
az = self.az_full_deg * DEG
el = (self.el_deg[:, None] * DEG) * np.ones_like(az)
c = np.cos(el)
return np.stack([c * np.sin(az), -c * np.cos(az), np.sin(el)], axis=-1)
def _init_lookup(self) -> None:
"""Таблицы для проверки порядка точек и поиска кольца по элевации."""
self._sin_el = np.sin(self.el_deg * DEG).astype(np.float32)
self._el_order = np.argsort(self.el_deg)
self._el_asc = self.el_deg[self._el_order]
def column_slice(self, half_fov_deg: float) -> slice:
"""Непрерывный диапазон столбцов внутри ±half_fov по азимуту."""
inside = np.flatnonzero(np.abs(self.az_grid_deg) <= half_fov_deg)
if inside.size == 0:
return slice(0, self.n_az)
return slice(int(inside[0]), int(inside[-1]) + 1)
def sub(self, cols: slice) -> "ScanLayout":
"""Урезанная по азимуту копия решётки (для обработки только переднего сектора)."""
start = cols.start or 0
stop = cols.stop if cols.stop is not None else self.n_az
out = ScanLayout.__new__(ScanLayout)
out.el_deg = self.el_deg
out.az_step_deg = self.az_step_deg
out.az0_deg = float(self.az_grid_deg[start])
out.col_shift = self.col_shift
out.az_resid_deg = self.az_resid_deg
out.n_az = stop - start
out.n_echo = self.n_echo
out.wrap = False # вырезанный сектор больше не смыкается
out.indexed = self.indexed
out.n_rings = self.n_rings
out.n_points = self.n_points
out.n_geometric = 0
out._init_lookup()
out.az_grid_deg = self.az_grid_deg[start:stop]
out.gather_ok = self.gather_ok[:, start:stop]
out.gather = self.gather[:, start:stop]
out.az_full_deg = self.az_full_deg[:, start:stop]
out.dirs = np.ascontiguousarray(self.dirs[:, start:stop])
out.el_step_deg = self.el_step_deg
return out
# ------------------------------------------------------------------ проекция
def project(self, pc: PointCloud2, cols: slice | None = None) -> RangeImage:
"""Облако точек → выпрямленный дальностный образ.
`cols` задаёт нужный диапазон **выходных** столбцов. Тяжёлая
арифметика (корень по 900 тыс. точек) выполняется только над теми
сырыми столбцами, которые в этот диапазон попадут с учётом скоса
каналов, — на круговом скане это экономит почти всё время стадии.
Кадр с нарушенным порядком точек (см. докстроку модуля) раскладывается
по углам точек: результат тот же, что дал бы целый кадр.
"""
w = self.n_az
start = 0 if cols is None else (cols.start or 0)
stop = w if cols is None else (cols.stop if cols.stop is not None else w)
if self.indexed and pc.n_points == self.n_points:
img = self._project_indexed(pc, start, stop)
if img is not None:
return img
self.n_geometric += 1
return self._project_geometric(pc, start, stop)
def _project_indexed(self, pc: PointCloud2, start: int, stop: int) -> RangeImage | None:
"""Быстрый путь по порядку точек. None — порядок в кадре нарушен."""
n, w, e = self.n_rings, self.n_az, self.n_echo
lo = start + int(self.col_shift.min())
hi = stop + int(self.col_shift.max())
if self.wrap:
raw_cols = np.arange(lo, hi) % w
else:
lo = max(lo, 0)
hi = min(hi, w)
raw_cols = None
pts = pc.points
def cube(name: str) -> np.ndarray:
a = pts[name].reshape(w, e, n)
a = a[raw_cols] if raw_cols is not None else a[lo:hi]
return a.transpose(2, 0, 1)
x, y, z = cube("x"), cube("y"), cube("z")
good = (x != 0) | (y != 0) | (z != 0)
r = np.sqrt(x * x + y * y + z * z, dtype=np.float32)
good &= np.isfinite(r) # драйверы, отдающие «нет эха» как NaN
r = np.where(good, r, np.float32(0.0))
# Порядок: элевация каждой точки должна совпасть с элевацией её кольца.
# У «нет эха» x = y = z = r = 0, и отклонение тоже ноль.
dev = np.abs(z - r * self._sin_el[:, None, None])
if np.any(dev > r * np.float32(ORDER_TOL_DEG * DEG) + np.float32(1e-3)):
return None
if e == 1:
r_near = r[..., 0]
r_far = r[..., 0]
it = cube("intensity")[..., 0]
valid = good[..., 0]
else:
inten = cube("intensity")
near_i = np.argmin(np.where(good, r, np.float32(np.inf)), axis=-1)[..., None]
far_i = np.argmax(r, axis=-1)[..., None]
r_near = np.take_along_axis(r, near_i, -1)[..., 0]
r_far = np.take_along_axis(r, far_i, -1)[..., 0]
it = np.take_along_axis(inten, near_i, -1)[..., 0]
valid = good.any(axis=-1)
# выпрямление скоса каналов, с поправкой на смещение окна
g = self.gather[:, start:stop]
ok = self.gather_ok[:, start:stop]
if raw_cols is not None:
g = (g - lo) % w
else:
g = g - lo
ok = ok & (g >= 0) & (g < (hi - lo))
np.clip(g, 0, hi - lo - 1, out=g)
r_near = np.take_along_axis(r_near, g, 1)
r_far = np.take_along_axis(r_far, g, 1)
it = np.take_along_axis(it, g, 1)
valid = np.take_along_axis(valid, g, 1) & ok
r_near = np.where(valid, r_near, np.float32(0.0))
r_far = np.where(valid, r_far, np.float32(0.0))
return RangeImage(pc.stamp, np.ascontiguousarray(r_near),
np.ascontiguousarray(r_far), np.ascontiguousarray(it),
np.ascontiguousarray(valid))
def _project_geometric(self, pc: PointCloud2, start: int, stop: int) -> RangeImage:
"""Раскладка по углам: кольцо — по элевации точки, столбец — по азимуту.
Не опирается на порядок точек вовсе. Если в ячейку попало несколько
точек (двойное эхо, вставленный предмет поверх фона), ближняя идёт в
`r_near`, дальняя — в `r_far`, как у целого кадра.
"""
n, wid = self.n_rings, stop - start
pts = pc.points
x = np.asarray(pts["x"], np.float32)
y = np.asarray(pts["y"], np.float32)
z = np.asarray(pts["z"], np.float32)
good = ((x != 0) | (y != 0) | (z != 0)) & np.isfinite(x) & np.isfinite(y) \
& np.isfinite(z)
idx = np.flatnonzero(good)
x, y, z = x[idx], y[idx], z[idx]
# грубый отбор сектора по азимуту, без поправки кольца
step = self.az_step_deg
az = np.degrees(np.arctan2(x, -y))
jf = (az - np.float32(self.az0_deg)) / np.float32(step)
if self.wrap:
jf = np.mod(jf, self.n_az)
margin = float(np.abs(self.az_resid_deg).max()) / abs(step) + 1.0
sel = np.flatnonzero((jf > start - margin) & (jf < stop - 1 + margin))
idx, x, y, z, az = idx[sel], x[sel], y[sel], z[sel], az[sel]
r = np.sqrt(x * x + y * y + z * z)
el = np.degrees(np.arcsin(np.clip(z / np.maximum(r, np.float32(1e-6)), -1.0, 1.0)))
# ближайшее кольцо по элевации
k = np.clip(np.searchsorted(self._el_asc, el), 1, n - 1)
k -= (el - self._el_asc[k - 1]) < (self._el_asc[k] - el)
h = self._el_order[k]
ok = np.abs(el - self.el_deg[h]) <= np.maximum(self.el_step_deg[h], 0.2)
j = np.rint((az - self.az_resid_deg[h] - self.az0_deg) / step).astype(np.int64)
if self.wrap:
j %= self.n_az
ok &= (j >= start) & (j < stop)
sel = np.flatnonzero(ok)
r_near = np.zeros(n * wid, np.float32)
r_far = np.zeros(n * wid, np.float32)
it = np.zeros(n * wid, np.float32)
valid = np.zeros(n * wid, bool)
if sel.size:
cell = h[sel] * wid + (j[sel] - start)
rr = r[sel]
# сортировка по (ячейка, дальность): первая в ячейке — ближняя
mm = np.minimum(rr * 1000.0, (1 << 20) - 1).astype(np.int64)
order = np.argsort(cell * (1 << 20) + mm)
cs, rs = cell[order], rr[order]
brk = np.flatnonzero(cs[1:] != cs[:-1])
first = np.concatenate(([0], brk + 1))
last = np.concatenate((brk, [cs.size - 1]))
r_near[cs[first]] = rs[first]
r_far[cs[last]] = rs[last]
valid[cs[first]] = True
if "intensity" in pts.dtype.names:
src = np.asarray(pts["intensity"], np.float32)[idx[sel]][order]
it[cs[first]] = src[first]
return RangeImage(pc.stamp, r_near.reshape(n, wid), r_far.reshape(n, wid),
it.reshape(n, wid), valid.reshape(n, wid))
# ------------------------------------------------------------------ калибровка
@staticmethod
def calibrate(clouds: list[PointCloud2], n_rings: int | None = None) -> "ScanLayout":
"""Восстановить решётку по нескольким кадрам.
Определяются: число колец и эх, элевация каждого кольца, шаг развёртки,
азимутальный сдвиг каждого канала и целочисленное выпрямление образа.
"""
if not clouds:
raise ValueError("нужен хотя бы один кадр для калибровки")
pc0 = clouds[0]
if "ring" not in pc0.points.dtype.names:
# Без поля `ring` (синтетика организаторов): кольца — по гистограмме
# элевации, а в калибровку по порядку точек идут только кадры, где
# этот порядок цел. Нет таких — решётка строится целиком по углам.
el_ring = _ring_elevations(clouds)
if n_rings is None:
n_rings = el_ring.size
whole = [pc for pc in clouds if _is_organized(pc, el_ring)]
whole = [pc for pc in whole if pc.n_points == whole[0].n_points] if whole else []
if not whole:
return ScanLayout._calibrate_geometric(clouds, el_ring)
clouds, pc0 = whole, whole[0]
if n_rings is None:
n_rings = int(pc0.points["ring"].max()) + 1
n_cols, rem = divmod(pc0.n_points, n_rings)
if rem:
raise ValueError(f"{pc0.n_points} точек не делится на {n_rings} колец")
# Направление луча задано сенсором и в каждом кадре одно и то же:
# кадры нужны только чтобы закрыть лучи, не вернувшие эхо. Поэтому
# берётся первое конечное значение, а не медиана по стопке кадров —
# та стоила 1.4 с из 2.8 с всей калибровки и ничего не уточняла:
# ниже и шаг развёртки, и сдвиг канала берутся медианой по тысячам
# столбцов, где шум одного отсчёта всё равно усредняется.
az = el = None
for pc in clouds:
x = pc.points["x"].reshape(n_cols, n_rings).T.astype(np.float32)
y = pc.points["y"].reshape(n_cols, n_rings).T.astype(np.float32)
z = pc.points["z"].reshape(n_cols, n_rings).T.astype(np.float32)
ok = (x != 0) | (y != 0) | (z != 0)
with _quiet():
a = np.where(ok, np.degrees(np.arctan2(x, -y)), np.nan)
e = np.where(ok, np.degrees(np.arctan2(z, np.hypot(x, y))), np.nan)
if az is None:
az, el = a, e
continue
gap = np.isnan(az)
if not gap.any():
break
az[gap] = a[gap]
el[gap] = e[gap]
az = az.astype(np.float64)
el = el.astype(np.float64)
n_echo = _detect_echoes(az)
n_az = n_cols // n_echo
if n_echo > 1:
with _quiet():
az = np.nanmean(az.reshape(n_rings, n_az, n_echo), axis=2)
el = np.nanmean(el.reshape(n_rings, n_az, n_echo), axis=2)
# общий шаг развёртки: медиана по кольцам от робастной оценки наклона
slopes = []
for h in range(n_rings):
row = az[h]
idx = np.flatnonzero(np.isfinite(row))
if idx.size < 50:
continue
d = np.diff(np.unwrap(np.radians(row[idx]))) / np.diff(idx)
slopes.append(np.median(np.degrees(d)))
if not slopes:
raise ValueError("недостаточно валидных лучей для калибровки развёртки")
step = float(np.median(slopes))
# смещение каждого канала относительно общей развёртки
j = np.arange(n_az, dtype=np.float64)
base = step * j
with _quiet():
c_ring = np.nanmedian(_wrap180(az - base[None, :]), axis=1) # (H,)
c_ring = _fill_linear(c_ring)
c0 = float(np.median(c_ring))
# круговой скан: развёртка покрывает полные 360°
wrap = abs(step) * n_az > 350.0
# начало отсчёта выбирается так, чтобы «вперёд» (азимут 0) был в середине
# образа — иначе шов ±180° разрезал бы рабочий сектор пополам
if wrap:
k = int(np.rint(-c0 / step)) - n_az // 2
c0 = _wrap180(c0 + step * k)
# разница берётся по кратчайшей дуге: иначе шов ±180° даёт сдвиг в пол-оборота
shift = np.rint(_wrap180(c0 - c_ring) / step).astype(np.int64)
resid = _wrap180(c_ring + step * shift - c0)
with _quiet():
el_ring = _fill_linear(np.nanmedian(el, axis=1))
return ScanLayout(el_ring, step, float(c0), shift, resid, n_az, n_echo, wrap)
@staticmethod
def _calibrate_geometric(clouds: list[PointCloud2], el_ring: np.ndarray) -> "ScanLayout":
"""Решётка только по углам точек, когда ни в одном кадре нет целого порядка.
Кольцо — ближайшая элевация из `el_ring`. Шаг развёртки — самая частая
разность соседних азимутов внутри кольца, сдвиг кольца внутри шага —
круговое среднее фазы его азимутов. Столбец растёт, азимут убывает — как
у Pandar128 в наших бэгах, чтобы образ не отразился зеркально.
"""
n = el_ring.size
el_desc = np.sort(el_ring)[::-1]
order = np.argsort(el_desc)
asc = el_desc[order]
azs, hs = [], []
for pc in clouds[:6]:
x, y, z, ok = _xyz64(pc)
x, y, z = x[ok], y[ok], z[ok]
el = np.degrees(np.arctan2(z, np.hypot(x, y)))
k = np.clip(np.searchsorted(asc, el), 1, n - 1)
k -= (el - asc[k - 1]) < (asc[k] - el)
h = order[k]
on = np.abs(el - el_desc[h]) < ORDER_TOL_DEG
azs.append(np.degrees(np.arctan2(x[on], -y[on])))
hs.append(h[on])
az = np.concatenate(azs)
h = np.concatenate(hs)
diffs = []
for ring in range(n):
a = np.unique(np.round(az[h == ring], 4))
if a.size > 20:
diffs.append(np.diff(a))
if not diffs:
raise ValueError("недостаточно валидных лучей для калибровки развёртки")
d = np.concatenate(diffs)
d = d[d > 1e-3]
vals, cnt = np.unique(np.round(d, 3), return_counts=True)
step = float(vals[np.argmax(cnt)])
step = float(np.median(d[np.abs(d - step) < 0.1 * step]))
ph = np.exp(2j * np.pi * az / step)
s = (np.bincount(h, weights=ph.real, minlength=n)
+ 1j * np.bincount(h, weights=ph.imag, minlength=n))
ref = float(np.angle(s.sum()) / (2 * np.pi) * step)
frac = np.where(np.abs(s) > 0, np.angle(s) / (2 * np.pi) * step, ref)
resid = (frac - ref + step / 2) % step - step / 2
lo, hi = np.percentile(az, [0.05, 99.95])
wrap = bool(hi - lo > 350.0)
if wrap:
n_az = int(round(360.0 / step))
az0 = ref + step * round((180.0 - ref) / step)
else:
az0 = ref + step * round((hi - ref) / step)
n_az = int(round((az0 - lo) / step)) + 1
return ScanLayout(el_desc, -step, float(az0), np.zeros(n, np.int64), resid,
n_az, 1, wrap, indexed=False)
# ------------------------------------------------------------------ сериализация
def save(self, path: str | Path) -> None:
np.savez_compressed(path, el_deg=self.el_deg, az_step_deg=self.az_step_deg,
az0_deg=self.az0_deg, col_shift=self.col_shift,
az_resid_deg=self.az_resid_deg, n_az=self.n_az,
n_echo=self.n_echo, wrap=self.wrap, indexed=self.indexed)
@staticmethod
def load(path: str | Path) -> "ScanLayout":
d = np.load(path)
return ScanLayout(d["el_deg"], float(d["az_step_deg"]), float(d["az0_deg"]),
d["col_shift"], d["az_resid_deg"], int(d["n_az"]),
int(d["n_echo"]), bool(d["wrap"]) if "wrap" in d else False,
bool(d["indexed"]) if "indexed" in d else True)
def __repr__(self) -> str:
kind = "" if self.indexed else ", по углам точек"
return (f"ScanLayout(колец={self.n_rings}, азимутов={self.n_az}, эх={self.n_echo}, "
f"сектор {self.az_grid_deg.min():.1f}°…{self.az_grid_deg.max():.1f}°, "
f"шаг {abs(self.az_step_deg):.3f}°, "
f"элевация {self.el_deg.min():.1f}°…{self.el_deg.max():.1f}°, "
f"скос каналов {np.ptp(self.col_shift)} стлб{kind})")
# ---------------------------------------------------------------------- вспомогательное
@contextmanager
def _quiet():
"""Пустые срезы и деление на ноль при калибровке — норма, а не ошибка."""
with warnings.catch_warnings(), np.errstate(all="ignore"):
warnings.simplefilter("ignore", RuntimeWarning)
yield
def _wrap180(a):
"""Привести угол(ы) в градусах к полуинтервалу (−180, 180]."""
return -((-np.asarray(a, np.float64) + 180.0) % 360.0 - 180.0)
def _xyz64(pc: PointCloud2):
"""Координаты в float64 и маска точек с эхом (не ноль и не NaN)."""
p = pc.points
x = p["x"].astype(np.float64)
y = p["y"].astype(np.float64)
z = p["z"].astype(np.float64)
ok = ((x != 0) | (y != 0) | (z != 0)) & np.isfinite(x) & np.isfinite(y) & np.isfinite(z)
return x, y, z, ok
def _ring_elevations(clouds: list[PointCloud2], max_frames: int = 6) -> np.ndarray:
"""Элевации колец по гистограмме, без поля `ring` и без опоры на порядок.
У лазерного канала элевация постоянна до 0.0001°, поэтому точки кольца
ложатся в один бин в 0.002°; соседние кольца Pandar128 разнесены на 0.086°
и больше. Разброс float32 может расщепить кольцо на соседние бины — они
сливаются. Вставленные точки (синтетика) рассыпаны по элевации и дают
мелкие группы, которые отсекает порог по весу. Порядок — сверху вниз,
как нумерует каналы Hesai.
"""
parts = []
for pc in clouds[:max_frames]:
x, y, z, ok = _xyz64(pc)
parts.append(np.degrees(np.arctan2(z[ok], np.hypot(x[ok], y[ok]))))
el = np.concatenate(parts)
if el.size == 0:
raise ValueError("нет ни одной точки с эхом для калибровки колец")
q, cnt = np.unique(np.rint(el / 0.002).astype(np.int64), return_counts=True)
grp = np.concatenate(([0], np.cumsum(np.diff(q) > 5)))
w = np.bincount(grp, weights=cnt)
c = np.bincount(grp, weights=cnt * q * 0.002) / w
top = np.sort(w)[-min(64, w.size):]
keep = w >= 0.25 * np.median(top)
if keep.sum() < 2:
raise ValueError("не удалось выделить кольца по элевации")
return np.sort(c[keep])[::-1]
def _is_organized(pc: PointCloud2, el_ring: np.ndarray) -> bool:
"""Цел ли порядок точек: элевация каждой точки = элевация её кольца."""
n = el_ring.size
if pc.n_points == 0 or pc.n_points % n:
return False
x, y, z, ok = _xyz64(pc)
el = np.degrees(np.arctan2(z, np.hypot(x, y))).reshape(-1, n)
ok = ok.reshape(-1, n)
for table in (el_ring, el_ring[::-1]):
if np.all(np.abs(el - table[None, :])[ok] < ORDER_TOL_DEG):
return True
return False
def _detect_echoes(az: np.ndarray) -> int:
"""Двойное эхо: соседние столбцы делят азимут (проверяется по каждому кольцу)."""
if az.shape[1] < 4:
return 1
a, b = az[:, 0::2], az[:, 1::2]
m = np.isfinite(a) & np.isfinite(b)
if m.sum() < 100:
return 1
return 2 if np.mean(np.abs(a[m] - b[m]) < 1e-3) > 0.8 else 1
def _fill_linear(a: np.ndarray) -> np.ndarray:
"""Линейно достроить NaN-пропуски по индексу (сетка равномерная)."""
a = np.asarray(a, np.float64).copy()
bad = ~np.isfinite(a)
if bad.all():
raise ValueError("нет ни одного валидного угла для калибровки")
if bad.any():
idx = np.arange(a.size)
a[bad] = np.interp(idx[bad], idx[~bad], a[~bad])
return a

38
ros2_ws/src/flyguard/flyguard/ros_conv.py Normal file
View file

@ -0,0 +1,38 @@
"""Преобразование sensor_msgs/PointCloud2 из rclpy во внутреннее представление.
Внутри ноды сообщение уже разобрано транспортом, поэтому CDR-парсер не нужен:
достаточно посмотреть на поля и построить структурированный numpy-массив
поверх готового буфера, без копирования.
"""
from __future__ import annotations
import numpy as np
from .cdr import PointCloud2, point_dtype
def from_ros(msg) -> PointCloud2:
"""sensor_msgs.msg.PointCloud2 → flyguard.cdr.PointCloud2 (без копирования)."""
fields = [(f.name, f.offset, f.datatype, f.count) for f in msg.fields]
fields.sort(key=lambda f: f[1])
dt = point_dtype(fields, msg.point_step)
buf = msg.data if isinstance(msg.data, (bytes, bytearray, memoryview)) else \
np.asarray(msg.data, np.uint8).tobytes()
n = msg.height * msg.width
pts = np.frombuffer(buf, dtype=dt, count=n)
stamp = msg.header.stamp.sec + msg.header.stamp.nanosec * 1e-9
return PointCloud2(stamp=stamp, frame_id=msg.header.frame_id,
height=msg.height, width=msg.width,
point_step=msg.point_step, is_dense=msg.is_dense, points=pts)
def has_required_fields(msg) -> tuple[bool, str]:
"""Проверить, что в облаке есть всё необходимое конвейеру."""
names = {f.name for f in msg.fields}
need = {"x", "y", "z", "intensity"}
missing = need - names
if missing:
return False, f"в облаке нет полей: {', '.join(sorted(missing))}"
if msg.height * msg.width == 0:
return False, "пустое облако"
return True, ""

418
ros2_ws/src/flyguard/flyguard/synth.py Normal file
View file

@ -0,0 +1,418 @@
"""Синтетические препятствия: трассировка лучей в реальные кадры.
Разметки в датасете нет, а организаторы прямо предупредили, что приватный тест
собран добавлением синтезированных препятствий в новые проезды. Поэтому свой
полигон строится тем же способом: берётся настоящий кадр пустого тоннеля,
в него трассировкой лучей вставляется предмет заданного размера на заданной
дистанции, и получается **размеченный** пример с точно известным ответом.
Вставка идёт в исходное облако точек, а не в готовый дальностный образ, поэтому
через конвейер проходит ровно тот же путь, что и настоящие данные, начиная
с ретины.
Модель сенсора намеренно пессимистична: добавляется шум дальности, а
вероятность несостоявшегося эха растёт с расстоянием и с углом падения. Лучше
недооценить свой детектор, чем на защите обнаружить, что полигон был слишком
добрым. Яркость вставки берётся из самой записи — из возвратов на тех же
лучах: абсолютной шкалы интенсивности в этих данных нет, и любое назначенное
число делает вставку узнаваемой (см. длинный комментарий ниже).
"""
from __future__ import annotations
from dataclasses import dataclass, field
import numpy as np
from .cdr import PointCloud2
from .geometry import RailPlane
from .retina import ScanLayout
INF = np.float32(np.inf)
# --------------------------------------------------------------------------- тела
@dataclass
class Box:
"""Параллелепипед в координатах пути, стоящий на плоскости рельсов."""
length: float # вдоль пути, м
width: float # поперёк, м
height: float # вверх от рельса, м
h_base: float = 0.0
def intersect(self, od, ou, oh, dd, du, dh) -> np.ndarray:
lo = np.array([-self.length / 2, -self.width / 2, self.h_base], np.float32)
hi = np.array([self.length / 2, self.width / 2, self.h_base + self.height], np.float32)
t0 = np.full(od.shape, -INF, np.float32)
t1 = np.full(od.shape, INF, np.float32)
for o, d, a, b in ((od, dd, lo[0], hi[0]), (ou, du, lo[1], hi[1]),
(oh, dh, lo[2], hi[2])):
with np.errstate(divide="ignore", invalid="ignore"):
ta = (a - o) / d
tb = (b - o) / d
lo_t = np.where(d != 0, np.minimum(ta, tb), np.where((o >= a) & (o <= b), -INF, INF))
hi_t = np.where(d != 0, np.maximum(ta, tb), np.where((o >= a) & (o <= b), INF, -INF))
t0 = np.maximum(t0, lo_t)
t1 = np.minimum(t1, hi_t)
hit = (t1 >= np.maximum(t0, 0.0)) & np.isfinite(t0)
return np.where(hit, np.maximum(t0, 0.0), INF)
@dataclass
class Cylinder:
"""Вертикальный цилиндр — человек, столб, ведро."""
radius: float
height: float
h_base: float = 0.0
def intersect(self, od, ou, oh, dd, du, dh) -> np.ndarray:
a = dd * dd + du * du
b = 2.0 * (od * dd + ou * du)
c = od * od + ou * ou - self.radius ** 2
disc = b * b - 4 * a * c
ok = (disc > 0) & (a > 1e-9)
sq = np.sqrt(np.where(ok, disc, 0.0))
with np.errstate(divide="ignore", invalid="ignore"):
t = (-b - sq) / (2 * a)
t2 = (-b + sq) / (2 * a)
t = np.where(t > 0, t, t2)
h = oh + t * dh
ok &= (t > 0) & (h >= self.h_base) & (h <= self.h_base + self.height)
return np.where(ok, t, INF)
@dataclass
class Sphere:
radius: float
h_centre: float
def intersect(self, od, ou, oh, dd, du, dh) -> np.ndarray:
oz = oh - self.h_centre
a = dd * dd + du * du + dh * dh
b = 2.0 * (od * dd + ou * du + oz * dh)
c = od * od + ou * ou + oz * oz - self.radius ** 2
disc = b * b - 4 * a * c
ok = disc > 0
sq = np.sqrt(np.where(ok, disc, 0.0))
with np.errstate(divide="ignore", invalid="ignore"):
t = (-b - sq) / (2 * a)
return np.where(ok & (t > 0), t, INF)
@dataclass
class ObjectModel:
"""Предмет: набор тел плюс отражательные свойства."""
name: str
parts: list = field(default_factory=list)
reflectivity: float = 0.3 # 0…1, доля отражённого света
def intersect(self, od, ou, oh, dd, du, dh) -> np.ndarray:
t = np.full(od.shape, INF, np.float32)
for p in self.parts:
t = np.minimum(t, p.intersect(od, ou, oh, dd, du, dh))
return t
@property
def size(self) -> tuple[float, float]:
"""Грубые габариты (ширина, высота) для отчётов."""
w = h = 0.0
for p in self.parts:
if isinstance(p, Box):
w = max(w, p.width); h = max(h, p.h_base + p.height)
elif isinstance(p, Cylinder):
w = max(w, 2 * p.radius); h = max(h, p.h_base + p.height)
elif isinstance(p, Sphere):
w = max(w, 2 * p.radius); h = max(h, p.h_centre + p.radius)
return w, h
def catalogue() -> dict[str, ObjectModel]:
"""Набор предметов, встречающихся в тоннеле, от крупных к мелким."""
return {
"человек_стоя": ObjectModel("человек_стоя", [
Cylinder(radius=0.22, height=1.45),
Sphere(radius=0.11, h_centre=1.60)], reflectivity=0.35),
"человек_сидя": ObjectModel("человек_сидя", [
Box(0.45, 0.50, 0.85)], reflectivity=0.35),
"человек_лежа": ObjectModel("человек_лежа", [
Box(length=1.80, width=0.50, height=0.30, h_base=0.0)], reflectivity=0.35),
"ящик": ObjectModel("ящик", [Box(0.60, 0.60, 0.60)], reflectivity=0.40),
"чемодан": ObjectModel("чемодан", [Box(0.25, 0.45, 0.55)], reflectivity=0.30),
"ведро": ObjectModel("ведро", [Cylinder(radius=0.15, height=0.35)], reflectivity=0.45),
"камень": ObjectModel("камень", [Sphere(radius=0.11, h_centre=0.11)], reflectivity=0.20),
"бутылка": ObjectModel("бутылка", [Cylinder(radius=0.045, height=0.30)],
reflectivity=0.25),
"кабель": ObjectModel("кабель", [Box(2.20, 0.06, 0.06)], reflectivity=0.15),
"каска": ObjectModel("каска", [Sphere(radius=0.14, h_centre=0.10)], reflectivity=0.55),
}
# --------------------------------------------------------------------------- вставка
@dataclass
class Placement:
"""Где стоит предмет."""
d: float # вперёд от сенсора, м
u: float = 0.0 # поперёк от оси пути, м
yaw_deg: float = 0.0 # поворот вокруг вертикали (для вытянутых тел)
def ray_dirs_track(layout: ScanLayout, plane: RailPlane):
"""Направления лучей в координатах пути: (dd, du, dh) и начало (0, 0, H)."""
dirs = layout.dirs
dd = -dirs[..., 1].astype(np.float32)
du = dirs[..., 0].astype(np.float32)
dh = (dirs[..., 2] - plane.a * dd - plane.b * du).astype(np.float32)
return dd, du, dh, np.float32(plane.height)
# Паспортные данные Pandar128E3X (руководство v4p5, п. 1.4 и Приложение A):
# дальность 0.3…200 м при отражательной способности 10 %, вероятность
# обнаружения на паспортной дальности PoD = 70 %, точность ±2 см на 1…200 м.
SPEC_REFLECTIVITY = 0.10
SPEC_POD = 0.70
HARD_MAX_RANGE = 230.0 # дальше в датасете возвратов не встречается
# Расходимость луча в руководстве не указана; принята равной угловому шагу
# решётки (0.1° по азимуту, 0.125° по элевации в полосе высокого разрешения) —
# это верхняя оценка, дающая консервативный результат для мелких предметов.
BEAM_DIV_H = 1.75e-3 # рад
BEAM_DIV_V = 2.18e-3 # рад
_CHANNEL_RANGE: np.ndarray | None = None
# Интенсивность вставки. Здесь нельзя придумать ни формулы, ни числа.
#
# Сначала стояла ламбертова ρ·cosθ/r². Прибор, однако, отдаёт не принятую
# энергию, а отражательную способность с компенсацией дальности: медиана по
# облаку держится 8…9 от 5 до 50 м и как 1/r² не падает. Вставка получалась на
# 55 м в двадцать раз тусклее настоящего предмета, а за 110 м упиралась в
# нижний срез шкалы — и «тускло» становилось безошибочным признаком предмета.
#
# Замена на постоянную яркость 100·ρ·√cosθ, привязанную к настоящему предмету
# (34.5 на 55.9 м), просто перевернула артефакт: 35 против 3…7 у обстановки на
# каждой полосе дальности, AUC по одной интенсивности 0.96…0.97.
#
# Причина в том, что абсолютной шкалы тут нет. Медиана яркости кандидатов
# обстановки по бэгам: 3.0, 4.0, 4.0, 4.0, 7.0 — а в `doubleT_obstacle`, где
# лежит настоящий предмет, 23.5 при 34.5 у самого предмета. Разница между
# записями впятеро больше, чем контраст предмета к фону внутри записи. Любое
# абсолютное число, назначенное вставке, оказывается подарком детектору — в ту
# или в другую сторону.
#
# Поэтому вставка берёт яркость **реальных возвратов с тех же самых лучей** —
# того, что предмет заслонил; где эха не было, из возвратов вдоль остальных его
# лучей. Признак становится неинформативным, и полигон меряет геометрию и
# движение, то есть то, что мы моделируем честно. Оценка заниженная: настоящий
# предмет в записи был в 1.47 раза ярче окружения. На линии с откалиброванной
# яркостью этот запас можно вернуть — замером, а не верой.
#
# Отражательная способность никуда не делась: она определяет, вернётся ли эхо
# вообще (`dropout_probability`), а это и есть её настоящая роль.
INTEN_SPREAD = 0.12 # разброс отсчёта, логнормальный, ≈ ±12 %
class IntensityEnv:
"""Яркость реальных возвратов кадра, разложенная по дальности.
Нужна, чтобы луч предмета, ушедший в пустоту, получил яркость такую же, как
у настоящих возвратов С ТОЙ ЖЕ дальности, а не с ближних и ярких. Считается
один раз на кадр и переиспользуется всеми сценариями: в сборе выборки их
135 на кадр, и пересчитывать корни по полутора миллионам точек для каждого
незачем.
"""
EDGES = np.array([0, 20, 40, 60, 90, 130, 180, 260], np.float32)
def __init__(self, pc: PointCloud2):
q = pc.points
it = np.asarray(q["intensity"], np.float32)
x, y, z = q["x"], q["y"], q["z"]
r2 = x * x + y * y + z * z
ok = (it > 0) & (r2 > 1.0) & np.isfinite(r2)
r = np.sqrt(r2[ok], dtype=np.float32)
v = it[ok]
b = np.clip(np.searchsorted(self.EDGES, r, side="right") - 1,
0, self.EDGES.size - 2)
order = np.argsort(b, kind="stable")
b_s, v_s = b[order], v[order]
cut = np.searchsorted(b_s, np.arange(self.EDGES.size - 1), side="left")
cut = np.append(cut, b_s.size)
self._pools = [v_s[cut[i]:cut[i + 1]] for i in range(self.EDGES.size - 1)]
self._all = v_s
def sample(self, d: float, n: int, rng: np.random.Generator) -> np.ndarray:
i = int(np.clip(np.searchsorted(self.EDGES, d, side="right") - 1,
0, self.EDGES.size - 2))
pool = self._pools[i]
if pool.size < 32: # на этой дальности возвратов нет
pool = self._all
if pool.size == 0:
return np.full(n, 5.0, np.float32)
return rng.choice(pool, size=n).astype(np.float32)
def local_intensity(prev: np.ndarray, pool: np.ndarray, frame: np.ndarray,
rng: np.random.Generator, env: "IntensityEnv | None" = None,
d: float = 0.0) -> np.ndarray:
"""Яркость вставки по окружению. Отражения среди аргументов нет намеренно.
Дальность участвует ровно в одном качестве — какую полосу реальных
возвратов брать для лучей, ушедших в пустоту. Никакого закона яркости от
дальности здесь не задаётся.
"""
v = np.asarray(prev, np.float32).copy()
miss = ~(v > 0)
if miss.any():
n = int(miss.sum())
if env is not None:
v[miss] = env.sample(d, n, rng)
else:
src = pool[pool > 0] if pool.size else np.empty(0, np.float32)
if src.size == 0: # все лучи предмета в пустоту
wide = frame[::997]
src = wide[wide > 0]
v[miss] = (rng.choice(src, size=n) if src.size else np.float32(5.0))
v = v * rng.lognormal(0.0, INTEN_SPREAD, v.shape)
return np.clip(v, 1.0, 255.0).astype(np.float32)
def channel_max_range() -> np.ndarray:
"""Паспортная дальность каждого канала при 10 % отражения, (128,).
Каналы сильно неравноправны: 34–65 «дальнобойные» и берут 200 м, а 98–128
смотрят в землю и рассчитаны только на ближнее и среднее поле. Без учёта
этого синтетический полигон завышал бы дальность обнаружения для предметов,
попадающих в нижние каналы.
"""
global _CHANNEL_RANGE
if _CHANNEL_RANGE is None:
import csv
from pathlib import Path
path = Path(__file__).with_name("data") / "pandar128_channels.csv"
try:
rows = sorted(csv.DictReader(open(path, encoding="utf-8")),
key=lambda r: int(r["channel"]))
_CHANNEL_RANGE = np.array([float(r["max_range_10pct_m"]) for r in rows],
np.float32)
except (OSError, KeyError, ValueError):
_CHANNEL_RANGE = np.full(128, 200.0, np.float32)
return _CHANNEL_RANGE
def dropout_probability(r: np.ndarray, reflectivity: float,
ang_w: np.ndarray | float = 1.0,
ang_h: np.ndarray | float = 1.0,
max_range: np.ndarray | float = 200.0) -> np.ndarray:
"""Вероятность, что эхо не вернётся.
Принятая мощность падает как ρ·A/r², где A — доля пятна луча, закрытая
предметом. Отсюда «эффективная дальность» r·√(ρ_паспорт/(ρ·A)), которую
остаётся сравнить с паспортной дальностью **этого канала**. Переход сделан
логистическим и смещён так, чтобы на паспортной дальности вероятность
обнаружения равнялась заявленным 70 %.
Заполнение пятна важно именно для мелких предметов: на 200 м луч шириной
1.75 мрад покрывает 35 см, и бутылка диаметром 9 см отражает лишь четверть
его энергии — поэтому она пропадает намного раньше человека, хотя по
геометрии в неё ещё попадают лучи.
"""
fill = np.clip(ang_w / BEAM_DIV_H, 0.05, 1.0) * np.clip(ang_h / BEAM_DIV_V, 0.05, 1.0)
rho = max(reflectivity, 0.02) * fill
eff = r * np.sqrt(SPEC_REFLECTIVITY / rho)
width = 0.12 * np.asarray(max_range, np.float32)
r50 = np.asarray(max_range, np.float32) + width * np.log(SPEC_POD / (1 - SPEC_POD))
p_detect = 1.0 / (1.0 + np.exp((eff - r50) / np.maximum(width, 1e-3)))
p_detect = np.where(r > HARD_MAX_RANGE, 0.0, p_detect)
return np.clip(1.0 - p_detect, 0.0, 1.0).astype(np.float32)
def inject(pc: PointCloud2, layout: ScanLayout, plane: RailPlane,
obj: ObjectModel, place: Placement, *,
rng: np.random.Generator | None = None,
range_noise: float = 0.02, cols: slice | None = None,
env: "IntensityEnv | None" = None) -> tuple[PointCloud2, dict]:
"""Вставить предмет в облако точек. Возвращает (новое облако, разметка)."""
rng = rng or np.random.default_rng()
n_rings, n_az, n_echo = layout.n_rings, layout.n_az, layout.n_echo
dd, du, dh, H = ray_dirs_track(layout, plane)
# начало луча в системе предмета: сенсор в (0,0,H), предмет в (d, u, 0)
od = np.full(dd.shape, -np.float32(place.d), np.float32)
ou = np.full(dd.shape, -np.float32(place.u), np.float32)
oh = np.full(dd.shape, H, np.float32)
if place.yaw_deg:
c, s = np.cos(np.radians(place.yaw_deg)), np.sin(np.radians(place.yaw_deg))
od, ou = c * od + s * ou, -s * od + c * ou
dd, du = c * dd + s * du, -s * dd + c * du
t = obj.intersect(od, ou, oh, dd, du, dh)
hit = np.isfinite(t) & (t > 1.0)
if not hit.any():
return pc, dict(hit_rays=0, d=place.d, u=place.u, name=obj.name)
# шум дальности и пропуски эха
t = t + rng.normal(0.0, range_noise, t.shape).astype(np.float32)
w_obj, h_obj = obj.size
ang_w = w_obj / np.maximum(t, 1.0)
ang_h = h_obj / np.maximum(t, 1.0)
ch_range = channel_max_range()
per_ray_range = (ch_range[:n_rings, None] if ch_range.size >= n_rings
else np.float32(200.0))
p_drop = dropout_probability(t, obj.reflectivity, ang_w, ang_h, per_ray_range)
hit &= rng.random(t.shape) > p_drop
pts = pc.points.copy()
ring_i, col_i = np.nonzero(hit)
raw_col = layout.gather[ring_i, col_i] # выпрямленный столбец → сырой
new_r = t[ring_i, col_i]
# плоский индекс точки: (сырой столбец · число эх + эхо) · число колец + кольцо
base = (raw_col * n_echo) * n_rings + ring_i
fx, fy, fz = pts["x"], pts["y"], pts["z"] # виды на поля, запись идёт в pts
# предмет виден, только если он ближе уже зарегистрированного эха
r_exist = np.full(base.shape, INF, np.float32)
for e in range(n_echo):
idx = base + e * n_rings
ex, ey, ez = fx[idx], fy[idx], fz[idx]
have = (ex != 0) | (ey != 0) | (ez != 0)
r_e = np.where(have, np.sqrt(ex * ex + ey * ey + ez * ez), INF)
r_exist = np.minimum(r_exist, r_e)
closer = new_r < r_exist
# яркость того, что было на лучах предмета: на выбранных — то, что он
# заслонил, остальные идут в запасной набор для лучей без эха
prev_sel = pts["intensity"][base[closer]]
pool = pts["intensity"][base]
ring_i, col_i, base, new_r = ring_i[closer], col_i[closer], base[closer], new_r[closer]
n_written = int(base.size)
if n_written:
dir_sel = layout.dirs[ring_i, col_i]
nx = (dir_sel[:, 0] * new_r).astype(np.float32)
ny = (dir_sel[:, 1] * new_r).astype(np.float32)
nz = (dir_sel[:, 2] * new_r).astype(np.float32)
inten = local_intensity(prev_sel, pool, pts["intensity"], rng,
env=env, d=place.d)
for e in range(n_echo):
idx = base + e * n_rings
fx[idx] = nx
fy[idx] = ny
fz[idx] = nz
pts["intensity"][idx] = inten.astype(np.float32)
out = PointCloud2(stamp=pc.stamp, frame_id=pc.frame_id, height=pc.height,
width=pc.width, point_step=pc.point_step,
is_dense=pc.is_dense, points=pts)
w, hgt = obj.size
return out, dict(hit_rays=int(n_written), d=float(place.d), u=float(place.u),
name=obj.name, width=w, height=hgt,
reflectivity=obj.reflectivity,
# индексы лучей, в которые предмет реально записан: по ним
# разметка кандидата точная, а не «по дальности примерно»
rays=(ring_i, col_i))

121
ros2_ws/src/flyguard/flyguard/track_readout.py Normal file
View file

@ -0,0 +1,121 @@
"""Считывание по ТРЕКУ — тот же контур MBON, но на входе накопленная история.
Считывание в `mbon_readout.py` смотрит на один кадр: контур кандидата, его
тень, яркость, опору накопителя. Этого хватает вблизи и не хватает вдали —
за 120 м предмет даёт пять-шесть лучей, и по одному кадру он неотличим от
куска стены, попавшего в габарит.
Но у трека есть то, чего у кандидата нет в принципе: **история**. Поезд
проехал двадцать метров, а предмет, если он настоящий, остался на том же
месте в тоннеле. Воронка потерь (EXPERIMENTS п. 12) показывает, что именно
здесь и теряется дальнее обнаружение: на 120…160 м трек заводится в 62 %
наблюдений, а до решения доходит 38 % — улика есть, порога не набирает.
Поэтому вход здесь другой: сводки за всю жизнь трека, а не последнее
значение, — средний и максимальный вес наблюдения, средний и максимальный
покадровый отсчёт MBON, наклон числа лучей по времени, разбросы положения.
**ЧЕМ ЭТО КОНЧИЛОСЬ.** Обученная модель не обгоняет ни один из признаков,
которые ей же и скормили: leave-one-bag-out AUC 0.873 против 0.880 у одного
среднего веса наблюдения и 0.895 у среднего отсчёта считывания. Причина — не в
схеме: наблюдений 9 299, но это сотни кадров подряд по одному и тому же треку,
и независимых примеров тут сотни, а не тысячи. Поэтому в решении участвует
**не модель, а прямо среднее** (`descending.track_score`, по умолчанию
`w_mean`), а всё в этом файле — инструмент замера. Разбор — EXPERIMENTS п. 15.
Отдельно: `s_std` и `u_std` в решении использовать нельзя. На полигоне разброс
привязки к месту выглядит лучшим признаком дальней полосы (AUC 0.869 за 160 м),
а на настоящем объекте он перевёрнут (0.223): вставка ставится на дальность,
посчитанную из той же оценки собственного движения, что двигает мировую
координату, и разброса у неё нет по построению. Признаки оставлены в
дескрипторе нарочно — чтобы проверку можно было повторить (п. 15.4).
"""
from __future__ import annotations
import numpy as np
from .central_complex import Track, expected_rays
from .mbon_readout import MbonConfig, MbonReadout
TRACK_FEATURES = (
"log_d", # дальность сейчас
"evidence", # накопленная улика — то, чем решают сегодня
"hits", # подтверждений
"age", # возраст, кадров
"hit_rate", # подтверждений на кадр жизни
"misses", # промахов подряд
"closed", # log(первая дальность / нынешняя): насколько подъехали
"novelty", # сглаженная новизна из памяти тоннеля
"width", "height", "h",
"speed_lat", # модуль поперечной скорости
"rays_now", # лучей к ожидаемым для этой дальности
"rays_mean", # то же, в среднем за жизнь
"rays_slope", # и наклон по времени
"w_mean", "w_max", # вес наблюдения
"p_mean", "p_max", # покадровый отсчёт считывания
# Оба разброса — ТОЛЬКО для замеров: на синтетике сильны, на реальном
# объекте перевёрнуты. См. шапку файла и EXPERIMENTS п. 15.4.
"s_std", # разброс места в тоннеле, м
"u_std", # разброс поперечного положения, м
)
def _std(n: int, s: float, sq: float) -> float:
"""Разброс по накопленным суммам. Отрицательную дисперсию даёт
округление, а не данные, поэтому она просто отсекается."""
if n < 2:
return 0.0
var = sq / n - (s / n) ** 2
return float(np.sqrt(var)) if var > 0.0 else 0.0
def describe_track(t: Track, s_now: float) -> np.ndarray:
"""Вектор трека для считывания. Порядок — как в `TRACK_FEATURES`."""
d = max(t.distance(s_now), 1.0)
n = max(t.n_obs, 1)
den = t.n_obs * t.i_sq - t.i_sum * t.i_sum
slope = ((t.n_obs * t.ri_sum - t.i_sum * t.r_sum) / den
if t.n_obs > 1 and abs(den) > 1e-9 else 0.0)
return np.array((
np.log(d),
t.evidence,
float(t.hits),
float(t.age),
t.hits / max(t.age, 1),
float(t.misses),
np.log(max(t.first_d, 1.0) / d),
t.novelty,
t.width, t.height, t.h,
abs(t.speed_lat),
t.last_n_rays / expected_rays(d),
t.r_sum / n,
slope,
t.w_sum / n, t.w_max,
t.p_sum / n, t.p_max,
_std(t.n_obs, t.s_sum, t.s_sq),
_std(t.n_obs, t.u_sum, t.u_sq),
), np.float32)
class TrackReadout(MbonReadout):
"""Тот же разрежённый код, другой вход и другой учитель.
Ёмкость меньше, чем у покадрового считывания: примеров тут на порядок
меньше (треков, а не кандидатов), и восемь тысяч клеток на такой выборке
заучивают её наизусть. Значение выбирается развёрткой в
`tools/train_track.py`, а не на глаз.
"""
def __init__(self, cfg: MbonConfig | None = None,
n_pn: int = len(TRACK_FEATURES)):
super().__init__(cfg or MbonConfig(n_kc=2_000, seed=20260922),
n_pn=n_pn)
def annotate_tracks(self, tracks: list[Track], s_now: float) -> None:
"""Проставить каждому треку отсчёт в `extra`-поле `p_track`."""
if not tracks:
return
X = np.stack([describe_track(t, s_now) for t in tracks])
p = 1.0 / (1.0 + np.exp(-self.logit(X)))
for t, pp in zip(tracks, p):
t.p_track = float(pp)

150
ros2_ws/src/flyguard/launch/detect.launch.py Normal file
View file

@ -0,0 +1,150 @@
"""Запуск детектора FlyGuard.
ros2 launch flyguard detect.launch.py
ros2 launch flyguard detect.launch.py rviz:=true brain_view:=true
ros2 launch flyguard detect.launch.py bag:=/data/for_hackathon/doubleT_obstacle
ros2 launch flyguard detect.launch.py bag:=/data/squareT_platform_squareT_switch start:=18
Если указан `bag`, launch сам поднимет `ros2 bag play` — тогда для демонстрации
достаточно одной команды; `start` — с какой секунды записи начать.
"""
from pathlib import Path
from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch.actions import (DeclareLaunchArgument, ExecuteProcess, OpaqueFunction,
RegisterEventHandler, TimerAction)
from launch.conditions import IfCondition
from launch.event_handlers import OnProcessIO
from launch.substitutions import (LaunchConfiguration, PathJoinSubstitution,
PythonExpression)
from launch_ros.actions import Node
def generate_launch_description() -> LaunchDescription:
share = Path(get_package_share_directory("flyguard"))
default_memory = share / "artifacts" / "mushroom_body.npz"
default_mbon = share / "artifacts" / "mbon_readout.npz"
args = [
DeclareLaunchArgument("input_topic", default_value="/lidar_points",
description="топик с облаком точек лидара"),
DeclareLaunchArgument("memory_path",
default_value=str(default_memory) if default_memory.exists() else "",
description="обученная память тоннеля (.npz)"),
DeclareLaunchArgument("mbon_path",
default_value=str(default_mbon) if default_mbon.exists() else "",
description="обученное считывание MBON (.npz); пусто — ручная формула"),
DeclareLaunchArgument("fov_deg", default_value="30.0"),
DeclareLaunchArgument("half_width", default_value="1.2",
description="полуширина габарита, м"),
DeclareLaunchArgument("best_effort", default_value="false",
description="QoS подписки: true — BEST_EFFORT, false — RELIABLE. "
"Кадры лидара крупные (до 24 МБ), и при BEST_EFFORT "
"потеря одного UDP-фрагмента отбрасывает всё сообщение"),
DeclareLaunchArgument("queue_depth", default_value="10",
description="глубина очереди подписки"),
DeclareLaunchArgument("async_worker", default_value="false",
description="обрабатывать в отдельном потоке; по умолчанию "
"в колбэке — так не теряется выработка на GIL"),
DeclareLaunchArgument("brain_view", default_value="false",
description="публиковать схему мозга мухи с активностью"),
DeclareLaunchArgument("brain_scale", default_value="1",
description="масштаб вида мозга: 1 — 1180 пикселей, "
"2–3 — для экрана и видео в 2K/4K"),
DeclareLaunchArgument("debug_cloud", default_value="false"),
DeclareLaunchArgument("rviz", default_value="false"),
DeclareLaunchArgument("bag", default_value="",
description="путь к bag-файлу; пусто — не запускать проигрывание"),
DeclareLaunchArgument("rate", default_value="1.0",
description="скорость проигрывания bag"),
DeclareLaunchArgument("start", default_value="0.0",
description="с какой секунды записи начать; узлу нужно "
"около двух секунд на калибровку и разгон"),
DeclareLaunchArgument("device", default_value="auto",
description="где считать сетчатку, ламину и кластеризацию: "
"auto — видеокарта, если есть (контейнер с --gpus all), "
"иначе процессор; cuda; cpu. Результат одинаков"),
DeclareLaunchArgument("log_level", default_value="info"),
]
detector = Node(
package="flyguard",
executable="detector",
name="flyguard",
output="screen",
# BLAS в один поток: иначе потоки OpenBLAS крутятся вхолостую, и узел
# занимает 3 ядра вместо 0.4 при том же времени кадра (docker/Dockerfile)
additional_env={"OPENBLAS_NUM_THREADS": "1", "OMP_NUM_THREADS": "1",
"MKL_NUM_THREADS": "1"},
arguments=["--ros-args", "--log-level", LaunchConfiguration("log_level")],
parameters=[
PathJoinSubstitution([str(share), "config", "flyguard.yaml"]),
{
"input_topic": LaunchConfiguration("input_topic"),
"memory_path": LaunchConfiguration("memory_path"),
"mbon_path": LaunchConfiguration("mbon_path"),
"fov_deg": LaunchConfiguration("fov_deg"),
"half_width": LaunchConfiguration("half_width"),
"best_effort": LaunchConfiguration("best_effort"),
"queue_depth": LaunchConfiguration("queue_depth"),
"async_worker": LaunchConfiguration("async_worker"),
"brain_view": LaunchConfiguration("brain_view"),
"brain_scale": LaunchConfiguration("brain_scale"),
"publish_debug_cloud": LaunchConfiguration("debug_cloud"),
"device": LaunchConfiguration("device"),
},
],
)
# Со схемой мозга — свой конфиг: схема справа во всю высоту. Без неё пустая
# панель картинки отъедала бы у трёхмерного вида треть окна.
rviz_cfg = PathJoinSubstitution([str(share / "config"), PythonExpression([
"'flyguard_brain.rviz' if '", LaunchConfiguration("brain_view"),
"'.lower() in ('true', '1') else 'flyguard.rviz'"])])
rviz = Node(
package="rviz2", executable="rviz2", name="rviz2", output="log",
condition=IfCondition(LaunchConfiguration("rviz")),
arguments=["-d", rviz_cfg],
)
# --read-ahead-queue-size 10: по умолчанию проигрыватель сперва читает
# вперёд 1000 сообщений — при кадре в 24 МБ это 24 ГБ, и пока он читает,
# первые секунды записи «просрочиваются» и не публикуются вовсе (замерено:
# 119 кадров из 201 без флага).
# --clock: время записи в /clock для пульта (flyguard-keys: пауза и
# перемотка). Узел и RViz живут по своим часам и его не слушают.
play = ExecuteProcess(
cmd=["ros2", "bag", "play", LaunchConfiguration("bag"),
"--rate", LaunchConfiguration("rate"), "--read-ahead-queue-size", "10",
"--start-offset", LaunchConfiguration("start"), "--clock", "20"],
output="screen",
# пустой аргумент `bag` означает «не запускать проигрывание»
condition=IfCondition(PythonExpression(["'", LaunchConfiguration("bag"), "' != ''"])),
)
# Проигрывание стартует, когда узел подписался: без этого первые кадры
# записи уходят в пустоту (замерено: 244 кадра из 252 против 252 из 252 у
# раздельного запуска). Фиксированной паузы мало: с видеокартой узел
# поднимается дольше — импорт PyTorch, контекст CUDA, прогрев. Поэтому ждём
# строку журнала «подписка на» и ещё 1.5 с на обнаружение DDS; не пришла за
# 30 с — запускаем всё равно.
started = {"play": False}
def start_play():
if started["play"]:
return []
started["play"] = True
return [play]
def on_output(event):
if not started["play"] and "подписка на" in event.text.decode("utf-8", "ignore"):
return [TimerAction(period=1.5, actions=[OpaqueFunction(
function=lambda context: start_play())])]
return None
wait_node = RegisterEventHandler(OnProcessIO(target_action=detector, on_stdout=on_output,
on_stderr=on_output))
fallback = TimerAction(period=30.0, actions=[OpaqueFunction(
function=lambda context: start_play())])
return LaunchDescription(args + [detector, rviz, wait_node, fallback])

35
ros2_ws/src/flyguard/package.xml Normal file
View file

@ -0,0 +1,35 @@
<?xml version="1.0"?>
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
<name>flyguard</name>
<version>1.0.0</version>
<description>
Обнаружение посторонних объектов в габарите беспилотного поезда по данным 3D-лидара.
Архитектура повторяет вычислительные схемы зрительной системы и грибовидных тел
Drosophila melanogaster, взятые из коннектома.
</description>
<maintainer email="team@flyguard.local">FlyGuard</maintainer>
<license>MIT</license>
<exec_depend>rclpy</exec_depend>
<exec_depend>sensor_msgs</exec_depend>
<exec_depend>std_msgs</exec_depend>
<exec_depend>visualization_msgs</exec_depend>
<exec_depend>diagnostic_msgs</exec_depend>
<exec_depend>geometry_msgs</exec_depend>
<exec_depend>tf2_ros</exec_depend>
<exec_depend>flyguard_msgs</exec_depend>
<exec_depend>rosbag2_interfaces</exec_depend>
<exec_depend>rosgraph_msgs</exec_depend>
<exec_depend>builtin_interfaces</exec_depend>
<exec_depend>python3-numpy</exec_depend>
<exec_depend>python3-scipy</exec_depend>
<test_depend>ament_copyright</test_depend>
<test_depend>ament_flake8</test_depend>
<test_depend>python3-pytest</test_depend>
<export>
<build_type>ament_python</build_type>
</export>
</package>

0
ros2_ws/src/flyguard/resource/flyguard Normal file
View file

4
ros2_ws/src/flyguard/setup.cfg Normal file
View file

@ -0,0 +1,4 @@
[develop]
script_dir=$base/lib/flyguard
[install]
install_scripts=$base/lib/flyguard

35
ros2_ws/src/flyguard/setup.py Normal file
View file

@ -0,0 +1,35 @@
from glob import glob
from setuptools import find_packages, setup
package_name = "flyguard"
setup(
name=package_name,
version="1.0.0",
packages=find_packages(exclude=["test"]),
data_files=[
("share/ament_index/resource_index/packages", ["resource/" + package_name]),
("share/" + package_name, ["package.xml"]),
("share/" + package_name + "/launch", glob("launch/*.launch.py")),
("share/" + package_name + "/config", glob("config/*")),
("share/" + package_name + "/artifacts", glob("artifacts/*")),
],
# Таблица каналов лидара и атлас нейронов лежат внутри пакета: без этого
# в образ попадают только .py, и вид мозга молча остаётся без данных.
package_data={package_name: ["data/*"]},
include_package_data=True,
install_requires=["setuptools"],
zip_safe=True,
maintainer="FlyGuard",
maintainer_email="team@flyguard.local",
description="Обнаружение посторонних объектов в тоннеле метро по данным 3D-лидара",
license="MIT",
tests_require=["pytest"],
entry_points={
"console_scripts": [
"detector = flyguard.node:main",
"player_keys = flyguard.player_keys:main",
],
},
)

1271
ros2_ws/src/flyguard/test/test_pipeline.py Normal file
View file

File diff suppressed because it is too large Load diff

1180
ros2_ws/src/flyguard/test/test_pipeline.py.orig Normal file
View file

File diff suppressed because it is too large Load diff

14
ros2_ws/src/flyguard_msgs/CMakeLists.txt Normal file
View file

@ -0,0 +1,14 @@
cmake_minimum_required(VERSION 3.8)
project(flyguard_msgs)
find_package(ament_cmake REQUIRED)
find_package(std_msgs REQUIRED)
find_package(rosidl_default_generators REQUIRED)
rosidl_generate_interfaces(${PROJECT_NAME}
"msg/DetectedObject.msg"
"msg/ObstacleStatus.msg"
DEPENDENCIES std_msgs
)
ament_package()

12
ros2_ws/src/flyguard_msgs/msg/DetectedObject.msg Normal file
View file

@ -0,0 +1,12 @@
# Один обнаруженный посторонний объект в габарите приближения поезда.
float32 distance # расстояние вперёд по пути, м
float32 lateral # смещение от оси пути, м (положительное вправо)
float32 height # высота нижней точки над головкой рельса, м
float32 width # поперечный размер, м
float32 size_v # вертикальный размер, м
float32 confidence # уверенность, 0..1
float32 novelty # ответ MBON: 1 — незнакомая форма, 0 — штатная конструкция
int32 n_rays # сколько лучей лидара попало в объект
int32 track_id # идентификатор трека, стабилен между кадрами
float32 time_to_collision # время до контакта при текущей скорости, с (inf, если стоим)

17
ros2_ws/src/flyguard_msgs/msg/ObstacleStatus.msg Normal file
View file

@ -0,0 +1,17 @@
# Состояние пути перед беспилотным поездом за один лидарный кадр.
#
# Минимально необходимое для бортовой системы — поля detected и distance:
# «путь свободен» либо «впереди препятствие на N метров».
std_msgs/Header header
bool detected # есть ли посторонний объект в габарите
bool emergency # объект близко и подтверждён надёжно: экстренное торможение
float32 distance # до ближайшего объекта, м (inf, если путь свободен)
float32 time_to_collision # до ближайшего объекта, с
float32 confidence # уверенность по ближайшему объекту, 0..1
float32 speed # оценка скорости поезда по лидару, м/с (одометрии нет)
float32 stopping_distance # требуемый тормозной путь при текущей скорости, м
float32 processing_ms # время обработки кадра, мс
DetectedObject[] objects # все подтверждённые объекты, ближний первым

21
ros2_ws/src/flyguard_msgs/package.xml Normal file
View file

@ -0,0 +1,21 @@
<?xml version="1.0"?>
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
<name>flyguard_msgs</name>
<version>1.0.0</version>
<description>Сообщения FlyGuard: статус пути перед беспилотным поездом.</description>
<maintainer email="team@flyguard.local">FlyGuard</maintainer>
<license>MIT</license>
<buildtool_depend>ament_cmake</buildtool_depend>
<buildtool_depend>rosidl_default_generators</buildtool_depend>
<depend>std_msgs</depend>
<exec_depend>rosidl_default_runtime</exec_depend>
<member_of_group>rosidl_interface_packages</member_of_group>
<export>
<build_type>ament_cmake</build_type>
</export>
</package>

77
tests/test_pipeline.py
View file

@ -4,7 +4,7 @@
выпрямление скоса каналов, геометрия плоскости пути, связность с учётом
глубины, кодирование грибовидного тела и вставка синтетического предмета.
pytest ros2_ws/src/flyguard/test
pytest tests
"""
from __future__ import annotations
@ -209,6 +209,81 @@ def test_feature_count_mismatch_is_explicit():
mb.encode(np.zeros((1, mb.n_pn + 1), np.float32))
# --------------------------------------------------------------------------- мост в ROS
class _FakeField:
def __init__(self, name, offset, datatype, count=1):
self.name, self.offset, self.datatype, self.count = name, offset, datatype, count
class _FakeStamp:
def __init__(self, sec, nanosec):
self.sec, self.nanosec = sec, nanosec
class _FakeHeader:
def __init__(self, stamp, frame_id):
self.stamp, self.frame_id = stamp, frame_id
class _FakeMsg:
"""Достаточная имитация sensor_msgs/PointCloud2 для проверки моста."""
def __init__(self, pts, fields, point_step):
self.header = _FakeHeader(_FakeStamp(1788354623, 110040384), "hesai_lidar")
self.height, self.width = 1, pts.shape[0]
self.point_step = point_step
self.is_dense = False
self.fields = fields
self.data = pts.tobytes()
def test_ros_bridge_matches_offline_parser():
"""Путь через rclpy обязан давать то же, что и офлайн-разбор CDR."""
from flyguard import ros_conv
fields_spec = [("x", 0, 7, 1), ("y", 4, 7, 1), ("z", 8, 7, 1),
("intensity", 12, 7, 1), ("ring", 16, 4, 1), ("timestamp", 18, 8, 1)]
dt = point_dtype(fields_spec, 26)
rng = np.random.default_rng(11)
pts = np.zeros(256, dt)
pts["x"] = rng.normal(size=256).astype(np.float32)
pts["y"] = rng.normal(size=256).astype(np.float32)
pts["z"] = rng.normal(size=256).astype(np.float32)
pts["intensity"] = rng.uniform(0, 255, 256).astype(np.float32)
pts["ring"] = np.arange(256, dtype=np.uint16) % 128
msg = _FakeMsg(pts, [_FakeField(*f) for f in fields_spec], 26)
got = ros_conv.from_ros(msg)
assert got.n_points == 256
assert got.frame_id == "hesai_lidar"
assert got.stamp == pytest.approx(1788354623.110040384, abs=1e-6)
for name in ("x", "y", "z", "intensity", "ring"):
assert np.array_equal(got.points[name], pts[name])
def test_ros_bridge_sorts_fields_by_offset():
"""Порядок полей в сообщении не гарантирован — мост обязан его нормализовать."""
from flyguard import ros_conv
fields_spec = [("x", 0, 7, 1), ("y", 4, 7, 1), ("z", 8, 7, 1), ("intensity", 12, 7, 1)]
dt = point_dtype(fields_spec, 16)
pts = np.zeros(8, dt)
pts["z"] = np.arange(8, dtype=np.float32)
shuffled = [_FakeField(*f) for f in reversed(fields_spec)]
got = ros_conv.from_ros(_FakeMsg(pts, shuffled, 16))
assert np.array_equal(got.points["z"], pts["z"])
def test_ros_bridge_reports_missing_fields():
from flyguard import ros_conv
msg = _FakeMsg(np.zeros(4, np.dtype([("x", "f4")])), [_FakeField("x", 0, 7, 1)], 4)
ok, why = ros_conv.has_required_fields(msg)
assert not ok and "intensity" in why
# --------------------------------------------------------- фигура и фон, привыкание
def _cand(**kw):

6
tools/_bootstrap.py
View file

@ -8,9 +8,9 @@ from pathlib import Path
ROOT = Path(__file__).resolve().parents[1]
PKG = ROOT
# Записи лидара в репозиторий не кладутся (десятки гигабайт). По умолчанию
# ищем их рядом: сначала внутри выгрузки, потом на уровень выше — так работает
# и у того, кто держит выгрузку внутри основного проекта, и у того, кто
# распаковал её отдельно. Переопределяется переменной FLYGUARD_DATA.
# ищем их рядом: сначала внутри репозитория, потом на уровень выше — так
# работает и у того, кто держит клон внутри основного проекта, и у того, кто
# распаковал его отдельно. Переопределяется переменной FLYGUARD_DATA.
_env = os.environ.get("FLYGUARD_DATA")
if _env:
DATA = Path(_env)

177
tools/build_brain_atlas.py Normal file
View file

@ -0,0 +1,177 @@
"""Атлас нейронов FlyWire: соматические координаты + привязка к стадиям FlyGuard.
Берёт публичные выгрузки коннектома FAFB v783 (Codex) и сводит их в один
компактный файл: положение каждого нейрона во фронтальной проекции и номер
стадии конвейера, которой он соответствует. Дальше вид мозга рисуется как
облако из 139 тысяч точек, подсвеченное живой активностью — без какой-либо
симуляции: активность берётся из наших же стадий, а коннектом даёт только
анатомию и принадлежность клеток.
Привязка не выдумана: FlyGuard с самого начала собран из конкретных типов
клеток, и все они есть в выгрузке поимённо — LC11 (127 нейронов), LPLC2 (210),
HS (6), VS (16), T4 (6243), T5 (6002), клетки Кеньона (5177), MBON (96),
APL (2), гигантское волокно DNp01 (2).
Входные файлы (скачиваются сами, около 9 МБ):
coordinates.csv.gz root_id → положение сомы
classification.csv.gz super_class / class / sub_class / сторона
neurons.csv.gz нейропиль (group)
consolidated_cell_types.csv.gz имя типа клетки
python tools/build_brain_atlas.py
Данные FlyWire распространяются под CC-BY 4.0 (Dorkenwald et al., Nature 2024;
Schlegel et al., Nature 2024).
"""
from __future__ import annotations
import argparse
import csv
import gzip
import re
import urllib.request
from pathlib import Path
import numpy as np
import _bootstrap as B # noqa: F401
BASE = "https://storage.googleapis.com/flywire-data/codex/data/fafb/783"
FILES = ("coordinates", "classification", "neurons", "consolidated_cell_types")
# Порядок важен: стадии проверяются сверху вниз, первая подошедшая выигрывает.
# Именованный тип клетки сильнее нейропиля, нейропиль сильнее общего класса.
STAGES = (
"retina", # 0 R1–R8, омматидиальная решётка
"lamina", # 1 L1/L2, ON/OFF и центр-окружение
"medulla", # 2 T4/T5, элементарные детекторы движения
"lptc", # 3 HS/VS, широкопольный поток → собственная скорость
"looming", # 4 LPLC2, надвигание
"lobula", # 5 LC11, мелкий объект
"mushroom", # 6 KC / APL / MBON, новизна
"central", # 7 PB/FB/EB/NO, накопление улик
"descending", # 8 нисходящие, решение
"other", # 9 остальной мозг — контекст, рисуется тускло
)
OTHER = len(STAGES) - 1
CX_NEUROPILS = {"PB", "FB", "EB", "NO", "AB"}
def _download(dest: Path) -> None:
dest.mkdir(parents=True, exist_ok=True)
for name in FILES:
p = dest / f"{name}.csv.gz"
if p.exists() and p.stat().st_size > 1000:
continue
print(f" качаю {name}.csv.gz …", flush=True)
urllib.request.urlretrieve(f"{BASE}/{name}.csv.gz", p)
def _read(dest: Path, name: str):
with gzip.open(dest / f"{name}.csv.gz", "rt", encoding="utf-8") as f:
yield from csv.DictReader(f)
def classify(cell_type: str, group: str, sub_class: str, super_class: str) -> int:
"""Номер стадии FlyGuard для одного нейрона."""
t = cell_type or ""
if t in ("DNp01", "DNp02", "DNp11") or super_class == "descending":
return STAGES.index("descending")
if t == "LC11":
return STAGES.index("lobula")
if t == "LPLC2":
return STAGES.index("looming")
if re.fullmatch(r"HS[ENS]|VS\d+", t):
return STAGES.index("lptc")
if re.fullmatch(r"T[45][a-d]", t):
return STAGES.index("medulla")
if t in ("L1", "L2") or sub_class == "lamina_monopolar":
return STAGES.index("lamina")
if sub_class == "photo_receptor":
return STAGES.index("retina")
if t.startswith(("KC", "MBON")) or t == "APL":
return STAGES.index("mushroom")
parts = set(group.split(".")) if group else set()
if any(p.startswith("MB_") for p in parts):
return STAGES.index("mushroom")
if parts & CX_NEUROPILS:
return STAGES.index("central")
if "LA" in parts:
return STAGES.index("lamina")
# Остальная оптическая доля: медулла отвечает за движение, лобула — за форму.
if "LOP" in parts:
return STAGES.index("lptc")
if "LO" in parts:
return STAGES.index("lobula")
if "ME" in parts:
return STAGES.index("medulla")
return OTHER
def main() -> None:
ap = argparse.ArgumentParser(description=__doc__)
ap.add_argument("--src", default=str(B.DATA / "flywire"))
ap.add_argument("--out", default=str(B.ROOT / "ros2_ws" / "src" / "flyguard" /
"flyguard" / "data" / "brain_atlas.npz"))
ap.add_argument("--width", type=int, default=1180)
ap.add_argument("--height", type=int, default=620)
ap.add_argument("--margin", type=int, default=18)
args = ap.parse_args()
src = Path(args.src)
_download(src)
pos: dict[str, tuple[int, int, int]] = {}
for r in _read(src, "coordinates"):
rid = r["root_id"]
if rid in pos:
continue # берём первую точку на нейрон
x, y, z = (int(v) for v in r["position"].strip("[]").split())
pos[rid] = (x, y, z)
group = {r["root_id"]: r["group"] for r in _read(src, "neurons")}
ctype = {r["root_id"]: r["primary_type"]
for r in _read(src, "consolidated_cell_types")}
rid_list, stage_list, side_list = [], [], []
for r in _read(src, "classification"):
rid = r["root_id"]
if rid not in pos:
continue
rid_list.append(rid)
stage_list.append(classify(ctype.get(rid, ""), group.get(rid, ""),
r["sub_class"], r["super_class"]))
side_list.append({"left": 0, "right": 1}.get(r["side"], 2))
xyz = np.array([pos[r] for r in rid_list], np.float64)
stage = np.array(stage_list, np.int8)
side = np.array(side_list, np.int8)
# Фронтальная проекция: x — влево-вправо, y — вверх-вниз, z — вглубь.
# Масштаб общий по обеим осям, иначе мозг растянется.
W, H, m = args.width, args.height, args.margin
lo, hi = xyz[:, :2].min(0), xyz[:, :2].max(0)
k = min((W - 2 * m) / (hi[0] - lo[0]), (H - 2 * m) / (hi[1] - lo[1]))
px = np.rint((xyz[:, 0] - lo[0]) * k + (W - (hi[0] - lo[0]) * k) / 2)
py = np.rint((xyz[:, 1] - lo[1]) * k + (H - (hi[1] - lo[1]) * k) / 2)
z = xyz[:, 2]
depth = np.rint(255 * (z - z.min()) / max(np.ptp(z), 1.0))
np.savez_compressed(
args.out,
px=px.astype(np.int16), py=py.astype(np.int16),
stage=stage, side=side, depth=depth.astype(np.uint8),
stages=np.array(STAGES), shape=np.array([H, W], np.int32))
print(f"\nнейронов в атласе: {len(rid_list)} холст {W}×{H}")
for i, name in enumerate(STAGES):
n = int((stage == i).sum())
print(f" {i} {name:<11s} {n:6d}")
print(f"\nсохранено: {args.out} "
f"({Path(args.out).stat().st_size / 1024:.0f} КБ)")
if __name__ == "__main__":
main()

124
tools/compare_centreline.py Normal file
View file

@ -0,0 +1,124 @@
"""Сравнение способов оценить осевую линию пути: свод тоннеля против полотна.
Свод виден далеко, но в кривой уходит из поля зрения; полотно видно только
вблизи, зато прямо задаёт положение пути. Скрипт печатает и рисует оба профиля,
чтобы выбрать источник и границы применимости.
python tools/compare_centreline.py --bag data/for_hackathon/roundT_doubleT
"""
from __future__ import annotations
import argparse
import matplotlib
matplotlib.use("Agg")
import matplotlib.pyplot as plt # noqa: E402
import numpy as np # noqa: E402
import _bootstrap as B # noqa: F401,E402
from flyguard.bag import Bag # noqa: E402
from flyguard.geometry import TrackFrame, fit_rail_plane # noqa: E402
from flyguard.retina import ScanLayout # noqa: E402
def slices(d_lo=8.0, d_hi=200.0, n=30):
e = np.geomspace(d_lo, d_hi, n + 1)
return list(zip(e[:-1], e[1:]))
def bore_centre(tf):
out = []
for lo, hi in slices():
m = tf.valid & (tf.d >= lo) & (tf.d < hi) & (tf.h > 0.6) & (tf.h < 3.2)
n = int(m.sum())
if n < 60:
continue
u = tf.u[m]
a, b = np.percentile(u, (3, 97))
if b - a < 1.5:
continue
out.append((0.5 * (lo + hi), 0.5 * (a + b), n))
return np.array(out) if out else np.zeros((0, 3))
def bed_centre(tf, track=0.0):
"""Центр полотна: полоса вокруг уровня рельсов, узкая по ширине."""
out = []
ref = track
for lo, hi in slices(8.0, 140.0, 26):
m = (tf.valid & (tf.d >= lo) & (tf.d < hi)
& (tf.h > -0.20) & (tf.h < 0.45) & (np.abs(tf.u - ref) < 2.6))
n = int(m.sum())
if n < 40:
continue
u = tf.u[m]
a, b = np.percentile(u, (5, 95))
if not (0.8 < b - a < 5.0):
continue
c = 0.5 * (a + b)
out.append((0.5 * (lo + hi), c, n))
ref = c # следующий срез ищем рядом с найденным
return np.array(out) if out else np.zeros((0, 3))
def fit_arc(d, u, w):
"""u ≈ c1·d + c2·d² (без свободного члена: под поездом ось проходит через 0)."""
A = np.stack([d, d * d], axis=1) * w[:, None]
c, *_ = np.linalg.lstsq(A, u * w, rcond=None)
return c
def main() -> None:
ap = argparse.ArgumentParser(description=__doc__)
ap.add_argument("--bag", required=True)
ap.add_argument("--frames", type=int, default=8)
ap.add_argument("--stride", type=int, default=20)
args = ap.parse_args()
bag = Bag(args.bag)
layout = ScanLayout.calibrate([pc for _, pc in bag.frames(start=2, stop=14)])
cols = layout.column_slice(40.0)
lay = layout.sub(cols)
fig, axes = plt.subplots(1, 2, figsize=(13, 5), dpi=110)
plane = None
radii_b, radii_d = [], []
for _, pc in bag.frames(stop=args.frames * args.stride, stride=args.stride):
img = layout.project(pc).crop(cols)
plane = fit_rail_plane(img, lay, prev=plane)
tf = TrackFrame(img, lay, plane)
bo = bore_centre(tf)
be = bed_centre(tf)
dd = np.linspace(0, 200, 100)
if bo.shape[0] >= 4:
u0 = bo[0, 1]
c = fit_arc(bo[:, 0], bo[:, 1] - u0, np.sqrt(bo[:, 2]))
axes[0].plot(bo[:, 0], bo[:, 1] - u0, ".", ms=4)
axes[0].plot(dd, c[0] * dd + c[1] * dd ** 2, lw=1, alpha=0.6)
radii_b.append(1 / (2 * c[1]) if c[1] else np.inf)
if be.shape[0] >= 4:
c = fit_arc(be[:, 0], be[:, 1], np.sqrt(be[:, 2]))
axes[1].plot(be[:, 0], be[:, 1], ".", ms=4)
axes[1].plot(dd, c[0] * dd + c[1] * dd ** 2, lw=1, alpha=0.6)
radii_d.append(1 / (2 * c[1]) if c[1] else np.inf)
for ax, t, r in ((axes[0], "центр свода тоннеля", radii_b),
(axes[1], "центр полотна пути", radii_d)):
ax.axhline(1.7, color="r", ls="--", lw=0.8); ax.axhline(-1.7, color="r", ls="--", lw=0.8)
med = np.median(r) if r else np.nan
spread = np.std(r) if len(r) > 1 else np.nan
ax.set_title(f"{t}\nR медиана {med:.0f} м, разброс {spread:.0f} м, оценок {len(r)}")
ax.set_xlabel("вперёд, м"); ax.set_ylabel("смещение оси, м")
ax.set_ylim(-14, 14); ax.set_xlim(0, 200)
print(f"{t}: R = " + ", ".join(f"{x:.0f}" for x in r))
B.FIGURES.mkdir(parents=True, exist_ok=True)
out = B.FIGURES / f"centreline_{bag.path.name}.png"
fig.suptitle(bag.path.name)
fig.tight_layout(); fig.savefig(out)
print("сохранено:", out)
if __name__ == "__main__":
main()

55
tools/find_obstacle.py Normal file
View file

@ -0,0 +1,55 @@
"""Scan a bag for points inside the train gauge ahead of the sensor.
Sensor frame: forward = -Y, lateral = X, up = Z.
"""
from __future__ import annotations
import sys
import numpy as np
sys.path.insert(0, r"C:\Games\Study\AI_Lidar\tools")
from probe_bag import frames # noqa: E402
def main(db, n=None, half_width=1.5, z_lo=-0.2, z_hi=2.2, d_min=5.0, d_max=250.0):
print("frame ts floor_z n_gauge nearest clusters(d[m] x[m] z[m] npts)")
for k, (ts, m) in enumerate(frames(db, limit=n)):
p = m["points"]
x = p["x"].astype(np.float32)
y = p["y"].astype(np.float32)
z = p["z"].astype(np.float32)
v = ~((x == 0) & (y == 0) & (z == 0))
d = -y # forward distance
# estimate floor/rail level from near-field points under the sensor
near = v & (d > 4) & (d < 25) & (np.abs(x) < 2.0)
floor = np.percentile(z[near], 2) if near.sum() > 100 else np.nan
sel = v & (d > d_min) & (d < d_max) & (np.abs(x) < half_width) \
& (z > floor + z_lo) & (z < floor + z_hi)
idx = np.flatnonzero(sel)
info = ""
if idx.size:
ds = d[idx]
order = np.argsort(ds)
ds = ds[order]
idx = idx[order]
# crude 1-D clustering along the corridor
splits = np.flatnonzero(np.diff(ds) > 1.0) + 1
parts = np.split(np.arange(ds.size), splits)
chunks = []
for pr in parts:
if pr.size < 8:
continue
ii = idx[pr]
chunks.append((float(ds[pr].min()), float(np.median(x[ii])),
float(np.median(z[ii]) - floor), int(pr.size)))
chunks.sort(key=lambda c: -c[3])
info = " ".join(f"[{c[0]:6.1f} {c[1]:+5.2f} {c[2]:+5.2f} {c[3]:5d}]" for c in chunks[:4])
print(f"{k:5d} {ts/1e9:.2f} {floor:8.2f} {idx.size:8d} "
f"{(d[idx].min() if idx.size else float('nan')):8.1f} {info}")
if __name__ == "__main__":
main(sys.argv[1], int(sys.argv[2]) if len(sys.argv) > 2 else None)

90
tools/probe_detect.py Normal file
View file

@ -0,0 +1,90 @@
"""Прогон геометрической части конвейера: сколько кандидатов и где.
Это опорный эксперимент: на `doubleT_obstacle` обязан стабильно находиться
реальный объект на ~55 м, на пустых бэгах — считается поток ложных кандидатов.
python tools/probe_detect.py --bag data/for_hackathon/doubleT_obstacle --frames 40 --verbose
python tools/probe_detect.py --all --frames 60
"""
from __future__ import annotations
import argparse
import time
import numpy as np
import _bootstrap as B # noqa: F401
from flyguard import lamina
from flyguard.bag import Bag, find_bags
from flyguard.geometry import TrackFrame, fit_corridor, fit_rail_plane
from flyguard.lobula import find_candidates
from flyguard.retina import ScanLayout
def run(bag_path, n_frames: int, stride: int, fov: float, verbose: bool,
**kw) -> dict:
bag = Bag(bag_path)
layout = ScanLayout.calibrate([pc for _, pc in bag.frames(start=2, stop=14)])
cols = layout.column_slice(fov)
lay = layout.sub(cols)
plane = corridor = None
per_frame, times = [], []
hits = []
for k, (_, pc) in enumerate(bag.frames(stop=n_frames * stride, stride=stride)):
t0 = time.perf_counter()
img = layout.project(pc).crop(cols)
plane = fit_rail_plane(img, lay, prev=plane)
tf = TrackFrame(img, lay, plane)
corridor = fit_corridor(tf, prev=corridor)
lam = lamina.process(tf.r, tf.valid)
cands = find_candidates(tf, lam, corridor, **kw)
times.append(time.perf_counter() - t0)
per_frame.append(len(cands))
hits.extend(cands)
if verbose:
head = " ".join(
f"[{c.d:6.1f}м u{c.u:+5.2f} h{c.h:4.2f} {c.width:4.2f}×{c.height:4.2f}м "
f"n={c.n_rays:4d} gap={c.gap:6.1f} def={c.floor_deficit:6.1f}]"
for c in cands[:4])
print(f" кадр {k*stride:4d}: {len(cands):3d} канд. {head}")
n = np.array(per_frame)
ds = np.array([c.d for c in hits]) if hits else np.zeros(0)
res = dict(name=bag.path.name, frames=len(n),
mean=float(n.mean()) if n.size else 0.0,
median=float(np.median(n)) if n.size else 0.0,
p95=float(np.percentile(n, 95)) if n.size else 0.0,
zero=float(np.mean(n == 0)) if n.size else 0.0,
total=int(n.sum()),
t_ms=float(np.median(times) * 1e3),
far=int((ds > 60).sum()))
print(f"{res['name']:40s} кадров {res['frames']:4d} | кандидатов/кадр "
f"среднее {res['mean']:6.2f} медиана {res['median']:4.0f} p95 {res['p95']:5.0f} | "
f"пустых кадров {res['zero']:5.1%} | дальше 60 м {res['far']:5d} | "
f"{res['t_ms']:6.1f} мс/кадр")
return res
def main() -> None:
ap = argparse.ArgumentParser(description=__doc__)
ap.add_argument("--bag")
ap.add_argument("--all", action="store_true")
ap.add_argument("--frames", type=int, default=50)
ap.add_argument("--stride", type=int, default=2)
ap.add_argument("--fov", type=float, default=25.0)
ap.add_argument("--verbose", action="store_true")
ap.add_argument("--h-lo", type=float, default=0.28)
ap.add_argument("--min-rays", type=int, default=4)
ap.add_argument("--half-width", type=float, default=1.7)
args = ap.parse_args()
kw = dict(h_lo=args.h_lo, min_rays=args.min_rays, half_width=args.half_width)
bags = find_bags(B.DATA / "for_hackathon") if args.all else [args.bag]
for b in bags:
run(b, args.frames, args.stride, args.fov, args.verbose, **kw)
if __name__ == "__main__":
main()

86
tools/render_brain.py Normal file
View file

@ -0,0 +1,86 @@
"""Отрисовка схемы мозга мухи по кадрам бэга — проверка и материал для видео.
python tools/render_brain.py --bag data/for_hackathon/doubleT_obstacle --frames 8
python tools/render_brain.py --bag ... --video docs/figures/brain.mp4 --limit 200
"""
from __future__ import annotations
import argparse
import numpy as np
import _bootstrap as B # noqa: F401
from flyguard.bag import Bag
from flyguard.brain_view import BrainView
from flyguard.mushroom_body import MushroomBody
from flyguard.pipeline import FlyGuard, Params
def main() -> None:
ap = argparse.ArgumentParser(description=__doc__)
ap.add_argument("--bag", required=True)
ap.add_argument("--memory")
ap.add_argument("--limit", type=int, default=120)
ap.add_argument("--frames", type=int, default=6, help="сколько кадров сохранить в PNG")
ap.add_argument("--video", default=None)
ap.add_argument("--style", default="hybrid",
choices=("scheme", "cloud", "hybrid"),
help="схема | облако нейронов | панели + облако")
args = ap.parse_args()
if args.style == "scheme":
view = BrainView()
elif args.style == "cloud":
from flyguard.brain_atlas import NeuronCloud
view = NeuronCloud()
else:
from flyguard.brain_hybrid import BrainHybrid
view = BrainHybrid()
if not view.enabled:
raise SystemExit("нужен OpenCV, а для облака — ещё атлас нейронов "
"(python tools/build_brain_atlas.py)")
import cv2
memory = MushroomBody.load(args.memory) if args.memory else None
fg = FlyGuard(Params(enable_looming=True), memory=memory)
bag = Bag(args.bag)
B.FIGURES.mkdir(parents=True, exist_ok=True)
writer = None
saved = 0
processed = 0
# кадры калибровки решётки не доходят до отрисовки, поэтому шаг считается
# от реально обрабатываемых, иначе при малом --limit не сохранится ничего
usable = max(args.limit - Params().calib_frames, 1)
every = max(1, usable // max(args.frames, 1))
for k, (_, pc) in enumerate(bag.frames(stop=args.limit)):
res = fg.process(pc, keep_debug=True)
if res is None:
continue
processed += 1
img = view.render(res)
if img is None:
continue
if args.video:
if writer is None:
fourcc = cv2.VideoWriter_fourcc(*"mp4v")
writer = cv2.VideoWriter(args.video, fourcc, 10.0,
(img.shape[1], img.shape[0]))
writer.write(img)
if processed % every == 0 and saved < args.frames:
out = B.FIGURES / f"brain_{args.style}_{bag.path.name}_{k:04d}.png"
cv2.imwrite(str(out), img)
saved += 1
print(f" кадр {k:4d}: {out.name} "
f"{'ПРЕПЯТСТВИЕ %.0f м' % res.decision.distance if res.decision.detected else 'путь свободен'}"
f" {res.total_ms:.1f} мс")
if writer is not None:
writer.release()
print("видео:", args.video)
print(f"обработано кадров: {processed}, сохранено картинок: {saved}")
if __name__ == "__main__":
main()

103
tools/rviz_layout.py Normal file
View file

@ -0,0 +1,103 @@
"""Раскладка панелей RViz: строка «QMainWindow State» для конфига без Qt.
Где и какой ширины стоят панели, RViz хранит в конфиге сериализацией Qt —
шестнадцатеричной строкой `QMainWindow::saveState()`. Руками её не набрать, а
Qt с привязками для Python в образе нет (и из WSL нет выхода к apt). Формат
при этом простой и стабильный, он собран здесь по исходникам Qt 5.15
(`QDockAreaLayout::saveState`, `QDockAreaLayoutInfo::saveState`): маркер
версии, маркер доков, по каждой зоне — размер и список панелей с именем,
флагами, положением и размером. Панель находится по имени объекта, а у RViz
это имя панели или дисплея («Displays», «Мозг мухи»).
python tools/rviz_layout.py
печатает строки для обоих конфигов: `config/flyguard.rviz` (панель Displays
слева) и `config/flyguard_brain.rviz` (плюс схема мозга справа во всю высоту).
"""
from __future__ import annotations
import argparse
import struct
QWIDGETSIZE_MAX = 16777215
LEFT, RIGHT = 0, 1 # QInternal::DockPosition
VERTICAL = 2 # Qt::Vertical — панели в зоне идут столбиком
CORNERS = (4, 4, 8, 8) # углы окна: верхняя, верхняя, нижняя, нижняя зона
def _i32(v: int) -> bytes:
return struct.pack(">i", v)
def _qstring(s: str) -> bytes:
b = s.encode("utf-16-be")
return struct.pack(">I", len(b)) + b
def state(areas, central: int, height: int) -> str:
"""areas — [(зона, ширина, [имя панели, ...])], central — ширина трёхмерного вида."""
out = _i32(0xff) + _i32(0) + b"\xfd" + _i32(len(areas))
for pos, width, docks in areas:
out += _i32(pos) + _i32(width) + _i32(height)
out += b"\xfc" + bytes([VERTICAL]) + _i32(len(docks))
step = height // len(docks)
for k, name in enumerate(docks):
out += b"\xfb" + _qstring(name) + b"\x01" # видима, не плавает
out += _i32(k * step) + _i32(step) + _i32(100) + _i32(QWIDGETSIZE_MAX)
out += _i32(central) + _i32(height)
for c in CORNERS:
out += _i32(c)
return out.hex()
def parse(hexs: str):
"""Обратный разбор — проверка, что строка собрана без хвостов."""
b = bytes.fromhex(hexs)
p = 0
def rd(fmt):
nonlocal p
v = struct.unpack_from(fmt, b, p)
p += struct.calcsize(fmt)
return v
assert rd(">ii") == (0xff, 0) and rd(">B") == (0xfd,)
n, = rd(">i")
areas = []
for _ in range(n):
pos, w, h = rd(">iii")
marker, _, cnt = rd(">BBi")
assert marker == 0xfc
docks = []
for _ in range(cnt):
assert rd(">B") == (0xfb,)
ln, = rd(">I")
docks.append(b[p:p + ln].decode("utf-16-be"))
p += ln
rd(">Biiii")
areas.append((pos, w, h, docks))
central = rd(">ii")
rd(">iiii")
assert p == len(b), "строка не разобрана до конца"
return areas, central
def main() -> None:
ap = argparse.ArgumentParser(description=__doc__,
formatter_class=argparse.RawDescriptionHelpFormatter)
ap.add_argument("--width", type=int, default=1880, help="ширина окна, как Width в конфиге")
ap.add_argument("--height", type=int, default=900, help="высота зоны панелей")
ap.add_argument("--left", type=int, default=300, help="ширина панели Displays")
ap.add_argument("--brain", type=int, default=820, help="ширина схемы мозга")
a = ap.parse_args()
plain = state([(LEFT, a.left, ["Displays"])], a.width - a.left - 4, a.height)
left = a.left - 20
brain = state([(LEFT, left, ["Displays"]), (RIGHT, a.brain, ["Мозг мухи"])],
a.width - left - a.brain - 8, a.height)
for name, s in (("flyguard.rviz", plain), ("flyguard_brain.rviz", brain)):
parse(s)
print(f"{name}:\n QMainWindow State: {s}")
if __name__ == "__main__":
main()

109
tools/stream_new_data.py Normal file
View file

@ -0,0 +1,109 @@
"""Потоковый прогон большого бэга прямо из tar-архива.
`new_data` — это 90 ГБ в 221 шарде, и распаковывать его целиком некуда. Скрипт
достаёт по одному шарду во временный каталог, прогоняет конвейер, забирает
дескрипторы кандидатов и удаляет шард. Состояние конвейера (поза, коридор,
собственное движение, треки) переносится между шардами, поэтому запись
обрабатывается как одна непрерывная поездка.
python tools/stream_new_data.py --shards 60 --out data/cache/new_data_candidates.npz
"""
from __future__ import annotations
import argparse
import re
import shutil
import tarfile
import tempfile
import time
from pathlib import Path
import numpy as np
import _bootstrap as B # noqa: F401
from flyguard.bag import Bag
from flyguard.mushroom_body import describe
from flyguard.pipeline import FlyGuard, Params
SHARD_RE = re.compile(r"_(\d+)\.db3$")
def shard_members(tar_path: Path):
"""Список (номер, TarInfo) шардов, отсортированный по номеру."""
out = []
with tarfile.open(tar_path, "r:") as t:
for m in t:
mm = SHARD_RE.search(m.name)
if mm and m.isfile():
out.append((int(mm.group(1)), m.name, m.size))
out.sort()
return out
def main() -> None:
ap = argparse.ArgumentParser(description=__doc__)
ap.add_argument("--tar", default=r"C:\Games\Study\AI_Lidar\датасет\new_data")
ap.add_argument("--out", default=str(B.CACHE / "new_data_candidates.npz"))
ap.add_argument("--shards", type=int, default=0, help="0 — все")
ap.add_argument("--stride", type=int, default=1, help="прореживание кадров")
ap.add_argument("--workdir", default=None)
args = ap.parse_args()
tar_path = Path(args.tar)
B.CACHE.mkdir(parents=True, exist_ok=True)
members = shard_members(tar_path)
if args.shards:
members = members[:args.shards]
print(f"шардов к обработке: {len(members)} из архива {tar_path.name}")
work = Path(args.workdir) if args.workdir else Path(tempfile.mkdtemp(prefix="flyguard_"))
work.mkdir(parents=True, exist_ok=True)
fg = FlyGuard(Params(), memory=None)
rows, dists, frames_total = [], [], 0
t_start = time.time()
with tarfile.open(tar_path, "r:") as tar:
for k, (num, name, size) in enumerate(members):
shard_dir = work / "bag"
if shard_dir.exists():
shutil.rmtree(shard_dir)
shard_dir.mkdir(parents=True)
target = shard_dir / Path(name).name
src = tar.extractfile(name)
with open(target, "wb") as dst:
shutil.copyfileobj(src, dst, length=1 << 22)
n_here = 0
try:
bag = Bag(shard_dir)
for _, pc in bag.frames(stride=args.stride):
res = fg.process(pc)
n_here += 1
if res is None:
continue
for c in res.candidates:
rows.append(describe(c))
dists.append(c.d)
except Exception as exc: # шард может быть обрезан
print(f" шард {num}: пропущен ({exc})")
frames_total += n_here
target.unlink(missing_ok=True)
if k % 5 == 0 or k == len(members) - 1:
el = time.time() - t_start
print(f" шард {k+1}/{len(members)} (#{num}) кадров {n_here:4d} | "
f"всего кадров {frames_total:6d} кандидатов {len(rows):7d} | "
f"{el/60:5.1f} мин", flush=True)
shutil.rmtree(work, ignore_errors=True)
X = np.stack(rows).astype(np.float32) if rows else np.zeros((0, 21), np.float32)
np.savez_compressed(args.out, X=X, d=np.asarray(dists, np.float32),
frames=frames_total)
print(f"готово: {frames_total} кадров, {X.shape[0]} кандидатов → {args.out}")
print(f"время: {(time.time()-t_start)/60:.1f} мин")
if __name__ == "__main__":
main()

124
tools/viz_frame.py Normal file
View file

@ -0,0 +1,124 @@
"""Диагностические картинки одного кадра: дальностный образ, срезы, вид сверху.
python tools/viz_frame.py --bag data/for_hackathon/doubleT_obstacle --frame 40
"""
from __future__ import annotations
import argparse
import matplotlib
matplotlib.use("Agg")
import matplotlib.pyplot as plt # noqa: E402
import numpy as np # noqa: E402
import _bootstrap as B # noqa: F401,E402
from flyguard.bag import Bag # noqa: E402
from flyguard.retina import ScanLayout # noqa: E402
def load(bag_path: str, frame: int, n_calib: int = 12):
bag = Bag(bag_path)
clouds = [pc for _, pc in bag.frames(start=2, stop=2 + n_calib)]
layout = ScanLayout.calibrate(clouds)
_, pc = next(bag.frames(start=frame, stop=frame + 1))
return bag, layout, layout.project(pc)
def main() -> None:
ap = argparse.ArgumentParser(description=__doc__)
ap.add_argument("--bag", required=True)
ap.add_argument("--frame", type=int, default=40)
ap.add_argument("--fov", type=float, default=30.0, help="полусектор по азимуту, °")
ap.add_argument("--out", default=None)
args = ap.parse_args()
bag, layout, img = load(args.bag, args.frame)
cols = layout.column_slice(args.fov)
lay = layout.sub(cols)
sub = img.crop(cols)
xyz = sub.xyz(lay)
x, y, z = xyz[..., 0], xyz[..., 1], xyz[..., 2]
d = -y # вперёд
v = sub.valid
# уровень пола по ближней зоне под сенсором
near = v & (d > 4) & (d < 25) & (np.abs(x) < 2.0)
z_rail = float(np.percentile(z[near], 2)) if near.sum() > 200 else np.nan
h = z - z_rail
fig = plt.figure(figsize=(19, 12), dpi=110)
fig.suptitle(f"{bag.path.name} кадр {args.frame} "
f"пол z={z_rail:.2f} м валидных {v.mean():.0%}", fontsize=13)
# 1. дальностный образ
ax = fig.add_subplot(3, 2, 1)
disp = np.where(v, sub.r_near, np.nan)
im = ax.imshow(disp, aspect="auto", cmap="turbo", vmin=0, vmax=120,
extent=[lay.az_grid_deg[0], lay.az_grid_deg[-1],
lay.el_deg[-1], lay.el_deg[0]])
ax.set_title("дальностный образ (выпрямленный), м")
ax.set_xlabel("азимут, °"); ax.set_ylabel("элевация, °")
fig.colorbar(im, ax=ax, fraction=0.03)
# 2. интенсивность
ax = fig.add_subplot(3, 2, 2)
im = ax.imshow(np.where(v, sub.inten, np.nan), aspect="auto", cmap="magma",
vmin=0, vmax=60,
extent=[lay.az_grid_deg[0], lay.az_grid_deg[-1],
lay.el_deg[-1], lay.el_deg[0]])
ax.set_title("интенсивность")
ax.set_xlabel("азимут, °"); ax.set_ylabel("элевация, °")
fig.colorbar(im, ax=ax, fraction=0.03)
# 3. вид сверху
ax = fig.add_subplot(3, 2, 3)
m = v & (d > 0) & (d < 200)
ax.scatter(d[m], x[m], s=0.05, c=h[m], cmap="viridis", vmin=-1, vmax=4, linewidths=0)
for s in (-1.5, 1.5):
ax.axhline(s, color="r", lw=0.8, ls="--")
ax.set_xlim(0, 200); ax.set_ylim(-8, 8)
ax.set_title("вид сверху (цвет — высота над головкой рельса)")
ax.set_xlabel("расстояние вперёд, м"); ax.set_ylabel("поперёк, м")
# 4. вид сбоку
ax = fig.add_subplot(3, 2, 4)
ax.scatter(d[m], h[m], s=0.05, c=np.abs(x[m]), cmap="plasma", vmin=0, vmax=4, linewidths=0)
ax.axhline(0, color="k", lw=0.8)
ax.axhline(2.1, color="r", lw=0.8, ls="--")
ax.set_xlim(0, 200); ax.set_ylim(-3, 6)
ax.set_title("вид сбоку (цвет — |поперёк|)")
ax.set_xlabel("расстояние вперёд, м"); ax.set_ylabel("высота над рельсом, м")
# 5. поперечные сечения
ax = fig.add_subplot(3, 2, 5)
for lo, hi, c in [(5, 15, "tab:blue"), (30, 45, "tab:orange"),
(60, 80, "tab:green"), (100, 130, "tab:red")]:
s = v & (d >= lo) & (d < hi)
ax.scatter(x[s], h[s], s=0.3, alpha=0.4, color=c, label=f"{lo}–{hi} м", linewidths=0)
ax.add_patch(plt.Rectangle((-1.5, 0), 3.0, 2.1, fill=False, ec="r", lw=1.5))
ax.set_xlim(-6, 6); ax.set_ylim(-2, 6); ax.set_aspect("equal")
ax.legend(fontsize=8, markerscale=8)
ax.set_title("поперечные сечения (красное — габарит 3.0 × 2.1 м)")
ax.set_xlabel("поперёк, м"); ax.set_ylabel("высота, м")
# 6. профиль дальности по лучам вдоль пути
ax = fig.add_subplot(3, 2, 6)
bins = np.arange(0, 205, 2.0)
for lo, hi, name in [(-1.0, 1.0, "|az|<1°"), (-5, 5, "|az|<5°"), (-20, 20, "|az|<20°")]:
sel = (lay.az_grid_deg >= lo) & (lay.az_grid_deg <= hi)
dd = d[:, sel][v[:, sel]]
ax.hist(dd, bins=bins, histtype="step", label=name, log=True)
ax.set_title("распределение дальностей по секторам")
ax.set_xlabel("расстояние вперёд, м"); ax.set_ylabel("число лучей")
ax.legend(fontsize=8)
out = args.out or (B.FIGURES / f"frame_{bag.path.name}_{args.frame:04d}.png")
B.FIGURES.mkdir(parents=True, exist_ok=True)
fig.tight_layout(rect=(0, 0, 1, 0.97))
fig.savefig(out)
print("сохранено:", out)
if __name__ == "__main__":
main()

100
tools/viz_object.py Normal file
View file

@ -0,0 +1,100 @@
"""Крупный план участка дальностного образа вокруг заданной точки пространства.
Нужен, чтобы понять, как реальный объект выглядит в ретинотопической решётке:
размер в лучах, контраст по дальности, окклюзионная тень за ним.
python tools/viz_object.py --bag data/for_hackathon/doubleT_obstacle \
--frame 40 --dist 55 --lat -0.8
"""
from __future__ import annotations
import argparse
import matplotlib
matplotlib.use("Agg")
import matplotlib.pyplot as plt # noqa: E402
import numpy as np # noqa: E402
import _bootstrap as B # noqa: F401,E402
from flyguard.bag import Bag # noqa: E402
from flyguard.retina import ScanLayout # noqa: E402
def main() -> None:
ap = argparse.ArgumentParser(description=__doc__)
ap.add_argument("--bag", required=True)
ap.add_argument("--frame", type=int, default=40)
ap.add_argument("--dist", type=float, required=True, help="расстояние вперёд, м")
ap.add_argument("--lat", type=float, default=0.0, help="смещение поперёк, м")
ap.add_argument("--pad-az", type=float, default=4.0, help="окно по азимуту, °")
ap.add_argument("--pad-el", type=float, default=4.0, help="окно по элевации, °")
args = ap.parse_args()
bag = Bag(args.bag)
layout = ScanLayout.calibrate([pc for _, pc in bag.frames(start=2, stop=14)])
_, pc = next(bag.frames(start=args.frame, stop=args.frame + 1))
img = layout.project(pc)
az_c = np.degrees(np.arctan2(args.lat, args.dist))
el_c = 0.0
ja = np.flatnonzero(np.abs(layout.az_grid_deg - az_c) <= args.pad_az)
ie = np.flatnonzero(np.abs(layout.el_deg - el_c) <= args.pad_el)
js, je = slice(ja[0], ja[-1] + 1), slice(ie[0], ie[-1] + 1)
r = np.where(img.valid, img.r_near, np.nan)[je, js]
it = np.where(img.valid, img.inten, np.nan)[je, js]
gap = np.where(img.valid, img.r_far - img.r_near, np.nan)[je, js]
az = layout.az_grid_deg[js]
el = layout.el_deg[je]
ext = [az[0], az[-1], el[-1], el[0]]
print(f"окно: азимут {az.min():.2f}…{az.max():.2f}° ({az.size} стлб), "
f"элевация {el.min():.2f}…{el.max():.2f}° ({el.size} колец)")
band = r[np.isfinite(r)]
print(f"дальности в окне: p5={np.percentile(band, 5):.1f} p50={np.percentile(band, 50):.1f} "
f"p95={np.percentile(band, 95):.1f} max={band.max():.1f}")
near = np.isfinite(r) & (np.abs(r - args.dist) < 5)
print(f"лучей на дистанции {args.dist}±5 м: {near.sum()}, "
f"доля окна {near.mean():.1%}")
print(f"пустых лучей в окне: {np.mean(~np.isfinite(r)):.1%}")
fig, axes = plt.subplots(2, 2, figsize=(15, 9), dpi=110)
fig.suptitle(f"{bag.path.name} кадр {args.frame}: окрестность точки "
f"({args.dist:.0f} м вперёд, {args.lat:+.1f} м поперёк)")
ax = axes[0, 0]
im = ax.imshow(r, extent=ext, aspect="auto", cmap="turbo", interpolation="nearest")
ax.set_title("дальность, м"); fig.colorbar(im, ax=ax, fraction=0.04)
ax = axes[0, 1]
lo, hi = np.nanpercentile(r, [2, 98])
im = ax.imshow(r, extent=ext, aspect="auto", cmap="turbo", vmin=lo, vmax=hi,
interpolation="nearest")
ax.set_title(f"дальность, растянутая шкала {lo:.0f}…{hi:.0f} м")
fig.colorbar(im, ax=ax, fraction=0.04)
ax = axes[1, 0]
im = ax.imshow(it, extent=ext, aspect="auto", cmap="magma", interpolation="nearest")
ax.set_title("интенсивность"); fig.colorbar(im, ax=ax, fraction=0.04)
ax = axes[1, 1]
im = ax.imshow(gap, extent=ext, aspect="auto", cmap="cividis", vmin=0, vmax=10,
interpolation="nearest")
ax.set_title("разнос эх (дальнее − ближнее), м")
fig.colorbar(im, ax=ax, fraction=0.04)
for ax in axes.ravel():
ax.set_xlabel("азимут, °"); ax.set_ylabel("элевация, °")
ax.invert_xaxis()
ax.axvline(az_c, color="w", lw=0.6, ls=":")
ax.axhline(el_c, color="w", lw=0.6, ls=":")
B.FIGURES.mkdir(parents=True, exist_ok=True)
out = B.FIGURES / f"object_{bag.path.name}_{args.frame:04d}_{int(args.dist)}m.png"
fig.tight_layout(rect=(0, 0, 1, 0.95))
fig.savefig(out)
print("сохранено:", out)
if __name__ == "__main__":
main()