Compare commits
No commits in common. "main" and "main" have entirely different histories.
110 changed files with 489 additions and 17410 deletions
12
.gitignore
vendored
12
.gitignore
vendored
|
|
@ -2,21 +2,11 @@ __pycache__/
|
||||||
*.py[cod]
|
*.py[cod]
|
||||||
.venv/
|
.venv/
|
||||||
.pytest_cache/
|
.pytest_cache/
|
||||||
# записи лидара — только в корне: flyguard/data и .../flyguard/data нужны пакету
|
data/
|
||||||
/data/
|
|
||||||
artifacts/mbon_folds/
|
artifacts/mbon_folds/
|
||||||
artifacts/track_folds/
|
|
||||||
docs/figures/*.png
|
docs/figures/*.png
|
||||||
!docs/figures/curve_gauge.png
|
|
||||||
!docs/figures/doubleT_mount_shadow.png
|
|
||||||
docs/figures/*.mp4
|
docs/figures/*.mp4
|
||||||
|
|
||||||
# сборка ROS 2 и колёса PyTorch для сборки образа без сети
|
|
||||||
ros2_ws/build/
|
|
||||||
ros2_ws/install/
|
|
||||||
ros2_ws/log/
|
|
||||||
docker/wheels/*.whl
|
|
||||||
|
|
||||||
# Project memory & chat transfer
|
# Project memory & chat transfer
|
||||||
PROJECT_MEMORY.md
|
PROJECT_MEMORY.md
|
||||||
CHAT_MEMORY.md
|
CHAT_MEMORY.md
|
||||||
|
|
|
||||||
177
Dockerfile
177
Dockerfile
|
|
@ -1,109 +1,96 @@
|
||||||
# FlyGuard — обнаружение посторонних объектов в тоннеле метро по данным 3D-лидара.
|
# ============================================================================
|
||||||
#
|
# FlyGuard: Bionic Obstacle Detection Core (LCT-2026, Case 05)
|
||||||
# Образ самодостаточен: во время работы сеть не нужна, все зависимости ставятся
|
# Hesai Pandar128 LiDAR (128 beams, 10 Hz / 100ms)
|
||||||
# на этапе сборки. Базовый образ — официальный ROS 2 Humble на Ubuntu 22.04,
|
# Universal Production Image: NVIDIA CUDA 12.x / Ada Lovelace / Graceful CPU Fallback
|
||||||
# ровно та среда, что указана в ТЗ.
|
# ============================================================================
|
||||||
#
|
|
||||||
# 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
|
|
||||||
|
|
||||||
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 \
|
RUN apt-get update && apt-get install -y --no-install-recommends \
|
||||||
python3-numpy \
|
libgomp1 \
|
||||||
python3-scipy \
|
ca-certificates \
|
||||||
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/*
|
&& rm -rf /var/lib/apt/lists/*
|
||||||
|
|
||||||
# PyTorch с CUDA 12.8 — плотные стадии конвейера на видеокарте (сетчатка,
|
RUN useradd -m -u 1000 -s /bin/bash flyguard && \
|
||||||
# ламина, кластеризация; flyguard/gpu.py). Колёса несут свою среду CUDA, от
|
mkdir -p /app /data /app/artifacts && \
|
||||||
# машины нужен только драйвер NVIDIA (у стенда 580 — это до CUDA 13.0).
|
chown -R flyguard:flyguard /app /data
|
||||||
# 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)
|
WORKDIR /app
|
||||||
ENV NVIDIA_VISIBLE_DEVICES=all \
|
COPY --chown=flyguard:flyguard requirements.txt /app/
|
||||||
NVIDIA_DRIVER_CAPABILITIES=all
|
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
|
||||||
|
|
||||||
# Сначала манифесты — слой с зависимостями переживает правки кода
|
USER flyguard
|
||||||
COPY ros2_ws/src/flyguard/package.xml ros2_ws/src/flyguard/package.xml
|
VOLUME ["/data", "/app/artifacts"]
|
||||||
COPY ros2_ws/src/flyguard_msgs/package.xml ros2_ws/src/flyguard_msgs/package.xml
|
ENTRYPOINT ["/app/docker-entrypoint.sh"]
|
||||||
COPY ros2_ws/src/flyguard_msgs/CMakeLists.txt ros2_ws/src/flyguard_msgs/CMakeLists.txt
|
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/ ломает пакет.
|
# Stage 2: Universal Production Image with NVIDIA GPU acceleration & CPU fallback
|
||||||
RUN . /opt/ros/humble/setup.sh \
|
# (Default target when building with: docker build -t flyguard:latest .)
|
||||||
&& cd ros2_ws \
|
# ----------------------------------------------------------------------------
|
||||||
&& colcon build --cmake-args -DCMAKE_BUILD_TYPE=Release \
|
FROM nvidia/cuda:12.4.1-runtime-ubuntu22.04 AS production
|
||||||
&& rm -rf build log
|
|
||||||
|
|
||||||
COPY docker/entrypoint.sh /entrypoint.sh
|
ENV DEBIAN_FRONTEND=noninteractive \
|
||||||
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 \
|
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 рассчитаны на сотни килобайт
|
# Install Python 3.11, pip, and system runtime libraries (OpenMP for LightGBM/Torch)
|
||||||
# и теряли половину кадров ещё до подписчика. Профиль поднимает сегмент
|
RUN apt-get update && apt-get install -y --no-install-recommends \
|
||||||
# разделяемой памяти и буферы сокетов; снимается через FLYGUARD_DDS_PROFILE=0.
|
software-properties-common \
|
||||||
ENV FASTRTPS_DEFAULT_PROFILES_FILE=/opt/flyguard/fastdds_large.xml
|
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"]
|
# Create non-root user for container security compliance
|
||||||
CMD ["ros2", "launch", "flyguard", "detect.launch.py"]
|
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"]
|
||||||
|
|
|
||||||
|
|
@ -1,7 +1,7 @@
|
||||||
# ============================================================================
|
# ============================================================================
|
||||||
# FlyGuard GPU: NVIDIA RTX / CUDA 12 Production Image (Кейс 05, ЛЦТ-2026)
|
# FlyGuard GPU: NVIDIA RTX / CUDA 12 Production Image (Кейс 05, ЛЦТ-2026)
|
||||||
# Архитектура: Ada Lovelace (RTX 4070 Ti Super 16GB) / Ampere / Turing
|
# Архитектура: 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
|
FROM nvidia/cuda:12.4.1-runtime-ubuntu22.04 AS production
|
||||||
|
|
||||||
|
|
|
||||||
|
|
@ -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
941
README.md
|
|
@ -1,740 +1,241 @@
|
||||||
# FlyGuard — обнаружение посторонних объектов в тоннеле метро по данным 3D-лидара
|
# FlyGuard · ML-ядро
|
||||||
|
|
||||||
Решение кейса «Система обнаружения посторонних объектов для беспилотных поездов в тоннеле метро»
|
Обнаружение посторонних объектов в тоннеле метро по данным 3D-лидара
|
||||||
(хакатон «Лидеры цифровой трансформации 2026», направление «Город»; заказчик — Департамент
|
Hesai Pandar128. Кейс 05, ЛЦТ-2026.
|
||||||
транспорта Москвы / ГУП «Московский метрополитен»).
|
|
||||||
|
|
||||||
На вход — поток облаков точек Hesai Pandar128. На выход — ответ на единственный важный вопрос:
|
Это **ядро обработки**: облако точек на входе, решение о препятствии на выходе.
|
||||||
**«путь свободен» или «впереди препятствие на N метров»**.
|
Узел ROS 2, транспорт, контейнер и визуализация живут отдельно и сюда не
|
||||||
|
входят — ядро от них не зависит и проверяется без ROS вообще.
|
||||||
## Быстрый старт для жюри
|
|
||||||
|
|
||||||
Образ — файлом, если на стенде нет сети ([`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`) скрипт подбирает сам.
|
|
||||||
|
|
||||||
---
|
---
|
||||||
|
|
||||||
## В двух словах
|
## Чем это не является
|
||||||
|
|
||||||
ТЗ формулирует главный вызов так:
|
Не нейросетевой детектор общего назначения. Конвейер собран по схемам
|
||||||
|
зрительной системы дрозофилы, и каждая стадия — это конкретный нейропиль с
|
||||||
|
конкретной функцией, а не слой, подобранный перебором:
|
||||||
|
|
||||||
> «Возможно, нужно научиться хорошо описывать нормальный тоннель, а затем искать всё, что в него
|
```
|
||||||
> не вписывается.»
|
облако точек 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 собрана из её вычислительных схем, взятых из
|
встречаются две раскладки скана (3600 азимутов на 360° и 1200 на 120°) и две
|
||||||
коннектома (FlyWire / hemibrain):
|
высоты установки (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` |
|
| Реальный объект 0.67 × 1.35 м на 55 м | **99.5 %** кадров | `tools/check_obstacle.py` |
|
||||||
| Нельзя путать своё движение с чужим объектом | T4/T5 → LPTC, широкопольный оптический поток | `medulla.py` |
|
| Синтетика организаторов, 10 предметов | **9 из 10** верно | `tools/eval_org_synth.py` |
|
||||||
| Увидеть заранее, независимо от размера | LPLC2 — детектор надвигания | `medulla.py` |
|
| Ложные тревоги, leave-one-bag-out | **5.7 трека на км**, 8.4 % кадров | `tools/evaluate.py --mbon-dir` |
|
||||||
| Мелкий объект = 5 лучей на кадр | кольцевой аттрактор эллипсоидного тела: накопление улик | `central_complex.py` |
|
| То же без обученного считывания | 11.9 на км, 20.3 % кадров | там же, без `--mbon-dir` |
|
||||||
| На 170 м контраст к фону равен нулю | веерное тело: улики копятся в координатах мира, а не кадра | `fan_body.py` |
|
| На незнакомой линии (памяти нет) | **12.8 на км**; без считывания 36.1 | `tools/evaluate.py --no-memory` |
|
||||||
| Не реагировать на штатные конструкции | депрессия синапсов KC→MBON | `mushroom_body.py` |
|
| Вторая половина `new_data`: другой день, 3.41 км, не видена при обучении | **11.7 на км**, 17.9 % кадров | `tools/eval_new_data.py` |
|
||||||
| Сенсор закреплён нежёстко | жужжальца и оцеллии: стабилизация «взгляда» | `geometry.py` |
|
| Дальность (полигон, 15 560 наблюдений) | человек стоя: рабочая дальность **100 м**, P@50 = 0.72, P@100 = 0.52, P@150 = 0.46 | `tools/plot_benchmark.py` |
|
||||||
| 100 мс на кадр, слабый CPU | разрежённый бинарный код, 0.1 % активных клеток | `mushroom_body.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)
|
Проверка всегда **leave-one-bag-out**: память обучается на всех записях, кроме
|
||||||
и [docs/CONNECTOME.md](docs/CONNECTOME.md).
|
проверяемой. Иначе цифры лгут — подавлять конструкции, которые сам же и
|
||||||
|
запомнил, умеет кто угодно, а на приватном тесте будет новый участок.
|
||||||
|
|
||||||
---
|
---
|
||||||
|
|
||||||
## Архитектура
|
## Что честно не работает
|
||||||
|
|
||||||
```
|
Разобрано замерами, подробности — в `docs/EXPERIMENTS.md`:
|
||||||
ROS 2 bag → /lidar_points (PointCloud2, 0.3–0.9 млн точек, 10 Гц)
|
|
||||||
│
|
|
||||||
├─ RETINA омматидиальная решётка → дальностный образ 128 × N
|
|
||||||
├─ HALTERES плоскость пути: крен, тангаж, высота сенсора
|
|
||||||
├─ LAMINA диспаритет 1/R, ON/OFF, центр-окружение на 3 масштабах
|
|
||||||
├─ MEDULLA/LP T4/T5 → LPTC: скорость поезда без одометрии; LPLC2: надвигание
|
|
||||||
├─ LOBULA LC11: кандидаты; разрез по контрасту отделяет предмет от стены
|
|
||||||
├─ MUSHROOM BODY новизна: подавление знакомой обстановки тоннеля
|
|
||||||
├─ FAN-SHAPED BODY накопление лучей в координатах пути: улика там, где нет контраста
|
|
||||||
├─ CENTRAL COMPLEX накопление улик в координатах пути, треки
|
|
||||||
└─ DESCENDING два порога: предупреждение и экстренное торможение
|
|
||||||
│
|
|
||||||
▼
|
|
||||||
/flyguard/obstacle · /flyguard/markers · /flyguard/brain · /flyguard/diagnostics
|
|
||||||
```
|
|
||||||
|
|
||||||
Детально — [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
|
* `docs/ALGORITHM.md` — что делает каждая стадия и почему именно так.
|
||||||
docker build -t flyguard .
|
* `docs/EXPERIMENTS.md` — все замеры, включая отрицательные результаты.
|
||||||
```
|
* `docs/CONNECTOME.md` — что взято из коннектома как число, а что как идея.
|
||||||
|
|
||||||
Образ основан на `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.
|
|
||||||
|
|
|
||||||
|
|
@ -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
|
|
||||||
}
|
|
||||||
]
|
|
||||||
File diff suppressed because one or more lines are too long
Binary file not shown.
|
|
@ -8,7 +8,7 @@ services:
|
||||||
pipeline:
|
pipeline:
|
||||||
build:
|
build:
|
||||||
context: .
|
context: .
|
||||||
dockerfile: Dockerfile.offline
|
dockerfile: Dockerfile
|
||||||
image: flyguard:latest
|
image: flyguard:latest
|
||||||
container_name: flyguard-pipeline
|
container_name: flyguard-pipeline
|
||||||
command: ["pipeline", "--all", "--verbose"]
|
command: ["pipeline", "--all", "--verbose"]
|
||||||
|
|
@ -209,7 +209,7 @@ services:
|
||||||
cpu-test:
|
cpu-test:
|
||||||
build:
|
build:
|
||||||
context: .
|
context: .
|
||||||
dockerfile: Dockerfile.offline
|
dockerfile: Dockerfile
|
||||||
target: cpu
|
target: cpu
|
||||||
image: flyguard:cpu
|
image: flyguard:cpu
|
||||||
container_name: flyguard-cpu-test
|
container_name: flyguard-cpu-test
|
||||||
|
|
|
||||||
|
|
@ -46,11 +46,11 @@ CMD="${1:-run}"
|
||||||
case "$CMD" in
|
case "$CMD" in
|
||||||
build)
|
build)
|
||||||
echo "==> Сборка универсального образа FlyGuard ($IMAGE_NAME)..."
|
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)
|
build-cpu)
|
||||||
echo "==> Сборка облегченного CPU-образа FlyGuard (flyguard: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)
|
run|default)
|
||||||
shift 2>/dev/null || true
|
shift 2>/dev/null || true
|
||||||
|
|
|
||||||
|
|
@ -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"]
|
|
||||||
|
|
@ -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
|
|
||||||
|
|
@ -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
|
|
||||||
|
|
@ -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
|
|
||||||
|
|
@ -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"
|
|
||||||
|
|
@ -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}"
|
|
||||||
|
|
@ -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
|
|
||||||
|
|
@ -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 "$@"
|
|
||||||
|
|
@ -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>
|
|
||||||
|
|
@ -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>
|
|
||||||
|
|
@ -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()
|
|
||||||
|
|
@ -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 "$@"
|
|
||||||
|
|
@ -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 "ВИДЕОКАРТА")
|
|
||||||
|
|
@ -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
|
|
||||||
|
|
@ -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}}'
|
|
||||||
|
|
@ -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
|
|
||||||
|
|
@ -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
1
docker/wheels/.gitignore
vendored
|
|
@ -1 +0,0 @@
|
||||||
*.whl
|
|
||||||
|
|
@ -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 файлов.
|
|
||||||
После сборки папку можно очистить.
|
|
||||||
|
|
@ -194,18 +194,6 @@ LC11 у мухи — детектор мелкого объекта: возбу
|
||||||
проценты клеток Кеньона. Измерено: одна фигура вместо всех даёт 6.4 ложных трека на километр
|
проценты клеток Кеньона. Измерено: одна фигура вместо всех даёт 6.4 ложных трека на километр
|
||||||
против 9.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 — память нормального тоннеля
|
### 3.7. MUSHROOM BODY — память нормального тоннеля
|
||||||
|
|
||||||
Геометрия честно сообщает обо всём, что торчит в габарит, и вместе с препятствиями выдаёт
|
Геометрия честно сообщает обо всём, что торчит в габарит, и вместе с препятствиями выдаёт
|
||||||
|
|
@ -335,16 +323,3 @@ $$q = \underbrace{\min\!\left(\frac{n_\text{лучей}}{n_\text{ожид}(d)},1
|
||||||
компенсируются стабилизацией плоскости, но в модель движения не входят.
|
компенсируются стабилизацией плоскости, но в модель движения не входят.
|
||||||
6. **LPLC2 считается, но пока не влияет на решение** — канал надвигания подготовлен
|
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 м).
|
|
||||||
|
|
|
||||||
|
|
@ -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 мс без изменения
|
|
||||||
результата (проверено побитовым сравнением).
|
|
||||||
|
|
@ -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. Источники
|
## 4. Источники
|
||||||
|
|
||||||
* Dorkenwald S. и др. **Neuronal wiring diagram of an adult brain.** Nature, 2024 —
|
* Dorkenwald S. и др. **Neuronal wiring diagram of an adult brain.** Nature, 2024 —
|
||||||
|
|
|
||||||
|
|
@ -34,7 +34,7 @@ FlyGuard поставляется в виде универсального пр
|
||||||
|
|
||||||
### Универсальный образ с GPU-ускорением (по умолчанию)
|
### Универсальный образ с GPU-ускорением (по умолчанию)
|
||||||
```bash
|
```bash
|
||||||
docker build -t flyguard:latest -f Dockerfile.offline .
|
docker build -t flyguard:latest .
|
||||||
```
|
```
|
||||||
* **База**: `nvidia/cuda:12.4.1-runtime-ubuntu22.04`
|
* **База**: `nvidia/cuda:12.4.1-runtime-ubuntu22.04`
|
||||||
* **Python**: 3.11, PyTorch CUDA 12.1+, cuDNN, OpenMP `libgomp1`
|
* **Python**: 3.11, PyTorch CUDA 12.1+, cuDNN, OpenMP `libgomp1`
|
||||||
|
|
@ -43,7 +43,7 @@ docker build -t flyguard:latest -f Dockerfile.offline .
|
||||||
|
|
||||||
### Облегченный CPU-образ (опционально, ~200 МБ)
|
### Облегченный CPU-образ (опционально, ~200 МБ)
|
||||||
```bash
|
```bash
|
||||||
docker build --target cpu -t flyguard:cpu -f Dockerfile.offline .
|
docker build --target cpu -t flyguard:cpu .
|
||||||
```
|
```
|
||||||
|
|
||||||
---
|
---
|
||||||
|
|
|
||||||
|
|
@ -252,86 +252,6 @@ numba являются ламина и оценка движения — вме
|
||||||
проекция сократила стадию с **29 до 7 мс**, причём результат совпадает с полной
|
проекция сократила стадию с **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 м
|
## 7.3. Почему далёкий предмет теряется — и что нужно для 200 м
|
||||||
|
|
||||||
ТЗ просит 300 м как «отлично» и 200 м как «очень хорошо». Разберём честно, чего
|
ТЗ просит 300 м как «отлично» и 200 м как «очень хорошо». Разберём честно, чего
|
||||||
|
|
@ -419,7 +339,7 @@ ROS 2 и `ros2 bag play`. Счётчики берутся из самой нод
|
||||||
| Кадров с тревогой | **9.4 %** | 17.2 % |
|
| Кадров с тревогой | **9.4 %** | 17.2 % |
|
||||||
| Посторонних тревог на кадр (полигон) | **0.055** | 0.171 |
|
| Посторонних тревог на кадр (полигон) | **0.055** | 0.171 |
|
||||||
| Реальный объект на 55 м | 98.9 % | 98.9 % |
|
| Реальный объект на 55 м | 98.9 % | 98.9 % |
|
||||||
| Задержка, медиана в контейнере | 32 мс | 33 мс |
|
| Задержка, медиана | 32 мс | 33 мс |
|
||||||
|
|
||||||
**Включено по умолчанию.** Размен здесь принципиально лучше, чем у разделения фигуры
|
**Включено по умолчанию.** Размен здесь принципиально лучше, чем у разделения фигуры
|
||||||
и фона (п. 9.4): там было вчетверо больше ложных за +29 % дальности, здесь — в 1.75
|
и фона (п. 9.4): там было вчетверо больше ложных за +29 % дальности, здесь — в 1.75
|
||||||
|
|
@ -3461,384 +3381,3 @@ gpu_background=True)` в узле). Конвейер тем временем с
|
||||||
дальше ведёт те же кандидаты и решения, что процессор; упавший прогрев оставляет
|
дальше ведёт те же кандидаты и решения, что процессор; упавший прогрев оставляет
|
||||||
процессор и причину в `gpu_error`. `docker/demo_test.sh` для замера скорости
|
процессор и причину в `gpu_error`. `docker/demo_test.sh` для замера скорости
|
||||||
ждёт видеокарту (`WAIT_GPU=0` — не ждать, как на стенде).
|
ждёт видеокарту (`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
|
|
||||||
оставлены прежними.
|
|
||||||
|
|
|
||||||
Binary file not shown.
|
Before Width: | Height: | Size: 124 KiB |
Binary file not shown.
|
Before Width: | Height: | Size: 263 KiB |
|
|
@ -6,7 +6,6 @@
|
||||||
"""
|
"""
|
||||||
from __future__ import annotations
|
from __future__ import annotations
|
||||||
|
|
||||||
import logging
|
|
||||||
import re
|
import re
|
||||||
import sqlite3
|
import sqlite3
|
||||||
from contextlib import closing
|
from contextlib import closing
|
||||||
|
|
@ -16,8 +15,6 @@ from typing import Iterator
|
||||||
|
|
||||||
from .cdr import PointCloud2, parse_pointcloud2
|
from .cdr import PointCloud2, parse_pointcloud2
|
||||||
|
|
||||||
logger = logging.getLogger("flyguard.bag")
|
|
||||||
|
|
||||||
_SHARD_RE = re.compile(r"_(\d+)\.db3$")
|
_SHARD_RE = re.compile(r"_(\d+)\.db3$")
|
||||||
_POINTCLOUD_TYPE = "sensor_msgs/msg/PointCloud2"
|
_POINTCLOUD_TYPE = "sensor_msgs/msg/PointCloud2"
|
||||||
|
|
||||||
|
|
@ -47,7 +44,6 @@ class Bag:
|
||||||
self.shards = shards
|
self.shards = shards
|
||||||
self.topics = self._scan_topics()
|
self.topics = self._scan_topics()
|
||||||
self.topic = topic or self._pick_topic()
|
self.topic = topic or self._pick_topic()
|
||||||
self.n_frames_failed = 0 # обновляется по ходу последнего frames()
|
|
||||||
|
|
||||||
@staticmethod
|
@staticmethod
|
||||||
def _shard_key(p: Path) -> tuple[int, str]:
|
def _shard_key(p: Path) -> tuple[int, str]:
|
||||||
|
|
@ -87,13 +83,9 @@ class Bag:
|
||||||
"""Выдать (timestamp_ns, облако) для сообщений выбранного топика.
|
"""Выдать (timestamp_ns, облако) для сообщений выбранного топика.
|
||||||
|
|
||||||
Индексация сквозная по всему бэгу; шарды читаются по порядку, внутри
|
Индексация сквозная по всему бэгу; шарды читаются по порядку, внутри
|
||||||
шарда — по возрастанию времени. Сообщение, которое не удаётся разобрать
|
шарда — по возрастанию времени.
|
||||||
(битый CDR у реального сенсора), пропускается с предупреждением в лог,
|
|
||||||
а не останавливает чтение бэга — `start`/`stop` считают по позиции
|
|
||||||
сообщения в бэге, а не по числу успешно разобранных.
|
|
||||||
"""
|
"""
|
||||||
idx = 0
|
idx = 0
|
||||||
self.n_frames_failed = 0
|
|
||||||
for shard in self.shards:
|
for shard in self.shards:
|
||||||
# closing(), а не сам connection: у sqlite3 `with` управляет
|
# closing(), а не сам connection: у sqlite3 `with` управляет
|
||||||
# транзакцией и файл остаётся открытым — на Windows его потом
|
# транзакцией и файл остаётся открытым — на Windows его потом
|
||||||
|
|
@ -119,15 +111,7 @@ class Bag:
|
||||||
if stop is not None and idx >= stop:
|
if stop is not None and idx >= stop:
|
||||||
return
|
return
|
||||||
if idx >= start and (idx - start) % stride == 0:
|
if idx >= start and (idx - start) % stride == 0:
|
||||||
try:
|
yield ts, parse_pointcloud2(blob)
|
||||||
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
|
idx += 1
|
||||||
|
|
||||||
def describe(self) -> str:
|
def describe(self) -> str:
|
||||||
|
|
|
||||||
|
|
@ -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)
|
|
||||||
|
|
@ -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)
|
|
||||||
|
|
@ -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
|
|
||||||
|
|
@ -4,8 +4,7 @@
|
||||||
* офлайн-эксперименты на машине без ROS (Windows);
|
* офлайн-эксперименты на машине без ROS (Windows);
|
||||||
* прямое чтение rosbag внутри контейнера, минуя `ros2 bag play`.
|
* прямое чтение rosbag внутри контейнера, минуя `ros2 bag play`.
|
||||||
|
|
||||||
Внутри ROS-ноды сообщение приходит уже разобранным; его переводит в тот же
|
Внутри ROS-ноды сообщение приходит уже разобранным, и этот модуль не используется.
|
||||||
`PointCloud2` функция `from_ros_message`.
|
|
||||||
"""
|
"""
|
||||||
from __future__ import annotations
|
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,
|
return PointCloud2(stamp=sec + nsec * 1e-9, frame_id=frame_id, height=height,
|
||||||
width=width, point_step=point_step, is_dense=is_dense,
|
width=width, point_step=point_step, is_dense=is_dense,
|
||||||
points=points)
|
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)
|
|
||||||
|
|
|
||||||
|
|
@ -152,13 +152,11 @@ class CentralComplex:
|
||||||
mbon_power: float = 1.0, mbon_blend: float = 1.0,
|
mbon_power: float = 1.0, mbon_blend: float = 1.0,
|
||||||
nov_fade_from: float = 0.0, nov_fade_to: float = 160.0,
|
nov_fade_from: float = 0.0, nov_fade_to: float = 160.0,
|
||||||
lying_exempt: bool = False, top_from: float = float("inf"),
|
lying_exempt: bool = False, top_from: float = float("inf"),
|
||||||
flat: tuple[float, float, float, float] = (0.0, 0.0, 0.0, 1.0),
|
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.use_shape = use_shape
|
||||||
self.lying_exempt = lying_exempt
|
self.lying_exempt = lying_exempt
|
||||||
self.top_from = top_from # низ выше этого — предмет в верхней секции габарита
|
self.top_from = top_from # низ выше этого — предмет в верхней секции габарита
|
||||||
self.flat = flat # (высота, верх, дальность, множитель), см. _quality
|
self.flat = flat # (высота, верх, дальность, множитель), см. _quality
|
||||||
self.hover = hover # (пол, низ от, доля в габарите, |u| до, дальность до, протяжённость до, ширина от), см. _quality
|
|
||||||
self.mbon_power = mbon_power
|
self.mbon_power = mbon_power
|
||||||
self.mbon_blend = mbon_blend
|
self.mbon_blend = mbon_blend
|
||||||
self.nov_fade_from = nov_fade_from
|
self.nov_fade_from = nov_fade_from
|
||||||
|
|
@ -235,7 +233,7 @@ class CentralComplex:
|
||||||
t.misses = 0
|
t.misses = 0
|
||||||
w = _quality(c, self.use_shape, self.mbon_power, self.mbon_blend,
|
w = _quality(c, self.use_shape, self.mbon_power, self.mbon_blend,
|
||||||
self.nov_fade_from, self.nov_fade_to,
|
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.observe(c, self.s_world + c.d, w)
|
||||||
t.evidence = min(1.0, t.evidence + self.gain * w)
|
t.evidence = min(1.0, t.evidence + self.gain * w)
|
||||||
if dt > 1e-3:
|
if dt > 1e-3:
|
||||||
|
|
@ -265,7 +263,7 @@ class CentralComplex:
|
||||||
u_raw=float(c.extra.get("u_raw", c.u)))
|
u_raw=float(c.extra.get("u_raw", c.u)))
|
||||||
w = _quality(c, self.use_shape, self.mbon_power, self.mbon_blend,
|
w = _quality(c, self.use_shape, self.mbon_power, self.mbon_blend,
|
||||||
self.nov_fade_from, self.nov_fade_to,
|
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.observe(c, self.s_world + c.d, w)
|
||||||
t.evidence = self.gain * w
|
t.evidence = self.gain * w
|
||||||
t.hits = 1
|
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,
|
mbon_blend: float = 1.0, nov_fade_from: float = 0.0,
|
||||||
nov_fade_to: float = 160.0, lying_exempt: bool = False,
|
nov_fade_to: float = 160.0, lying_exempt: bool = False,
|
||||||
top_from: float = float("inf"),
|
top_from: float = float("inf"),
|
||||||
flat: tuple[float, float, float, float] = (0.0, 0.0, 0.0, 1.0),
|
flat: tuple[float, float, float, float] = (0.0, 0.0, 0.0, 1.0)) -> float:
|
||||||
hover: tuple[float, ...] = (0.0, 0.6, 0.9, 1.2, 80.0, 1.5, 0.0)) -> float:
|
|
||||||
"""Вес одного наблюдения: сколько улики оно добавляет.
|
"""Вес одного наблюдения: сколько улики оно добавляет.
|
||||||
|
|
||||||
Дальний объект даёт мало лучей не потому, что он сомнительный, а потому что
|
Дальний объект даёт мало лучей не потому, что он сомнительный, а потому что
|
||||||
|
|
@ -331,8 +328,6 @@ def _quality(c: Candidate, use_shape: bool = True, mbon_power: float = 1.0,
|
||||||
# дальности. Пояс над вагоном в исправном тоннеле пуст, поэтому нижний
|
# дальности. Пояс над вагоном в исправном тоннеле пуст, поэтому нижний
|
||||||
# край для висящего поднят до 0.5. EXPERIMENTS п. 19.
|
# край для висящего поднят до 0.5. EXPERIMENTS п. 19.
|
||||||
top = c.h_min >= top_from
|
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))
|
support = float(np.clip(c.n_rays / expected_rays(c.d), 0.5 if top else 0.25, 1.0))
|
||||||
|
|
||||||
# Контраст к фону — главная улика вблизи и недоступная вдали. На 170 м
|
# Контраст к фону — главная улика вблизи и недоступная вдали. На 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:
|
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)
|
pm = float(np.clip(p, 1e-4, 1.0)) ** max(mbon_power, 1e-3)
|
||||||
b = float(np.clip(mbon_blend, 0.0, 1.0))
|
b = float(np.clip(mbon_blend, 0.0, 1.0))
|
||||||
hand = pm ** b * max(hand, 1e-4) ** (1.0 - b)
|
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))
|
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,
|
def novelty_gain(novelty: float, lo: float = 0.15, hi: float = 0.50,
|
||||||
floor: float = 0.05) -> float:
|
floor: float = 0.05) -> float:
|
||||||
"""Ответ MBON → множитель улики.
|
"""Ответ MBON → множитель улики.
|
||||||
|
|
|
||||||
Binary file not shown.
Binary file not shown.
Binary file not shown.
|
|
@ -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
|
|
||||||
|
|
|
@ -206,15 +206,13 @@ class GpuStages:
|
||||||
if bool(t.any(dev > tol)):
|
if bool(t.any(dev > tol)):
|
||||||
return None
|
return None
|
||||||
|
|
||||||
# яркость — не обязательное поле, без неё нули (как в retina.py)
|
|
||||||
has_i = "intensity" in pts.dtype.names
|
|
||||||
if e == 1:
|
if e == 1:
|
||||||
r_near = r[..., 0]
|
r_near = r[..., 0]
|
||||||
r_far = 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]
|
valid = good[..., 0]
|
||||||
else:
|
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)
|
inf = t.full((), float("inf"), dtype=r.dtype, device=self.dev)
|
||||||
near_i = t.argmin(t.where(good, r, inf), dim=-1, keepdim=True)
|
near_i = t.argmin(t.where(good, r, inf), dim=-1, keepdim=True)
|
||||||
far_i = t.argmax(r, dim=-1, keepdim=True)
|
far_i = t.argmax(r, dim=-1, keepdim=True)
|
||||||
|
|
|
||||||
|
|
@ -292,10 +292,7 @@ def find_candidates(tf: TrackFrame, lam: LaminaOutput, corridor: Corridor, *,
|
||||||
platform_h_hi: float = 1.25,
|
platform_h_hi: float = 1.25,
|
||||||
h_top: float = 0.0, half_width_top: float = 0.0,
|
h_top: float = 0.0, half_width_top: float = 0.0,
|
||||||
top_d_max: float = 90.0, top_detect: bool = False,
|
top_d_max: float = 90.0, top_detect: bool = False,
|
||||||
top_min_rays: int = 3, cluster=None,
|
top_min_rays: int = 3, cluster=None) -> list[Candidate]:
|
||||||
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]:
|
|
||||||
"""Выделить кандидатов: связные пятна лучей, попавших в габарит.
|
"""Выделить кандидатов: связные пятна лучей, попавших в габарит.
|
||||||
|
|
||||||
Кластеризация ведётся по **расширенной** области, а членство в габарите
|
Кластеризация ведётся по **расширенной** области, а членство в габарите
|
||||||
|
|
@ -303,9 +300,6 @@ def find_candidates(tf: TrackFrame, lam: LaminaOutput, corridor: Corridor, *,
|
||||||
срезанный границей коридора, неотличим от предмета: обрезка сама создаёт
|
срезанный границей коридора, неотличим от предмета: обрезка сама создаёт
|
||||||
компактное пятно нужного размера. Доля лучей, оставшихся внутри габарита
|
компактное пятно нужного размера. Доля лучей, оставшихся внутри габарита
|
||||||
(`containment`), сразу показывает, предмет это целиком или край стены.
|
(`containment`), сразу показывает, предмет это целиком или край стены.
|
||||||
|
|
||||||
`small_rays` > 0 включает канал малых целей (`_small_targets`): пятно из
|
|
||||||
двух-трёх лучей, висящее в пустом пространстве габарита.
|
|
||||||
"""
|
"""
|
||||||
if masks is None:
|
if masks is None:
|
||||||
masks = gauge_mask(tf, corridor, half_width=half_width, h_lo=h_lo,
|
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)
|
labels, n = (cluster or cluster_by_depth)(context, tf.r, col_reach=dilate_cols + 1)
|
||||||
if n == 0:
|
if n == 0:
|
||||||
return []
|
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:
|
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),
|
need = np.where(dmean >= min_rays_far_from, float(min_rays_far),
|
||||||
float(min_rays))
|
float(min_rays))
|
||||||
keep = np.flatnonzero(inner >= need)
|
keep = np.flatnonzero(inner >= need)
|
||||||
if keep.size == 0 and not small:
|
if keep.size == 0:
|
||||||
return []
|
return []
|
||||||
if keep.size > max_candidates:
|
if keep.size > max_candidates:
|
||||||
keep = keep[np.argsort(inner[keep])[::-1][: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:
|
if cand is not None:
|
||||||
out.append(cand)
|
out.append(cand)
|
||||||
|
|
||||||
for sel, n_ctx in small:
|
if use_top:
|
||||||
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,
|
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,
|
half_width_top=half_width_top, top_d_max=top_d_max,
|
||||||
min_rays=top_min_rays, col_reach=dilate_cols + 1)
|
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
|
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,
|
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,
|
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]:
|
col_reach: int, margin_u: float = 0.3, below: float = 0.4) -> list[Candidate]:
|
||||||
|
|
|
||||||
765
flyguard/node.py
765
flyguard/node.py
|
|
@ -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()
|
|
||||||
|
|
@ -27,7 +27,7 @@ from .lobula import Candidate, cluster_by_depth, find_candidates, gauge_mask
|
||||||
from .medulla import EgoMotion, EgoMotionEstimator, EmdBank, looming
|
from .medulla import EgoMotion, EgoMotionEstimator, EmdBank, looming
|
||||||
from .mbon_readout import MbonReadout
|
from .mbon_readout import MbonReadout
|
||||||
from .mushroom_body import Habituation, HabituationConfig, MushroomBody, NoMemory
|
from .mushroom_body import Habituation, HabituationConfig, MushroomBody, NoMemory
|
||||||
from .retina import RangeImage, ScanLayout, forward_azimuth, rotate_cloud
|
from .retina import RangeImage, ScanLayout
|
||||||
|
|
||||||
|
|
||||||
@dataclass
|
@dataclass
|
||||||
|
|
@ -153,34 +153,6 @@ class Params:
|
||||||
# это тихий режим для линии с обученной памятью. EXPERIMENTS п. 12.3.
|
# это тихий режим для линии с обученной памятью. EXPERIMENTS п. 12.3.
|
||||||
nov_fade_from: float = 90.0
|
nov_fade_from: float = 90.0
|
||||||
nov_fade_to: float = 160.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 — не
|
# дальностью из логита вычитается типичный для полосы. 0 — не
|
||||||
# поправлять. Замер — EXPERIMENTS п. 12.3.
|
# поправлять. Замер — EXPERIMENTS п. 12.3.
|
||||||
|
|
@ -326,9 +298,6 @@ class FlyGuard:
|
||||||
self._track_readout = track_readout
|
self._track_readout = track_readout
|
||||||
self._init_temporal()
|
self._init_temporal()
|
||||||
self._calib: list[PointCloud2] = []
|
self._calib: list[PointCloud2] = []
|
||||||
# Поворот облака вокруг вертикали, если у драйвера «вперёд» — не −Y
|
|
||||||
# (определяется по дальним эхам при калибровке, см. forward_azimuth)
|
|
||||||
self.forward_deg = 0.0
|
|
||||||
self.frames_seen = 0
|
self.frames_seen = 0
|
||||||
self._last_stamp: float | None = None
|
self._last_stamp: float | None = None
|
||||||
self.time_jumps = 0 # сколько раз время записи скакало
|
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
|
top_from=(self.p.h_hi if self.p.h_top > self.p.h_hi
|
||||||
else float("inf")),
|
else float("inf")),
|
||||||
flat=(self.p.flat_h, self.p.flat_top, self.p.flat_d,
|
flat=(self.p.flat_h, self.p.flat_top, self.p.flat_d,
|
||||||
self.p.flat_w),
|
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(
|
self.dn = (DescendingNeurons(
|
||||||
warn_evidence=self.p.warn_evidence,
|
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:
|
def _ensure_layout(self, pc: PointCloud2) -> bool:
|
||||||
"""Накопить кадры и откалибровать решётку. False — ещё не готов."""
|
"""Накопить кадры и откалибровать решётку. False — ещё не готов."""
|
||||||
if self.layout is not None:
|
if self.layout is not None:
|
||||||
|
|
@ -451,9 +411,6 @@ class FlyGuard:
|
||||||
self._calib.append(pc)
|
self._calib.append(pc)
|
||||||
if len(self._calib) < self.p.calib_frames:
|
if len(self._calib) < self.p.calib_frames:
|
||||||
return False
|
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.layout_full = ScanLayout.calibrate(self._calib)
|
||||||
self._calib.clear()
|
self._calib.clear()
|
||||||
self.cols = self.layout_full.column_slice(self.p.fov_deg)
|
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:
|
def process(self, pc: PointCloud2, *, keep_debug: bool = False) -> FrameResult | None:
|
||||||
"""Обработать один кадр. None, пока идёт калибровка решётки."""
|
"""Обработать один кадр. None, пока идёт калибровка решётки."""
|
||||||
t = _Timer()
|
t = _Timer()
|
||||||
rotated = bool(self.forward_deg)
|
|
||||||
if rotated:
|
|
||||||
pc = rotate_cloud(pc, self.forward_deg)
|
|
||||||
if not self._ensure_layout(pc):
|
if not self._ensure_layout(pc):
|
||||||
return None
|
return None
|
||||||
if self.forward_deg and not rotated:
|
|
||||||
# поворот найден на этом кадре при калибровке: сам кадр ещё не повёрнут
|
|
||||||
pc = rotate_cloud(pc, self.forward_deg)
|
|
||||||
# Скачок времени записи — перемотка, `ros2 bag play --loop`, разрыв в
|
# Скачок времени записи — перемотка, `ros2 bag play --loop`, разрыв в
|
||||||
# записи. Треки, одометрия и ось пути относятся к другому месту, и
|
# записи. Треки, одометрия и ось пути относятся к другому месту, и
|
||||||
# с ними предмет с прошлого круга всплыл бы там, где его нет.
|
# с ними предмет с прошлого круга всплыл бы там, где его нет.
|
||||||
|
|
@ -603,14 +554,7 @@ class FlyGuard:
|
||||||
half_width_top=self.p.half_width_top,
|
half_width_top=self.p.half_width_top,
|
||||||
top_d_max=self.p.top_d_max,
|
top_d_max=self.p.top_d_max,
|
||||||
top_detect=self.p.top_detect,
|
top_detect=self.p.top_detect,
|
||||||
cluster=self._cluster,
|
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"):
|
with t("mushroom"):
|
||||||
cands = self.memory.annotate(cands)
|
cands = self.memory.annotate(cands)
|
||||||
|
|
|
||||||
|
|
@ -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()
|
|
||||||
|
|
@ -219,16 +219,13 @@ class ScanLayout:
|
||||||
if np.any(dev > r * np.float32(ORDER_TOL_DEG * DEG) + np.float32(1e-3)):
|
if np.any(dev > r * np.float32(ORDER_TOL_DEG * DEG) + np.float32(1e-3)):
|
||||||
return None
|
return None
|
||||||
|
|
||||||
# Яркость — не обязательное поле: у драйвера без неё облако всё равно
|
|
||||||
# раскладывается, а яркость считается нулевой (как на медленном пути)
|
|
||||||
has_i = "intensity" in pts.dtype.names
|
|
||||||
if e == 1:
|
if e == 1:
|
||||||
r_near = r[..., 0]
|
r_near = r[..., 0]
|
||||||
r_far = 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]
|
valid = good[..., 0]
|
||||||
else:
|
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]
|
near_i = np.argmin(np.where(good, r, np.float32(np.inf)), axis=-1)[..., None]
|
||||||
far_i = np.argmax(r, axis=-1)[..., None]
|
far_i = np.argmax(r, axis=-1)[..., None]
|
||||||
r_near = np.take_along_axis(r, near_i, -1)[..., 0]
|
r_near = np.take_along_axis(r, near_i, -1)[..., 0]
|
||||||
|
|
@ -332,13 +329,10 @@ class ScanLayout:
|
||||||
if not clouds:
|
if not clouds:
|
||||||
raise ValueError("нужен хотя бы один кадр для калибровки")
|
raise ValueError("нужен хотя бы один кадр для калибровки")
|
||||||
pc0 = clouds[0]
|
pc0 = clouds[0]
|
||||||
if not _ring_ordered(pc0):
|
if "ring" not in pc0.points.dtype.names:
|
||||||
# Без поля `ring` (синтетика организаторов) или с ним, но не
|
# Без поля `ring` (синтетика организаторов): кольца — по гистограмме
|
||||||
# «столбец за столбцом»: драйвер отдаёт только точки с эхом,
|
# элевации, а в калибровку по порядку точек идут только кадры, где
|
||||||
# раскладывает кадр по кольцам или перемешивает. Кольца — по
|
# этот порядок цел. Нет таких — решётка строится целиком по углам.
|
||||||
# гистограмме элевации, а в калибровку по порядку точек идут только
|
|
||||||
# кадры, где этот порядок цел. Нет таких — решётка строится целиком
|
|
||||||
# по углам.
|
|
||||||
el_ring = _ring_elevations(clouds)
|
el_ring = _ring_elevations(clouds)
|
||||||
if n_rings is None:
|
if n_rings is None:
|
||||||
n_rings = el_ring.size
|
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]
|
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:
|
def _is_organized(pc: PointCloud2, el_ring: np.ndarray) -> bool:
|
||||||
"""Цел ли порядок точек: элевация каждой точки = элевация её кольца."""
|
"""Цел ли порядок точек: элевация каждой точки = элевация её кольца."""
|
||||||
n = el_ring.size
|
n = el_ring.size
|
||||||
|
|
|
||||||
|
|
@ -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, ""
|
|
||||||
|
|
@ -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 — так во всех тестах
|
|
||||||
|
|
@ -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
|
|
||||||
|
|
@ -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)
|
|
||||||
|
|
@ -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
|
|
||||||
|
|
@ -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"
|
|
||||||
|
|
@ -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)
|
|
||||||
|
|
@ -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)
|
|
||||||
|
|
@ -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)
|
|
||||||
|
|
@ -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
|
|
||||||
|
|
@ -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)
|
|
||||||
|
|
@ -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))
|
|
||||||
Binary file not shown.
Binary file not shown.
Binary file not shown.
|
|
@ -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,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
|
|
||||||
|
|
@ -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)
|
|
||||||
|
|
@ -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
|
|
||||||
|
|
@ -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)
|
|
||||||
|
|
@ -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))
|
|
||||||
|
|
@ -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())
|
|
||||||
|
|
@ -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)
|
|
||||||
|
|
@ -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
|
|
||||||
|
|
@ -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
|
|
||||||
|
|
@ -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)
|
|
||||||
|
|
@ -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
|
|
||||||
|
|
@ -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()
|
|
||||||
|
|
@ -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
|
|
||||||
|
|
@ -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()
|
|
||||||
|
|
@ -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
|
|
||||||
|
|
@ -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, ""
|
|
||||||
|
|
@ -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))
|
|
||||||
|
|
@ -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)
|
|
||||||
|
|
@ -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])
|
|
||||||
|
|
@ -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>
|
|
||||||
|
|
@ -1,4 +0,0 @@
|
||||||
[develop]
|
|
||||||
script_dir=$base/lib/flyguard
|
|
||||||
[install]
|
|
||||||
install_scripts=$base/lib/flyguard
|
|
||||||
|
|
@ -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",
|
|
||||||
],
|
|
||||||
},
|
|
||||||
)
|
|
||||||
File diff suppressed because it is too large
Load diff
|
|
@ -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()
|
|
||||||
|
|
@ -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, если стоим)
|
|
||||||
|
|
@ -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 # все подтверждённые объекты, ближний первым
|
|
||||||
|
|
@ -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>
|
|
||||||
|
|
@ -4,12 +4,10 @@
|
||||||
выпрямление скоса каналов, геометрия плоскости пути, связность с учётом
|
выпрямление скоса каналов, геометрия плоскости пути, связность с учётом
|
||||||
глубины, кодирование грибовидного тела и вставка синтетического предмета.
|
глубины, кодирование грибовидного тела и вставка синтетического предмета.
|
||||||
|
|
||||||
pytest tests
|
pytest ros2_ws/src/flyguard/test
|
||||||
"""
|
"""
|
||||||
from __future__ import annotations
|
from __future__ import annotations
|
||||||
|
|
||||||
import sqlite3
|
|
||||||
import struct
|
|
||||||
import sys
|
import sys
|
||||||
from pathlib import Path
|
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)
|
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):
|
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))
|
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):
|
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="датасет не распакован")
|
@pytest.mark.skipif(not DATA.exists(), reason="датасет не распакован")
|
||||||
def test_real_bag_projects_without_angular_error():
|
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)
|
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 лежит рядом с тестами
|
_TOOLS = ROOT / "tools" # выгрузка: tools лежит рядом с тестами
|
||||||
|
|
@ -885,106 +622,6 @@ def test_gauge_mask_filters_platform_surface():
|
||||||
assert inside[1] # препятствие на колее сохранено
|
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():
|
def test_descending_far_soft_warning_triggers_without_emergency():
|
||||||
"""На 130 м устойчивый трек поднимает предупреждение, но не экстренное торможение."""
|
"""На 130 м устойчивый трек поднимает предупреждение, но не экстренное торможение."""
|
||||||
from flyguard.central_complex import CentralComplex, Track
|
from flyguard.central_complex import CentralComplex, Track
|
||||||
|
|
|
||||||
|
|
@ -8,9 +8,9 @@ from pathlib import Path
|
||||||
ROOT = Path(__file__).resolve().parents[1]
|
ROOT = Path(__file__).resolve().parents[1]
|
||||||
PKG = ROOT
|
PKG = ROOT
|
||||||
# Записи лидара в репозиторий не кладутся (десятки гигабайт). По умолчанию
|
# Записи лидара в репозиторий не кладутся (десятки гигабайт). По умолчанию
|
||||||
# ищем их рядом: сначала внутри репозитория, потом на уровень выше — так
|
# ищем их рядом: сначала внутри выгрузки, потом на уровень выше — так работает
|
||||||
# работает и у того, кто держит клон внутри основного проекта, и у того, кто
|
# и у того, кто держит выгрузку внутри основного проекта, и у того, кто
|
||||||
# распаковал его отдельно. Переопределяется переменной FLYGUARD_DATA.
|
# распаковал её отдельно. Переопределяется переменной FLYGUARD_DATA.
|
||||||
_env = os.environ.get("FLYGUARD_DATA")
|
_env = os.environ.get("FLYGUARD_DATA")
|
||||||
if _env:
|
if _env:
|
||||||
DATA = Path(_env)
|
DATA = Path(_env)
|
||||||
|
|
|
||||||
|
|
@ -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()
|
|
||||||
|
|
@ -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
Loading…
Reference in a new issue