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

висящие предметы в габарите, устойчивость к формату облака, замер ресурсов, быстрый старт

Browse source
This commit is contained in:
Данил Омелечко 2026-09-27 01:49:31 +03:00
parent b10f419693
commit e241bec58d
21 changed files with 1074 additions and 36 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

62
README.md
View file

@ -7,6 +7,44 @@
На вход — поток облаков точек Hesai Pandar128. На выход — ответ на единственный важный вопрос: На вход — поток облаков точек Hesai Pandar128. На выход — ответ на единственный важный вопрос:
**«путь свободен» или «впереди препятствие на N метров»**. **«путь свободен» или «впереди препятствие на N метров»**.
## Быстрый старт для жюри
Образ — файлом, если на стенде нет сети (архив — в разделе «Стенд без интернета»), или
сборкой из репозитория там, где сеть есть:
```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`, результат тот же.
- Топик с облаком узел находит сам, какое бы имя у него ни было.
- `--ipc host` обязателен: без него кадры с хоста до контейнера не доходят вовсе.
- Результат для программ — топики `/flyguard/obstacle`, `/flyguard/detected`,
`/flyguard/distance` (раздел «Выходные данные»); RViz и схема мозга мухи — раздел
«Демонстрация со схемой мозга мухи».
--- ---
## В двух словах ## В двух словах
@ -301,6 +339,25 @@ ros2 topic echo /flyguard/obstacle
До этой правки узел на синтетике не обработал бы ни одного кадра: калибровка решётки До этой правки узел на синтетике не обработал бы ни одного кадра: калибровка решётки
требовала поле `ring`. требовала поле `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 ```bash
@ -384,6 +441,7 @@ ros2 launch flyguard detect.launch.py fov_deg:=35.0 half_width:=1.5
| `half_width` | `1.2` | полуширина габарита, м. Так его задают организаторы в синтетике 24.09: «у края» — до 1.13 м от оси, «вне габарита, но близко» — с 1.14 м. При прежних 1.6 второй давал ложную тревогу, см. [EXPERIMENTS](docs/EXPERIMENTS.md) п. 18.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_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` — выключить | | `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 |
| `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` — выключить | | `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 | | `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` | зона поиска по дальности, м | | `d_min` / `d_max` | `4.0` / `220.0` | зона поиска по дальности, м |
@ -419,12 +477,13 @@ ros2 launch flyguard detect.launch.py fov_deg:=35.0 half_width:=1.5
| Метрика | Значение | | Метрика | Значение |
|---|---| |---|---|
| Реальный объект (0.67 × 1.35 м на 55 м) | обнаружен в **99.5 %** кадров | | Реальный объект (0.67 × 1.35 м на 55 м) | обнаружен в **99.5 %** кадров |
| Синтетика организаторов (`cloud_with_fake_obj`, 10 предметов) | **9 из 10** верно: из восьми предметов в габарите найдены семь (2×2 посередине — с 95 м, 2×2 сверху — с 87 м, длинный на рельсах — с 79 м, 0.3 на рельсе — с 76 м, стержень 5 см с потолка — только с 13 м), оба предмета вне габарита — без тревоги; не найден 2×2, заходящий в габарит на 0.3 м. Ложных тревог за 151 с — три коротких (`tools/eval_org_synth.py`, EXPERIMENTS п. 18–19) | | Синтетика организаторов (`cloud_with_fake_obj`, 10 предметов) | **9 из 10** верно: из восьми предметов в габарите найдены семь (2×2 посередине — с 95 м, 2×2 сверху — с 87 м, длинный на рельсах — с 79 м, 0.3 на рельсе — с 76 м, 0.3 у края — с 63 м, 0.3 висящий посередине — с 45 м, стержень 5 см с потолка — только с 13 м), оба предмета вне габарита — без тревоги; не найден 2×2, заходящий в габарит на 0.3 м. Ложных тревог за 151 с — три коротких (`tools/eval_org_synth.py`, EXPERIMENTS п. 18–19) |
| Ложные тревоги, leave-one-bag-out | **5.7 разных ложных трека на километр** (8.4 % кадров) — без обученного считывания 11.9 | | Ложные тревоги, leave-one-bag-out | **5.7 разных ложных трека на километр** (8.4 % кадров) — без обученного считывания 11.9 |
| То же на незнакомой линии, памяти нет | **12.8 на км** (20.7 % кадров) — без считывания 36.1 | | То же на незнакомой линии, памяти нет | **12.8 на км** (20.7 % кадров) — без считывания 36.1 |
| То же на второй половине `new_data` — другой день, 3.41 км, ни в каком виде не видена при обучении, памяти нет | **11.7 на км** (17.9 % кадров) | | То же на второй половине `new_data` — другой день, 3.41 км, ни в каком виде не видена при обучении, памяти нет | **11.7 на км** (17.9 % кадров) |
| Время обработки кадра | итоговый образ в контейнере: `doubleT_obstacle` **33.4 / 37.9 мс** (медиана / p95), синтетика **43.5 / 48.8 мс** (требование 100 мс) | | Время обработки кадра | итоговый образ в контейнере: `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 | | То же на ядре уровня стенда жюри (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** | | Приём в контейнере | **201 из 201** и **252 из 252**, отброшено **0** |
| Рабочая дальность, размеченный полигон | **100 м** для человека стоя, **122 м** сидя, **100 м** для чемодана, 20 м для человека лёжа | | Рабочая дальность, размеченный полигон | **100 м** для человека стоя, **122 м** сидя, **100 м** для чемодана, 20 м для человека лёжа |
| Обнаружение на 50 м (при видимости) | **0.72** человек стоя; **0.50** человек лёжа — против 0.27 до пола в колее | | Обнаружение на 50 м (при видимости) | **0.72** человек стоя; **0.50** человек лёжа — против 0.27 до пола в колее |
@ -440,6 +499,7 @@ ros2 launch flyguard detect.launch.py fov_deg:=35.0 half_width:=1.5
| Вклад дальнего порога тревоги | P@150 0.33 → 0.37, человек сидя 0.22 → 0.24; ложных на знакомой линии 8.0 → 8.0, на незнакомой 20.3 → 21.3 | | Вклад дальнего порога тревоги | 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.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 | | Вклад габарита 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 |
| Вклад сброса при скачке времени | вторая половина `new_data` (70 с выпавших кадров): 17.4 → 13.8 ложного трека на км; на записях без разрывов не меняет ничего. EXPERIMENTS п. 18.9 | | Вклад сброса при скачке времени | вторая половина `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 | | Вклад штрафа за плоское у пола и прохода по верхней секции | синтетика 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 | | Вклад пола в колее (с переобучением считывания) | лёжа 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 |

2
artifacts/benchmark.json
View file

File diff suppressed because one or more lines are too long

45
docker/demo.sh Normal file
View file

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

71
docker/usage_test.sh Normal file
View file

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

119
docs/EXPERIMENTS.md
View file

@ -3528,3 +3528,122 @@ gpu_background=True)` в узле). Конвейер тем временем с
`rclpy` (`ModuleNotFoundError: No module named 'rclpy'`), так что живой `rclpy` (`ModuleNotFoundError: No module named 'rclpy'`), так что живой
прогон `ros2 bag play` с узлом (п. 22) не выполним здесь; логика узла прогон `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
flyguard/central_complex.py
View file

@ -152,11 +152,13 @@ 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
@ -233,7 +235,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.lying_exempt, self.top_from, self.flat, self.hover)
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:
@ -263,7 +265,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.lying_exempt, self.top_from, self.flat, self.hover)
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
@ -309,7 +311,8 @@ 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)) -> float: 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:
"""Вес одного наблюдения: сколько улики оно добавляет. """Вес одного наблюдения: сколько улики оно добавляет.
Дальний объект даёт мало лучей не потому, что он сомнительный, а потому что Дальний объект даёт мало лучей не потому, что он сомнительный, а потому что
@ -387,6 +390,21 @@ 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)

6
flyguard/gpu.py
View file

@ -206,13 +206,15 @@ 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] it = cube("intensity")[..., 0] if has_i else t.zeros_like(r_near)
valid = good[..., 0] valid = good[..., 0]
else: else:
inten = cube("intensity") inten = cube("intensity") if has_i else t.zeros_like(r)
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)

12
flyguard/node.py
View file

@ -65,6 +65,8 @@ class FlyGuardNode(Node):
("h_hi", 2.3), ("h_hi", 2.3),
("h_top", 3.3), ("h_top", 3.3),
("half_width_top", 1.0), ("half_width_top", 1.0),
# висящее посреди габарита: пол вероятности считывания, 0 — выключить
("hover_floor", 0.5),
("top_d_max", 90.0), ("top_d_max", 90.0),
("h_lo_core", 0.16), ("h_lo_core", 0.16),
("core_from", 30.0), ("core_from", 30.0),
@ -101,6 +103,7 @@ class FlyGuardNode(Node):
h_lo=float(self.par["h_lo"]), h_hi=float(self.par["h_hi"]), h_lo=float(self.par["h_lo"]), h_hi=float(self.par["h_hi"]),
h_top=float(self.par["h_top"]), h_top=float(self.par["h_top"]),
half_width_top=float(self.par["half_width_top"]), half_width_top=float(self.par["half_width_top"]),
hover_floor=float(self.par["hover_floor"]),
top_d_max=float(self.par["top_d_max"]), top_d_max=float(self.par["top_d_max"]),
h_lo_core=float(self.par["h_lo_core"]), h_lo_core=float(self.par["h_lo_core"]),
core_from=float(self.par["core_from"]), core_from=float(self.par["core_from"]),
@ -227,6 +230,7 @@ class FlyGuardNode(Node):
self._alarm = False # была ли тревога на прошлом кадре self._alarm = False # была ли тревога на прошлом кадре
self._alarm_frames = 0 self._alarm_frames = 0
self._jumps = 0 # скачки времени записи, о которых уже сказано self._jumps = 0 # скачки времени записи, о которых уже сказано
self._told_forward = False # сказано ли, что облако пришлось поворачивать
self._latest = None self._latest = None
self._lock = threading.Lock() self._lock = threading.Lock()
@ -343,10 +347,16 @@ class FlyGuardNode(Node):
res = self.fg.process(pc, keep_debug=need_debug) res = self.fg.process(pc, keep_debug=need_debug)
if res is None: if res is None:
self.get_logger().info( self.get_logger().info(
f"калибровка решётки лучей: {self.fg.frames_seen}/{self.fg.p.calib_frames}", f"калибровка решётки лучей: {self.fg.calib_progress}/{self.fg.p.calib_frames}",
throttle_duration_sec=2.0) throttle_duration_sec=2.0)
return 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: if self.fg.time_jumps != self._jumps:
self._jumps = self.fg.time_jumps self._jumps = self.fg.time_jumps
self._alarm = False self._alarm = False

39
flyguard/pipeline.py
View file

@ -27,7 +27,7 @@ from .lobula import Candidate, cluster_by_depth, find_candidates, gauge_mask
from .medulla import EgoMotion, EgoMotionEstimator, EmdBank, looming from .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 from .retina import RangeImage, ScanLayout, forward_azimuth, rotate_cloud
@dataclass @dataclass
@ -153,6 +153,20 @@ 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 # ширина от, м: уже — провод, кронштейн, тонкая деталь
# Поправка оценки модели на распространённость предметов: за этой # Поправка оценки модели на распространённость предметов: за этой
# дальностью из логита вычитается типичный для полосы. 0 — не # дальностью из логита вычитается типичный для полосы. 0 — не
# поправлять. Замер — EXPERIMENTS п. 12.3. # поправлять. Замер — EXPERIMENTS п. 12.3.
@ -298,6 +312,9 @@ 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 # сколько раз время записи скакало
@ -332,7 +349,11 @@ 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,
@ -403,6 +424,11 @@ 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:
@ -411,6 +437,9 @@ 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)
@ -467,8 +496,14 @@ 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`, разрыв в
# записи. Треки, одометрия и ось пути относятся к другому месту, и # записи. Треки, одометрия и ось пути относятся к другому месту, и
# с ними предмет с прошлого круга всплыл бы там, где его нет. # с ними предмет с прошлого круга всплыл бы там, где его нет.

77
flyguard/retina.py
View file

@ -219,13 +219,16 @@ 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] it = cube("intensity")[..., 0] if has_i else np.zeros_like(r_near)
valid = good[..., 0] valid = good[..., 0]
else: else:
inten = cube("intensity") 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] 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]
@ -329,10 +332,13 @@ class ScanLayout:
if not clouds: if not clouds:
raise ValueError("нужен хотя бы один кадр для калибровки") raise ValueError("нужен хотя бы один кадр для калибровки")
pc0 = clouds[0] pc0 = clouds[0]
if "ring" not in pc0.points.dtype.names: if not _ring_ordered(pc0):
# Без поля `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
@ -553,6 +559,65 @@ 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

2
flyguard/ros_conv.py
View file

@ -29,7 +29,7 @@ def from_ros(msg) -> PointCloud2:
def has_required_fields(msg) -> tuple[bool, str]: def has_required_fields(msg) -> tuple[bool, str]:
"""Проверить, что в облаке есть всё необходимое конвейеру.""" """Проверить, что в облаке есть всё необходимое конвейеру."""
names = {f.name for f in msg.fields} names = {f.name for f in msg.fields}
need = {"x", "y", "z", "intensity"} need = {"x", "y", "z"} # яркость не обязательна: без неё нули
missing = need - names missing = need - names
if missing: if missing:
return False, f"в облаке нет полей: {', '.join(sorted(missing))}" return False, f"в облаке нет полей: {', '.join(sorted(missing))}"

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

@ -36,6 +36,11 @@ flyguard:
h_top: 3.3 h_top: 3.3
half_width_top: 1.0 half_width_top: 1.0
top_d_max: 90.0 top_d_max: 90.0
# Висящее посреди габарита, ближе 80 м: пол вероятности считывания MBON.
# Модель учили на стоящих предметах, и предмет в воздухе она гасит; куб
# 0.3 м на высоте 1.2 м синтетики находится с 63 м вместо 31, ложных
# тревог столько же. 0 — выключить. EXPERIMENTS п. 24.
hover_floor: 0.5
# Пол между рельсами (|u| <= 0.85 м) опущен, но только дальше core_from: # Пол между рельсами (|u| <= 0.85 м) опущен, но только дальше core_from:
# лежащий человек высотой 0.30 м при поле 0.28 м виден верхушкой в два # лежащий человек высотой 0.30 м при поле 0.28 м виден верхушкой в два
# сантиметра. Ближе пол прежний — там в полосу 0.16…0.28 м попадают # сантиметра. Ближе пол прежний — там в полосу 0.16…0.28 м попадают

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

@ -152,11 +152,13 @@ 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
@ -233,7 +235,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.lying_exempt, self.top_from, self.flat, self.hover)
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:
@ -263,7 +265,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.lying_exempt, self.top_from, self.flat, self.hover)
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
@ -309,7 +311,8 @@ 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)) -> float: 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:
"""Вес одного наблюдения: сколько улики оно добавляет. """Вес одного наблюдения: сколько улики оно добавляет.
Дальний объект даёт мало лучей не потому, что он сомнительный, а потому что Дальний объект даёт мало лучей не потому, что он сомнительный, а потому что
@ -387,6 +390,21 @@ 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)

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

@ -206,13 +206,15 @@ 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] it = cube("intensity")[..., 0] if has_i else t.zeros_like(r_near)
valid = good[..., 0] valid = good[..., 0]
else: else:
inten = cube("intensity") inten = cube("intensity") if has_i else t.zeros_like(r)
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)

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

@ -65,6 +65,8 @@ class FlyGuardNode(Node):
("h_hi", 2.3), ("h_hi", 2.3),
("h_top", 3.3), ("h_top", 3.3),
("half_width_top", 1.0), ("half_width_top", 1.0),
# висящее посреди габарита: пол вероятности считывания, 0 — выключить
("hover_floor", 0.5),
("top_d_max", 90.0), ("top_d_max", 90.0),
("h_lo_core", 0.16), ("h_lo_core", 0.16),
("core_from", 30.0), ("core_from", 30.0),
@ -101,6 +103,7 @@ class FlyGuardNode(Node):
h_lo=float(self.par["h_lo"]), h_hi=float(self.par["h_hi"]), h_lo=float(self.par["h_lo"]), h_hi=float(self.par["h_hi"]),
h_top=float(self.par["h_top"]), h_top=float(self.par["h_top"]),
half_width_top=float(self.par["half_width_top"]), half_width_top=float(self.par["half_width_top"]),
hover_floor=float(self.par["hover_floor"]),
top_d_max=float(self.par["top_d_max"]), top_d_max=float(self.par["top_d_max"]),
h_lo_core=float(self.par["h_lo_core"]), h_lo_core=float(self.par["h_lo_core"]),
core_from=float(self.par["core_from"]), core_from=float(self.par["core_from"]),
@ -227,6 +230,7 @@ class FlyGuardNode(Node):
self._alarm = False # была ли тревога на прошлом кадре self._alarm = False # была ли тревога на прошлом кадре
self._alarm_frames = 0 self._alarm_frames = 0
self._jumps = 0 # скачки времени записи, о которых уже сказано self._jumps = 0 # скачки времени записи, о которых уже сказано
self._told_forward = False # сказано ли, что облако пришлось поворачивать
self._latest = None self._latest = None
self._lock = threading.Lock() self._lock = threading.Lock()
@ -343,10 +347,16 @@ class FlyGuardNode(Node):
res = self.fg.process(pc, keep_debug=need_debug) res = self.fg.process(pc, keep_debug=need_debug)
if res is None: if res is None:
self.get_logger().info( self.get_logger().info(
f"калибровка решётки лучей: {self.fg.frames_seen}/{self.fg.p.calib_frames}", f"калибровка решётки лучей: {self.fg.calib_progress}/{self.fg.p.calib_frames}",
throttle_duration_sec=2.0) throttle_duration_sec=2.0)
return 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: if self.fg.time_jumps != self._jumps:
self._jumps = self.fg.time_jumps self._jumps = self.fg.time_jumps
self._alarm = False self._alarm = False

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

@ -27,7 +27,7 @@ from .lobula import Candidate, cluster_by_depth, find_candidates, gauge_mask
from .medulla import EgoMotion, EgoMotionEstimator, EmdBank, looming from .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 from .retina import RangeImage, ScanLayout, forward_azimuth, rotate_cloud
@dataclass @dataclass
@ -153,6 +153,20 @@ 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 # ширина от, м: уже — провод, кронштейн, тонкая деталь
# Поправка оценки модели на распространённость предметов: за этой # Поправка оценки модели на распространённость предметов: за этой
# дальностью из логита вычитается типичный для полосы. 0 — не # дальностью из логита вычитается типичный для полосы. 0 — не
# поправлять. Замер — EXPERIMENTS п. 12.3. # поправлять. Замер — EXPERIMENTS п. 12.3.
@ -298,6 +312,9 @@ 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 # сколько раз время записи скакало
@ -332,7 +349,11 @@ 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,
@ -403,6 +424,11 @@ 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:
@ -411,6 +437,9 @@ 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)
@ -467,8 +496,14 @@ 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`, разрыв в
# записи. Треки, одометрия и ось пути относятся к другому месту, и # записи. Треки, одометрия и ось пути относятся к другому месту, и
# с ними предмет с прошлого круга всплыл бы там, где его нет. # с ними предмет с прошлого круга всплыл бы там, где его нет.

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

@ -219,13 +219,16 @@ 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] it = cube("intensity")[..., 0] if has_i else np.zeros_like(r_near)
valid = good[..., 0] valid = good[..., 0]
else: else:
inten = cube("intensity") 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] 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]
@ -329,10 +332,13 @@ class ScanLayout:
if not clouds: if not clouds:
raise ValueError("нужен хотя бы один кадр для калибровки") raise ValueError("нужен хотя бы один кадр для калибровки")
pc0 = clouds[0] pc0 = clouds[0]
if "ring" not in pc0.points.dtype.names: if not _ring_ordered(pc0):
# Без поля `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
@ -553,6 +559,65 @@ 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

2
ros2_ws/src/flyguard/flyguard/ros_conv.py
View file

@ -29,7 +29,7 @@ def from_ros(msg) -> PointCloud2:
def has_required_fields(msg) -> tuple[bool, str]: def has_required_fields(msg) -> tuple[bool, str]:
"""Проверить, что в облаке есть всё необходимое конвейеру.""" """Проверить, что в облаке есть всё необходимое конвейеру."""
names = {f.name for f in msg.fields} names = {f.name for f in msg.fields}
need = {"x", "y", "z", "intensity"} need = {"x", "y", "z"} # яркость не обязательна: без неё нули
missing = need - names missing = need - names
if missing: if missing:
return False, f"в облаке нет полей: {', '.join(sorted(missing))}" return False, f"в облаке нет полей: {', '.join(sorted(missing))}"

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

@ -279,7 +279,16 @@ def test_ros_bridge_reports_missing_fields():
msg = _FakeMsg(np.zeros(4, np.dtype([("x", "f4")])), [_FakeField("x", 0, 7, 1)], 4) msg = _FakeMsg(np.zeros(4, np.dtype([("x", "f4")])), [_FakeField("x", 0, 7, 1)], 4)
ok, why = ros_conv.has_required_fields(msg) ok, why = ros_conv.has_required_fields(msg)
assert not ok and "intensity" in why 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, "")
# --------------------------------------------------------- фигура и фон, привыкание # --------------------------------------------------------- фигура и фон, привыкание
@ -570,6 +579,94 @@ 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 лежит рядом с тестами

99
tests/test_pipeline.py
View file

@ -281,7 +281,16 @@ def test_ros_bridge_reports_missing_fields():
msg = _FakeMsg(np.zeros(4, np.dtype([("x", "f4")])), [_FakeField("x", 0, 7, 1)], 4) msg = _FakeMsg(np.zeros(4, np.dtype([("x", "f4")])), [_FakeField("x", 0, 7, 1)], 4)
ok, why = ros_conv.has_required_fields(msg) ok, why = ros_conv.has_required_fields(msg)
assert not ok and "intensity" in why 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, "")
# --------------------------------------------------------- фигура и фон, привыкание # --------------------------------------------------------- фигура и фон, привыкание
@ -572,6 +581,94 @@ 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 лежит рядом с тестами

284
tools/check_formats.py Normal file
View file

@ -0,0 +1,284 @@
"""Устойчивость к формату облака: тот же проезд в других обличьях.
Контрольная запись может прийти не в том виде, что выданные: другой драйвер,
другая раскладка, другой порядок и типы полей. Инструмент берёт настоящие
кадры, переписывает их, прогоняет через 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()