Dan4ick/Lidar_Muxa
1
0
Fork 2
Code Issues Pull requests Projects Releases Packages Wiki Activity Actions

Compare commits

base: Dan4ick:main
Branches Tags
Dan4ick:main
SUDOZOVCHIK:main
..
compare: SUDOZOVCHIK:main
Branches Tags
SUDOZOVCHIK:main
Dan4ick:main

No commits in common. "main" and "main" have entirely different histories.
main ... main

110 changed files with 489 additions and 17410 deletions
Show stats Expand all files Collapse all files

12
.gitignore vendored
View file

@ -2,21 +2,11 @@ __pycache__/
*.py[cod]
.venv/
.pytest_cache/
# записи лидара — только в корне: flyguard/data и .../flyguard/data нужны пакету
/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

177
Dockerfile
View file

@ -1,109 +1,96 @@
# FlyGuard — обнаружение посторонних объектов в тоннеле метро по данным 3D-лидара.
#
# Образ самодостаточен: во время работы сеть не нужна, все зависимости ставятся
# на этапе сборки. Базовый образ — официальный ROS 2 Humble на Ubuntu 22.04,
# ровно та среда, что указана в ТЗ.
#
# docker build -t flyguard .
# 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
# ============================================================================
# FlyGuard: Bionic Obstacle Detection Core (LCT-2026, Case 05)
# Hesai Pandar128 LiDAR (128 beams, 10 Hz / 100ms)
# Universal Production Image: NVIDIA CUDA 12.x / Ada Lovelace / Graceful CPU Fallback
# ============================================================================
ARG DEBIAN_FRONTEND=noninteractive
# ----------------------------------------------------------------------------
# Stage 1: Optional lightweight CPU-only image (build with: --target cpu)
# ----------------------------------------------------------------------------
FROM python:3.11-slim-bookworm AS cpu
ENV DEBIAN_FRONTEND=noninteractive \
PYTHONUNBUFFERED=1 \
PYTHONDONTWRITEBYTECODE=1 \
PYTHONPATH="/app:/app/tools" \
FLYGUARD_DATA="/data" \
FLYGUARD_DEVICE="cpu"
# Численное ядро берётся из репозиториев 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 \
libgomp1 \
ca-certificates \
&& 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)"
RUN useradd -m -u 1000 -s /bin/bash flyguard && \
mkdir -p /app /data /app/artifacts && \
chown -R flyguard:flyguard /app /data
# Видеокарта для CUDA и для RViz (OpenGL через драйвер NVIDIA при --gpus all)
ENV NVIDIA_VISIBLE_DEVICES=all \
NVIDIA_DRIVER_CAPABILITIES=all
WORKDIR /app
COPY --chown=flyguard:flyguard requirements.txt /app/
RUN pip install --no-cache-dir --upgrade pip && \
pip install --no-cache-dir -r requirements.txt
WORKDIR /opt/flyguard
COPY --chown=flyguard:flyguard . /app/
RUN chmod +x /app/docker-entrypoint.sh
# Сначала манифесты — слой с зависимостями переживает правки кода
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
USER flyguard
VOLUME ["/data", "/app/artifacts"]
ENTRYPOINT ["/app/docker-entrypoint.sh"]
CMD ["default"]
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
# ----------------------------------------------------------------------------
# Stage 2: Universal Production Image with NVIDIA GPU acceleration & CPU fallback
# (Default target when building with: docker build -t flyguard:latest .)
# ----------------------------------------------------------------------------
FROM nvidia/cuda:12.4.1-runtime-ubuntu22.04 AS production
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 \
ENV DEBIAN_FRONTEND=noninteractive \
PYTHONUNBUFFERED=1 \
RCUTILS_COLORIZED_OUTPUT=1
PYTHONDONTWRITEBYTECODE=1 \
PYTHONPATH="/app:/app/tools" \
FLYGUARD_DATA="/data" \
FLYGUARD_DEVICE="auto" \
NVIDIA_VISIBLE_DEVICES=all \
NVIDIA_DRIVER_CAPABILITIES=compute,utility
# Транспорт под кадр в 24 МБ: умолчания Fast DDS рассчитаны на сотни килобайт
# и теряли половину кадров ещё до подписчика. Профиль поднимает сегмент
# разделяемой памяти и буферы сокетов; снимается через FLYGUARD_DDS_PROFILE=0.
ENV FASTRTPS_DEFAULT_PROFILES_FILE=/opt/flyguard/fastdds_large.xml
# Install Python 3.11, pip, and system runtime libraries (OpenMP for LightGBM/Torch)
RUN apt-get update && apt-get install -y --no-install-recommends \
software-properties-common \
ca-certificates \
libgomp1 \
curl \
&& add-apt-repository -y ppa:deadsnakes/ppa \
&& apt-get update && apt-get install -y --no-install-recommends \
python3.11 \
python3.11-distutils \
&& curl -sS https://bootstrap.pypa.io/get-pip.py | python3.11 \
&& ln -sf /usr/bin/python3.11 /usr/bin/python3 \
&& ln -sf /usr/bin/python3.11 /usr/bin/python \
&& apt-get clean && rm -rf /var/lib/apt/lists/*
ENTRYPOINT ["/entrypoint.sh"]
CMD ["ros2", "launch", "flyguard", "detect.launch.py"]
# Create non-root user for container security compliance
RUN useradd -m -u 1000 -s /bin/bash flyguard && \
mkdir -p /app /data /app/artifacts && \
chown -R flyguard:flyguard /app /data
WORKDIR /app
# Cache layer: install Python dependencies with PyTorch CUDA 12.1+ wheels
COPY --chown=flyguard:flyguard requirements-gpu.txt /app/
RUN pip install --no-cache-dir --upgrade pip setuptools wheel && \
pip install --no-cache-dir -r requirements-gpu.txt
# Copy source code and artifacts
COPY --chown=flyguard:flyguard . /app/
RUN chmod +x /app/docker-entrypoint.sh
USER flyguard
VOLUME ["/data", "/app/artifacts"]
# Container healthcheck: verifies Python runtime and graceful device detection
HEALTHCHECK --interval=30s --timeout=10s --start-period=5s --retries=3 \
CMD python3 -c "import flyguard; from flyguard.device import get_device_info; print('healthy', get_device_info())" || exit 1
ENTRYPOINT ["/app/docker-entrypoint.sh"]
CMD ["default"]

2
Dockerfile.gpu
View file

@ -1,7 +1,7 @@
# ============================================================================
# FlyGuard GPU: NVIDIA RTX / CUDA 12 Production Image (Кейс 05, ЛЦТ-2026)
# Архитектура: Ada Lovelace (RTX 4070 Ti Super 16GB) / Ampere / Turing
# Полная совместимость с Dockerfile.offline (Stage: production)
# Полная совместимость с Dockerfile (Stage: production)
# ============================================================================
FROM nvidia/cuda:12.4.1-runtime-ubuntu22.04 AS production

96
Dockerfile.offline
View file

@ -1,96 +0,0 @@
# ============================================================================
# FlyGuard: Bionic Obstacle Detection Core (LCT-2026, Case 05)
# Hesai Pandar128 LiDAR (128 beams, 10 Hz / 100ms)
# Universal Production Image: NVIDIA CUDA 12.x / Ada Lovelace / Graceful CPU Fallback
# ============================================================================
# ----------------------------------------------------------------------------
# Stage 1: Optional lightweight CPU-only image (build with: --target cpu)
# ----------------------------------------------------------------------------
FROM python:3.11-slim-bookworm AS cpu
ENV DEBIAN_FRONTEND=noninteractive \
PYTHONUNBUFFERED=1 \
PYTHONDONTWRITEBYTECODE=1 \
PYTHONPATH="/app:/app/tools" \
FLYGUARD_DATA="/data" \
FLYGUARD_DEVICE="cpu"
RUN apt-get update && apt-get install -y --no-install-recommends \
libgomp1 \
ca-certificates \
&& rm -rf /var/lib/apt/lists/*
RUN useradd -m -u 1000 -s /bin/bash flyguard && \
mkdir -p /app /data /app/artifacts && \
chown -R flyguard:flyguard /app /data
WORKDIR /app
COPY --chown=flyguard:flyguard requirements.txt /app/
RUN pip install --no-cache-dir --upgrade pip && \
pip install --no-cache-dir -r requirements.txt
COPY --chown=flyguard:flyguard . /app/
RUN chmod +x /app/docker-entrypoint.sh
USER flyguard
VOLUME ["/data", "/app/artifacts"]
ENTRYPOINT ["/app/docker-entrypoint.sh"]
CMD ["default"]
# ----------------------------------------------------------------------------
# Stage 2: Universal Production Image with NVIDIA GPU acceleration & CPU fallback
# (Default target when building with: docker build -t flyguard:latest .)
# ----------------------------------------------------------------------------
FROM nvidia/cuda:12.4.1-runtime-ubuntu22.04 AS production
ENV DEBIAN_FRONTEND=noninteractive \
PYTHONUNBUFFERED=1 \
PYTHONDONTWRITEBYTECODE=1 \
PYTHONPATH="/app:/app/tools" \
FLYGUARD_DATA="/data" \
FLYGUARD_DEVICE="auto" \
NVIDIA_VISIBLE_DEVICES=all \
NVIDIA_DRIVER_CAPABILITIES=compute,utility
# Install Python 3.11, pip, and system runtime libraries (OpenMP for LightGBM/Torch)
RUN apt-get update && apt-get install -y --no-install-recommends \
software-properties-common \
ca-certificates \
libgomp1 \
curl \
&& add-apt-repository -y ppa:deadsnakes/ppa \
&& apt-get update && apt-get install -y --no-install-recommends \
python3.11 \
python3.11-distutils \
&& curl -sS https://bootstrap.pypa.io/get-pip.py | python3.11 \
&& ln -sf /usr/bin/python3.11 /usr/bin/python3 \
&& ln -sf /usr/bin/python3.11 /usr/bin/python \
&& apt-get clean && rm -rf /var/lib/apt/lists/*
# Create non-root user for container security compliance
RUN useradd -m -u 1000 -s /bin/bash flyguard && \
mkdir -p /app /data /app/artifacts && \
chown -R flyguard:flyguard /app /data
WORKDIR /app
# Cache layer: install Python dependencies with PyTorch CUDA 12.1+ wheels
COPY --chown=flyguard:flyguard requirements-gpu.txt /app/
RUN pip install --no-cache-dir --upgrade pip setuptools wheel && \
pip install --no-cache-dir -r requirements-gpu.txt
# Copy source code and artifacts
COPY --chown=flyguard:flyguard . /app/
RUN chmod +x /app/docker-entrypoint.sh
USER flyguard
VOLUME ["/data", "/app/artifacts"]
# Container healthcheck: verifies Python runtime and graceful device detection
HEALTHCHECK --interval=30s --timeout=10s --start-period=5s --retries=3 \
CMD python3 -c "import flyguard; from flyguard.device import get_device_info; print('healthy', get_device_info())" || exit 1
ENTRYPOINT ["/app/docker-entrypoint.sh"]
CMD ["default"]

941
README.md
View file

@ -1,740 +1,241 @@
# FlyGuard — обнаружение посторонних объектов в тоннеле метро по данным 3D-лидара
# FlyGuard · ML-ядро
Решение кейса «Система обнаружения посторонних объектов для беспилотных поездов в тоннеле метро»
(хакатон «Лидеры цифровой трансформации 2026», направление «Город»; заказчик — Департамент
транспорта Москвы / ГУП «Московский метрополитен»).
Обнаружение посторонних объектов в тоннеле метро по данным 3D-лидара
Hesai Pandar128. Кейс 05, ЛЦТ-2026.
На вход — поток облаков точек Hesai Pandar128. На выход — ответ на единственный важный вопрос:
**«путь свободен» или «впереди препятствие на N метров»**.
## Быстрый старт для жюри
Образ — файлом, если на стенде нет сети ([`flyguard_image.tar`, 4.7 ГБ, Google Drive](https://drive.google.com/file/d/1fJLz3b0-mmJL1YsTFRozKp1CIKm5f0NX/view?usp=sharing);
скачать из терминала — раздел «Стенд без интернета»), или сборкой из репозитория там, где
сеть есть:
```bash
docker load -i flyguard_image.tar
```
```bash
docker build -t flyguard .
```
Узел — в одном терминале, запись — в другом:
```bash
docker run --rm -it --gpus all --network host --ipc host flyguard
```
```bash
ros2 bag play --read-ahead-queue-size 10 /путь/к/записи
```
В консоли узла (пример — запись `doubleT_obstacle`, сокращено):
```
вычисления: видеокарта NVIDIA GeForce RTX 4070 Ti SUPER (16 ГБ, CUDA 12.8) — …
калибровка решётки лучей: 1/12
ПРЕПЯТСТВИЕ: 55.7 м, уверенность 0.36, объектов 1
путь свободен
итог: принято кадров 201, обработано 190, на калибровку 11, пропущено 0; кадров с тревогой 189
```
- Без видеокарты — та же команда без `--gpus all`, результат тот же.
- Топик с облаком узел находит сам, какое бы имя у него ни было, и сам подстраивается под QoS
записи: если она публикуется «best effort», переподписывается так же.
- `--ipc host` обязателен: без него кадры с хоста до контейнера не доходят вовсе.
- Результат для программ — топики `/flyguard/obstacle`, `/flyguard/detected`,
`/flyguard/distance` (раздел «Выходные данные»); RViz и схема мозга мухи — раздел
«Демонстрация со схемой мозга мухи».
- Всё сразу — узел, проигрывание, RViz и мозг мухи — одной командой из папки репозитория:
`bash docker/demo.sh /папка/с/записями имя_записи`; флаги видеокарты и доступ к экрану
(`xhost`) скрипт подбирает сам.
Это **ядро обработки**: облако точек на входе, решение о препятствии на выходе.
Узел ROS 2, транспорт, контейнер и визуализация живут отдельно и сюда не
входят — ядро от них не зависит и проверяется без ROS вообще.
---
## В двух словах
## Чем это не является
ТЗ формулирует главный вызов так:
Не нейросетевой детектор общего назначения. Конвейер собран по схемам
зрительной системы дрозофилы, и каждая стадия — это конкретный нейропиль с
конкретной функцией, а не слой, подобранный перебором:
> «Возможно, нужно научиться хорошо описывать нормальный тоннель, а затем искать всё, что в него
> не вписывается.»
```
облако точек 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 два порога с гистерезисом → решение
```
Это дословное описание того, чем занимается мозг **Drosophila melanogaster**. Муха решает ровно
нашу задачу — на лету, без разметки, без одометрии, с мизерным бюджетом нейронов и в жёстком
реальном времени. Поэтому архитектура FlyGuard собрана из её вычислительных схем, взятых из
коннектома (FlyWire / hemibrain):
Ничего про геометрию сенсора не захардкожено: решётка лучей, высота установки,
крен и тангаж **калибруются по самим данным** на первых кадрах. В записях
встречаются две раскладки скана (3600 азимутов на 360° и 1200 на 120°) и две
высоты установки (1.31 и 1.70 м) — ядро работает с обеими без правок.
| Проблема кейса | Схема мухи | Модуль |
---
## Что нужно интеграции: один класс, один вызов
```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` — чтобы такое
число нельзя было случайно привести в отчёте.
---
## Где мы сейчас
| Метрика | Значение | Чем измерено |
|---|---|---|
| Разметки нет, почти всё — пустой тоннель | грибовидное тело: 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` |
| Реальный объект 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` |
Подробный разбор с привязкой к типам нейронов — в [docs/ALGORITHM.md](docs/ALGORITHM.md)
и [docs/CONNECTOME.md](docs/CONNECTOME.md).
Проверка всегда **leave-one-bag-out**: память обучается на всех записях, кроме
проверяемой. Иначе цифры лгут — подавлять конструкции, которые сам же и
запомнил, умеет кто угодно, а на приватном тесте будет новый участок.
---
## Архитектура
## Что честно не работает
```
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
```
Разобрано замерами, подробности — в `docs/EXPERIMENTS.md`:
Детально — [docs/ARCHITECTURE.md](docs/ARCHITECTURE.md).
* **За 200 м на этих участках не увидит никто**: прямая видимость в тоннелях
121–167 м, дальше линия взгляда упирается в стену кривой. На отдельных
перегонах и того меньше — 49–90 м.
* **Мелкие предметы на большой дальности невозможны с этим сенсором**: ведро
(0.1 м²) на 160–190 м даёт один луч, каска и бутылка — ноль.
* **За 80–90 м прирельсовая зона не наблюдается вовсе**: луч скользит по
полотну, и самая низкая видимая точка у оси пути оказывается выше головки
рельса на 0.1–0.6 м.
* Привыкание внутри проезда сделано и **отвергнуто замером** — п. 10.
* **Яркость как признак мы себе не засчитываем.** Абсолютной шкалы
интенсивности в записях нет: медиана по кандидатам обстановки 3…7 в пяти
бэгах и 23.5 в шестом. Вставка берёт яркость реальных возвратов с тех же
лучей, то есть признак намеренно обесточен, и дальность из-за этого
занижена — настоящий предмет был в 1.47 раза ярче окружения. Разбор — п. 11.1.
---
## Сборка
## Документация
```bash
docker build -t flyguard .
```
Образ основан на `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` на Google Drive](https://drive.google.com/file/d/1fJLz3b0-mmJL1YsTFRozKp1CIKm5f0NX/view?usp=sharing),
4.7 ГБ (4 732 318 208 байт). Из терминала — без браузера и без предупреждения Drive о большом
файле:
```bash
curl -L -o flyguard_image.tar "https://drive.usercontent.google.com/download?id=1fJLz3b0-mmJL1YsTFRozKp1CIKm5f0NX&export=download&confirm=t"
```
```bash
sha256sum flyguard_image.tar # 515bdbc527ccfcaf81c22aed9bc766a7e5438d8c52d4a7501438c9dfe4a3d104
```
Слои в нём уже сжаты (gzip), поэтому `gzip` поверх не нужен, а `docker load -i` принимает и
`.tar`, и `.tar.gz`. Архив самодостаточный: внутри все 74 слоя образа, хеш каждого сходится,
так что чистому движку больше ничего не нужно. Проверено: `docker load` в Docker Desktop 29.8
(Windows 11, WSL 2) — 5.5 мин, образ работает с `--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`.
Контрольная запись может прийти и в другом виде, поэтому формат проверяется инструментом:
настоящие кадры переписываются в 15 других обличий, проходят через тот же разбор CDR, что в
узле, и через конвейер, а решения сравниваются с исходными:
```bash
python tools/check_formats.py
```
| Обличье | Итог |
|---|---|
| «нет эха» — NaN вместо нулей; шапка 128 × N; поля в другом порядке и с выравниванием; яркость `uint8`; координаты `float64`; без `ring` и `timestamp`; только точки с эхом без `ring`; 5 Гц; одинаковое время кадров | решения те же |
| без поля `intensity` | было: падение на каждом кадре. Яркость теперь не обязательна, без неё — нули |
| `ring` есть, но точки разложены по кольцам или идут только с эхом | было: падение на каждом кадре. Порядок проверяется по полю `ring`, и при другом порядке решётка строится по углам |
| `ring` есть, порядок перемешан | было: дальности врали на десятки метров. Исправлено тем же |
| система координат с осью X вперёд (REP-103) | было: узел молча не видел ничего — рабочий сектор смотрел в стену. Теперь «вперёд» находится по дальним эхам: далеко лидар видит только вдоль тоннеля. Поворот признаётся, только если в переднем секторе дальних эх почти нет (меньше 2 %), а в другом — больше половины; на всех выданных записях в переднем секторе 100 % дальних эх. Узел пишет в журнал, что поворачивает кадры |
После правок все обличья дают те же решения, что исходный кадр: на `doubleT_obstacle` — в
каждом кадре и с той же дальностью.
### Демонстрация со схемой мозга мухи
```bash
xhost +local:
docker run --rm -it --gpus all --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
```
То же одной командой, с подбором флагов видеокарты под машину:
`bash docker/demo.sh /path/to/bags doubleT_obstacle`.
Топик `/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` — выключить |
| `hover_floor` | `0.5` | висящее посреди габарита: для кандидата целиком в габарите, низом выше 0.6 м, ближе 80 м, компактного вдоль пути (до 1.5 м) и не тоньше 0.2 м вероятность считывания не ниже этой. Считывание учили на стоящих предметах, и куб 0.3 м на высоте 1.2 м оно гасило: находился с 31 м, с правилом — с 63 м, ложных тревог столько же. `0` — выключить, см. [EXPERIMENTS](docs/EXPERIMENTS.md) п. 24 |
| `small_rays` | `2` | канал малых целей (LC11): кандидат из двух-трёх лучей, если он висит в пустоте посреди габарита — вся связная компонента не больше 4 лучей (`small_ctx`), низ выше 0.5 м над рельсом (`small_h`), не дальше 0.8 м от оси (`small_u`), фон за ним дальше 5 м (`small_gap`), дальность 45–100 м (`small_d_from` / `small_d_to`). Обычный порог — четыре луча, а куб 0.3 м организаторов с 57 м ложится в две ячейки образа: находился с 45 м, с каналом — с 76 м, ложных тревог столько же. `0` — выключить, см. [EXPERIMENTS](docs/EXPERIMENTS.md) п. 26 |
| `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-мегабайтный кадр. Если же сама запись публикуется «best effort», надёжный подписчик с ней по правилам DDS несовместим и не получил бы ни кадра, — узел это видит и сам переподписывается «best effort» (EXPERIMENTS п. 27) |
| `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 м, 0.3 висящий посередине — с 76 м, 0.3 у края — с 63 м, стержень 5 см с потолка — только с 13 м), оба предмета вне габарита — без тревоги; не найден 2×2, заходящий в габарит на 0.3 м. Ложных тревог за 151 с — три коротких (`tools/eval_org_synth.py`, EXPERIMENTS п. 18–19, 26) |
| Ложные тревоги, 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 |
| Использование процессора и видеокарты (узел в своём контейнере, запись — в другом, `docker/usage_test.sh`) | узел занимает **0.39–0.42 ядра** в медиане с видеокартой и **0.52–0.56** без неё; память 1.3–1.4 ГБ и 0.6–0.9 ГБ; видеокарта — **0.4–0.8 ГБ** памяти и +4–6 п.п. загрузки к фону (пик 13–18 %). Кадр 30–31 мс с видеокартой, 44 мс без неё, приняты все кадры: 201 из 201 и 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 |
| Вклад пола вероятности для висящего посреди габарита | куб 0.3 м у края на высоте 1.2 м: 31 → 63 м; висящий посередине: 43 → 45 м. Ложных треков столько же на синтетике (3), знакомой линии (5.7 на км), незнакомой (12.8) и `new_data` (11.7); полигон парно +10 / −0. Считывание учили на стоящих предметах, и предмет в воздухе оно гасило. EXPERIMENTS п. 24 |
| Вклад канала малых целей (LC11) | куб 0.3 м, висящий посередине габарита: **45 → 76 м**. С 57 м его точки ложатся в две ячейки образа, а кандидату нужно четыре луча; канал берёт пятно из двух-трёх лучей, если оно висит в пустоте: вся компонента не больше четырёх лучей, низ выше 0.5 м, не дальше 0.8 м от оси, фон за ним дальше 5 м. Ложных столько же на синтетике (3), знакомой линии (5.7 на км), незнакомой (12.8) и `new_data` (11.7); полигон парно 0 / −0, посторонних 758 = 758; время кадра то же. EXPERIMENTS п. 26 |
| Вклад сброса при скачке времени | вторая половина `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/` — один и тот же код из одного
источника. Корневой `Dockerfile` и `docker/Dockerfile` собирают один и тот же образ. В корне
лежат ещё `Dockerfile.offline`, `Dockerfile.gpu` и `docker-compose.yml` для прогона ядра по
записям без ROS и `Dockerfile.ros2` с узлом `flyguard/flyguard_ros2_node.py`.
## Лицензия
MIT.
* `docs/ALGORITHM.md` — что делает каждая стадия и почему именно так.
* `docs/EXPERIMENTS.md` — все замеры, включая отрицательные результаты.
* `docs/CONNECTOME.md` — что взято из коннектома как число, а что как идея.

37
artifacts/ablation.json
View file

@ -1,37 +0,0 @@
[
{
"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
}
]

2
artifacts/benchmark.json
View file

File diff suppressed because one or more lines are too long

BIN
artifacts/track_readout.npz
View file

Binary file not shown.

4
docker-compose.yml
View file

@ -8,7 +8,7 @@ services:
pipeline:
build:
context: .
dockerfile: Dockerfile.offline
dockerfile: Dockerfile
image: flyguard:latest
container_name: flyguard-pipeline
command: ["pipeline", "--all", "--verbose"]
@ -209,7 +209,7 @@ services:
cpu-test:
build:
context: .
dockerfile: Dockerfile.offline
dockerfile: Dockerfile
target: cpu
image: flyguard:cpu
container_name: flyguard-cpu-test

4
docker-run.sh
View file

@ -46,11 +46,11 @@ CMD="${1:-run}"
case "$CMD" in
build)
echo "==> Сборка универсального образа FlyGuard ($IMAGE_NAME)..."
docker build -t "$IMAGE_NAME" -f "$DIR/Dockerfile.offline" "$DIR"
docker build -t "$IMAGE_NAME" -f "$DIR/Dockerfile" "$DIR"
;;
build-cpu)
echo "==> Сборка облегченного CPU-образа FlyGuard (flyguard:cpu)..."
docker build --target cpu -t flyguard:cpu -f "$DIR/Dockerfile.offline" "$DIR"
docker build --target cpu -t flyguard:cpu -f "$DIR/Dockerfile" "$DIR"
;;
run|default)
shift 2>/dev/null || true

109
docker/Dockerfile
View file

@ -1,109 +0,0 @@
# 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
View file

@ -1,67 +0,0 @@
#!/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
View file

@ -1,71 +0,0 @@
#!/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
View file

@ -1,23 +0,0 @@
#!/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
View file

@ -1,14 +0,0 @@
#!/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"

48
docker/demo.sh
View file

@ -1,48 +0,0 @@
#!/usr/bin/env bash
# Показ одной командой: узел, проигрывание записи, RViz со схемой мозга мухи.
#
# bash docker/demo.sh /папка/с/записями [запись] [с какой секунды]
# bash docker/demo.sh /root/bags cloud_with_fake_obj
# bash docker/demo.sh /root/bags doubleT_obstacle
#
# Видеокарта для RViz и CUDA выбирается сама: под WSL 2 — через /dev/dxg (одного
# --gpus all там мало, RViz рисовал бы процессором), на Linux с NVIDIA — --gpus all,
# со встроенной графикой — /dev/dri. Пульт проигрывания — из второго терминала:
# docker exec -it flyguard_demo flyguard-keys /data/<запись>
# Масштаб схемы мозга: BRAIN_SCALE=1|2|3 (по умолчанию 2 — под экран 2K).
set -euo pipefail
BAGS=${1:?"укажите папку с записями: bash docker/demo.sh /папка/с/записями [запись]"}
BAG=${2:-cloud_with_fake_obj}
START=${3:-0}
IMAGE=${IMAGE:-flyguard}
if [ ! -d "$BAGS/$BAG" ]; then
echo "нет записи $BAGS/$BAG" >&2
exit 1
fi
if [ -e /dev/dxg ]; then
GPU=(--device /dev/dxg -v /usr/lib/wsl:/usr/lib/wsl:ro -e LD_LIBRARY_PATH=/usr/lib/wsl/lib)
WHERE="WSL 2, видеокарта через /dev/dxg"
elif command -v nvidia-smi >/dev/null 2>&1; then
GPU=(--gpus all)
WHERE="NVIDIA, --gpus all"
elif [ -e /dev/dri ]; then
GPU=(--device /dev/dri)
WHERE="встроенная графика, /dev/dri"
else
GPU=()
WHERE="без видеокарты: считает и рисует процессор"
fi
TTY=(-it)
[ -t 0 ] || TTY=() # без терминала (из другого скрипта) — без -it
xhost +local: >/dev/null 2>&1 || true
docker rm -f flyguard_demo >/dev/null 2>&1 || true
echo "показ: $BAG с $START с, $WHERE"
exec docker run --rm "${TTY[@]}" --name flyguard_demo --network host --ipc host "${GPU[@]}" \
-e DISPLAY="${DISPLAY:-:0}" -v /tmp/.X11-unix:/tmp/.X11-unix -v "$BAGS":/data:ro \
"$IMAGE" ros2 launch flyguard detect.launch.py bag:=/data/"$BAG" start:="$START" \
rviz:=true brain_view:=true brain_scale:="${BRAIN_SCALE:-2}"

135
docker/demo_test.sh
View file

@ -1,135 +0,0 @@
#!/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
View file

@ -1,60 +0,0 @@
#!/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
View file

@ -1,77 +0,0 @@
<?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
View file

@ -1,57 +0,0 @@
<?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
View file

@ -1,59 +0,0 @@
"""Скачать колёса 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
View file

@ -1,10 +0,0 @@
#!/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
View file

@ -1,51 +0,0 @@
"""Чем рисует 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
View file

@ -1,41 +0,0 @@
#!/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
View file

@ -1,64 +0,0 @@
#!/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
View file

@ -1,69 +0,0 @@
#!/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

71
docker/usage_test.sh
View file

@ -1,71 +0,0 @@
#!/usr/bin/env bash
# Сколько узел занимает процессора и видеокарты в реальном времени.
# Узел — в своём контейнере, запись проигрывается из другого (как у жюри), поэтому
# процессор проигрывателя в замер не попадает.
#
# bash docker/usage_test.sh /путь/к/записи --gpus all
# bash docker/usage_test.sh /путь/к/записи # без видеокарты
#
# Всё после записи — флаги видеокарты для контейнера узла. Загрузка видеокарты
# берётся из nvidia-smi всей машины: сначала 5 с без узла, потом во время
# проигрывания; в итоге — прибавка к фону.
set -u
BAG="${1:?укажите путь к записи на хосте}"
shift
GPU=("$@")
here="$(cd "$(dirname "$0")" && pwd)"
bag_dir="$(cd "$(dirname "$BAG")" && pwd)"
bag_name="$(basename "$BAG")"
tmp="$(mktemp -d)"
SMI="$(command -v nvidia-smi || true)"
[ -z "$SMI" ] && [ -x /usr/lib/wsl/lib/nvidia-smi ] && SMI=/usr/lib/wsl/lib/nvidia-smi
gpu_sample() { # $1 — файл, $2 — секунд
[ -n "$SMI" ] || return 0
timeout "$2" "$SMI" --query-gpu=utilization.gpu,memory.used \
--format=csv,noheader,nounits -lms 500 > "$1" 2>/dev/null || true
}
gpu_sample "$tmp/gpu_idle.csv" 5
docker rm -f fg_usage >/dev/null 2>&1 || true
docker run -d --name fg_usage --network host --ipc host "${GPU[@]}" flyguard >/dev/null
for _ in $(seq 1 40); do
docker logs fg_usage 2>&1 | grep -q "вычисления: видеокарта\|вычисления: процессор" && break
sleep 1
done
docker logs fg_usage 2>&1 | grep -o "вычисления: [^,(]*" | tail -1
( while docker inspect fg_usage >/dev/null 2>&1; do
docker stats --no-stream --format '{{.CPUPerc}} {{.MemUsage}}' fg_usage 2>/dev/null
done ) > "$tmp/cpu.txt" &
CS=$!
gpu_sample "$tmp/gpu_run.csv" 600 &
GS=$!
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 -E "кадр|итог|принято|обработ" | head -8
docker kill --signal=INT fg_usage >/dev/null 2>&1
sleep 3
kill "$CS" "$GS" 2>/dev/null
pkill -f "nvidia-smi --query-gpu" 2>/dev/null || true
docker logs fg_usage 2>&1 | grep -E "итог" | sed 's/.*\] //'
docker rm -f fg_usage >/dev/null 2>&1
echo
echo "=== узел: процессор (100 % = одно ядро) и память ==="
awk '{gsub("%","",$1); if ($1+0 > 1) print $1+0}' "$tmp/cpu.txt" | sort -n | awk '
{v[NR]=$1}
END {if (NR) {i = int(NR * 0.95) + 1; if (i > NR) i = NR
printf "процессор: медиана %.0f %%, 95 %% замеров ниже %.0f %% (%d замеров)\n", v[int(NR/2)+1], v[i], NR}}'
awk '{print $2}' "$tmp/cpu.txt" | sort -h | tail -1 | sed 's/^/память, наибольшая: /'
if [ -s "$tmp/gpu_run.csv" ]; then
echo "=== видеокарта (вся машина, прибавка к фону) ==="
awk -F', *' 'NR==FNR {u0+=$1; m0+=$2; n0++; next} {u[++n]=$1; if ($2>mx) mx=$2}
END {asort(u); printf "загрузка: фон %.0f %%, при работе медиана %.0f %%, наибольшая %.0f %%\n", u0/n0, u[int(n/2)+1], u[n];
printf "память видеокарты: +%.0f МБ к фону (%.0f МБ)\n", mx - m0/n0, m0/n0}' \
"$tmp/gpu_idle.csv" "$tmp/gpu_run.csv"
fi
rm -rf "$tmp"

1
docker/wheels/.gitignore vendored
View file

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

28
docker/wheels/README.md
View file

@ -1,28 +0,0 @@
# Колёса для сборки без сети
Если там, где собирается образ, нет доступа к 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 файлов.
После сборки папку можно очистить.

25
docs/ALGORITHM.md
View file

@ -194,18 +194,6 @@ LC11 у мухи — детектор мелкого объекта: возбу
проценты клеток Кеньона. Измерено: одна фигура вместо всех даёт 6.4 ложных трека на километр
против 9.4 **при одинаковой дальности обнаружения**.
**Канал малых целей.** Кандидату нужно четыре луча (за 90 м — три): вблизи обрывок из трёх
лучей — это кусок чего-то большего. Но LC11 у мухи настроен как раз на цель в один-три
омматидия на пустом фоне, и у нас такая цель есть: куб 0.3 м организаторов, висящий посреди
габарита, с 57 м даёт в образе две ячейки. Поэтому у лобулы второй вход
(`lobula._small_targets`): пятно из двух-трёх лучей становится кандидатом, если вся его
связная компонента — не больше четырёх лучей (висит в пустоте, а не отколото от стены или
свода), низ выше 0.5 м над рельсом, |u| ≤ 0.8 м (у края вдали не отличить «внутри» от
«снаружи»), фон за ним дальше 5 м, дальность 45–100 м. Вес такому наблюдению даёт ручная
формула без опоры снизу и без считывания MBON — двухлучевых кандидатов в его выборке не было;
знакомость остаётся. Куб находится с 76 м вместо 45, ложных тревог столько же на всех
проверках (EXPERIMENTS п. 26).
### 3.7. MUSHROOM BODY — память нормального тоннеля
Геометрия честно сообщает обо всём, что торчит в габарит, и вместе с препятствиями выдаёт
@ -335,16 +323,3 @@ $$q = \underbrace{\min\!\left(\frac{n_\text{лучей}}{n_\text{ожид}(d)},1
компенсируются стабилизацией плоскости, но в модель движения не входят.
6. **LPLC2 считается, но пока не влияет на решение** — канал надвигания подготовлен
и визуализируется, его вклад в улику ещё не откалиброван.
7. **Предмет у края габарита вблизи теряется, особенно у платформы.** На полигоне при смещении
0.9 м от оси крупные предметы видны в 46 % кадров на 0–15 м и в 34 % на 15–40 м (на оси —
100 % и 82 %), у платформы на 0–15 м — в 12 %. Предмет склеивается с кромкой платформы или
продольной конструкцией у стены в одну компоненту длиной 4–140 м, и она выбрасывается целиком:
разрез по контрасту ближе 55 м запрещён. Разрешить его для растянутых компонент
(`near_long`) — +15 п.п. у края на 15–40 м ценой 12.8 → 16.8 ложных трека на км на незнакомой
линии; не принято (EXPERIMENTS п. 12.7, 26.4).
8. **Предмет, который стоит в основном снаружи и заходит в габарит на десятки сантиметров,
не находится** (на синтетике организаторов — ящик 2 × 2 м, заходящий на 0.3 м). По одному
кадру это та же картина, что шкаф или колонна у края габарита; на пустых записях такая
подпись встречается в 16 местах (EXPERIMENTS п. 20.3).
9. **Прозрачное и тонкое.** Стекло для 905 нм почти прозрачно; стержень 5 см с потолка
находится только с 13 м — он тоньше шага развёртки (0.1° — 17 см на 100 м).

178
docs/ARCHITECTURE.md
View file

@ -1,178 +0,0 @@
# Архитектура решения
Цепочка, которую просит ТЗ: **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,36 +147,6 @@ 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 —

4
docs/DOCKER.md
View file

@ -34,7 +34,7 @@ FlyGuard поставляется в виде универсального пр
### Универсальный образ с GPU-ускорением (по умолчанию)
```bash
docker build -t flyguard:latest -f Dockerfile.offline .
docker build -t flyguard:latest .
```
* **База**: `nvidia/cuda:12.4.1-runtime-ubuntu22.04`
* **Python**: 3.11, PyTorch CUDA 12.1+, cuDNN, OpenMP `libgomp1`
@ -43,7 +43,7 @@ docker build -t flyguard:latest -f Dockerfile.offline .
### Облегченный CPU-образ (опционально, ~200 МБ)
```bash
docker build --target cpu -t flyguard:cpu -f Dockerfile.offline .
docker build --target cpu -t flyguard:cpu .
```
---

463
docs/EXPERIMENTS.md
View file

@ -252,86 +252,6 @@ 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 м как «очень хорошо». Разберём честно, чего
@ -419,7 +339,7 @@ ROS 2 и `ros2 bag play`. Счётчики берутся из самой нод
| Кадров с тревогой | **9.4 %** | 17.2 % |
| Посторонних тревог на кадр (полигон) | **0.055** | 0.171 |
| Реальный объект на 55 м | 98.9 % | 98.9 % |
| Задержка, медиана в контейнере | 32 мс | 33 мс |
| Задержка, медиана | 32 мс | 33 мс |
**Включено по умолчанию.** Размен здесь принципиально лучше, чем у разделения фигуры
и фона (п. 9.4): там было вчетверо больше ложных за +29 % дальности, здесь — в 1.75
@ -3461,384 +3381,3 @@ gpu_background=True)` в узле). Конвейер тем временем с
дальше ведёт те же кандидаты и решения, что процессор; упавший прогрев оставляет
процессор и причину в `gpu_error`. `docker/demo_test.sh` для замера скорости
ждёт видеокарту (`WAIT_GPU=0` — не ждать, как на стенде).
## 22. Узел `tools/flyguard_ros2_node.py` доведён до рабочего состояния
П. 16.7 фиксировал узел как нерабочий («отдаёт конвейеру массив numpy вместо
разобранного `PointCloud2`, публикует все треки, а не решение, и у трека нет
полей рамки») и оставлял его за интеграцией. В командном репозитории на 24.09
не было ни `ros2_ws/.../node.py`, ни `export_team.py`, и этот узел был единственным,
поэтому переписан начисто:
* **Вход.** [`flyguard/cdr.py`](../flyguard/cdr.py) получил `from_ros_message`:
переводит уже разобранное rclpy-сообщение в тот же `PointCloud2`, что и
офлайн-парсер, по общему `point_dtype`. Порядок точек и NaN сохраняются —
`sensor_msgs_py.read_points(skip_nans=True)` их выбрасывал, а решётка
`retina.py` раскладывает облако по позиции в массиве (азимут × кольцо), и
выброшенная точка сдвигала бы всю решётку. Тест —
`test_ros_message_keeps_every_point_in_order`.
* **Решение, не треки.** Узел зовёт [`flyguard/export.py`](../flyguard/export.py)
(`export_frame`) — тот же модуль, что уже решал разворот рамки в кривой и
зеркальный `yaw` (п. 16.7) — и публикует `Decision`, а не сырые треки
`CentralComplex`. Рамки идут и в `vision_msgs/Detection3DArray`, и в
`visualization_msgs/MarkerArray` (кубы + подписи «ID | дальность | TTC»), из
одного и того же `ExportResult`, так что они не могут разойтись.
* **Не падает от битого кадра.** `pointcloud_callback` оборачивает
`fg.process()` в `try/except`: следующий кадр придёт через 100 мс, а не
превратит узел в мёртвый процесс на записи с реальным Pandar128E3X, где
структура сообщения может отличаться от тестовых бэгов.
* **Память и считывания подключаются по умолчанию.** Раньше узел собирал
`FlyGuard` без `memory`/`readout` вообще — конвейер работал в режиме «всё
ново», без обученной памяти тоннеля. Теперь по умолчанию берутся
`artifacts/mushroom_body.npz` и `artifacts/mbon_readout.npz`, если они есть
(параметры `memory_path`/`readout_path` позволяют переопределить путь или
явно отключить значением `-`).
* **Устройство по умолчанию — `cpu`**, как и решил п. 16.7: на GPU
ускоряется только `Lamina`, а `device=auto` в узле интеграции — источник
тихих ошибок, если жюри не даёт GPU.
Не проверено на самом Pandar128E3X и не прогнано в реальном `ros2_ws` — только
модульным тестом и прогоном логики колбэка на заглушках `rclpy`/`vision_msgs`
(тестовое окружение без ROS2). Перед демо нужен прогон `ros2 bag play` живой
записи с этим узлом.
## 23. `flyguard/bag.py`: битый CDR-пакет больше не обрывает чтение бэга
`Bag.frames()` звал `parse_pointcloud2(blob)` без защиты — одно повреждённое
сообщение (реальный сенсор, не тестовый бэг) роняло исключение наружу и
обрывало генератор, а с ним — весь прогон `tools/evaluate.py` /
`tools/run_pipeline.py` на этой записи, вместе со всеми уже накопленными
кадрами. Теперь разбор каждого сообщения обёрнут в `try/except`: битый кадр
логируется (`flyguard.bag`, номер кадра, файл шарда, тип исключения) и
пропускается, `start`/`stop` по-прежнему считают по позиции сообщения в
бэге, а не по числу успешно разобранных — окно `frames(start=..., stop=...)`
не съезжает. Число пропусков доступно как `Bag.n_frames_failed` после
прохода. Тест — `test_bag_skips_a_corrupted_message_and_keeps_reading`
(набор тогда вырос до 46/46), с отдельным минимальным CDR-кодировщиком в
`tests/test_pipeline.py` для валидного `PointCloud2` без зависимости от
реальных бэгов.
Два других пункта из того же списка критики остаются открытыми — их нельзя
закрыть в этом окружении:
* **Три неопознанных ложных трека (п. 17.7)** требуют визуального разбора по
самим бэгам/RViz — самих файлов записей в `data/` в этой рабочей копии нет.
Позже разобраны покадрово — п. 18.10: не препятствия.
* **ROS2-узел на реальном стенде** — в этом окружении не установлен пакет
`rclpy` (`ModuleNotFoundError: No module named 'rclpy'`), так что живой
прогон `ros2 bag play` с узлом (п. 22) не выполним здесь; логика узла
проверена только на заглушках.
## 24. Висящее посреди габарита: пол вероятности считывания (27.09)
На синтетике организаторов маленькие кубы находились поздно: № 2 (0.3 м посередине)
лидар видит со 118 м, а первая тревога была на 43 м; № 4 (0.3 м у края) — с 85 м и
31 м. Контрольная запись, скорее всего, собрана так же, как синтетика, поэтому это
прямая потеря дальности.
### 24.1. Где теряются
Разбор по поясам дальности (кандидат, ближайший к эталону, и вес его наблюдения):
| № | пояс | кандидат | низ над рельсом | доля в габарите | MBON | вес |
|---|---|---|---|---|---|---|
| 2 | 20–30 м | 6 из 6 | 1.21 м | 1.00 | 0.68 | 0.56 |
| 2 | 30–40 м | 6 из 6 | 1.22 м | 1.00 | 0.45 | 0.30 |
| 2 | 40–50 м | 6 из 6 | 1.39 м | 1.00 | 0.53 | 0.33 |
| 2 | 50–60 м | 3 из 6 | 1.43 м | 1.00 | **0.08** | 0.02 |
| 4 | 20–30 м | 6 из 6 | 1.25 м | 1.00 | 0.24 | 0.12 |
| 4 | 40–50 м | 6 из 6 | 1.23 м | 1.00 | **0.13** | 0.05 |
| 4 | 60–70 м | 6 из 6 | 1.35 м | 1.00 | **0.03** | 0.00 |
Оба куба **висят в воздухе на высоте 1.2 м**. Кандидаты рождаются исправно — у № 4 до
70 м, целиком в габарите, новизна высокая. Гасит их обученное считывание MBON: его учили
на стоящих предметах полигона (человек, чемодан, ящик), и низ в метре над рельсом для него —
примета конструкции на стене. При `mbon_blend = 1` вес наблюдения — это и есть вероятность
модели в степени 1.5, и с 0.02–0.13 улика не копится. Та же причина, по которой висящее в
верхней секции решает ручная формула (п. 19.3), только здесь предмет в основной секции.
### 24.2. Правило и как оно сужалось
Для кандидата, который целиком в габарите (доля ≥ 0.9) и низом выше 0.6 м над рельсом,
вероятность считывания не опускается ниже пола `hover_floor`. Каждое сужение — по замеру:
| вариант | № 2 | № 4 | ложные на синтетике | знакомая линия | незнакомая | `new_data` |
|---|---|---|---|---|---|---|
| без правила | 43 м | 31 м | 3 (24 кадра) | 5.7 / км, 8.37 % | 12.8 / км, 20.67 % | 11.7 / км, 17.9 % |
| пол 0.5 на любой дальности | 48 м | 63 м | **12 (163 кадра)** | | | |
| пол 0.6 | 50 м | 65 м | **14 (203 кадра)** | | | |
| пол 0.5, только \|u\| ≤ 0.9 | 48 м | **31 м** | 8 (62 кадра) | | | |
| пол 0.5, ближе 80 м | 48 м | 63 м | 3 (24 кадра) | **6.7 / км**, 9.54 % | **16.2 / км**, 24.69 % | |
| + протяжённость вдоль пути ≤ 1.5 м | 48 м | 63 м | 3 (24 кадра) | 5.7 / км, 8.37 % | **14.4 / км**, 22.43 % | **13.2 / км**, 20.7 % |
| **+ ширина ≥ 0.2 м — принято** | **45 м** | **63 м** | **3 (24 кадра)** | **5.7 / км, 8.37 %** | **12.8 / км, 20.67 %** | **11.7 / км, 18.0 %** |
* **Дальность.** Без предела правило поднимало предметы на стенах той же высоты на 90–180 м:
вдали ось пути неточна (п. 20.1), и стена заходит в габарит. Оба куба выигрывают ближе
70 м, поэтому предел — 80 м.
* **Протяжённость вдоль пути.** Лишний трек на `doubleT_platform` — предмет у края
(u −0.93) на высоте метра, 0.2 × 0.3 м в сечении, но **6.4 м вдоль пути**: поручень или
труба у платформы. У кубов протяжённость 0.04–0.30 м на любой дальности.
* **Ширина.** Без памяти тоннеля (незнакомая линия) правило поднимало тонкие детали 11–17 см
на высоте 1.2–1.7 м — у гермозатвора, провод или кронштейн. У кубов ширина 0.23–0.34 м.
Порог подобран по незнакомой линии, а `new_data` в подборе не участвовал и проверил его
независимо: 40 ложных треков на 3.41 км — ровно столько же, сколько без правила.
Что проверено с принятыми настройками (`hover_floor 0.5`, `hover_d 80`, `hover_depth 1.5`,
`hover_width 0.2`):
| проверка | без правила | с правилом |
|---|---|---|
| синтетика организаторов | 9 из 10; № 2 с 43 м, № 4 с 31 м | 9 из 10; **№ 2 с 45 м, № 4 с 63 м** |
| ложные треки на синтетике | 3 (24 кадра) | 3 (24 кадра) |
| знакомая линия (LOO) | 5.7 на км, 8.37 % кадров | 5.7 на км, 8.37 % |
| незнакомая линия, памяти нет | 12.8 на км, 20.67 % | 12.8 на км, 20.67 % |
| вторая половина `new_data` | 40 треков, 11.7 на км, 17.9 % | 40 треков, 11.7 на км, 18.0 % |
| полигон, парно | — | 0→1: 10 (человек стоя, 0–40 м), 1→0: 0; посторонних 758 = 758 |
Правило включено по умолчанию; `hover_floor: 0` в `config/flyguard.yaml` или
`hover_floor:=0` при запуске — выключить. Настоящее лечение — переобучить считывание с
висящими предметами на полигоне; это не успевалось к сдаче и требует тех же четырёх проверок.
## 25. Устойчивость к формату облака (27.09)
Контрольную запись мы не увидим, и формат у неё может оказаться другим: синтетика
организаторов 24.09 уже пришла без `ring` и с перемешанными точками, и узел тогда не обработал
бы ни одного кадра (п. 18.1). `tools/check_formats.py` проверяет это заранее: настоящие кадры
переписываются в другом виде, проходят через тот же разбор CDR, что в узле, и через конвейер,
а решения сравниваются с исходными покадрово — есть ли тревога и на какой дальности.
Две записи: `doubleT_obstacle` (круговой скан, два эха, `ring` и `timestamp`, реальный
предмет) и первые 400–600 кадров синтетики (скан 120°, без `ring`).
| Обличье | `doubleT_obstacle` до правок | синтетика до правок | после правок |
|---|---|---|---|
| NaN вместо нулей у «нет эха» | те же решения | те же | те же |
| шапка 128 × N вместо плоской | те же | те же | те же |
| поля в другом порядке, точка 32 байта с выравниванием | те же | те же | те же |
| яркость `uint8` | те же | те же | те же |
| координаты `float64` | те же | те же | те же |
| без `ring` и `timestamp` | те же | — | те же |
| только точки с эхом, без `ring` | те же | те же | те же |
| 5 Гц (каждый второй кадр) | 98.9 % кадров | 90.7 % | так и есть: улика копится вдвое реже |
| одинаковое время у всех кадров | те же | те же | те же |
| **без `intensity`** | **сбой на каждом кадре** | 96.8 % | те же; на синтетике 97.2 % — яркость входит в признаки считывания |
| **`ring` есть, точки по кольцам** | **сбой на каждом кадре** | — | те же |
| **`ring` есть, только точки с эхом** | **сбой: не делится на 128 колец** | — | те же |
| **`ring` есть, порядок перемешан** | **дальность врёт на 39 м** | — | те же |
| **ось X вперёд (REP-103)** | **не та тревога: на 51 м дальше** | **ни одной тревоги** | те же, на обеих записях до кадра; зеркальная система координат — 97.7 % |
Что было не так и что сделано:
* **Яркость считалась обязательной.** Быстрый путь сетчатки и её вариант на видеокарте брали
поле `intensity` без проверки, а проверка ROS-моста отбрасывала облако без него. Теперь
яркость — нули, как уже было на медленном пути (`retina.py`, `gpu.py`, `ros_conv.py`).
* **Поле `ring` считалось гарантией порядка.** Калибровка резала кадр на столбцы по 128
точек, если поле `ring` есть, и не проверяла, так ли идут точки. Теперь проверяет по самому
полю (`_ring_ordered`: в каждом блоке тот же порядок колец), и при любом другом порядке
решётка строится по углам точек — тем путём, которым уже читается синтетика.
* **«Вперёд» было зашито как −Y.** Драйвер с другой системой координат поворачивал рабочий
сектор в стену. `forward_azimuth` ищет «вперёд» по дальним эхам (дальше 30 м): далеко
лидар видит только вдоль тоннеля, назад мешает сам поезд. Поворот признаётся только
явный — в переднем секторе ±30° меньше 2 % дальних эх, в одном из трёх других больше
половины, — и тогда каждый кадр поворачивается (`rotate_cloud`), а узел пишет об этом в
журнал. На всех семи выданных записях, в начале и на 150-м кадре, в переднем секторе 100 %
дальних эх: поворот не срабатывает ни разу.
Цена: в редком случае поворота — копия кадра (около 10 мс на 24 МБ); раскладка по углам,
как и прежде, дороже раскладки по порядку (п. 18.1). Тестов 63: калибровка при другом
порядке точек, облако без яркости, поворот и его обратимость.
## 26. Канал малых целей в лобуле — LC11 для цели в два луча (28.09)
Вопрос: что ещё в «мозге» ограничивает дальность на синтетике организаторов. Разбор по поясам
дальности для каждого предмета — кандидат, ближайший к эталону, его вес и улика трека
(как в п. 24.1, но до 130 м и для всех десяти) — дал четыре разных причины:
| № | предмет | виден с | первое срабатывание | что теряет его дальше |
|---:|---|---:|---:|---|
| 2 | 0.3 висит посередине | 118 м | 45 м | **лобула**: кандидат не рождается |
| 4 | 0.3 висит у края | 85 м | 63 м | считывание MBON (0.02–0.06) и край габарита |
| 8 | 2×2 сверху | 130 м | 87 м | верхняя секция кончается на 90 м |
| 9 | 2×0.2 на рельсах | 129 м | 79 м | вдали он плоский (2 см по высоте) и неотличим от порога гермозатвора |
### 26.1. Куб в две ячейки
С 57 до 118 м у куба № 2 в эталоне четыре точки на кадр, а в дальностном образе от них
остаются **две** — одно кольцо, два столбца. Вставленные организаторами точки не лежат на
элевациях колец (п. 18.2), сетчатка раскладывает их по ближайшему кольцу, и пары ложатся в
одну ячейку. Кандидату нужно четыре луча (за 90 м — три, п. 12.5), поэтому его нет вовсе, и
никакое считывание или накопление тут ни при чём. Кольцевая сетка Pandar128 у горизонта —
0.13°, куб 0.3 м на 100 м — 0.17°: настоящий прибор дал бы те же один-два луча по высоте.
### 26.2. Правило
LC11 у мухи настроен на цель в один-три омматидия на пустом фоне. В лобуле появился такой же
канал (`lobula._small_targets`): пятно из двух-трёх лучей становится кандидатом, если
* вся связная компонента, считая контекст над габаритом и по сторонам, не больше 4 лучей —
цель висит в пустоте, а не отколота от стены, свода или пола;
* низ выше 0.5 м над рельсом, и вся она в основной секции габарита;
* |u| ≤ 0.8 м;
* фон за ней дальше 5 м (контраст ламины, переведённый в метры);
* дальность 45–100 м: ближе предмет даёт четыре луча и без канала, дальше ось пути неточна.
Вес такого наблюдения (`central_complex._small_quality`) — опора с нижним краем 0.5 (как у
висящего в верхней секции), контраст, целостность и знакомость. Считывание MBON его не судит:
кандидатов из двух лучей в его выборке не было ни одного. Опора снизу не требуется — канал как
раз для висящего. Дальше решает, как и для всего, центральный комплекс: трек должен
удерживаться на своём месте кадр за кадром.
Почему |u| ≤ 0.8, а не вся ширина, — замерено: на всей ширине куб у края (№ 4) находится с 76 м
вместо 63, но куб вне габарита (№ 5) даёт ложную тревогу с 81 м. Вдали ось пути ошибается на
0.2–0.3 м (п. 20.1), и по одному кадру «внутри у края» от «снаружи у края» не отличить.
Дальше 90 м канал для № 2 не срабатывает не из-за порогов: путь уходит в кривую, и куб в
образе сливается со стеной той же дальности — компонента в 40 тысяч лучей от 4 до 150 м,
контраст ламины ноль. Первый малый кандидат — на 90 м, где за кубом впервые открывается фон.
### 26.3. Замер
Все прогоны парные, остальные настройки — умолчания.
| проверка | без канала | с каналом |
|---|---|---|
| синтетика организаторов | 9 из 10; № 2 с 45.0 м | 9 из 10; **№ 2 с 76.3 м**, остальные — те же дальности |
| ложные треки на синтетике | 3 (24 кадра) | 3 (24 кадра) |
| знакомая линия (LOO) | 5.7 на км, 8.37 % | 5.7 на км, 8.37 % |
| незнакомая линия, памяти нет | 12.8 на км, 20.67 % | 12.8 на км, 20.67 % |
| вторая половина `new_data` | 40 треков, 11.7 на км, 18.0 % | 40 треков, 11.7 на км, 18.0 % |
| полигон, парно | — | 0→1: 0, 1→0: 0; посторонних 758 = 758 |
| `doubleT_obstacle` | 99.5 % | 99.5 % |
| кадр на синтетике, процессор | 52.0 / 55.8 и 52.9 / 61.9 мс | 52.5 / 56.3 и 52.2 / 56.1 мс |
Малых кандидатов за всё время: 17 на 151 с синтетики, 2 на шести записях с поезда, 5 на 5650
кадрах `new_data`, 84 на полигоне (15 560 наблюдений, ни одно решение не изменилось).
Стоящие предметы канал не трогает по построению — у них опора снизу. Время кадра не
изменилось: канал считает два `bincount` по уже готовым меткам кластеризации и собирает
кандидатов только для крошечных компонент.
Что проверено и не взято:
* **верхняя секция до 130 м** (`top_d_max=130`): «2×2 сверху» 87 → 93 м — мало, чтобы
рисковать сводом на незнакомой линии;
* **канал на всю ширину габарита** — выше;
* **переобучить считывание MBON с висящими предметами** — настоящее лечение для № 4 (и замена
пола из п. 24), но по воронке выигрыш — 63 → около 70 м на одном предмете, а менялось бы всё
считывание; не успевается с четырьмя проверками до сдачи.
Отдельное наблюдение о синтетике: предмет № 3 («0.3 на рельсе») вдали висит в воздухе — на
81 м его точки на 3.9 м выше, чем вблизи, на 60–80 м подъём около 10 %, чего путь метро не
допускает. Похоже, генератор ставит предмет по высоте того, что видно в этом месте. Если
контрольная запись собрана так же, лежащее на пути вдали может выглядеть висящим, поэтому
висящее посреди габарита стоит видеть — и пол из п. 24, и этот канал об этом.
### 26.4. Чего не находим — сводка на 28.09
Доля кадров с тревогой, когда предмет виден (полигон, итоговая конфигурация, крупные предметы —
человек стоя, сидя, лёжа, чемодан, ящик, ведро):
| смещение от оси | 0–15 м | 15–40 м | 40–55 м | 55–90 м | 90–130 м | 130–170 м |
|---|---:|---:|---:|---:|---:|---:|
| 0.0 м | 1.00 | 0.82 | 0.60 | 0.70 | 0.30 | 0.25 |
| 0.9 м (у края) | **0.46** | **0.34** | 0.60 | 0.43 | 0.40 | 0.29 |
У края вблизи хуже всего у платформы: на 0–15 м кандидат рождается в 21 % видимых наблюдений
(без платформы — в 82 %). Разбор ящика у края на записи без платформы: на 12–45 м его лучи —
часть компоненты в 37–41 тысячу лучей длиной 4–140 м (продольная конструкция у стены), и она
выбрасывается целиком; причина та же, что в п. 12.7.
`near_long` из п. 12.7 перемерен на итоговой конфигурации (`--set near_long=8`), парно:
| проверка | без | `near_long = 8` |
|---|---|---|
| у края, 15–40 м | 0.34 | 0.49 |
| на оси, 55–90 м | 0.70 | 0.66 |
| полигон: переворотов | — | 0→1: 168, 1→0: 40 |
| полигон: посторонних тревог | 758 | 983 |
| знакомая линия | 5.7 на км, 8.37 % | 6.6 на км, 9.12 % |
| незнакомая линия | 12.8 на км, 20.67 % | 16.8 на км, 22.68 % |
| вторая половина `new_data` | 11.7 на км, 18.0 % | 14.4 на км, 21.3 % |
| синтетика организаторов | 9 из 10, 3 ложных трека | то же |
Не принято и теперь: четверть лишних ложных тревог за полосу, где при служебном торможении
поезд и так в пределах тормозного пути.
Остальное, чего мы не находим:
* **ниже пола габарита** (0.28 м, в колее дальше 30 м — 0.16 м): камень 22 см, каска, бутылка,
кабель 6 см — на полигоне 0 на любой дальности. Это граница габарита, а не слепота: по ответу
организаторов лежащее в жёлобе препятствием не считается, а их предметы на рельсах (0.3 м и
брус 0.2 м) находятся;
* **заходящее в габарит снаружи** (№ 6) — п. 20.3;
* **вдали плоское на полотне** — брус 2 × 0.2 м организаторов за 80 м (2 см по высоте в
образе) неотличим от порога гермозатвора; находится с 79 м;
* **тонкое** — стержень 5 см, с 13 м; **прозрачное** — стекло (README, «Про стекло честно»);
* **за изгибом тоннеля** — дальше 130–170 м обнаружение 0.25–0.45, за 170 м около нуля: линии
взгляда нет (п. 3);
* **лежащий человек за 90 м** — 0.04 на оси, **ведро за 90 м** — 0;
* **ближе 4 м** (`d_min`) и в тени деталей самого поезда (п. 18.11); первые 11 кадров после
запуска — калибровка решётки.
Не проверялось вовсе, данных нет: предмет, движущийся вдоль пути навстречу (тележка, человек
бегом), дым, пыль, вода на полотне.
Выключить — `small_rays: 0` в `config/flyguard.yaml` или `--set small_rays=0` у инструментов.
Тестов 66: канал рождает кандидата из двух лучей в пустоте и не рождает его у стены, у края
габарита, у полотна, с близким фоном и вне своего диапазона дальности; вес не зависит от MBON и
гасится знакомостью.
## 27. Чужая запись: QoS «best effort», подсказка про сеть, образ 28.09
Вопрос пользователя: запустит ли жюри решение на своей записи. Формат облака, имя топика,
раскладка скана и высота лидара уже подстраиваются сами (п. 18.1, 25). Оставались две вещи,
при которых узел молча не получает ни одного кадра.
### 27.1. Запись, опубликованная «best effort»
По правилам DDS надёжный подписчик с издателем «best effort» несовместим — соединения нет
вовсе. Узел подписывается надёжно (`best_effort: false`: при «best effort» потеря одного
UDP-фрагмента выбрасывает весь 24-мегабайтный кадр), а `ros2 bag play` публикует с тем QoS,
который записан в бэге. Все шесть выданных записей и синтетика организаторов записаны надёжно
(`reliability: 1` в `metadata.yaml`), но драйвер лидара обычно публикует облако как «best
effort», и запись с живого поезда может прийти такой.
Теперь сторож входа, увидев издателя «best effort» на своём топике, переподписывается так же и
пишет об этом в журнал (`node._match_publisher_qos`); при надёжном издателе ничего не меняется.
Проверка — та же запись, проигранная «best effort» (`--qos-profile-overrides-path`):
| образ | принято кадров |
|---|---:|
| прежний (26.09) | **0 из 252** |
| новый | **252 из 252**, в журнале — предупреждение о переподписке |
### 27.2. Контейнер без `--network host`
Без него обнаружение DDS до хоста не доходит, издателя узел не видит и молча ждёт — снаружи это
неотличимо от пустого тоннеля. Если издателя нет ни на одном топике 15 с, узел пишет раз в
30 с, что при проигрывании на хосте контейнер нужен с `--network host --ipc host`. Проверено:
контейнер без флага — предупреждение через 15 с.
### 27.3. Образ и архив
Образ пересобран поверх прежнего (`flyguard:2609`, 26.09: слои apt и PyTorch те же, код —
заново) своим dockerd в WSL: `c49ded640d29`. Архив `flyguard_image.tar` — 4 732 318 208 байт,
sha256 `515bdbc527ccfcaf81c22aed9bc766a7e5438d8c52d4a7501438c9dfe4a3d104`; 65 blob, хеш
каждого сходится, индекс указывает на `c49ded640d29`, 74 слоя.
В контейнере:
| проверка | результат |
|---|---|
| `doubleT_obstacle`, видеокарта (4 прогона, старый и новый образ попеременно) | 201 из 201, тревога в 189 кадрах из 190, 55.8 м — во всех |
| синтетика, процессор | 1510 из 1510; «0.3 посередине» — `ПРЕПЯТСТВИЕ: 76.3 м` в консоли узла |
| ядро уровня жюри (квота 0.61), синтетика, старый / новый образ парно | 69.9 / 80.9 и 67.8 / 77.5 мс против 68.9 / 77.7 и 69.4 / 79.0 мс; 1510 из 1510 во всех четырёх |
| ядро уровня жюри, `doubleT_obstacle`, старый / новый образ парно, по три прогона | медиана 59.5, 66.9, 72.0 мс против 63.2, 71.2, 55.6; принято 185, 201, 174 из 201 против 170, 198, 201 (26.09–27.09: 55 / 65 мс, 201 из 201) |
Абсолютное время в WSL 28.09 гуляло между одинаковыми прогонами сильнее, чем меняет код: тот
же прежний образ на видеокарте — 29.2 и 45.0 мс на `doubleT_obstacle`, новый на процессоре на
синтетике — 44.8 и 60.2 мс; кадры терялись и у прежнего образа (на `doubleT_obstacle` до 27 из
201), чего 26–27.09 не было ни разу. WSL перезапускался, кэш страниц у него почти пуст, и запись
(24 МБ на кадр, 240 МБ/с) каждый раз читается с диска, заполненного на 98 %: проигрыватель
отстаёт и отдаёт кадры пачками, а узел под квотой не успевает их принять. Поэтому о цене правок судим по парным прогонам (они совпадают) и по
прямому замеру канала малых целей — около 1 мс на кадр (п. 26.3). Цифры времени в README
оставлены прежними.

BIN
docs/figures/curve_gauge.png
View file

Binary file not shown.
Side by side

Before

Width:  |  Height:  |  Size: 124 KiB

BIN
docs/figures/doubleT_mount_shadow.png
View file

Binary file not shown.
Side by side

Before

Width:  |  Height:  |  Size: 263 KiB

20
flyguard/bag.py
View file

@ -6,7 +6,6 @@
"""
from __future__ import annotations
import logging
import re
import sqlite3
from contextlib import closing
@ -16,8 +15,6 @@ 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"
@ -47,7 +44,6 @@ class Bag:
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]:
@ -87,13 +83,9 @@ class Bag:
"""Выдать (timestamp_ns, облако) для сообщений выбранного топика.
Индексация сквозная по всему бэгу; шарды читаются по порядку, внутри
шарда — по возрастанию времени. Сообщение, которое не удаётся разобрать
(битый CDR у реального сенсора), пропускается с предупреждением в лог,
а не останавливает чтение бэга — `start`/`stop` считают по позиции
сообщения в бэге, а не по числу успешно разобранных.
шарда — по возрастанию времени.
"""
idx = 0
self.n_frames_failed = 0
for shard in self.shards:
# closing(), а не сам connection: у sqlite3 `with` управляет
# транзакцией и файл остаётся открытым — на Windows его потом
@ -119,15 +111,7 @@ class Bag:
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
yield ts, parse_pointcloud2(blob)
idx += 1
def describe(self) -> str:

364
flyguard/brain_atlas.py
View file

@ -1,364 +0,0 @@
"""Мозг как облако из 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
View file

@ -1,145 +0,0 @@
"""Гибридный вид: что видит глаз — и что от этого происходит в мозге.
Сверху четыре панели ретинотопической решётки, как их видит муха: дальностный
образ, 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
View file

@ -1,369 +0,0 @@
"""Схема мозга дрозофилы, подсвеченная живой активностью конвейера.
Каждая стадия 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

21
flyguard/cdr.py
View file

@ -4,8 +4,7 @@
* офлайн-эксперименты на машине без ROS (Windows);
* прямое чтение rosbag внутри контейнера, минуя `ros2 bag play`.
Внутри ROS-ноды сообщение приходит уже разобранным; его переводит в тот же
`PointCloud2` функция `from_ros_message`.
Внутри ROS-ноды сообщение приходит уже разобранным, и этот модуль не используется.
"""
from __future__ import annotations
@ -129,21 +128,3 @@ def parse_pointcloud2(blob: bytes | memoryview) -> PointCloud2:
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)

52
flyguard/central_complex.py
View file

@ -152,13 +152,11 @@ class CentralComplex:
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),
hover: tuple[float, ...] = (0.0, 0.6, 0.9, 1.2, 80.0, 1.5, 0.0)):
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.hover = hover # (пол, низ от, доля в габарите, |u| до, дальность до, протяжённость до, ширина от), см. _quality
self.mbon_power = mbon_power
self.mbon_blend = mbon_blend
self.nov_fade_from = nov_fade_from
@ -235,7 +233,7 @@ class CentralComplex:
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, self.hover)
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:
@ -265,7 +263,7 @@ class CentralComplex:
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, self.hover)
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
@ -311,8 +309,7 @@ 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),
hover: tuple[float, ...] = (0.0, 0.6, 0.9, 1.2, 80.0, 1.5, 0.0)) -> float:
flat: tuple[float, float, float, float] = (0.0, 0.0, 0.0, 1.0)) -> float:
"""Вес одного наблюдения: сколько улики оно добавляет.
Дальний объект даёт мало лучей не потому, что он сомнительный, а потому что
@ -331,8 +328,6 @@ def _quality(c: Candidate, use_shape: bool = True, mbon_power: float = 1.0,
# дальности. Пояс над вагоном в исправном тоннеле пуст, поэтому нижний
# край для висящего поднят до 0.5. EXPERIMENTS п. 19.
top = c.h_min >= top_from
if c.extra and c.extra.get("small"):
return _small_quality(c, nov_fade_from, nov_fade_to)
support = float(np.clip(c.n_rays / expected_rays(c.d), 0.5 if top else 0.25, 1.0))
# Контраст к фону — главная улика вблизи и недоступная вдали. На 170 м
@ -392,21 +387,6 @@ def _quality(c: Candidate, use_shape: bool = True, mbon_power: float = 1.0,
# висящее в верхней секции модель не видела ни разу (обучена на стоящих
# предметах) и гасит его — здесь решает ручная формула
if p is not None and mbon_blend > 0.0 and not top:
# Висящее посреди габарита модель тоже не видела: её учили на стоящих
# предметах, и низ в метре над рельсом для неё — примета конструкции
# на стене. Кубы 0.3 м синтетики организаторов на высоте 1.2 м она
# гасит до 0.02–0.13 уже с 40–50 м, хотя кандидат целиком в габарите.
# Пол вероятности для такого кандидата; 0 — выключено. EXPERIMENTS п. 24.
# Только ближе `d_max`: вдали ось пути неточна, и в габарит заходят
# предметы на стенах той же высоты — с правилом без предела они давали
# ложные треки на 90–180 м. И только компактное вдоль пути: труба или
# поручень у платформы на той же высоте тянется на метры (6.4 м), а у
# висящего предмета протяжённость — десятки сантиметров.
floor, h_from, cont_min, u_max, d_max, depth_max, width_min = hover
if (floor > 0.0 and c.h_min >= h_from and c.containment >= cont_min
and abs(c.u) <= u_max and c.d <= d_max and c.depth <= depth_max
and c.width >= width_min):
p = max(float(p), floor)
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)
@ -426,30 +406,6 @@ def _quality(c: Candidate, use_shape: bool = True, mbon_power: float = 1.0,
return float(np.clip(hand * novel, 0.0, 1.0))
def _small_quality(c: Candidate, nov_fade_from: float, nov_fade_to: float) -> float:
"""Вес наблюдения из канала малых целей (`lobula._small_targets`).
Лобула уже потребовала от такого кандидата всё, что отличает висящий в
пустоте предмет от конструкции: крошечная компонента, низ над полотном,
ближе к оси, далёкий фон за ним. Поэтому из ручной формулы остаются опора
(с тем же нижним краем 0.5, что у висящего сверху: предмет в два луча — не
сомнительный, а маленький), контраст и целостность, а опора снизу и
компактность — нет: первая наказала бы как раз висящее, вторая у двух
лучей всегда единица. Обученное считывание MBON такой цели не судит: его
выборка собрана при обычном пороге, и кандидатов из двух лучей в ней не
было ни одного. Знакомость остаётся — память тоннеля по-прежнему гасит
то, что на этом месте видела.
"""
support = float(np.clip(c.n_rays / expected_rays(c.d), 0.5, 1.0))
contrast = float(np.clip(c.gap / 3.0, 0.2, 1.0))
whole = float(np.clip((c.containment - 0.25) / 0.45, 0.05, 1.0))
novel = novelty_gain(c.novelty)
if nov_fade_from > 0.0 and c.d > nov_fade_from:
span = max(nov_fade_to - nov_fade_from, 1e-3)
novel = novel ** (1.0 - min((c.d - nov_fade_from) / span, 1.0))
return float(np.clip(support * contrast * whole * novel, 0.0, 1.0))
def novelty_gain(novelty: float, lo: float = 0.15, hi: float = 0.50,
floor: float = 0.05) -> float:
"""Ответ MBON → множитель улики.

BIN
flyguard/data/brain_atlas.npz
View file

Binary file not shown.

BIN
flyguard/data/brain_atlas_x2.npz
View file

Binary file not shown.

BIN
flyguard/data/brain_atlas_x3.npz
View file

Binary file not shown.

129
flyguard/data/pandar128_channels.csv
View file

@ -1,129 +0,0 @@
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

6
flyguard/gpu.py
View file

@ -206,15 +206,13 @@ class GpuStages:
if bool(t.any(dev > tol)):
return None
# яркость — не обязательное поле, без неё нули (как в retina.py)
has_i = "intensity" in pts.dtype.names
if e == 1:
r_near = r[..., 0]
r_far = r[..., 0]
it = cube("intensity")[..., 0] if has_i else t.zeros_like(r_near)
it = cube("intensity")[..., 0]
valid = good[..., 0]
else:
inten = cube("intensity") if has_i else t.zeros_like(r)
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)

100
flyguard/lobula.py
View file

@ -292,10 +292,7 @@ def find_candidates(tf: TrackFrame, lam: LaminaOutput, corridor: Corridor, *,
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,
small_rays: int = 0, small_ctx: int = 4, small_h: float = 0.5,
small_u: float = 0.8, small_gap: float = 5.0,
small_d: tuple[float, float] = (45.0, 100.0)) -> list[Candidate]:
top_min_rays: int = 3, cluster=None) -> list[Candidate]:
"""Выделить кандидатов: связные пятна лучей, попавших в габарит.
Кластеризация ведётся по **расширенной** области, а членство в габарите
@ -303,9 +300,6 @@ def find_candidates(tf: TrackFrame, lam: LaminaOutput, corridor: Corridor, *,
срезанный границей коридора, неотличим от предмета: обрезка сама создаёт
компактное пятно нужного размера. Доля лучей, оставшихся внутри габарита
(`containment`), сразу показывает, предмет это целиком или край стены.
`small_rays` > 0 включает канал малых целей (`_small_targets`): пятно из
двух-трёх лучей, висящее в пустом пространстве габарита.
"""
if masks is None:
masks = gauge_mask(tf, corridor, half_width=half_width, h_lo=h_lo,
@ -348,14 +342,6 @@ def find_candidates(tf: TrackFrame, lam: LaminaOutput, corridor: Corridor, *,
labels, n = (cluster or cluster_by_depth)(context, tf.r, col_reach=dilate_cols + 1)
if n == 0:
return []
# До разреза по фигуре: крошечную компоненту он не меняет, а метки после
# него перенумерованы.
small = (_small_targets(labels, n, inside, tf, lat, gap_img, h_hi=h_hi,
min_rays=min_rays, min_rays_far=min_rays_far,
min_rays_far_from=min_rays_far_from,
rays=small_rays, max_ctx=small_ctx, h_min=small_h,
u_max=small_u, gap_min=small_gap, d_range=small_d)
if small_rays > 0 else [])
if split_gap > 0:
# Контраст к окружению как признак фигуры. Если задан и порог движения,
# выполняются оба условия сразу: стена должна и приближаться как предмет,
@ -416,7 +402,7 @@ def find_candidates(tf: TrackFrame, lam: LaminaOutput, corridor: Corridor, *,
need = np.where(dmean >= min_rays_far_from, float(min_rays_far),
float(min_rays))
keep = np.flatnonzero(inner >= need)
if keep.size == 0 and not small:
if keep.size == 0:
return []
if keep.size > max_candidates:
keep = keep[np.argsort(inner[keep])[::-1][:max_candidates]]
@ -485,15 +471,7 @@ def find_candidates(tf: TrackFrame, lam: LaminaOutput, corridor: Corridor, *,
if cand is not None:
out.append(cand)
for sel, n_ctx in small:
cand = build(sel, n_ctx)
if cand is not None:
cand.extra["small"] = True
out.append(cand)
# `keep.size` — как было до канала малых целей: в кадре без обычных
# кандидатов проход по верхней секции не запускался и теперь не запускается
if use_top and keep.size:
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)
@ -502,78 +480,6 @@ def find_candidates(tf: TrackFrame, lam: LaminaOutput, corridor: Corridor, *,
return out
def _small_targets(labels: np.ndarray, n: int, inside: np.ndarray, tf: TrackFrame,
lat: np.ndarray, gap_img: np.ndarray, *, h_hi: float,
min_rays: int, min_rays_far: int, min_rays_far_from: float,
rays: int, max_ctx: int, h_min: float, u_max: float,
gap_min: float, d_range: tuple[float, float]) -> list:
"""Канал малых целей — то, чем LC11 и занят у мухи.
LC11 настроен на цель в один-три омматидия на пустом фоне и молчит, когда
пятно крупнее или сливается с окружением. Обычный порог кандидата (четыре
луча, за 90 м — три) такую цель не пропускает вовсе: куб 0.3 м из синтетики
организаторов висит посреди габарита, и с 57 до 118 м его четыре точки
ложатся в ДВЕ ячейки дальностного образа — одно кольцо, два столбца.
Кандидат не рождался, и первое срабатывание было на 45 м.
Два-три луча — мало, поэтому остальное требуется строго, и каждое
требование отсекает свой источник ложных тревог:
* вся связная компонента крошечная (`max_ctx` лучей, считая контекст над
габаритом и по сторонам): цель висит в пустоте, а не отколота от стены,
свода или пола;
* низ выше `h_min` над рельсом и вся — в основной секции габарита:
полотно и кромки на нём сюда не попадают, свисающее сверху ведёт свой
проход (`_hanging`);
* |u| не больше `u_max`: у края габарита вдали не различить, внутри
предмет или снаружи, — ошибка оси на 60–100 м 0.2–0.3 м (EXPERIMENTS
п. 20.1). На всей ширине куб у края находился с 76 м вместо 63, но куб
вне габарита давал ложную тревогу с 81 м;
* за ней далеко (`gap_min`, м): сзади должен быть фон, а не стена на той
же дальности;
* дальность в `d_range`: ближе предмет и так даёт четыре луча, дальше ось
пути неточна.
Вес такому наблюдению даёт `central_complex._quality` (ветка `small`), а
копится оно, как и всё, в центральном комплексе: одиночная вспышка трека
не поднимет, цель должна держаться на своём месте кадр за кадром.
Возвращает список (лучи в габарите, размер компоненты). EXPERIMENTS п. 26.
"""
flat = labels.ravel()
ins = inside.ravel()
counts = np.bincount(flat, minlength=n + 1)
inner = np.bincount(flat, weights=ins.astype(np.float64), minlength=n + 1)
counts[0] = 0
pick = (inner >= rays) & (counts <= max_ctx)
pick[0] = False
if not pick.any():
return []
idx = np.flatnonzero(pick[flat]) # лучи отобранных компонент
lab = flat[idx]
o = np.argsort(lab, kind="stable")
idx, lab = idx[o], lab[o]
d_all, h_all = tf.d.ravel(), tf.h.ravel()
u_all, g_all = lat.ravel(), gap_img.ravel()
out = []
for grp in np.split(idx, np.flatnonzero(np.diff(lab)) + 1):
sel = grp[ins[grp]]
d = float(d_all[sel].mean())
need = min_rays_far if (min_rays_far > 0 and d >= min_rays_far_from) else min_rays
if sel.size >= need:
continue # это обычный кандидат
if not (d_range[0] <= d <= d_range[1]):
continue
h = h_all[sel]
if h.min() < h_min or h.max() >= h_hi:
continue
if abs(float(np.median(u_all[sel]))) > u_max:
continue
if float(np.median(g_all[sel])) < gap_min:
continue
out.append((sel, grp.size))
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]:

765
flyguard/node.py
View file

@ -1,765 +0,0 @@
"""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),
# висящее посреди габарита: пол вероятности считывания, 0 — выключить
("hover_floor", 0.5),
# канал малых целей (LC11): кандидат из 2–3 лучей в пустоте габарита, 0 — выключить
("small_rays", 2),
("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"]),
hover_floor=float(self.par["hover_floor"]),
small_rays=int(self.par["small_rays"]),
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._started_at = time.monotonic()
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._told_forward = False # сказано ли, что облако пришлось поворачивать
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()
# Издателей не видно вовсе: запись ещё не запущена — или контейнер без
# --network host, и обнаружение DDS до хоста не доходит. Без подсказки
# второе выглядит как узел, который просто ждёт.
waited = time.monotonic() - self._started_at
if waited >= 15.0:
self.get_logger().warn(
f"облака точек нет уже {waited:.0f} с: издателя не видно ни на одном "
f"топике. Если запись проигрывается на хосте, контейнер нужен с "
f"--network host --ipc host", throttle_duration_sec=30.0)
return
self._match_publisher_qos()
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 _match_publisher_qos(self) -> None:
"""Издатель шлёт облако «best effort» — подписаться так же.
Надёжный подписчик с таким издателем по правилам DDS несовместим: кадры
не придут вовсе, и узел будет выглядеть как пустой тоннель. Наши записи
и синтетика организаторов записаны с надёжной доставкой, а
`ros2 bag play` публикует с записанным QoS, — но драйвер лидара обычно
публикует облако как «best effort», и запись с живого поезда может
прийти такой. Подписчик «best effort» принимает от издателей обоих
видов, однако крупный кадр надёжнее доставляется надёжно (конфиг,
`best_effort`), поэтому переключение — только по факту несовместимости.
"""
if self._qos.reliability != QoSReliabilityPolicy.RELIABLE:
return
loose = sorted({t for t in self._topics
for info in self.get_publishers_info_by_topic(t)
if info.qos_profile.reliability == QoSReliabilityPolicy.BEST_EFFORT})
if not loose:
return
self._qos = QoSProfile(history=self._qos.history, depth=self._qos.depth,
reliability=QoSReliabilityPolicy.BEST_EFFORT,
durability=self._qos.durability)
for sub in self.subs:
self.destroy_subscription(sub)
self.subs = [self.create_subscription(PointCloud2, t, self._on_cloud, self._qos,
raw=self.raw)
for t in self._topics]
self._pub_seen_at = None
self.get_logger().warn(
f"{', '.join(loose)}: издатель шлёт облако «best effort», а узел ждал "
f"надёжной доставки — такие не соединяются вовсе. Переподписался "
f"«best effort» (явно: best_effort:=true)")
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.calib_progress}/{self.fg.p.calib_frames}",
throttle_duration_sec=2.0)
return
if self.fg.forward_deg and not self._told_forward:
self._told_forward = True
self.get_logger().warn(
f"облако повёрнуто: «вперёд» у него на азимуте {self.fg.forward_deg:+.0f}° "
f"от −Y, как в выданных записях, — узел поворачивает кадры сам")
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()

62
flyguard/pipeline.py
View file

@ -27,7 +27,7 @@ from .lobula import Candidate, cluster_by_depth, find_candidates, gauge_mask
from .medulla import EgoMotion, EgoMotionEstimator, EmdBank, looming
from .mbon_readout import MbonReadout
from .mushroom_body import Habituation, HabituationConfig, MushroomBody, NoMemory
from .retina import RangeImage, ScanLayout, forward_azimuth, rotate_cloud
from .retina import RangeImage, ScanLayout
@dataclass
@ -153,34 +153,6 @@ class Params:
# это тихий режим для линии с обученной памятью. EXPERIMENTS п. 12.3.
nov_fade_from: float = 90.0
nov_fade_to: float = 160.0
# Висящее посреди габарита: пол вероятности считывания для кандидата,
# целиком внутри габарита (`hover_cont`), с низом выше `hover_h` над
# рельсом, |u| не больше `hover_u`, не дальше `hover_d`, компактного вдоль
# пути и не тоньше `hover_width`. Модель учили на стоящих предметах, и
# предмет в воздухе она гасит: куб 0.3 м на высоте 1.2 м у края габарита
# синтетики находился с 31 м, с правилом — с 63 м; ложных тревог на всех
# трёх проверках столько же. 0 — выключено. EXPERIMENTS п. 24.
hover_floor: float = 0.5
hover_h: float = 0.6
hover_cont: float = 0.9
hover_u: float = 1.2
hover_d: float = 80.0 # дальше — не действует: ось пути вдали неточна
hover_depth: float = 1.5 # протяжённость вдоль пути до, м: длиннее — труба, поручень
hover_width: float = 0.2 # ширина от, м: уже — провод, кронштейн, тонкая деталь
# Канал малых целей (LC11, см. lobula._small_targets): кандидат из двух-трёх
# лучей, если он висит в пустом пространстве габарита — вся связная
# компонента не больше `small_ctx` лучей, низ выше `small_h` над рельсом,
# |u| не больше `small_u`, фон за ним дальше `small_gap`, дальность от
# `small_d_from` до `small_d_to`. Куб 0.3 м посередине синтетики
# организаторов при обычном пороге в четыре луча находился с 45 м: с 57 м
# его точки ложатся в две ячейки образа. 0 — выключено. EXPERIMENTS п. 26.
small_rays: int = 2
small_ctx: int = 4
small_h: float = 0.5
small_u: float = 0.8
small_gap: float = 5.0
small_d_from: float = 45.0
small_d_to: float = 100.0
# Поправка оценки модели на распространённость предметов: за этой
# дальностью из логита вычитается типичный для полосы. 0 — не
# поправлять. Замер — EXPERIMENTS п. 12.3.
@ -326,9 +298,6 @@ class FlyGuard:
self._track_readout = track_readout
self._init_temporal()
self._calib: list[PointCloud2] = []
# Поворот облака вокруг вертикали, если у драйвера «вперёд» — не −Y
# (определяется по дальним эхам при калибровке, см. forward_azimuth)
self.forward_deg = 0.0
self.frames_seen = 0
self._last_stamp: float | None = None
self.time_jumps = 0 # сколько раз время записи скакало
@ -363,11 +332,7 @@ class FlyGuard:
top_from=(self.p.h_hi if self.p.h_top > self.p.h_hi
else float("inf")),
flat=(self.p.flat_h, self.p.flat_top, self.p.flat_d,
self.p.flat_w),
hover=(self.p.hover_floor, self.p.hover_h,
self.p.hover_cont, self.p.hover_u,
self.p.hover_d, self.p.hover_depth,
self.p.hover_width))
self.p.flat_w))
# без накопления улик трек подтверждается первым же наблюдением
self.dn = (DescendingNeurons(
warn_evidence=self.p.warn_evidence,
@ -438,11 +403,6 @@ class FlyGuard:
# ------------------------------------------------------------------ калибровка
@property
def calib_progress(self) -> int:
"""Сколько кадров уже набрано для калибровки решётки."""
return self.p.calib_frames if self.layout_full is not None else len(self._calib)
def _ensure_layout(self, pc: PointCloud2) -> bool:
"""Накопить кадры и откалибровать решётку. False — ещё не готов."""
if self.layout is not None:
@ -451,9 +411,6 @@ class FlyGuard:
self._calib.append(pc)
if len(self._calib) < self.p.calib_frames:
return False
self.forward_deg = forward_azimuth(self._calib)
if self.forward_deg:
self._calib = [rotate_cloud(c, self.forward_deg) for c in self._calib]
self.layout_full = ScanLayout.calibrate(self._calib)
self._calib.clear()
self.cols = self.layout_full.column_slice(self.p.fov_deg)
@ -510,14 +467,8 @@ class FlyGuard:
def process(self, pc: PointCloud2, *, keep_debug: bool = False) -> FrameResult | None:
"""Обработать один кадр. None, пока идёт калибровка решётки."""
t = _Timer()
rotated = bool(self.forward_deg)
if rotated:
pc = rotate_cloud(pc, self.forward_deg)
if not self._ensure_layout(pc):
return None
if self.forward_deg and not rotated:
# поворот найден на этом кадре при калибровке: сам кадр ещё не повёрнут
pc = rotate_cloud(pc, self.forward_deg)
# Скачок времени записи — перемотка, `ros2 bag play --loop`, разрыв в
# записи. Треки, одометрия и ось пути относятся к другому месту, и
# с ними предмет с прошлого круга всплыл бы там, где его нет.
@ -603,14 +554,7 @@ class FlyGuard:
half_width_top=self.p.half_width_top,
top_d_max=self.p.top_d_max,
top_detect=self.p.top_detect,
cluster=self._cluster,
small_rays=self.p.small_rays,
small_ctx=self.p.small_ctx,
small_h=self.p.small_h,
small_u=self.p.small_u,
small_gap=self.p.small_gap,
small_d=(self.p.small_d_from,
self.p.small_d_to))
cluster=self._cluster)
with t("mushroom"):
cands = self.memory.annotate(cands)

183
flyguard/player_keys.py
View file

@ -1,183 +0,0 @@
"""Пульт проигрывания записи: пауза, перемотка, шаг по кадру, скорость.
`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()

77
flyguard/retina.py
View file

@ -219,16 +219,13 @@ class ScanLayout:
if np.any(dev > r * np.float32(ORDER_TOL_DEG * DEG) + np.float32(1e-3)):
return None
# Яркость — не обязательное поле: у драйвера без неё облако всё равно
# раскладывается, а яркость считается нулевой (как на медленном пути)
has_i = "intensity" in pts.dtype.names
if e == 1:
r_near = r[..., 0]
r_far = r[..., 0]
it = cube("intensity")[..., 0] if has_i else np.zeros_like(r_near)
it = cube("intensity")[..., 0]
valid = good[..., 0]
else:
inten = cube("intensity") if has_i else np.zeros_like(r)
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]
@ -332,13 +329,10 @@ class ScanLayout:
if not clouds:
raise ValueError("нужен хотя бы один кадр для калибровки")
pc0 = clouds[0]
if not _ring_ordered(pc0):
# Без поля `ring` (синтетика организаторов) или с ним, но не
# «столбец за столбцом»: драйвер отдаёт только точки с эхом,
# раскладывает кадр по кольцам или перемешивает. Кольца — по
# гистограмме элевации, а в калибровку по порядку точек идут только
# кадры, где этот порядок цел. Нет таких — решётка строится целиком
# по углам.
if "ring" not in pc0.points.dtype.names:
# Без поля `ring` (синтетика организаторов): кольца — по гистограмме
# элевации, а в калибровку по порядку точек идут только кадры, где
# этот порядок цел. Нет таких — решётка строится целиком по углам.
el_ring = _ring_elevations(clouds)
if n_rings is None:
n_rings = el_ring.size
@ -559,65 +553,6 @@ def _ring_elevations(clouds: list[PointCloud2], max_frames: int = 6) -> np.ndarr
return np.sort(c[keep])[::-1]
def _ring_ordered(pc: PointCloud2) -> bool:
"""Идут ли точки столбец за столбцом с одним порядком колец (по полю `ring`).
Калибровка по полю `ring` раскладывает кадр на столбцы по n_rings точек и
без этой проверки молча ошибалась на любом другом порядке: драйвер, который
отдаёт только точки с эхом, падал на «не делится на 128 колец», кадр,
разложенный по кольцам, — на каждом кадре, а перемешанный давал неверные
дальности (`tools/check_formats.py`).
"""
if "ring" not in pc.points.dtype.names or pc.n_points == 0:
return False
ring = np.asarray(pc.points["ring"]).astype(np.int64)
n = int(ring.max()) + 1
if n < 2 or ring.size % n:
return False
cols = ring.reshape(-1, n)
first = cols[0]
return bool(np.array_equal(np.sort(first), np.arange(n)) and np.all(cols == first))
def forward_azimuth(clouds: list[PointCloud2], far: float = 30.0, half: float = 30.0,
min_points: int = 2000) -> float:
"""Куда у облака смотрит «вперёд», по дальним эхам: 0 — как у выданных записей.
У выданных записей вперёд −Y. Другой драйвер может повернуть систему
координат (по REP-103 вперёд +X), и тогда рабочий сектор смотрит в стену —
узел молча не видит ничего. Далеко лидар видит только вдоль тоннеля, а назад
мешает сам поезд, поэтому направление с большинством дальних эх — вперёд.
Поворот признаётся только явный: в переднем секторе дальних эх почти нет, а
в одном из трёх других — больше половины. Иначе 0, и облако не трогается.
"""
parts = []
for pc in clouds[:6]:
x, y, z, ok = _xyz64(pc)
m = ok & (np.hypot(x, y) > far)
parts.append(np.degrees(np.arctan2(x[m], -y[m])))
az = np.concatenate(parts) if parts else np.zeros(0)
if az.size < min_points:
return 0.0
share = {c: float(np.mean(np.abs(_wrap180(az - c)) <= half))
for c in (0.0, 90.0, 180.0, -90.0)}
if share[0.0] >= 0.02:
return 0.0
best = max((90.0, 180.0, -90.0), key=lambda c: share[c])
return best if share[best] >= 0.5 else 0.0
def rotate_cloud(pc: PointCloud2, az_deg: float) -> PointCloud2:
"""Повернуть облако вокруг вертикали так, чтобы азимут `az_deg` стал «вперёд»."""
pts = np.array(pc.points) # копия: буфер кадра только для чтения
f, r = -pts["y"].astype(np.float64), pts["x"].astype(np.float64)
c, s = np.cos(np.radians(az_deg)), np.sin(np.radians(az_deg))
pts["x"] = -f * s + r * c
pts["y"] = -(f * c + r * s)
return 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)
def _is_organized(pc: PointCloud2, el_ring: np.ndarray) -> bool:
"""Цел ли порядок точек: элевация каждой точки = элевация её кольца."""
n = el_ring.size

38
flyguard/ros_conv.py
View file

@ -1,38 +0,0 @@
"""Преобразование 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"} # яркость не обязательна: без неё нули
missing = need - names
if missing:
return False, f"в облаке нет полей: {', '.join(sorted(missing))}"
if msg.height * msg.width == 0:
return False, "пустое облако"
return True, ""

13
pyproject.toml
View file

@ -1,13 +0,0 @@
[tool.ruff]
target-version = "py311"
line-length = 96
extend-exclude = ["artifacts", "data", "__pycache__"]
[tool.ruff.lint]
select = ["E", "F", "W", "B", "UP", "SIM"]
ignore = [
"E501", # длину строк проверяет line-length, но многодетные формулы иногда шире
]
[tool.ruff.lint.per-file-ignores]
"tests/*" = ["E402"] # sys.path.insert перед импортом flyguard — так во всех тестах

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

@ -1,109 +0,0 @@
# Конфиг 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

138
ros2_ws/src/flyguard/config/flyguard.yaml
View file

@ -1,138 +0,0 @@
# Параметры ноды 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
# Висящее посреди габарита, ближе 80 м: пол вероятности считывания MBON.
# Модель учили на стоящих предметах, и предмет в воздухе она гасит; куб
# 0.3 м на высоте 1.2 м синтетики находится с 63 м вместо 31, ложных
# тревог столько же. 0 — выключить. EXPERIMENTS п. 24.
hover_floor: 0.5
# Канал малых целей (LC11): кандидат из двух-трёх лучей, если он висит в
# пустоте посреди габарита — вся связная компонента не больше 4 лучей, низ
# выше 0.5 м над рельсом, |u| до 0.8 м, фон за ним дальше 5 м, 45–100 м.
# Обычный порог — четыре луча, а куб 0.3 м организаторов с 57 м ложится в
# две ячейки образа: находился с 45 м, с каналом — с 76 м; ложных тревог
# столько же на всех проверках. 0 — выключить. EXPERIMENTS п. 26.
small_rays: 2
# Пол между рельсами (|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
View file

@ -1,124 +0,0 @@
# Конфиг 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
View file

@ -1,16 +0,0 @@
"""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
View file

@ -1,149 +0,0 @@
"""Чтение 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
View file

@ -1,364 +0,0 @@
"""Мозг как облако из 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
View file

@ -1,145 +0,0 @@
"""Гибридный вид: что видит глаз — и что от этого происходит в мозге.
Сверху четыре панели ретинотопической решётки, как их видит муха: дальностный
образ, 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
View file

@ -1,369 +0,0 @@
"""Схема мозга дрозофилы, подсвеченная живой активностью конвейера.
Каждая стадия 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
View file

@ -1,149 +0,0 @@
"""Разбор 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)

462
ros2_ws/src/flyguard/flyguard/central_complex.py
View file

@ -1,462 +0,0 @@
"""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),
hover: tuple[float, ...] = (0.0, 0.6, 0.9, 1.2, 80.0, 1.5, 0.0)):
self.use_shape = use_shape
self.lying_exempt = lying_exempt
self.top_from = top_from # низ выше этого — предмет в верхней секции габарита
self.flat = flat # (высота, верх, дальность, множитель), см. _quality
self.hover = hover # (пол, низ от, доля в габарите, |u| до, дальность до, протяжённость до, ширина от), см. _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, self.hover)
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, self.hover)
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),
hover: tuple[float, ...] = (0.0, 0.6, 0.9, 1.2, 80.0, 1.5, 0.0)) -> float:
"""Вес одного наблюдения: сколько улики оно добавляет.
Дальний объект даёт мало лучей не потому, что он сомнительный, а потому что
так устроена решётка, — поэтому число лучей нормируется на ожидаемое для
этой дальности. Остальные множители отделяют предмет от конструкции тоннеля:
предмет целиком помещается в габарит и компактен вдоль пути, а лоток или
стена тянутся дальше и в стороны. Новизна из грибовидного тела входит сюда
же множителем.
"""
# Висящее в верхней секции (`top_from`) — отдельная статья. Обе обученные
# части, считывание MBON и память тоннеля, собирались до того, как у
# габарита появилась верхняя секция, и таких форм не видели: их ответ там
# не значит ничего, и оба канала здесь выключены. Нормировка числа лучей
# рассчитана на предмет с человека, а свисающий кабель или стержень тонок
# по природе: у стержня 5 см опора упиралась в нижний край 0.25 на любой
# дальности. Пояс над вагоном в исправном тоннеле пуст, поэтому нижний
# край для висящего поднят до 0.5. EXPERIMENTS п. 19.
top = c.h_min >= top_from
if c.extra and c.extra.get("small"):
return _small_quality(c, nov_fade_from, nov_fade_to)
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:
# Висящее посреди габарита модель тоже не видела: её учили на стоящих
# предметах, и низ в метре над рельсом для неё — примета конструкции
# на стене. Кубы 0.3 м синтетики организаторов на высоте 1.2 м она
# гасит до 0.02–0.13 уже с 40–50 м, хотя кандидат целиком в габарите.
# Пол вероятности для такого кандидата; 0 — выключено. EXPERIMENTS п. 24.
# Только ближе `d_max`: вдали ось пути неточна, и в габарит заходят
# предметы на стенах той же высоты — с правилом без предела они давали
# ложные треки на 90–180 м. И только компактное вдоль пути: труба или
# поручень у платформы на той же высоте тянется на метры (6.4 м), а у
# висящего предмета протяжённость — десятки сантиметров.
floor, h_from, cont_min, u_max, d_max, depth_max, width_min = hover
if (floor > 0.0 and c.h_min >= h_from and c.containment >= cont_min
and abs(c.u) <= u_max and c.d <= d_max and c.depth <= depth_max
and c.width >= width_min):
p = max(float(p), floor)
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 _small_quality(c: Candidate, nov_fade_from: float, nov_fade_to: float) -> float:
"""Вес наблюдения из канала малых целей (`lobula._small_targets`).
Лобула уже потребовала от такого кандидата всё, что отличает висящий в
пустоте предмет от конструкции: крошечная компонента, низ над полотном,
ближе к оси, далёкий фон за ним. Поэтому из ручной формулы остаются опора
(с тем же нижним краем 0.5, что у висящего сверху: предмет в два луча — не
сомнительный, а маленький), контраст и целостность, а опора снизу и
компактность — нет: первая наказала бы как раз висящее, вторая у двух
лучей всегда единица. Обученное считывание MBON такой цели не судит: его
выборка собрана при обычном пороге, и кандидатов из двух лучей в ней не
было ни одного. Знакомость остаётся — память тоннеля по-прежнему гасит
то, что на этом месте видела.
"""
support = float(np.clip(c.n_rays / expected_rays(c.d), 0.5, 1.0))
contrast = float(np.clip(c.gap / 3.0, 0.2, 1.0))
whole = float(np.clip((c.containment - 0.25) / 0.45, 0.05, 1.0))
novel = novelty_gain(c.novelty)
if nov_fade_from > 0.0 and c.d > nov_fade_from:
span = max(nov_fade_to - nov_fade_from, 1e-3)
novel = novel ** (1.0 - min((c.d - nov_fade_from) / span, 1.0))
return float(np.clip(support * contrast * whole * 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
View file

Binary file not shown.

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

Binary file not shown.

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

Binary file not shown.

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

@ -1,129 +0,0 @@
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
View file

@ -1,219 +0,0 @@
"""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
View file

@ -1,150 +0,0 @@
"""Управление вычислительными устройствами (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
View file

@ -1,303 +0,0 @@
"""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
View file

@ -1,180 +0,0 @@
"""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
View file

@ -1,324 +0,0 @@
"""Система координат пути, плоскость рельсов и «ожидаемая дальность до пола».
Соответствие мухе — **жужжальца и оцеллии**. Прежде чем обрабатывать изображение,
муха стабилизирует взгляд: жужжальца дают угловые скорости, оцеллии — направление
на горизонт, и голова доворачивается так, чтобы зрительный мир не «плавал».
Здесь роль горизонта играет плоскость пути: она оценивается по самим данным
в каждом кадре, поэтому крепление сенсора не обязано быть жёстким, а качка
вагона не превращается в ложные срабатывания.
Ключевая величина дальше по конвейеру — **ожидаемая дальность до пола** для
каждого луча. Луч с отрицательной элевацией, если ему ничто не мешает, обязан
закончиться на плоскости пути на строго определённом расстоянии. Всё, что
обрывает его раньше, — предмет, стоящий на пути. Это даёт детектор, не зависящий
от абсолютного размера объекта и работающий на любой дальности.
Система координат пути (используется во всём проекте):
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))

502
ros2_ws/src/flyguard/flyguard/gpu.py
View file

@ -1,502 +0,0 @@
"""Видеокарта для плотных стадий конвейера: сетчатка, ламина, кластеризация.
Три стадии работают с целым образом 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
# яркость — не обязательное поле, без неё нули (как в retina.py)
has_i = "intensity" in pts.dtype.names
if e == 1:
r_near = r[..., 0]
r_far = r[..., 0]
it = cube("intensity")[..., 0] if has_i else t.zeros_like(r_near)
valid = good[..., 0]
else:
inten = cube("intensity") if has_i else t.zeros_like(r)
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
View file

@ -1,216 +0,0 @@
"""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)

627
ros2_ws/src/flyguard/flyguard/lobula.py
View file

@ -1,627 +0,0 @@
"""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,
small_rays: int = 0, small_ctx: int = 4, small_h: float = 0.5,
small_u: float = 0.8, small_gap: float = 5.0,
small_d: tuple[float, float] = (45.0, 100.0)) -> list[Candidate]:
"""Выделить кандидатов: связные пятна лучей, попавших в габарит.
Кластеризация ведётся по **расширенной** области, а членство в габарите
проверяется уже для готового пятна. Без этого вертикальный лоскут стены,
срезанный границей коридора, неотличим от предмета: обрезка сама создаёт
компактное пятно нужного размера. Доля лучей, оставшихся внутри габарита
(`containment`), сразу показывает, предмет это целиком или край стены.
`small_rays` > 0 включает канал малых целей (`_small_targets`): пятно из
двух-трёх лучей, висящее в пустом пространстве габарита.
"""
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 []
# До разреза по фигуре: крошечную компоненту он не меняет, а метки после
# него перенумерованы.
small = (_small_targets(labels, n, inside, tf, lat, gap_img, h_hi=h_hi,
min_rays=min_rays, min_rays_far=min_rays_far,
min_rays_far_from=min_rays_far_from,
rays=small_rays, max_ctx=small_ctx, h_min=small_h,
u_max=small_u, gap_min=small_gap, d_range=small_d)
if small_rays > 0 else [])
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 and not small:
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)
for sel, n_ctx in small:
cand = build(sel, n_ctx)
if cand is not None:
cand.extra["small"] = True
out.append(cand)
# `keep.size` — как было до канала малых целей: в кадре без обычных
# кандидатов проход по верхней секции не запускался и теперь не запускается
if use_top and keep.size:
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 _small_targets(labels: np.ndarray, n: int, inside: np.ndarray, tf: TrackFrame,
lat: np.ndarray, gap_img: np.ndarray, *, h_hi: float,
min_rays: int, min_rays_far: int, min_rays_far_from: float,
rays: int, max_ctx: int, h_min: float, u_max: float,
gap_min: float, d_range: tuple[float, float]) -> list:
"""Канал малых целей — то, чем LC11 и занят у мухи.
LC11 настроен на цель в один-три омматидия на пустом фоне и молчит, когда
пятно крупнее или сливается с окружением. Обычный порог кандидата (четыре
луча, за 90 м — три) такую цель не пропускает вовсе: куб 0.3 м из синтетики
организаторов висит посреди габарита, и с 57 до 118 м его четыре точки
ложатся в ДВЕ ячейки дальностного образа — одно кольцо, два столбца.
Кандидат не рождался, и первое срабатывание было на 45 м.
Два-три луча — мало, поэтому остальное требуется строго, и каждое
требование отсекает свой источник ложных тревог:
* вся связная компонента крошечная (`max_ctx` лучей, считая контекст над
габаритом и по сторонам): цель висит в пустоте, а не отколота от стены,
свода или пола;
* низ выше `h_min` над рельсом и вся — в основной секции габарита:
полотно и кромки на нём сюда не попадают, свисающее сверху ведёт свой
проход (`_hanging`);
* |u| не больше `u_max`: у края габарита вдали не различить, внутри
предмет или снаружи, — ошибка оси на 60–100 м 0.2–0.3 м (EXPERIMENTS
п. 20.1). На всей ширине куб у края находился с 76 м вместо 63, но куб
вне габарита давал ложную тревогу с 81 м;
* за ней далеко (`gap_min`, м): сзади должен быть фон, а не стена на той
же дальности;
* дальность в `d_range`: ближе предмет и так даёт четыре луча, дальше ось
пути неточна.
Вес такому наблюдению даёт `central_complex._quality` (ветка `small`), а
копится оно, как и всё, в центральном комплексе: одиночная вспышка трека
не поднимет, цель должна держаться на своём месте кадр за кадром.
Возвращает список (лучи в габарите, размер компоненты). EXPERIMENTS п. 26.
"""
flat = labels.ravel()
ins = inside.ravel()
counts = np.bincount(flat, minlength=n + 1)
inner = np.bincount(flat, weights=ins.astype(np.float64), minlength=n + 1)
counts[0] = 0
pick = (inner >= rays) & (counts <= max_ctx)
pick[0] = False
if not pick.any():
return []
idx = np.flatnonzero(pick[flat]) # лучи отобранных компонент
lab = flat[idx]
o = np.argsort(lab, kind="stable")
idx, lab = idx[o], lab[o]
d_all, h_all = tf.d.ravel(), tf.h.ravel()
u_all, g_all = lat.ravel(), gap_img.ravel()
out = []
for grp in np.split(idx, np.flatnonzero(np.diff(lab)) + 1):
sel = grp[ins[grp]]
d = float(d_all[sel].mean())
need = min_rays_far if (min_rays_far > 0 and d >= min_rays_far_from) else min_rays
if sel.size >= need:
continue # это обычный кандидат
if not (d_range[0] <= d <= d_range[1]):
continue
h = h_all[sel]
if h.min() < h_min or h.max() >= h_hi:
continue
if abs(float(np.median(u_all[sel]))) > u_max:
continue
if float(np.median(g_all[sel])) < gap_min:
continue
out.append((sel, grp.size))
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
View file

@ -1,390 +0,0 @@
"""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
View file

@ -1,398 +0,0 @@
"""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
View file

@ -1,524 +0,0 @@
"""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

765
ros2_ws/src/flyguard/flyguard/node.py
View file

@ -1,765 +0,0 @@
"""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),
# висящее посреди габарита: пол вероятности считывания, 0 — выключить
("hover_floor", 0.5),
# канал малых целей (LC11): кандидат из 2–3 лучей в пустоте габарита, 0 — выключить
("small_rays", 2),
("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"]),
hover_floor=float(self.par["hover_floor"]),
small_rays=int(self.par["small_rays"]),
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._started_at = time.monotonic()
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._told_forward = False # сказано ли, что облако пришлось поворачивать
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()
# Издателей не видно вовсе: запись ещё не запущена — или контейнер без
# --network host, и обнаружение DDS до хоста не доходит. Без подсказки
# второе выглядит как узел, который просто ждёт.
waited = time.monotonic() - self._started_at
if waited >= 15.0:
self.get_logger().warn(
f"облака точек нет уже {waited:.0f} с: издателя не видно ни на одном "
f"топике. Если запись проигрывается на хосте, контейнер нужен с "
f"--network host --ipc host", throttle_duration_sec=30.0)
return
self._match_publisher_qos()
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 _match_publisher_qos(self) -> None:
"""Издатель шлёт облако «best effort» — подписаться так же.
Надёжный подписчик с таким издателем по правилам DDS несовместим: кадры
не придут вовсе, и узел будет выглядеть как пустой тоннель. Наши записи
и синтетика организаторов записаны с надёжной доставкой, а
`ros2 bag play` публикует с записанным QoS, — но драйвер лидара обычно
публикует облако как «best effort», и запись с живого поезда может
прийти такой. Подписчик «best effort» принимает от издателей обоих
видов, однако крупный кадр надёжнее доставляется надёжно (конфиг,
`best_effort`), поэтому переключение — только по факту несовместимости.
"""
if self._qos.reliability != QoSReliabilityPolicy.RELIABLE:
return
loose = sorted({t for t in self._topics
for info in self.get_publishers_info_by_topic(t)
if info.qos_profile.reliability == QoSReliabilityPolicy.BEST_EFFORT})
if not loose:
return
self._qos = QoSProfile(history=self._qos.history, depth=self._qos.depth,
reliability=QoSReliabilityPolicy.BEST_EFFORT,
durability=self._qos.durability)
for sub in self.subs:
self.destroy_subscription(sub)
self.subs = [self.create_subscription(PointCloud2, t, self._on_cloud, self._qos,
raw=self.raw)
for t in self._topics]
self._pub_seen_at = None
self.get_logger().warn(
f"{', '.join(loose)}: издатель шлёт облако «best effort», а узел ждал "
f"надёжной доставки — такие не соединяются вовсе. Переподписался "
f"«best effort» (явно: best_effort:=true)")
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.calib_progress}/{self.fg.p.calib_frames}",
throttle_duration_sec=2.0)
return
if self.fg.forward_deg and not self._told_forward:
self._told_forward = True
self.get_logger().warn(
f"облако повёрнуто: «вперёд» у него на азимуте {self.fg.forward_deg:+.0f}° "
f"от −Y, как в выданных записях, — узел поворачивает кадры сам")
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()

670
ros2_ws/src/flyguard/flyguard/pipeline.py
View file

@ -1,670 +0,0 @@
"""Сборка конвейера: от облака точек до решения.
Порядок стадий повторяет путь сигнала в мозге мухи:
ретина → стабилизация → ламина → медулла/лобулярная пластинка
→ лобула → грибовидное тело → центральный комплекс → нисходящие нейроны
Конвейер держит состояние между кадрами (поза, коридор, собственное движение,
треки), поэтому один экземпляр обслуживает один поток лидарных данных.
"""
from __future__ import annotations
import threading
import time
from dataclasses import dataclass, field
import numpy as np
from . import lamina
from .cdr import PointCloud2
from .central_complex import CentralComplex
from .descending import Decision, DescendingNeurons
from .geometry import (STRAIGHT, Corridor, RailPlane, TrackFrame, fit_corridor,
fit_rail_plane)
from .fan_body import FanBody
from .lobula import Candidate, cluster_by_depth, find_candidates, gauge_mask
from .medulla import EgoMotion, EgoMotionEstimator, EmdBank, looming
from .mbon_readout import MbonReadout
from .mushroom_body import Habituation, HabituationConfig, MushroomBody, NoMemory
from .retina import RangeImage, ScanLayout, forward_azimuth, rotate_cloud
@dataclass
class Params:
"""Все настройки конвейера в одном месте."""
fov_deg: float = 30.0 # полусектор обработки по азимуту
# Полуширина габарита. 1.2, а не 1.6: так габарит задают организаторы в
# синтетике 24.09 — «у края» кончается на 1.13 м от оси, «вне габарита,
# но близко» начинается с 1.14. При 1.6 второй давал ложную тревогу, а
# в `doubleT_obstacle` в полосу заходили колонны между путями. Полигон
# парно +43 / −33 наблюдения, посторонних 793 → 742 (EXPERIMENTS п. 18.4).
half_width: float = 1.2
h_lo: float = 0.28 # нижняя граница габарита над рельсом, м
# Три правки габарита (Zhirik1337, 22.09), каждая мерилась отдельно на
# двух сценах и полигоне (EXPERIMENTS п. 16):
# h_lo_core — пол в колее: между рельсами (|u| ≤ core_width) нижняя
# граница опускается, чтобы не терять лежащего человека.
# ПРИНЯТО, но только дальше core_from: ближе в полосу
# 0.16…0.28 м попадают головки рельсов, рельс собирается
# в одну компоненту от кабины, и предмет выбрасывается
# вместе с ней. Лежачий P@50 0.27 → 0.50;
# k_sigma — габарит расширяется там, где ось пути известна хуже
# (в кривых вдали); 0 — не расширять. Отвергнуто;
# platform_filter — вырезать из габарита настил платформы. Отвергнуто:
# в тоннеле без платформы режет стену на обрывки.
h_lo_core: float = 0.16
core_width: float = 0.85 # полуширина колеи, м
core_from: float = 30.0 # ближе этой дальности пол в колее не опускается, м
k_sigma: float = 0.0
platform_filter: bool = False
h_hi: float = 2.3 # верхняя граница, м
# Верхняя секция габарита: от h_hi до h_top при |u| < half_width_top —
# то, что свисает со свода в путь вагона (см. lobula.gauge_mask). Вагон
# выше 3.5 м, а предмет «2×2 сверху» у организаторов висит низом на 3.0 м
# над рельсом — при верхе 2.3 м он был не виден вовсе. Верх 3.3, а не 3.7:
# у свода в настоящих тоннелях свои кабели и кронштейны, и ошибка наклона
# плоскости пути на 60–90 м опускает их на 0.1–0.3 м — при 3.7 они шли в
# тревоги (низ у них в основном выше 3.2 м, у предметов организаторов —
# 2.8 и 3.0). h_top <= h_hi — секции нет. EXPERIMENTS п. 18.4, 19.
h_top: float = 3.3
half_width_top: float = 1.0
top_d_max: float = 90.0 # дальше верхней секции нет, м
# Плоское у пола (см. central_complex._quality): вертикальный размер меньше
# flat_h, верх ниже flat_top над рельсом, не дальше flat_d — вес наблюдения
# умножается на flat_w. flat_h: 0 — выключено. EXPERIMENTS п. 19.
flat_h: float = 0.06
# Отдельный проход по верхней секции для свисающего со свода (см.
# lobula._hanging): в основном проходе оно склеено со сводом.
top_detect: bool = True
# Дальний трек (дальше far_confirm_from) поднимает тревогу, только прожив
# far_confirm кадров (см. descending.py). 0 — выключено. Замерено и
# отвергнуто: 15 кадров убирают короткие дальние фантомы, но полигон теряет
# 200 наблюдений, P@150 у человека стоя 0.46 → 0.37. EXPERIMENTS п. 19.
far_confirm: int = 0
far_confirm_from: float = 100.0
flat_top: float = 0.26
flat_d: float = 70.0
flat_w: float = 0.15
ctx_up: float = 4.0 # насколько контекст лезет выше габарита, м
# Разделение фигуры и фона по движению (см. lobula.split_by_figure):
# порог скорости сближения вдоль луча, в долях пройденного пути.
# Поднимает рабочую дальность с 62 до 80 м, но вчетверо увеличивает ложные
# тревоги: в тоннеле полно поверхностей, которые приближаются так же, как
# предмет. 0 — выключено; рабочее значение 0.6. Кривая — EXPERIMENTS п. 9.4.
split_adv: float = 0.0
# Разделение фигуры и фона по контрасту ламины (см. lobula.split_by_figure):
# порог «насколько ближе своего окружения», м. Гладкая стена даёт ноль по
# построению центр-окружения, предмет на стене — ступеньку. Именно это
# вытаскивает предмет из компоненты, растёкшейся вдоль стены круглого
# тоннеля, где покадровый тракт слеп целиком. 0 — выключено.
split_gap: float = 6.0
split_near: float = 55.0 # ближе этого резать только растянутое
# Разброс дальности внутри компоненты, выше которого её режут и в
# ближней зоне: предмет по дальности занимает десятки сантиметров,
# кромка платформы и стена — десятки метров. 0 — не резать вблизи
# вовсе. Замер — EXPERIMENTS п. 12.7.
near_long: float = 0.0
split_top: int = 1 # сколько фигур выносить из одной компоненты
# Накопление слабых улик в координатах пути (см. fan_body.py). Заменяет
# недоступный на большой дальности контраст к фону: на 140…185 м `gap`
# структурно равен нулю, и без накопления улика не растёт вовсе.
enable_accumulator: bool = True
acc_near: float = 55.0 # ближе этого покадровый тракт и так справляется
acc_gain: float = 1.5 # опора накопителя, засчитываемая за полный контраст
# Привыкание внутри проезда (см. mushroom_body.Habituation). Замысел: гасить
# формы, встретившиеся в НЕСКОЛЬКИХ разных точках пути, то есть штатную
# повторяющуюся обстановку, без единого прошлого проезда.
#
# ПО УМОЛЧАНИЮ ВЫКЛЮЧЕНО — измерено, что избирательности у механизма нет.
# При ёмкости, достаточной чтобы популяция не насыщалась (20 000 клеток),
# привыкание не меняет ничего: 7.5 ложных трека на км против тех же 7.5.
# Весь эффект маленькой популяции (5.9 на км) оказался насыщением, которое
# давит без разбора: новизна вставленного предмета падала до 0.24 при
# медианной новизне кандидата 0.43…0.71, то есть предмет подавлялся
# СИЛЬНЕЕ обстановки. Огрубление признаков и резкий отсчёт MBON не помогли.
# Разбор — EXPERIMENTS п. 10.
enable_habituation: bool = False
hab_rate: float = 0.25 # депрессия за одно новое место
hab_place_m: float = 5.0 # насколько разнести места, чтобы считать разными
hab_recover_m: float = 800.0 # путь, за который привыкание сходит наполовину
hab_n_kc: int = 20000 # ёмкость короткой памяти, клеток Кеньона
hab_read_q: float = 0.75 # квантиль отсчёта MBON
hab_quant: float = 0.5 # огрубление признаков, в долях разброса
# Обученное считывание MBON (см. mbon_readout.py). Заменяет ручную формулу
# веса улики в центральном комплексе там, где модель передана конвейеру;
# без модели всё работает по-прежнему.
enable_mbon: bool = True
# Резкость: вес наблюдения p**power. Рабочая точка 1.5 выбрана сравнением
# при равной строгости (EXPERIMENTS п. 17.4): ложных треков на незнакомой
# линии 23.5 → 14.7 на км, на знакомой 6.5 → 5.7, на второй половине
# new_data 23.9 → 17.4, а полигон теряет 142 наблюдения из 7 600: 39
# вблизи у края габарита (пограничный случай п. 17.1–17.2) и 82 за 90 м.
# 2.0 срезает ещё 127 наблюдений ради 0.4 трека на км.
mbon_power: float = 1.5
mbon_blend: float = 1.0 # 1 — только модель, 0 — только ручная формула
# Гашение знакомости с дальностью: за `nov_fade_from` её вклад падает
# до нуля к `nov_fade_to`. За 90 м новизна измеримо ВРЕДНА — у
# вставленного человека на 120…185 м она 0.150 против 0.199 у
# обстановки, AUC 0.293, то есть признак перевёрнут. Гашение подняло
# P@100 с 0.31 до 0.48 и P@150 с нуля до 0.32 ценой 3.3 → 5.6 ложных
# трека на км на ЗНАКОМОЙ линии и ноля на незнакомой. 0 — не гасить,
# это тихий режим для линии с обученной памятью. EXPERIMENTS п. 12.3.
nov_fade_from: float = 90.0
nov_fade_to: float = 160.0
# Висящее посреди габарита: пол вероятности считывания для кандидата,
# целиком внутри габарита (`hover_cont`), с низом выше `hover_h` над
# рельсом, |u| не больше `hover_u`, не дальше `hover_d`, компактного вдоль
# пути и не тоньше `hover_width`. Модель учили на стоящих предметах, и
# предмет в воздухе она гасит: куб 0.3 м на высоте 1.2 м у края габарита
# синтетики находился с 31 м, с правилом — с 63 м; ложных тревог на всех
# трёх проверках столько же. 0 — выключено. EXPERIMENTS п. 24.
hover_floor: float = 0.5
hover_h: float = 0.6
hover_cont: float = 0.9
hover_u: float = 1.2
hover_d: float = 80.0 # дальше — не действует: ось пути вдали неточна
hover_depth: float = 1.5 # протяжённость вдоль пути до, м: длиннее — труба, поручень
hover_width: float = 0.2 # ширина от, м: уже — провод, кронштейн, тонкая деталь
# Канал малых целей (LC11, см. lobula._small_targets): кандидат из двух-трёх
# лучей, если он висит в пустом пространстве габарита — вся связная
# компонента не больше `small_ctx` лучей, низ выше `small_h` над рельсом,
# |u| не больше `small_u`, фон за ним дальше `small_gap`, дальность от
# `small_d_from` до `small_d_to`. Куб 0.3 м посередине синтетики
# организаторов при обычном пороге в четыре луча находился с 45 м: с 57 м
# его точки ложатся в две ячейки образа. 0 — выключено. EXPERIMENTS п. 26.
small_rays: int = 2
small_ctx: int = 4
small_h: float = 0.5
small_u: float = 0.8
small_gap: float = 5.0
small_d_from: float = 45.0
small_d_to: float = 100.0
# Поправка оценки модели на распространённость предметов: за этой
# дальностью из логита вычитается типичный для полосы. 0 — не
# поправлять. Замер — EXPERIMENTS п. 12.3.
mbon_prior_from: float = 0.0
# Пороги решения (см. descending.py). Вынесены сюда, потому что обученное
# считывание меняет цену улики: когда ложные тревоги падают, запас можно
# потратить на дальность, опустив порог. Замер — EXPERIMENTS п. 11.
# Считывание по треку (см. track_readout.py): геометрическое смешивание
# обученного отсчёта с уликой перед сравнением с порогом. 0 — решает
# только улика, как раньше. Замер — EXPERIMENTS п. 15.
# Правки решения (Zhirik1337, 22.09), обе выключены по замеру (п. 16):
# lying_exempt — не штрафовать за вытянутость низкий предмет в колее
# (лежащий человек вытянут вдоль пути, как конструкция).
# Правит ручную формулу, а при mbon_blend = 1 вес целиком
# от модели — не действует;
# far_channel — дальний канал предупреждения в обход порога. Выигрыш
# настоящий, но та же задача, что warn_far, втрое дороже:
# +3.0 и +7.1 ложных трека на км на двух сценах.
lying_exempt: bool = False
far_channel: bool = False
track_blend: float = 0.0
track_gate: float = 0.0 # жёсткий порог по тому же отсчёту
# Чем считать «насколько это предмет» по треку: `w_mean` — средний вес
# наблюдения за всю жизнь, `w_ema` — он же со скользящим забыванием,
# `p_mean` — средний покадровый отсчёт считывания, `model` — обученное
# считывание по треку (нужен файл модели). Замер — п. 15.
track_score: str = "w_mean"
warn_evidence: float = 0.5 # улика, с которой трек поднимает тревогу
clear_evidence: float = 0.3 # и до которой держится (гистерезис)
emergency_evidence: float = 0.75
# Наблюдений, без которых трек не считается. Три подтверждения стоили
# дальности на предметах среднего размера: на 60…100 м они видны через
# кадр, и третьего приходится ждать слишком долго. Два подтверждения
# подняли рабочую дальность человека сидя с 20 до 80 м, а чемодана —
# с 20 до 62 м, и не стоили ни одного лишнего ложного трека
# (3.3 на км в обоих случаях). Замер — EXPERIMENTS п. 11.3.
min_hits: int = 2
novelty_floor: float = 0.10
# Порог тревоги, линейно опускаемый с 0.5 до `warn_far` на отрезке от
# `warn_far_from` до 200 м. В п. 12.2 ровно эта правка была отвергнута:
# тогда улика далёкого трека колебалась около нуля, и опускать порог под
# нулевым сигналом было бессмысленно. После гашения знакомости и порога
# по лучам улика перестала быть нулём, и та же правка дала P@150
# 0.33 → 0.37 без потерь на 50 и 100 м ценой нуля ложных треков на
# знакомой линии и одного на незнакомой. Экстренного торможения она не
# касается: у него свой порог и условие d ≤ max(тормозной путь, 25 м).
# EXPERIMENTS п. 15.9.
warn_far: float = 0.30
warn_far_from: float = 90.0
leak_far: float = 0.12 # утечка улики на дальнем краю; равна leak — выключено
leak_far_from: float = 90.0 # с какой дальности она начинает падать
d_min: float = 4.0
d_max: float = 220.0
# Сколько лучей образуют кандидата. Вблизи обрывок из трёх лучей — это
# кусок чего-то большего, и он дробит настоящий предмет: разрешить
# тройки везде стоит P@50 у человека 0.70 → 0.64. Вдали три луча —
# всё, что есть (медиана на 150 м — шесть). Порог по дальности берёт
# дальний выигрыш целиком (P@150 0.32 → 0.37, человек сидя 80 → 100 м
# рабочей дальности) и не стоит ни одного лишнего ложного трека ни на
# знакомой линии, ни на незнакомой. EXPERIMENTS п. 12.5.
min_rays: int = 4
min_rays_far: int = 3 # 0 — не различать
min_rays_far_from: float = 90.0
calib_frames: int = 12
# Скачок времени записи больше этого (или назад) — перемотка, повтор или
# выпадение данных: треки, одометрия и ось пути начинаются заново. 0 — не
# сбрасывать никогда.
reset_gap_s: float = 2.0
novelty_gate: float = 0.0 # ниже этой новизны кандидат отбрасывается сразу
# Где считать плотные стадии — сетчатку, ламину, кластеризацию (gpu.py):
# 'auto' — видеокарта, если PyTorch её видит, иначе процессор; 'cuda';
# 'cpu'. Результат одинаков: на видеокарте те же операции в тех же типах.
# Любой сбой видеокарты переводит конвейер на процессор до конца работы,
# кадр досчитывается там же. Узел поднимает видеокарту в фоне
# (`gpu_background`), считая первые кадры на процессоре. EXPERIMENTS п. 21.
device: str = "auto"
enable_motion: bool = True
enable_memory: bool = True
enable_looming: bool = False # T4/T5 + LPLC2: нужны для оценки надвигания и для
# схемы мозга; на решение пока не влияют
# Переключатели для абляции: каждый отключает один механизм, оставляя
# остальные. Нужны, чтобы вклад механизма измерялся, а не декларировался.
use_corridor: bool = True # ось пути; False — прямой коридор вперёд
use_shape: bool = True # целостность, компактность, опора снизу
use_tracking: bool = True # накопление улик по кадрам
@dataclass
class FrameResult:
"""Всё, что конвейер узнал за один кадр."""
stamp: float
decision: Decision
candidates: list[Candidate]
ego: EgoMotion | None
plane: RailPlane
corridor: Corridor
tf: TrackFrame | None = None
lam: lamina.LaminaOutput | None = None
emd: np.ndarray | None = None # (4, h, w) отклики T4/T5
loom: np.ndarray | None = None # (h, w) отклик LPLC2
hab: float = 0.0 # привыкание популяции KC, 0…1
timings: dict = field(default_factory=dict)
@property
def total_ms(self) -> float:
return sum(self.timings.values())
class FlyGuard:
"""Полный конвейер обнаружения посторонних объектов."""
def __init__(self, params: Params | None = None,
memory: MushroomBody | None = None,
layout: ScanLayout | None = None,
readout: "MbonReadout | None" = None,
track_readout=None,
gpu_background: bool = False):
self.p = params or Params()
self.device = "cpu"
self.gpu = None # плотные стадии на видеокарте (gpu.GpuStages)
self.gpu_error: str | None = None
self._gpu_ready = None # (стадии, устройство) — готовы, ждут начала кадра
# gpu_background: видеокарта поднимается в своём потоке, а кадры тем
# временем считает процессор. Первый запуск ядер CUDA бывает долгим —
# в Docker под WSL 15 с на каждый новый контейнер, — и узел всё это время
# не был подписан и терял кадры. Зависни драйвер совсем — узел так и
# останется на процессоре, а не встанет.
self.gpu_pending = str(self.p.device).strip().lower() != "cpu"
if self.gpu_pending:
if gpu_background:
threading.Thread(target=self._bring_up_gpu, name="flyguard-gpu",
daemon=True).start()
else:
self._bring_up_gpu()
self._take_gpu()
self.memory = memory if (memory is not None and self.p.enable_memory) else NoMemory()
self.readout = readout if (readout is not None and self.p.enable_mbon) else None
self.layout_full = layout
self.layout: ScanLayout | None = None
self.cols: slice | None = None
self._track_readout = track_readout
self._init_temporal()
self._calib: list[PointCloud2] = []
# Поворот облака вокруг вертикали, если у драйвера «вперёд» — не −Y
# (определяется по дальним эхам при калибровке, см. forward_azimuth)
self.forward_deg = 0.0
self.frames_seen = 0
self._last_stamp: float | None = None
self.time_jumps = 0 # сколько раз время записи скакало
def _init_temporal(self) -> None:
"""Всё, что копится от кадра к кадру: треки, одометрия, ось, улики.
Решётка лучей сюда не входит — она от сенсора, а не от места на пути.
"""
track_readout = self._track_readout
self.plane: RailPlane | None = None
self._prev_r: np.ndarray | None = None
self.corridor: Corridor | None = None
self.ego_est = EgoMotionEstimator()
self.fan: FanBody | None = None
self.s_path = 0.0 # пройденный путь, м — координата привыкания
self.hab = (Habituation(HabituationConfig(
n_kc=self.p.hab_n_kc, sparsity=80.0 / max(self.p.hab_n_kc, 1),
rate=self.p.hab_rate, place_m=self.p.hab_place_m,
read_q=self.p.hab_read_q, quant=self.p.hab_quant,
recover_m=self.p.hab_recover_m))
if self.p.enable_habituation else None)
self.emd_bank = EmdBank()
self.cx = CentralComplex(use_shape=self.p.use_shape,
mbon_power=self.p.mbon_power,
mbon_blend=self.p.mbon_blend,
leak_far=self.p.leak_far,
leak_far_from=self.p.leak_far_from,
nov_fade_from=self.p.nov_fade_from,
nov_fade_to=self.p.nov_fade_to,
lying_exempt=self.p.lying_exempt,
top_from=(self.p.h_hi if self.p.h_top > self.p.h_hi
else float("inf")),
flat=(self.p.flat_h, self.p.flat_top, self.p.flat_d,
self.p.flat_w),
hover=(self.p.hover_floor, self.p.hover_h,
self.p.hover_cont, self.p.hover_u,
self.p.hover_d, self.p.hover_depth,
self.p.hover_width))
# без накопления улик трек подтверждается первым же наблюдением
self.dn = (DescendingNeurons(
warn_evidence=self.p.warn_evidence,
clear_evidence=self.p.clear_evidence,
emergency_evidence=self.p.emergency_evidence,
min_hits=self.p.min_hits,
novelty_floor=self.p.novelty_floor,
warn_far=self.p.warn_far,
warn_far_from=self.p.warn_far_from,
track_blend=self.p.track_blend,
track_gate=self.p.track_gate,
track_score=self.p.track_score,
track_readout=track_readout,
far_channel=self.p.far_channel,
far_confirm=self.p.far_confirm,
far_confirm_from=self.p.far_confirm_from) if self.p.use_tracking
else DescendingNeurons(warn_evidence=0.0, clear_evidence=0.0,
emergency_evidence=0.0, min_hits=1))
# ------------------------------------------------------------------ видеокарта
def _bring_up_gpu(self) -> None:
"""Проверить CUDA, прогреть стадии и отдать их конвейеру (`_take_gpu`)."""
try:
from .device import get_device
dev = get_device(self.p.device)
if dev.startswith("cuda"):
from .gpu import GpuStages
stages = GpuStages(dev)
stages.warmup()
self._gpu_ready = (stages, dev)
except Exception as e: # нет драйвера, не та видеокарта, нет памяти
self._gpu_off(e)
finally:
self.gpu_pending = False
def _take_gpu(self) -> None:
"""Перейти на видеокарту, если она готова. Только между кадрами.
Посреди кадра нельзя: ламина и кластеризация берут образ, который
сетчатка оставила на видеокарте.
"""
if self._gpu_ready is not None:
self.gpu, self.device = self._gpu_ready
self._gpu_ready = None
@property
def gpu_active(self) -> bool:
"""Плотные стадии на видеокарте — уже или с ближайшего кадра."""
return self.gpu is not None or self._gpu_ready is not None
def _gpu_off(self, exc: Exception) -> None:
"""Сбой видеокарты: до конца работы считать на процессоре."""
from .device import notify_cuda_error
notify_cuda_error(exc)
self.gpu = None
self.device = "cpu"
self.gpu_error = f"{type(exc).__name__}: {exc}"
def _cluster(self, mask: np.ndarray, r: np.ndarray, **kw):
"""Кластеризация с учётом глубины: видеокарта, при сбое — процессор."""
if self.gpu is not None:
try:
return self.gpu.cluster_by_depth(mask, r, **kw)
except Exception as e:
self._gpu_off(e)
return cluster_by_depth(mask, r, **kw)
# ------------------------------------------------------------------ калибровка
@property
def calib_progress(self) -> int:
"""Сколько кадров уже набрано для калибровки решётки."""
return self.p.calib_frames if self.layout_full is not None else len(self._calib)
def _ensure_layout(self, pc: PointCloud2) -> bool:
"""Накопить кадры и откалибровать решётку. False — ещё не готов."""
if self.layout is not None:
return True
if self.layout_full is None:
self._calib.append(pc)
if len(self._calib) < self.p.calib_frames:
return False
self.forward_deg = forward_azimuth(self._calib)
if self.forward_deg:
self._calib = [rotate_cloud(c, self.forward_deg) for c in self._calib]
self.layout_full = ScanLayout.calibrate(self._calib)
self._calib.clear()
self.cols = self.layout_full.column_slice(self.p.fov_deg)
self.layout = self.layout_full.sub(self.cols)
return True
# ------------------------------------------------------------------ накопление
def _accumulate(self, tf, inside, lat, ds: float,
cands: list[Candidate]) -> None:
"""Досыпать кадр в веерное тело и проставить опору каждому кандидату."""
if not self.p.enable_accumulator:
return
if self.fan is None:
self.fan = FanBody(d_max=self.p.d_max, half_width=self.p.half_width,
h_lo=self.p.h_lo, h_hi=self.p.h_hi,
d_near=self.p.acc_near)
self.fan.update(tf.d[inside], lat[inside], tf.h[inside], ds)
if not cands:
return
d = np.array([c.d for c in cands], np.float32)
u = np.array([c.u for c in cands], np.float32)
g = max(self.p.acc_gain, 1e-3)
for c, s in zip(cands, self.fan.support_at(d, u)):
c.extra["acc_support"] = float(s) / g
# ------------------------------------------------------------------ фигура и фон
MIN_DS = 0.25 # м: меньше — шум дальности забивает разницу
def _advance(self, tf, ds: float):
"""Скорость сближения вдоль каждого луча, в долях пройденного пути.
Вдоль фиксированного луча стена, параллельная движению, не приближается
вовсе: поезд едет, точка пересечения скользит по стене, дальность
остаётся прежней. Предмет, обращённый к поезду, приближается ровно на
пройденный путь. Отсюда 0 у фона и 1 у фигуры.
Сравниваются два соседних кадра в одной решётке: столбец отвечает
фиксированному азимуту, поэтому достаточно вычитания без перепроекции.
Рысканье в кривой за кадр меньше шага решётки (0.06° при радиусе
1300 м против шага 0.1°), поэтому им пренебрегаем.
"""
cur = np.where(tf.valid, tf.r, np.nan).astype(np.float32)
prev, self._prev_r = self._prev_r, cur
if prev is None or prev.shape != cur.shape or ds < self.MIN_DS:
return None
with np.errstate(invalid="ignore"):
a = (prev - cur) / ds
return np.nan_to_num(a, nan=0.0, posinf=0.0, neginf=0.0)
# ------------------------------------------------------------------ основной такт
def process(self, pc: PointCloud2, *, keep_debug: bool = False) -> FrameResult | None:
"""Обработать один кадр. None, пока идёт калибровка решётки."""
t = _Timer()
rotated = bool(self.forward_deg)
if rotated:
pc = rotate_cloud(pc, self.forward_deg)
if not self._ensure_layout(pc):
return None
if self.forward_deg and not rotated:
# поворот найден на этом кадре при калибровке: сам кадр ещё не повёрнут
pc = rotate_cloud(pc, self.forward_deg)
# Скачок времени записи — перемотка, `ros2 bag play --loop`, разрыв в
# записи. Треки, одометрия и ось пути относятся к другому месту, и
# с ними предмет с прошлого круга всплыл бы там, где его нет.
if (self._last_stamp is not None and self.p.reset_gap_s > 0
and not (-0.05 < pc.stamp - self._last_stamp < self.p.reset_gap_s)):
self._init_temporal()
self.time_jumps += 1
self._last_stamp = pc.stamp
self.frames_seen += 1
self._take_gpu()
with t("retina"):
img: RangeImage | None = None
if self.gpu is not None:
try:
img = self.gpu.project(self.layout_full, pc, self.cols)
except Exception as e:
self._gpu_off(e)
if img is None:
img = self.layout_full.project(pc, self.cols)
with t("stabilize"):
self.plane = fit_rail_plane(img, self.layout, prev=self.plane)
tf = TrackFrame(img, self.layout, self.plane)
with t("corridor"):
self.corridor = (fit_corridor(tf, prev=self.corridor) if self.p.use_corridor
else STRAIGHT)
with t("lamina"):
lam = None
if self.gpu is not None:
try:
lam = self.gpu.lamina() # по образу кадра, уже лежащему на видеокарте
except Exception as e:
self._gpu_off(e)
if lam is None:
lam = lamina.process(tf.r, tf.valid, device="cpu")
with t("ego"):
ego = (self.ego_est.update(tf, pc.stamp) if self.p.enable_motion
else EgoMotion(0.0, 0.0, 0.0, 0.0, 0.1))
with t("figure"):
advance = self._advance(tf, ego.ds)
emd = loom = None
if self.p.enable_looming:
with t("medulla"):
emd = self.emd_bank.update(lam.on)
loom = looming(emd)
with t("lobula"):
masks = gauge_mask(tf, self.corridor, half_width=self.p.half_width,
h_lo=self.p.h_lo, h_hi=self.p.h_hi,
d_min=self.p.d_min, d_max=self.p.d_max,
k_sigma=self.p.k_sigma,
h_lo_core=self.p.h_lo_core,
core_width=self.p.core_width,
core_from=self.p.core_from,
platform_filter=self.p.platform_filter,
h_top=self.p.h_top,
half_width_top=self.p.half_width_top,
top_d_max=self.p.top_d_max)
cands = find_candidates(tf, lam, self.corridor, masks=masks,
half_width=self.p.half_width, h_lo=self.p.h_lo,
h_hi=self.p.h_hi, ctx_up=self.p.ctx_up,
split_adv=self.p.split_adv,
split_gap=self.p.split_gap,
split_near=self.p.split_near,
near_long=self.p.near_long,
split_top=self.p.split_top, advance=advance,
d_min=self.p.d_min,
d_max=self.p.d_max, min_rays=self.p.min_rays,
min_rays_far=self.p.min_rays_far,
min_rays_far_from=self.p.min_rays_far_from,
k_sigma=self.p.k_sigma,
h_lo_core=self.p.h_lo_core,
core_width=self.p.core_width,
core_from=self.p.core_from,
platform_filter=self.p.platform_filter,
h_top=self.p.h_top,
half_width_top=self.p.half_width_top,
top_d_max=self.p.top_d_max,
top_detect=self.p.top_detect,
cluster=self._cluster,
small_rays=self.p.small_rays,
small_ctx=self.p.small_ctx,
small_h=self.p.small_h,
small_u=self.p.small_u,
small_gap=self.p.small_gap,
small_d=(self.p.small_d_from,
self.p.small_d_to))
with t("mushroom"):
cands = self.memory.annotate(cands)
self.s_path += ego.ds
if self.hab is not None:
self.hab.update(cands, self.s_path, ego.ds)
if self.p.novelty_gate > 0:
cands = [c for c in cands if c.novelty >= self.p.novelty_gate]
with t("fan"):
# Накопитель идёт ПОСЛЕ памяти и мимо неё: память обучена на
# покадровых кандидатах, а у накопленного скопления нет ни
# контраста к фону, ни интенсивности — её признаки для него
# не определены. Судить о нём надо по тому, что у него есть:
# по числу попаданий в одну точку пути и по размеру.
self._accumulate(tf, masks[0], masks[1], ego.ds, cands)
with t("readout"):
# Строго после накопителя: опора веерного тела входит в признаки
# считывания, и на большой дальности это его главный вход.
if self.readout is not None:
self.readout.annotate(cands, self.p.mbon_prior_from)
with t("central"):
self.cx.update(cands, ego.ds, ego.dt)
with t("descending"):
decision = self.dn.decide(self.cx, ego.speed)
return FrameResult(stamp=pc.stamp, decision=decision, candidates=cands,
ego=ego, plane=self.plane, corridor=self.corridor,
tf=tf if keep_debug else None,
lam=lam if keep_debug else None,
emd=emd, loom=loom,
hab=self.hab.level if self.hab is not None else 0.0,
timings=t.result)
class _Timer:
"""Замер времени по стадиям — нужен для отчёта о задержке."""
def __init__(self):
self.result: dict[str, float] = {}
self._name = None
self._t0 = 0.0
def __call__(self, name: str):
self._name = name
return self
def __enter__(self):
self._t0 = time.perf_counter()
return self
def __exit__(self, *exc):
self.result[self._name] = (time.perf_counter() - self._t0) * 1e3
return False

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

@ -1,183 +0,0 @@
"""Пульт проигрывания записи: пауза, перемотка, шаг по кадру, скорость.
`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()

655
ros2_ws/src/flyguard/flyguard/retina.py
View file

@ -1,655 +0,0 @@
"""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
# Яркость — не обязательное поле: у драйвера без неё облако всё равно
# раскладывается, а яркость считается нулевой (как на медленном пути)
has_i = "intensity" in pts.dtype.names
if e == 1:
r_near = r[..., 0]
r_far = r[..., 0]
it = cube("intensity")[..., 0] if has_i else np.zeros_like(r_near)
valid = good[..., 0]
else:
inten = cube("intensity") if has_i else np.zeros_like(r)
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 not _ring_ordered(pc0):
# Без поля `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 _ring_ordered(pc: PointCloud2) -> bool:
"""Идут ли точки столбец за столбцом с одним порядком колец (по полю `ring`).
Калибровка по полю `ring` раскладывает кадр на столбцы по n_rings точек и
без этой проверки молча ошибалась на любом другом порядке: драйвер, который
отдаёт только точки с эхом, падал на «не делится на 128 колец», кадр,
разложенный по кольцам, — на каждом кадре, а перемешанный давал неверные
дальности (`tools/check_formats.py`).
"""
if "ring" not in pc.points.dtype.names or pc.n_points == 0:
return False
ring = np.asarray(pc.points["ring"]).astype(np.int64)
n = int(ring.max()) + 1
if n < 2 or ring.size % n:
return False
cols = ring.reshape(-1, n)
first = cols[0]
return bool(np.array_equal(np.sort(first), np.arange(n)) and np.all(cols == first))
def forward_azimuth(clouds: list[PointCloud2], far: float = 30.0, half: float = 30.0,
min_points: int = 2000) -> float:
"""Куда у облака смотрит «вперёд», по дальним эхам: 0 — как у выданных записей.
У выданных записей вперёд −Y. Другой драйвер может повернуть систему
координат (по REP-103 вперёд +X), и тогда рабочий сектор смотрит в стену —
узел молча не видит ничего. Далеко лидар видит только вдоль тоннеля, а назад
мешает сам поезд, поэтому направление с большинством дальних эх — вперёд.
Поворот признаётся только явный: в переднем секторе дальних эх почти нет, а
в одном из трёх других — больше половины. Иначе 0, и облако не трогается.
"""
parts = []
for pc in clouds[:6]:
x, y, z, ok = _xyz64(pc)
m = ok & (np.hypot(x, y) > far)
parts.append(np.degrees(np.arctan2(x[m], -y[m])))
az = np.concatenate(parts) if parts else np.zeros(0)
if az.size < min_points:
return 0.0
share = {c: float(np.mean(np.abs(_wrap180(az - c)) <= half))
for c in (0.0, 90.0, 180.0, -90.0)}
if share[0.0] >= 0.02:
return 0.0
best = max((90.0, 180.0, -90.0), key=lambda c: share[c])
return best if share[best] >= 0.5 else 0.0
def rotate_cloud(pc: PointCloud2, az_deg: float) -> PointCloud2:
"""Повернуть облако вокруг вертикали так, чтобы азимут `az_deg` стал «вперёд»."""
pts = np.array(pc.points) # копия: буфер кадра только для чтения
f, r = -pts["y"].astype(np.float64), pts["x"].astype(np.float64)
c, s = np.cos(np.radians(az_deg)), np.sin(np.radians(az_deg))
pts["x"] = -f * s + r * c
pts["y"] = -(f * c + r * s)
return 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)
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
View file

@ -1,38 +0,0 @@
"""Преобразование 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"} # яркость не обязательна: без неё нули
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
View file

@ -1,418 +0,0 @@
"""Синтетические препятствия: трассировка лучей в реальные кадры.
Разметки в датасете нет, а организаторы прямо предупредили, что приватный тест
собран добавлением синтезированных препятствий в новые проезды. Поэтому свой
полигон строится тем же способом: берётся настоящий кадр пустого тоннеля,
в него трассировкой лучей вставляется предмет заданного размера на заданной
дистанции, и получается **размеченный** пример с точно известным ответом.
Вставка идёт в исходное облако точек, а не в готовый дальностный образ, поэтому
через конвейер проходит ровно тот же путь, что и настоящие данные, начиная
с ретины.
Модель сенсора намеренно пессимистична: добавляется шум дальности, а
вероятность несостоявшегося эха растёт с расстоянием и с углом падения. Лучше
недооценить свой детектор, чем на защите обнаружить, что полигон был слишком
добрым. Яркость вставки берётся из самой записи — из возвратов на тех же
лучах: абсолютной шкалы интенсивности в этих данных нет, и любое назначенное
число делает вставку узнаваемой (см. длинный комментарий ниже).
"""
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
View file

@ -1,121 +0,0 @@
"""Считывание по ТРЕКУ — тот же контур 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
View file

@ -1,150 +0,0 @@
"""Запуск детектора 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
View file

@ -1,35 +0,0 @@
<?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
View file

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

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

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

@ -1,35 +0,0 @@
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",
],
},
)

1468
ros2_ws/src/flyguard/test/test_pipeline.py
View file

File diff suppressed because it is too large Load diff

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

@ -1,14 +0,0 @@
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
View file

@ -1,12 +0,0 @@
# Один обнаруженный посторонний объект в габарите приближения поезда.
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
View file

@ -1,17 +0,0 @@
# Состояние пути перед беспилотным поездом за один лидарный кадр.
#
# Минимально необходимое для бортовой системы — поля 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
View file

@ -1,21 +0,0 @@
<?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>

365
tests/test_pipeline.py
View file

@ -4,12 +4,10 @@
выпрямление скоса каналов, геометрия плоскости пути, связность с учётом
глубины, кодирование грибовидного тела и вставка синтетического предмета.
pytest tests
pytest ros2_ws/src/flyguard/test
"""
from __future__ import annotations
import sqlite3
import struct
import sys
from pathlib import Path
@ -52,37 +50,6 @@ def test_point_dtype_rejects_overlapping_fields():
point_dtype([("x", 0, 8, 1), ("y", 4, 7, 1)], 26)
def test_ros_message_keeps_every_point_in_order():
"""Узел ROS: NaN-точки остаются на месте, иначе решётка азимут × кольцо съедет."""
from types import SimpleNamespace as NS
from flyguard.cdr import from_ros_message
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(spec, 26)
n_rings, n_cols = 4, 5
pts = np.zeros(n_rings * n_cols, dt)
pts["x"] = np.arange(pts.size, dtype=np.float32)
pts["ring"] = np.tile(np.arange(n_rings), n_cols)
pts["timestamp"] = 1.5 + np.arange(pts.size) * 1e-6
pts["x"][[3, 7]] = np.nan
msg = NS(header=NS(stamp=NS(sec=12, nanosec=500_000_000), frame_id="hesai_lidar"),
height=1, width=pts.size, point_step=26, is_bigendian=False, is_dense=False,
fields=[NS(name=n, offset=o, datatype=t, count=c) for n, o, t, c in spec],
data=pts.tobytes())
pc = from_ros_message(msg)
assert pc.n_points == n_rings * n_cols
assert pc.stamp == pytest.approx(12.5)
assert pc.frame_id == "hesai_lidar"
assert np.isnan(pc.points["x"][[3, 7]]).all()
assert pc.points["x"][8] == 8.0
assert np.array_equal(pc.points["ring"], pts["ring"])
assert np.array_equal(pc.points["timestamp"], pts["timestamp"])
# --------------------------------------------------------------------------- решётка
def _layout(n_rings=8, n_az=40, n_echo=2, shift=None):
@ -209,90 +176,6 @@ 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 "y" in why and "z" in why
def test_ros_bridge_accepts_cloud_without_intensity():
"""Яркость не обязательна: драйвер без неё не должен терять кадры."""
from flyguard import ros_conv
spec = [("x", 0, 7, 1), ("y", 4, 7, 1), ("z", 8, 7, 1)]
msg = _FakeMsg(np.zeros(4, point_dtype(spec, 12)), [_FakeField(*f) for f in spec], 12)
assert ros_conv.has_required_fields(msg) == (True, "")
# --------------------------------------------------------- фигура и фон, привыкание
def _cand(**kw):
@ -506,64 +389,6 @@ def test_injected_intensity_copies_the_surroundings():
# --------------------------------------------------------------------------- данные
def _write_minimal_bag(db_path, blobs: list[bytes]):
"""Собрать минимальный `.db3` со схемой rosbag2, нужной `flyguard.bag.Bag`."""
con = sqlite3.connect(db_path)
con.execute("CREATE TABLE topics (id INTEGER PRIMARY KEY, name TEXT, type TEXT)")
con.execute("CREATE TABLE messages (id INTEGER PRIMARY KEY, topic_id INTEGER, "
"timestamp INTEGER, data BLOB)")
con.execute("INSERT INTO topics VALUES (1, '/lidar_points', "
"'sensor_msgs/msg/PointCloud2')")
for i, blob in enumerate(blobs):
con.execute("INSERT INTO messages (topic_id, timestamp, data) VALUES (1, ?, ?)",
(i, blob))
con.commit()
con.close()
def _cdr_pointcloud2_blob(n_points: int) -> bytes:
"""Валидный CDR sensor_msgs/msg/PointCloud2: один float32 `x` на точку."""
def s(text: str) -> bytes:
b = text.encode() + b"\x00"
pad = (-len(b)) % 4
return struct.pack("<I", len(b)) + b + b"\x00" * pad
buf = bytearray(b"\x00\x00\x00\x00") # заголовок инкапсуляции CDR
buf += struct.pack("<ii", 0, 0) # header.stamp (sec, nanosec)
buf += s("hesai_lidar") # header.frame_id
buf += struct.pack("<II", 1, n_points) # height, width
buf += struct.pack("<I", 1) # число полей
buf += s("x")
buf += struct.pack("<I", 0) # offset
buf += struct.pack("<B", 7) # datatype=FLOAT32
buf += b"\x00\x00\x00" # паддинг octet -> uint32 (_align(4))
buf += struct.pack("<I", 1) # count
buf += struct.pack("<B", 0) # is_bigendian
buf += b"\x00\x00\x00" # паддинг octet -> uint32 (_align(4))
buf += struct.pack("<I", 4) # point_step
buf += struct.pack("<I", 4 * n_points) # row_step
data = np.arange(n_points, dtype="<f4").tobytes()
buf += struct.pack("<I", len(data)) + data
buf += struct.pack("<B", 1) # is_dense
return bytes(buf)
def test_bag_skips_a_corrupted_message_and_keeps_reading(tmp_path):
"""Один битый CDR-пакет (реальный сенсор) не должен обрывать чтение бэга."""
from flyguard.bag import Bag
good_a = _cdr_pointcloud2_blob(5)
good_b = _cdr_pointcloud2_blob(7)
broken = b"\x00\x00\x00\x00" + b"\xff" * 3 # обрывается на первом же string()
_write_minimal_bag(tmp_path / "chunk_0.db3", [good_a, broken, good_b])
bag = Bag(tmp_path)
frames = list(bag.frames())
assert [pc.n_points for _, pc in frames] == [5, 7]
assert bag.n_frames_failed == 1
@pytest.mark.skipif(not DATA.exists(), reason="датасет не распакован")
def test_real_bag_projects_without_angular_error():
"""На реальном бэге выпрямленная решётка обязана описывать лучи точно."""
@ -581,94 +406,6 @@ def test_real_bag_projects_without_angular_error():
assert np.all(img.r_far[img.valid] >= img.r_near[img.valid] - 1e-3)
@pytest.mark.skipif(not DATA.exists(), reason="датасет не распакован")
def test_real_bag_projects_without_intensity_field():
"""Облако без поля яркости раскладывается так же, яркость — нули."""
from flyguard.bag import Bag
from flyguard.cdr import PointCloud2
bag = Bag(next(p for p in DATA.iterdir() if p.is_dir()))
clouds = [pc for _, pc in bag.frames(start=2, stop=8)]
lay = ScanLayout.calibrate(clouds)
pc = clouds[-1]
names = [n for n in pc.points.dtype.names if n != "intensity" and not n.startswith("_pad")]
bare = np.zeros(pc.points.size, [(n, pc.points.dtype.fields[n][0]) for n in names])
for n in names:
bare[n] = pc.points[n]
pc_bare = PointCloud2(stamp=pc.stamp, frame_id=pc.frame_id, height=pc.height,
width=pc.width, point_step=bare.dtype.itemsize,
is_dense=pc.is_dense, points=bare)
full, got = lay.project(pc), lay.project(pc_bare)
assert np.array_equal(got.valid, full.valid)
assert np.array_equal(got.r_near, full.r_near)
assert not np.any(got.inten)
def _reordered(pc, order):
from flyguard.cdr import PointCloud2
pts = np.array(pc.points)[order]
return PointCloud2(stamp=pc.stamp, frame_id=pc.frame_id, height=1, width=pts.size,
point_step=pc.point_step, is_dense=pc.is_dense, points=pts)
@pytest.mark.skipif(not DATA.exists(), reason="датасет не распакован")
@pytest.mark.parametrize("how", ["по кольцам", "только с эхом"])
def test_ring_field_with_other_point_order_is_calibrated_by_angles(how):
"""Поле `ring` есть, но точки не «столбец за столбцом» — решётка по углам."""
from flyguard.bag import Bag
from flyguard.retina import _ring_ordered
bag = Bag(next(p for p in DATA.iterdir() if p.is_dir()))
raw = [pc for _, pc in bag.frames(start=2, stop=8)]
assert _ring_ordered(raw[0])
if how == "по кольцам":
clouds = [_reordered(pc, np.argsort(pc.points["ring"], kind="stable")) for pc in raw]
else:
p0 = raw[0].points
clouds = [_reordered(pc, np.flatnonzero((pc.points["x"] != 0) | (pc.points["y"] != 0)
| (pc.points["z"] != 0))) for pc in raw]
assert not _ring_ordered(clouds[0])
lay, ref_lay = ScanLayout.calibrate(clouds), ScanLayout.calibrate(raw)
# сравнивается рабочий сектор: по углам решётка покрывает только дугу, где
# есть эхо, а по порядку точек — все 360°
img = lay.project(clouds[-1], lay.column_slice(30.0))
ref = ref_lay.project(raw[-1], ref_lay.column_slice(30.0))
assert not lay.indexed
assert lay.az_step_deg == pytest.approx(ref_lay.az_step_deg, abs=1e-3)
assert abs(img.valid.mean() - ref.valid.mean()) < 0.05
assert abs(np.median(img.r_near[img.valid]) - np.median(ref.r_near[ref.valid])) < 0.5
@pytest.mark.skipif(not DATA.exists(), reason="датасет не распакован")
def test_rotated_cloud_is_turned_back_to_minus_y():
"""Драйвер с осью X вперёд (REP-103): узел находит поворот по дальним эхам."""
from flyguard.bag import Bag
from flyguard.retina import forward_azimuth, rotate_cloud
bag = Bag(next(p for p in DATA.iterdir() if p.is_dir()))
raw = [pc for _, pc in bag.frames(start=2, stop=8)]
assert forward_azimuth(raw) == 0.0
turned = []
for pc in raw: # REP-103: x' = −y, y' = x — вперёд смотрит +X
pts = np.array(pc.points)
pts["x"], pts["y"] = -pc.points["y"], pc.points["x"]
turned.append(_reordered(pc, slice(None)).__class__(
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))
az = forward_azimuth(turned)
assert az == 90.0
back = rotate_cloud(turned[-1], az)
assert np.allclose(back.points["x"], raw[-1].points["x"], atol=1e-3)
assert np.allclose(back.points["y"], raw[-1].points["y"], atol=1e-3)
# ------------------------------------------------------------ раскладка по бэгам
_TOOLS = ROOT / "tools" # выгрузка: tools лежит рядом с тестами
@ -885,106 +622,6 @@ def test_gauge_mask_filters_platform_surface():
assert inside[1] # препятствие на колее сохранено
class _LobulaFrame(_MockTrackFrame):
"""Кадр в координатах пути со всем, что читает `find_candidates`."""
def __init__(self, d, u, h, valid):
super().__init__(d, u, h, valid)
self.r = np.where(self.valid, self.d, 0.0).astype(np.float32) # «нет эха» — 0, как у сетчатки
self.floor_r = np.full(self.d.shape, np.inf, np.float32)
self.inten = np.zeros(self.d.shape, np.float32)
rows, cols = self.d.shape
self.layout = type("Layout", (), dict(
az_grid_deg=np.linspace(-2.0, 2.0, cols, dtype=np.float32),
el_deg=np.linspace(1.0, -1.0, rows, dtype=np.float32)))()
def _small_scene():
"""Два луча на 80 м посреди габарита на высоте 1.5 м; вокруг эха нет,
а ON-контраст ламины такой, что фон за ними на 20 м дальше."""
shape = (12, 40)
d = np.full(shape, 150.0, np.float32)
u = np.zeros(shape, np.float32)
h = np.full(shape, 1.5, np.float32)
valid = np.zeros(shape, bool)
on = np.zeros(shape, np.float32)
d[6, 20:22] = 80.0
u[6, 20:22] = (0.15, 0.30)
valid[6, 20:22] = True
on[6, 20:22] = 0.0025 # x = on·r = 0.2 → разрыв r·x/(1−x) = 20 м
return d, u, h, valid, on
def _find(d, u, h, valid, on, **kw):
from flyguard.geometry import STRAIGHT
from flyguard.lobula import find_candidates
tf = _LobulaFrame(d, u, h, valid)
lam = type("Lam", (), dict(on=on, hole=np.zeros(on.shape, np.float32)))()
return find_candidates(tf, lam, STRAIGHT, half_width=1.2, h_lo=0.28, h_hi=2.3,
**kw)
def test_small_target_hanging_in_free_space_becomes_a_candidate():
"""Канал малых целей: два луча в пустоте — кандидат, при обычном
пороге в четыре луча — ничего, и выключенный канал ничего не меняет."""
scene = _small_scene()
assert _find(*scene) == [] # по умолчанию выключен
assert _find(*scene, small_rays=0) == []
out = _find(*scene, small_rays=2)
assert len(out) == 1
c = out[0]
assert c.extra.get("small") and c.n_rays == 2
assert abs(c.d - 80.0) < 1e-3 and c.containment == 1.0 and c.gap > 15.0
def test_small_target_rejects_what_is_attached_near_the_edge_or_on_the_wall():
"""Те же два луча не проходят, если пятно — часть стены, стоит у края
габарита, фон за ним близко или дальность вне диапазона канала."""
d, u, h, valid, on = _small_scene()
# стенка той же дальности рядом, за краем габарита: компонента большая
wd, wu, wh, wv = d.copy(), u.copy(), h.copy(), valid.copy()
wd[2:11, 23] = 80.5
wu[2:11, 23] = 1.6
wh[2:11, 23] = np.linspace(0.5, 2.0, 9)
wv[2:11, 23] = True
assert _find(wd, wu, wh, wv, on, small_rays=2) == []
# у края габарита: вдали внутри он или снаружи — не различить
eu = u.copy()
eu[6, 20:22] = (1.0, 1.1)
assert _find(d, eu, h, valid, on, small_rays=2) == []
# фон вплотную — не висит в пустоте
assert _find(d, u, h, valid, on * 0.05, small_rays=2) == []
# у самого полотна — это не канал малых целей
lh = h.copy()
lh[6, 20:22] = 0.35
assert _find(d, u, lh, valid, on, small_rays=2) == []
# ближе диапазона канала предмет даёт четыре луча и без него
assert _find(d, u, h, valid, on, small_rays=2, small_d=(90.0, 130.0)) == []
def test_small_target_weight_ignores_mbon_but_not_familiarity():
"""Вес наблюдения малой цели: считывание MBON её не судит (двухлучевых
кандидатов в его выборке не было), а знакомость — судит."""
from flyguard.central_complex import _quality
c = _cand(d=80.0, n_rays=2, gap=20.0, containment=1.0, h_min=1.5,
depth=0.0, width=0.15, height=0.0)
c.novelty = 0.6
c.extra["mbon"] = 0.01
ordinary = _quality(c, mbon_power=1.5, mbon_blend=1.0)
c.extra["small"] = True
w = _quality(c, mbon_power=1.5, mbon_blend=1.0)
assert ordinary < 0.01 < 0.45 < w # модель больше не гасит
c.novelty = 0.1
assert _quality(c, mbon_power=1.5, mbon_blend=1.0) < 0.1 * w # память гасит
def test_descending_far_soft_warning_triggers_without_emergency():
"""На 130 м устойчивый трек поднимает предупреждение, но не экстренное торможение."""
from flyguard.central_complex import CentralComplex, Track

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
View file

@ -1,177 +0,0 @@
"""Атлас нейронов 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()

284
tools/check_formats.py
View file

@ -1,284 +0,0 @@
"""Устойчивость к формату облака: тот же проезд в других обличьях.
Контрольная запись может прийти не в том виде, что выданные: другой драйвер,
другая раскладка, другой порядок и типы полей. Инструмент берёт настоящие
кадры, переписывает их, прогоняет через CDR — тот же разбор, что в узле, — и
через конвейер и сравнивает решения с исходными: сколько кадров обработано,
были ли сбои, совпадает ли тревога и расстояние.
python tools/check_formats.py
python tools/check_formats.py --bag data/for_hackathon/doubleT_obstacle
python tools/check_formats.py --only "без intensity,ось X вперёд"
"""
from __future__ import annotations
import argparse
import struct
import time
from pathlib import Path
import numpy as np
import _bootstrap as B
from flyguard.bag import Bag
from flyguard.cdr import PointCloud2, parse_pointcloud2
from flyguard.mbon_readout import MbonReadout
from flyguard.mushroom_body import MushroomBody
from flyguard.pipeline import FlyGuard, Params
_ROS_TYPE = {("i", 1): 1, ("u", 1): 2, ("i", 2): 3, ("u", 2): 4,
("i", 4): 5, ("u", 4): 6, ("f", 4): 7, ("f", 8): 8}
def to_cdr(pc: PointCloud2) -> bytes:
"""Обратно в CDR sensor_msgs/msg/PointCloud2 — в том виде, в каком пишет ROS."""
out = bytearray(b"\x00\x01\x00\x00") # заголовок инкапсуляции
def align(n: int) -> None:
out.extend(b"\x00" * ((-(len(out) - 4)) % n))
def u32(v: int) -> None:
align(4)
out.extend(struct.pack("<I", v))
def string(s: str) -> None:
b = s.encode() + b"\x00"
u32(len(b))
out.extend(b)
sec = int(np.floor(pc.stamp))
align(4)
out.extend(struct.pack("<i", sec))
u32(int(round((pc.stamp - sec) * 1e9)) % 1_000_000_000)
string(pc.frame_id)
u32(pc.height)
u32(pc.width)
dt = pc.points.dtype
named = [n for n in dt.names if not n.startswith("_pad")]
u32(len(named))
for name in named:
ft, off = dt.fields[name][:2]
base = ft.subdtype[0] if ft.subdtype else ft
string(name)
u32(off)
out.append(_ROS_TYPE[(base.kind, base.itemsize)])
u32(int(np.prod(ft.shape)) if ft.shape else 1)
out.append(0) # is_bigendian
u32(dt.itemsize)
u32(dt.itemsize * pc.width)
data = np.ascontiguousarray(pc.points).tobytes()
u32(len(data))
out.extend(data)
out.append(1 if pc.is_dense else 0)
return bytes(out)
# ------------------------------------------------------------------ обличья кадра
def _like(pc: PointCloud2, pts: np.ndarray, *, height: int = 1, dense: bool | None = None,
stamp: float | None = None) -> PointCloud2:
return PointCloud2(stamp=pc.stamp if stamp is None else stamp, frame_id=pc.frame_id,
height=height, width=pts.size // height,
point_step=pts.dtype.itemsize,
is_dense=pc.is_dense if dense is None else dense, points=pts)
def _fields(pc: PointCloud2) -> list[str]:
return [n for n in pc.points.dtype.names if not n.startswith("_pad")]
def _recast(pc: PointCloud2, spec: list[tuple[str, str, int]], itemsize: int) -> np.ndarray:
"""Новая раскладка точки: (имя, тип, смещение); значения — по имени поля."""
names = [s[0] for s in spec if s[0] in pc.points.dtype.names]
dt = np.dtype({"names": names,
"formats": [s[1] for s in spec if s[0] in names],
"offsets": [s[2] for s in spec if s[0] in names],
"itemsize": itemsize})
out = np.zeros(pc.points.size, dt)
for n in names:
out[n] = pc.points[n]
return out
def _drop(pc: PointCloud2, gone: set[str]) -> np.ndarray:
keep = [n for n in _fields(pc) if n not in gone]
spec, off = [], 0
for n in keep:
t = pc.points.dtype.fields[n][0]
spec.append((n, t.str, off))
off += t.itemsize
return _recast(pc, spec, off)
def _echo(pc: PointCloud2) -> np.ndarray:
p = pc.points
return ((p["x"] != 0) | (p["y"] != 0) | (p["z"] != 0)) & np.isfinite(p["x"])
def as_is(pc, k):
return pc
def nan_for_no_return(pc, k):
pts = np.array(pc.points)
miss = ~_echo(pc)
for n in ("x", "y", "z"):
pts[n][miss] = np.nan
return _like(pc, pts, dense=False)
def header_128(pc, k):
n = pc.points.size
return _like(pc, np.array(pc.points), height=128) if n % 128 == 0 else pc
def ring_major(pc, k):
if "ring" not in pc.points.dtype.names:
return pc
order = np.argsort(pc.points["ring"], kind="stable")
return _like(pc, np.array(pc.points)[order])
def reordered_fields(pc, k):
spec = [("intensity", "<f4", 0), ("ring", "<u2", 4), ("x", "<f4", 8), ("y", "<f4", 12),
("z", "<f4", 16), ("timestamp", "<f8", 24)]
return _like(pc, _recast(pc, spec, 32))
def intensity_u8(pc, k):
pts = _drop(pc, set())
spec = [(n, ("|u1" if n == "intensity" else pts.dtype.fields[n][0].str), 0)
for n in pts.dtype.names]
off = 0
for i, (n, t, _) in enumerate(spec):
spec[i] = (n, t, off)
off += np.dtype(t).itemsize
out = _recast(pc, spec, off)
out["intensity"] = np.clip(np.asarray(pc.points["intensity"], np.float64), 0, 255)
return _like(pc, out)
def no_intensity(pc, k):
return _like(pc, _drop(pc, {"intensity"}))
def no_ring(pc, k):
return _like(pc, _drop(pc, {"ring", "timestamp"}))
def echo_only(pc, k):
return _like(pc, np.array(pc.points)[_echo(pc)], dense=True)
def echo_only_no_ring(pc, k):
pts = _drop(pc, {"ring", "timestamp"})
return _like(pc, pts[_echo(pc)], dense=True)
def shuffled(pc, k):
rng = np.random.default_rng(k)
return _like(pc, np.array(pc.points)[rng.permutation(pc.points.size)])
def float64_xyz(pc, k):
spec, off = [], 0
for n in _fields(pc):
t = "<f8" if n in ("x", "y", "z") else pc.points.dtype.fields[n][0].str
spec.append((n, t, off))
off += np.dtype(t).itemsize
return _like(pc, _recast(pc, spec, off))
def x_forward(pc, k):
"""REP-103: x вперёд. У выданных записей вперёд −Y — поворот на 90° вокруг z."""
pts = np.array(pc.points)
x, y = pts["x"].copy(), pts["y"].copy()
pts["x"], pts["y"] = -y, x
return _like(pc, pts)
def frozen_stamp(pc, k):
return _like(pc, pc.points, stamp=1000.0)
VARIANTS = {
"как есть": (as_is, 1),
"NaN вместо нулей": (nan_for_no_return, 1),
"шапка 128 × N": (header_128, 1),
"по кольцам": (ring_major, 1),
"поля в другом порядке, 32 байта": (reordered_fields, 1),
"intensity uint8": (intensity_u8, 1),
"без intensity": (no_intensity, 1),
"без ring и timestamp": (no_ring, 1),
"только точки с эхом": (echo_only, 1),
"только точки с эхом, без ring": (echo_only_no_ring, 1),
"перемешанный порядок": (shuffled, 1),
"координаты float64": (float64_xyz, 1),
"5 Гц": (as_is, 2),
"ось X вперёд": (x_forward, 1),
"одинаковое время кадров": (frozen_stamp, 1),
}
def run(bag: Path, n: int, fn, step: int, memory, readout) -> dict:
# кадры читаются заново для каждого обличья: 24-мегабайтные кадры
# doubleT_obstacle целиком в память не помещаются
fg = FlyGuard(Params(device="cpu"), memory=memory, readout=readout)
out, errors, ms = {}, [], []
for k, (_, pc) in enumerate(Bag(bag).frames(stop=n)):
if k % step:
continue
try:
blob = to_cdr(fn(pc, k))
t0 = time.perf_counter()
res = fg.process(parse_pointcloud2(blob))
ms.append((time.perf_counter() - t0) * 1e3)
except Exception as e: # noqa: BLE001 — сбой и есть результат проверки
errors.append(f"{type(e).__name__}: {e}")
continue
if res is not None:
d = res.decision
out[k] = (bool(d.detected), float(d.distance) if d.detected else np.nan)
return {"dec": out, "errors": errors, "ms": ms}
def main() -> None:
ap = argparse.ArgumentParser(description=__doc__)
ap.add_argument("--bag", action="append",
help="запись; можно несколько (по умолчанию doubleT_obstacle и синтетика)")
ap.add_argument("--frames", type=int, default=600, help="сколько кадров с начала")
ap.add_argument("--only", default="", help="обличья через запятую")
args = ap.parse_args()
bags = args.bag or [str(B.DATA / "for_hackathon" / "doubleT_obstacle"),
str(B.DATA / "cloud_with_fake_obj")]
only = [s.strip() for s in args.only.split(",") if s.strip()]
memory = MushroomBody.load(B.ARTIFACTS / "mushroom_body.npz")
readout = MbonReadout.load(B.ARTIFACTS / "mbon_readout.npz")
for bag in map(Path, bags):
first = next(pc for _, pc in Bag(bag).frames(stop=1))
print(f"\n== {bag.name}: до {args.frames} кадров, "
f"поля {', '.join(_fields(first))}, точек {first.points.size}")
base = run(bag, args.frames, as_is, 1, memory, readout)["dec"]
n_alarm = sum(v[0] for v in base.values())
print(f" исходно: обработано {len(base)}, с тревогой {n_alarm}")
print(f" {'обличье':34s} {'обраб.':>6s} {'сбоев':>5s} {'тревог':>6s} "
f"{'совпало':>7s} {'Δд, м':>6s} {'мс':>5s}")
for name, (fn, step) in VARIANTS.items():
if only and name not in only:
continue
r = run(bag, args.frames, fn, step, memory, readout)
dec = r["dec"]
common = [k for k in dec if k in base]
same = np.mean([dec[k][0] == base[k][0] for k in common]) if common else np.nan
dd = [abs(dec[k][1] - base[k][1]) for k in common if dec[k][0] and base[k][0]]
alarms = sum(v[0] for v in dec.values())
print(f" {name:34s} {len(dec):6d} {len(r['errors']):5d} {alarms:6d} "
f"{same:7.1%} {np.median(dd) if dd else float('nan'):6.2f} "
f"{np.median(r['ms']) if r['ms'] else float('nan'):5.0f}")
if r["errors"]:
print(f" первый сбой: {r['errors'][0][:160]}")
if __name__ == "__main__":
main()

Some files were not shown because too many files have changed in this diff Show more