аугментации бенчмарка, зонный пол в колее, фильтр платформы, экспорт RViz
- tools/make_benchmark.py: аугментации спавна d_start ∈ [40, 200] м, боковой дрейф v_lat, шум продольной координаты δs (устранение инверсии s_std), сценарии стоянки; - flyguard/lobula.py: зонный пол h_lo_core = 0.16 м в межрельсовой колее, динамическое расширение габарита в кривых W_eff(d) по Corridor.sigma(d), отсечение плоскости настила платформы; - flyguard/central_complex.py: поддержка лежащих препятствий в колее без штрафа за вытянутость формы; - flyguard/synth.py: добавлен класс «человек_лежа» (1.8×0.5×0.3 м); - flyguard/descending.py: дальний мягкий канал предупреждения на дистанциях >90 м; - flyguard/export.py: экспорт детекций в 3D BBox, уровни угрозы, маркеры RViz MarkerArray; - tests/test_pipeline.py, tests/run_tests.py: 38 юнит-тестов и автономный раннер.
This commit is contained in:
parent
3f8314e908
commit
062bdfd68c
10 changed files with 946 additions and 30 deletions
254
docs/IMPROVEMENT_PLAN.md
Normal file
254
docs/IMPROVEMENT_PLAN.md
Normal file
|
|
@ -0,0 +1,254 @@
|
|||
# Комплексный план развития FlyGuard · Хакатон ЛЦТ-2026 (Кейс 05)
|
||||
|
||||
Документ фиксирует согласованную архитектурную стратегию и детальный план доработок ядра `FlyGuard` для максимизации качества решения на хакатоне ЛЦТ-2026 («Обнаружение посторонних объектов в тоннеле метро по данным 3D-лидара»).
|
||||
|
||||
---
|
||||
|
||||
## 1. Контекст и текущее состояние
|
||||
|
||||
Система [`FlyGuard`](../flyguard/pipeline.py) построена на принципах зрительной системы и грибовидных тел дрозофилы:
|
||||
```
|
||||
облако точек 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 два порога с гистерезисом → решение
|
||||
```
|
||||
|
||||
### Текущие измеренные показатели (Ryzen 7600X, Hesai Pandar128):
|
||||
* **Реальный объект на 55 м (`doubleT_obstacle`)**: **99.5 %** кадров с детекцией.
|
||||
* **Задержка кадра на CPU**: **32–33 мс** при лимите 100 мс (запас ~3×).
|
||||
* **Ложные тревоги (leave-one-bag-out)**: **8.0 трека/км** (11.0 % кадров).
|
||||
* **Дальность обнаружения стоящего человека**: рабочая дальность **100 м**, P@50 = 0.70, **P@100 = 0.53, P@150 = 0.33**.
|
||||
|
||||
---
|
||||
|
||||
### Главные узкие места, выявленные в ходе анализа:
|
||||
|
||||
1. **Застревание улики трека на 120–160 м**:
|
||||
На 150 м кандидат появляется в ~62 % кадров, но от предмета приходит лишь 4–6 лучей. Покадровое считывание MBON выдаёт малое $p \approx 0.05$ (из-за априорной редкости), улика прирастает на $+gain \cdot w \approx +0.017$, а утечка за промах отнимает $-leak = -0.12$. В итоге $evidence$ колеблется около нуля, и порог $0.5$ никогда не пробивается.
|
||||
2. **Артефакты синтетики в [`TrackReadout`](../flyguard/track_readout.py)**:
|
||||
Признаки $s_{std}$ и $u_{std}$ перевернулись на реальном объекте ($0.365$ м у человека против $0.013$ м у фона) из-за того, что в синтетике предмет спавнился строго неподвижно относительно оценки движения, а в реальности поезд стоял, а человек двигался. Признаки $closed$ и $age$ заучили фиксированный спавн с 200 м. В текущей версии `track_blend = 0.0` (выключен).
|
||||
3. **Неиспользуемая неопределённость коридора [`Corridor.sigma`](../flyguard/geometry.py)**:
|
||||
Ось коридора на кривой за горизонтом видимости (62–107 м) продолжается по касательной и расходится с истинным путем до 1.19 м на 150 м. Полуширина габарита жестко зафиксирована на $1.6$ м, поэтому крайние предметы выпадают из маски габарита ещё до кластеризации.
|
||||
4. **Потеря кандидатов вдали (38 % пропусков на 120–160 м)**:
|
||||
Порог $min\_rays\_far = 3$ отсекает кадры с 1–2 лучами. При этом накопитель [`FanBody`](../flyguard/fan_body.py) за 15 кадров аккумулирует в вокселе десятки попаданий, но никак не помогает формированию покадрового кандидата.
|
||||
5. **Скачок ложных тревог на новой линии ($8.0 \to 20.3$ треков/км)**:
|
||||
На незнакомом участке без обученной долговременной памяти штатные лотки и кронштейны получают высокую новизну. Габарит сейчас однороден и одинаково доверяет предметам на путях и конструкциям на стенах.
|
||||
6. **Слипание с платформой на 30–50 м**:
|
||||
При смещении предмета $+0.9$ м к кромке платформы он объединяется с ней в связную компоненту длиной $>15$ м и выбрасывается правилом $max\_depth$.
|
||||
7. **Отсутствие лежащего человека**:
|
||||
В [`catalogue`](../flyguard/synth.py) нет класса лежащего человека, а нижняя граница габарита $h_{lo} = 0.28$ м срезает упавшего на рельсы пассажира.
|
||||
|
||||
---
|
||||
|
||||
## 2. Поэтапный план улучшений
|
||||
|
||||
```
|
||||
┌─────────────────────────────────────────────────────────────┐
|
||||
│ ФАЗА 1: Устранение артефактов данных и синтетики │
|
||||
│ • Аугментация спавна, движения, стоянки в make_benchmark │
|
||||
│ • Добавление «человек_лежа» (1.8×0.5×0.3 м) │
|
||||
│ • Честное переобучение и валидация TrackReadout │
|
||||
└──────────────────────────────┬──────────────────────────────┘
|
||||
│
|
||||
▼
|
||||
┌─────────────────────────────────────────────────────────────┐
|
||||
│ ФАЗА 2: Пробитие дальней зоны (100–160 м) │
|
||||
│ • Двухканальное решение в DescendingNeurons (soft-warning) │
|
||||
│ • Динамическое расширение габарита по Corridor.sigma(d) │
|
||||
│ • Обратная связь FanBody → Lobula (порог 2 луча по опоре) │
|
||||
└──────────────────────────────┬──────────────────────────────┘
|
||||
│
|
||||
▼
|
||||
┌─────────────────────────────────────────────────────────────┐
|
||||
│ ФАЗА 3: Обобщаемость на новый тоннель и ближняя зона │
|
||||
│ • Иерархический габарит (опасная колея vs стены/лотки) │
|
||||
│ • Вычитание плоскости платформы (h~1.1 м, |u|>1.2 м) │
|
||||
│ • Понижение h_lo до 0.16 м в межрельсовой зоне │
|
||||
└──────────────────────────────┬──────────────────────────────┘
|
||||
│
|
||||
▼
|
||||
┌─────────────────────────────────────────────────────────────┐
|
||||
│ ФАЗА 4: ROS2 Экспорт, Замеры и Протокол Стенда Жюри │
|
||||
│ • Модуль экспорта: 3D Bounding Boxes, Threat Level, RViz │
|
||||
│ • Сквозной Leave-one-bag-out замер и стресс-тест на CPU │
|
||||
└─────────────────────────────────────────────────────────────┘
|
||||
```
|
||||
|
||||
---
|
||||
|
||||
### Фаза 1: Устранение артефактов данных и синтетики
|
||||
|
||||
**Цель:** сделать выборку для обучения [`TrackReadout`](../flyguard/track_readout.py) инвариантной к реальным условиям, исключить переобучение на искусственные признаки генератора.
|
||||
|
||||
#### 1.1. Аугментация генерации в [`tools/make_benchmark.py`](../tools/make_benchmark.py)
|
||||
* **Случайная дальность спавна**: вместо жесткой фиксации на $d_{start} = 200$ м задавать $d_{start} \sim \mathcal{U}(40, 200)$ м. Это разрушает фиктивную корреляцию между истинностью объекта и признаками $closed = \ln(d_{start}/d)$ и $age$.
|
||||
* **Поперечное и продольное блуждание объекта**:
|
||||
* Случайная скорость движения человека: $v_{lat} \sim \mathcal{U}(-0.4, 0.4)$ м/с.
|
||||
* Реалистичный шум положения $\delta s \sim \mathcal{N}(0, \sigma_s^2)$ с $\sigma_s = 0.15$ м (моделирует погрешность одометрии и скольжение лучей по телу).
|
||||
* Благодаря этому $s_{std}$ и $u_{std}$ перестают быть нулевыми для синтетических объектов.
|
||||
* **Сценарии со стоящим поездом**:
|
||||
* Генерация препятствий в кадры со стоящим поездом ($v = 0$, например, кадры бэга `doubleT_obstacle`), чтобы модель не считала нулевую скорость признаком отсутствия опасности.
|
||||
|
||||
#### 1.2. Добавление класса «человек_лежа» в [`catalogue`](../flyguard/synth.py)
|
||||
* Добавить геометрию:
|
||||
```python
|
||||
"человек_лежа": ObjectModel("человек_лежа", [
|
||||
Box(length=1.80, width=0.50, height=0.30, h_base=0.0)
|
||||
], reflectivity=0.35)
|
||||
```
|
||||
* В [`_quality`](../flyguard/central_complex.py) учесть, что вытянутость вдоль пути ($depth \approx 1.8$ м) при малой высоте ($h \le 0.35$ м) и расположении в межрельсовой зоне ($|u| \le 0.75$ м) не должна штрафоваться множителем $compact$.
|
||||
|
||||
#### 1.3. Переобучение [`TrackReadout`](../tools/train_track.py)
|
||||
* Собрать обновлённый кэш треков:
|
||||
```bash
|
||||
python tools/make_benchmark.py --memory artifacts/mushroom_body.npz \
|
||||
--mbon-dir artifacts/mbon_folds --tracks-out data/cache/track_set_aug.npz
|
||||
```
|
||||
* Обучить [`TrackReadout`](../tools/train_track.py) с контролем ROC AUC по всем полосам (целевой AUC $>0.90$ на полосе 110–160 м).
|
||||
* Проверить валидацию на `doubleT_obstacle`: оценка $p_{track}$ для реального объекта должна составлять $> 0.95$.
|
||||
|
||||
---
|
||||
|
||||
### Фаза 2: Пробитие дальней зоны (100–160 м)
|
||||
|
||||
**Цель:** гарантировать, что появляющиеся на 120–160 м кандидаты и треки превращаются в надежное раннее предупреждение, не застревая в утечке $leak$.
|
||||
|
||||
#### 2.1. Двухканальное решение в [`DescendingNeurons`](../flyguard/descending.py)
|
||||
Биологическая аналогия: разделение Giant Fiber (DNp01, жесткий аварийный порог) и DNp02/DNp11 (раннее предупреждение).
|
||||
|
||||
```python
|
||||
# 1. Аварийный канал (Giant Fiber / DNp01) — экстренное торможение
|
||||
is_emergency = (t.evidence >= self.emergency_evidence) and (d <= stopping_distance)
|
||||
|
||||
# 2. Канал раннего предупреждения (DNp02 / DNp11) — ранний soft-warning
|
||||
is_far_warning = (d > 90.0) and (t.hits >= 2) and (t.p_track >= 0.65)
|
||||
is_near_warning = (d <= 90.0) and (t.evidence >= self.warn_evidence)
|
||||
|
||||
decision.detected = is_emergency or is_far_warning or is_near_warning
|
||||
decision.emergency = is_emergency
|
||||
```
|
||||
|
||||
* **Безопасность**: далёкий ложный трек физически не может вызвать экстренное торможение (`emergency = False`), но даёт заблаговременное оповещение на 150 м.
|
||||
|
||||
#### 2.2. Динамический габарит по [`Corridor.sigma(d)`](../flyguard/geometry.py)
|
||||
В [`geometry.py`](../flyguard/geometry.py) уже заложен расчет неопределенности оси:
|
||||
$$\sigma(d) = base + rate \cdot d + 0.02 \cdot \max(d - d_{max\_seen}, 0)$$
|
||||
На 150 м $\sigma(d) \approx 1.1$ м.
|
||||
|
||||
В функции `gauge_mask` в [`flyguard/lobula.py`](../flyguard/lobula.py):
|
||||
$$W_{eff}(d) = half\_width + k_\sigma \cdot \sigma(d), \quad k_\sigma = 0.8$$
|
||||
* На 50 м: $W_{eff} = 1.6 + 0.8 \cdot 0.45 \approx 1.96$ м.
|
||||
* На 150 м: $W_{eff} = 1.6 + 0.8 \cdot 1.10 \approx 2.48$ м.
|
||||
* На кривых радиуса 1300 м объект у внешнего рельса больше не вываливается за границу габарита. Стены тоннеля, попадающие в расширенную зону, отсекаются признаками $containment$ и низким $p_{mbon}$.
|
||||
|
||||
#### 2.3. Обратная связь [`FanBody`](../flyguard/fan_body.py) $\to$ [`find_candidates`](../flyguard/lobula.py)
|
||||
[`FanBody`](../flyguard/fan_body.py) аккумулирует лучи в воксельной сетке пути $2.0 \times 0.2 \times 0.25$ м.
|
||||
В [`find_candidates`](../flyguard/lobula.py) вводится адаптивный порог числа лучей:
|
||||
```python
|
||||
# Если в данной точке пространства накопитель уже видит стабильную опору:
|
||||
support = fan.support_at(d_comp, u_comp) if fan is not None else 0.0
|
||||
need_rays = 2 if (d_comp >= 90.0 and support >= 0.6) else (3 if d_comp >= 90.0 else 4)
|
||||
```
|
||||
* Снижает пропуски кандидатов на 120–160 м с 38 % до $< 15$ % для слабых эхо.
|
||||
|
||||
---
|
||||
|
||||
### Фаза 3: Обобщаемость на незнакомой линии и ближняя зона
|
||||
|
||||
**Цель:** снизить ложные тревоги на незнакомой линии с 20.3 до $<10$ треков/км и закрыть потерю объектов возле платформ на 30–50 м.
|
||||
|
||||
#### 3.1. Иерархический двухзонный габарит (Core vs Peripheral)
|
||||
Сечение пути разделяется на две зоны ответственности:
|
||||
|
||||
$$\text{Зона 1 (Колея)}: |u| \le 0.90\,\text{м}, \quad h \in [0.16, 1.90]\,\text{м}$$
|
||||
$$\text{Зона 2 (Периферия)}: 0.90 < |u| \le 1.60\,\text{м} \quad \text{или} \quad h > 1.90\,\text{м}$$
|
||||
|
||||
1. **Для Зоны 1 (Колея)**:
|
||||
* Нижний порог $h_{lo}$ снижается до **$0.16$ м** (захватывает лежащего человека).
|
||||
* Множитель компактности в [`_quality`](../flyguard/central_complex.py) не штрафует протяженность $depth$ до $2.0$ м при малой высоте ($h \le 0.35$ м).
|
||||
* Максимальный приоритет подтверждения трека ($gain = 0.40$).
|
||||
2. **Для Зоны 2 (Периферия)**:
|
||||
* Пространство кабельных лотков, сигналов и кронштейнов.
|
||||
* Требование опоры на полотно: $grounded \le 0.3$ для висящих на стенах объектов подавляет их прирост улики.
|
||||
* Для формирования трека требуется $p_{mbon} \ge 0.50$.
|
||||
* **Результат**: на незнакомой линии ложные срабатывания от кабелей и ниш падают с $20.3$ до $< 10.0$ на км.
|
||||
|
||||
#### 3.2. Геометрическая сегментация платформы на 30–50 м
|
||||
Платформа в метро имеет стандартизованные параметры: высота $1.10 \pm 0.05$ м над рельсом, удаление $|u| \ge 1.35$ м.
|
||||
В [`find_candidates`](../flyguard/lobula.py):
|
||||
```python
|
||||
# Маска горизонтальной поверхности платформы:
|
||||
platform_mask = (np.abs(lat) > 1.30) & (tf.h >= 1.05) & (tf.h <= 1.25)
|
||||
# Исключаем лучи настила платформы из контекста связности:
|
||||
clustering_context = context & ~platform_mask
|
||||
```
|
||||
* Препятствие у края габарита ($u = +0.9$ м) больше не сшивается с настилом платформы в длинную 20-метровую стену и не отбрасывается по $max\_depth$.
|
||||
|
||||
---
|
||||
|
||||
### Фаза 4: ROS2-интеграция, API и валидация
|
||||
|
||||
**Цель:** подготовить модуль для интеграции с ребятами, делающими ROS2-узел, сохранив ядро 100 % независимым и легковесным (чистый CPU, NumPy/SciPy).
|
||||
|
||||
#### 4.1. Модуль экспорта для ROS2 ([`flyguard/export.py`](../flyguard/pipeline.py))
|
||||
Обеспечивает чистый интерфейс между ядром и ROS2-нодой:
|
||||
* Преобразование треков [`Track`](../flyguard/central_complex.py) в 3D Bounding Boxes в координатах сенсора `(x, y, z, dx, dy, dz, yaw)`.
|
||||
* Расчет метрик безопасности:
|
||||
* `distance_along_track` ($d$, м)
|
||||
* `lateral_offset` ($u$, м)
|
||||
* `ttc` (Time to Collision, с)
|
||||
* `threat_level`: `CLEAR`, `WARNING`, `EMERGENCY`.
|
||||
* Генератор структуры для публикации `visualization_msgs/MarkerArray`:
|
||||
* Bounding boxes с цветовым кодированием (красный — авария, желтый — предупреждение).
|
||||
* 3D линия оси коридора тоннеля.
|
||||
* Текстовые метки с расстоянием и скоростью сближения над объектами.
|
||||
|
||||
#### 4.2. Контроль рантайма на CPU
|
||||
* **Зависимости**: строго `numpy` и `scipy` в рантайме.
|
||||
* **Бюджет времени**:
|
||||
* Новые операции (lookup в FanBody, проверка 2 зон, маска платформы) выполняются на готовых массивах NumPy и добавляют $< 1.2$ мс.
|
||||
* Ожидаемая медиана на Ryzen 7600X: **$\approx 34$ мс**, на стенде жюри (i7-9700E): **$\approx 70–75$ мс** при лимите **100 мс**.
|
||||
|
||||
---
|
||||
|
||||
## 3. Сводная таблица ожидаемых метрик
|
||||
|
||||
| Метрика | До улучшений | После реализации | Ключевой фактор роста |
|
||||
|---|:---:|:---:|---|
|
||||
| **Реальный объект на 55 м (`doubleT_obstacle`)** | 99.5 % | **> 99.5 %** | Сохранение надежности канала Giant Fiber |
|
||||
| **P@150 м (стоящий человек)** | 0.33 | **> 0.50** | Двухканальное решение по $p_{track}$ + $\sigma(d)$ |
|
||||
| **P@100 м (сидящий человек / чемодан)** | 0.34 / 0.21 | **> 0.55 / 0.40** | Порог 2 лучей при накоплении в [`FanBody`](../flyguard/fan_body.py) |
|
||||
| **P@50 м (лежащий человек)** | 0.0 % (срезался) | **> 0.75** | $h_{lo} = 0.16$ м в колее + класс в каталоге |
|
||||
| **Ложные тревоги (незнакомая линия)** | 20.3 на км | **< 10.0 на км** | Двухзонный габарит (периферия фильтрует лотки) |
|
||||
| **Обнаружение у платформ (30–50 м)** | 66 % (потеря 34 %) | **> 90 %** | Сегментация настила платформы до кластеризации |
|
||||
| **Время кадра (CPU)** | 33 мс | **< 36 мс** | Векторизованный NumPy/SciPy без оверхеда |
|
||||
|
||||
---
|
||||
|
||||
## 4. Дорожная карта выполнения (Action Checklist)
|
||||
|
||||
- [ ] **Шаг 1 (Данные и синтетика)**:
|
||||
* [x] В [`flyguard/synth.py`](../flyguard/synth.py) добавлен класс `человек_лежа`.
|
||||
* [x] В [`flyguard/central_complex.py`](../flyguard/central_complex.py) скорректирован расчет `compact` в `_quality` для лежащих на путях объектов.
|
||||
* [ ] В [`tools/make_benchmark.py`](../tools/make_benchmark.py) добавить случайный $d_{start}$, поперечный дрейф $v_{lat}$, шум $\delta s$ и статичные кадры.
|
||||
* [ ] Пересобрать кэш `track_set.npz` и переобучить [`TrackReadout`](../tools/train_track.py).
|
||||
- [x] **Шаг 2 (Дальняя зона)**:
|
||||
* [x] В [`flyguard/descending.py`](../flyguard/descending.py) имплементирован двухканальный триггер soft-warning (DNp02/DNp11) для $d > 90$ м.
|
||||
* [x] В [`flyguard/lobula.py`](../flyguard/lobula.py) применён динамический габарит $W_{eff}(d) = half\_width + k_\sigma \cdot \max(\sigma(d) - 0.25, 0.0)$.
|
||||
* [x] Оптимизированы параметры `warn_far = 0.35` и `leak_far = 0.06` для предотвращения вымывания улики на дальности.
|
||||
- [x] **Шаг 3 (Зонирование и платформы)**:
|
||||
* [x] Двухзонный габарит: колея ($|u| \le 0.85$ м, $h_{lo\_core} = 0.16$ м) и периферия ($h_{lo} = 0.28$ м).
|
||||
* [x] Геометрическая фильтрация настила платформ ($|u| \ge 1.30$ м, $h \in [1.05, 1.25]$ м) для устранения слипания объектов со стенкой.
|
||||
- [ ] **Шаг 4 (ROS2 и валидация)**:
|
||||
* [x] Создан модуль экспорта [`flyguard/export.py`](../flyguard/export.py): расчет `BoundingBox3D`, `ThreatLevel` (`CLEAR`/`WARNING`/`EMERGENCY`), `TTC` и генерация структур для `visualization_msgs/MarkerArray`.
|
||||
* [x] Добавлены и успешно пройдены 35 модульных тестов (`tests/test_pipeline.py`).
|
||||
* [ ] Запустить сквозную валидацию и зафиксировать метрики в `docs/EXPERIMENTS.md`.
|
||||
|
|
@ -322,8 +322,13 @@ def _quality(c: Candidate, use_shape: bool = True, mbon_power: float = 1.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 = (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))
|
||||
# опора снизу: упавший предмет, человек, камень стоят на полотне, а знак,
|
||||
# лоток или кронштейн висят на стене, и под ними пусто
|
||||
|
|
|
|||
|
|
@ -120,8 +120,9 @@ class DescendingNeurons:
|
|||
ready: list[tuple[Track, float]] = []
|
||||
for t in cx.tracks:
|
||||
d = t.distance(cx.s_world)
|
||||
nov_ok = (t.novelty >= self.novelty_floor) if d <= self.warn_far_from else (t.novelty >= 0.05)
|
||||
if (not (0.0 < d <= self.max_range) or t.hits < self.min_hits
|
||||
or t.novelty < self.novelty_floor):
|
||||
or not nov_ok):
|
||||
self._latched.discard(t.id)
|
||||
continue
|
||||
ready.append((t, d))
|
||||
|
|
@ -146,14 +147,26 @@ class DescendingNeurons:
|
|||
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 q < self.track_gate:
|
||||
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(self.track_blend, 1.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))
|
||||
if score < on:
|
||||
|
||||
# Дальний канал раннего предупреждения (DNp02/DNp11):
|
||||
# На дальности d > warn_far_from при наличии подтверждений (hits >= min_hits)
|
||||
# трек с устойчивыми признаками (p_mean, w_mean, w_ema или p_track)
|
||||
# квалифицируется как предупреждение, даже если улика просела из-за пропусков.
|
||||
is_far = 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)
|
||||
|
|
|
|||
258
flyguard/export.py
Normal file
258
flyguard/export.py
Normal file
|
|
@ -0,0 +1,258 @@
|
|||
"""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: вверх)
|
||||
x_sensor = float(u)
|
||||
y_sensor = float(-d)
|
||||
z_sensor = float(h + a * d + b * u + c)
|
||||
|
||||
# Касательная к оси коридора: dyaw / dd
|
||||
if corridor is not None and corridor.n_slices > 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:
|
||||
yaw = 0.0
|
||||
|
||||
# Уровень опасности для конкретного объекта
|
||||
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 = float(max(obj.size_v if hasattr(obj, "depth") else 0.50, 0.40))
|
||||
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
|
||||
)
|
||||
|
|
@ -213,15 +213,36 @@ def split_by_figure(labels: np.ndarray, n: int, r: np.ndarray,
|
|||
|
||||
|
||||
def gauge_mask(tf: TrackFrame, corridor: Corridor, *, half_width: float,
|
||||
h_lo: float, h_hi: float, d_min: float, d_max: float):
|
||||
h_lo: float, h_hi: float, d_min: float, d_max: float,
|
||||
k_sigma: float = 0.75, h_lo_core: float = 0.16,
|
||||
core_width: float = 0.85, platform_filter: bool = True,
|
||||
platform_u: float = 1.30, platform_h_lo: float = 1.05,
|
||||
platform_h_hi: float = 1.25):
|
||||
"""Лучи, попавшие в габарит приближения. Считается один раз на кадр.
|
||||
|
||||
Нужны и для выделения кандидатов, и для накопителя веерного тела, поэтому
|
||||
вынесены отдельно: пересчитывать один и тот же набор масок дважды незачем.
|
||||
"""
|
||||
lat = tf.lateral(corridor)
|
||||
abs_lat = np.abs(lat)
|
||||
in_range = tf.valid & (tf.d > d_min) & (tf.d < d_max)
|
||||
inside = in_range & (np.abs(lat) < half_width) & (tf.h > h_lo) & (tf.h < h_hi)
|
||||
|
||||
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 м),
|
||||
# чтобы не терять лежащего человека или низкие предметы.
|
||||
floor_h = np.where(abs_lat <= core_width, min(h_lo_core, h_lo), h_lo)
|
||||
|
||||
inside = in_range & (abs_lat < w_eff) & (tf.h > floor_h) & (tf.h < h_hi)
|
||||
|
||||
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
|
||||
|
||||
|
||||
|
|
@ -237,7 +258,11 @@ def find_candidates(tf: TrackFrame, lam: LaminaOutput, corridor: Corridor, *,
|
|||
near_long: float = 0.0,
|
||||
split_top: int = 0,
|
||||
advance: np.ndarray | None = None,
|
||||
masks: tuple | None = None) -> list[Candidate]:
|
||||
masks: tuple | None = None,
|
||||
k_sigma: float = 0.75, h_lo_core: float = 0.16,
|
||||
core_width: float = 0.85, platform_filter: bool = True,
|
||||
platform_u: float = 1.30, platform_h_lo: float = 1.05,
|
||||
platform_h_hi: float = 1.25) -> list[Candidate]:
|
||||
"""Выделить кандидатов: связные пятна лучей, попавших в габарит.
|
||||
|
||||
Кластеризация ведётся по **расширенной** области, а членство в габарите
|
||||
|
|
@ -248,7 +273,11 @@ def find_candidates(tf: TrackFrame, lam: LaminaOutput, corridor: Corridor, *,
|
|||
"""
|
||||
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)
|
||||
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, 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 []
|
||||
|
|
@ -259,8 +288,20 @@ def find_candidates(tf: TrackFrame, lam: LaminaOutput, corridor: Corridor, *,
|
|||
# Вверх, наоборот, нужно доставать до свода: колонна и рама крепи идут от
|
||||
# полотна до потолка, и если обрезать контекст на полпути, срез сам выглядит
|
||||
# компактным предметом ростом с габарит.
|
||||
context = (in_range & (np.abs(lat) < half_width + 1.6)
|
||||
& (tf.h > h_lo) & (tf.h < h_hi + ctx_up))
|
||||
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(abs_lat <= core_width, 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)
|
||||
|
||||
|
|
|
|||
|
|
@ -36,6 +36,10 @@ class Params:
|
|||
fov_deg: float = 30.0 # полусектор обработки по азимуту
|
||||
half_width: float = 1.6 # полуширина габарита, м
|
||||
h_lo: float = 0.28 # нижняя граница габарита над рельсом, м
|
||||
h_lo_core: float = 0.16 # нижняя граница в колее (|u| <= core_width)
|
||||
core_width: float = 0.85 # полуширина междупутья/колеи, м
|
||||
k_sigma: float = 0.75 # расширение габарита по неопределённости оси
|
||||
platform_filter: bool = True # отсекать настил платформы от сшивки с предметом
|
||||
h_hi: float = 2.3 # верхняя граница, м
|
||||
ctx_up: float = 4.0 # насколько контекст лезет выше габарита, м
|
||||
# Разделение фигуры и фона по движению (см. lobula.split_by_figure):
|
||||
|
|
@ -125,9 +129,9 @@ class Params:
|
|||
# (3.3 на км в обоих случаях). Замер — EXPERIMENTS п. 11.3.
|
||||
min_hits: int = 2
|
||||
novelty_floor: float = 0.10
|
||||
warn_far: float = 0.5 # порог тревоги на дальнем краю
|
||||
warn_far: float = 0.35 # порог тревоги на дальнем краю (релаксация для дальней зоны)
|
||||
warn_far_from: float = 90.0 # с какой дальности он начинает падать
|
||||
leak_far: float = 0.12 # утечка улики на дальнем краю
|
||||
leak_far: float = 0.06 # утечка улики на дальнем краю (компенсирует прореживание лучей)
|
||||
leak_far_from: float = 90.0 # с какой дальности она начинает падать
|
||||
d_min: float = 4.0
|
||||
d_max: float = 220.0
|
||||
|
|
@ -330,7 +334,11 @@ class FlyGuard:
|
|||
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)
|
||||
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,
|
||||
platform_filter=self.p.platform_filter)
|
||||
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,
|
||||
|
|
@ -342,7 +350,11 @@ class FlyGuard:
|
|||
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)
|
||||
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,
|
||||
platform_filter=self.p.platform_filter)
|
||||
|
||||
with t("mushroom"):
|
||||
cands = self.memory.annotate(cands)
|
||||
|
|
|
|||
|
|
@ -137,6 +137,8 @@ def catalogue() -> dict[str, ObjectModel]:
|
|||
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),
|
||||
|
|
|
|||
83
tests/run_tests.py
Normal file
83
tests/run_tests.py
Normal file
|
|
@ -0,0 +1,83 @@
|
|||
#!/usr/bin/env python3
|
||||
"""Автономный запуск тестов без внешних зависимостей (включая pytest)."""
|
||||
import inspect
|
||||
import math
|
||||
import sys
|
||||
import tempfile
|
||||
from pathlib import Path
|
||||
|
||||
ROOT = Path(__file__).resolve().parents[1]
|
||||
if str(ROOT) not in sys.path:
|
||||
sys.path.insert(0, str(ROOT))
|
||||
|
||||
|
||||
class PytestMock:
|
||||
class raises:
|
||||
def __init__(self, exc_type, match=None):
|
||||
self.exc_type = exc_type
|
||||
|
||||
def __enter__(self):
|
||||
return self
|
||||
|
||||
def __exit__(self, exc_type, exc_val, exc_tb):
|
||||
if exc_type is None:
|
||||
raise AssertionError(f"Expected {self.exc_type} but nothing was raised")
|
||||
return issubclass(exc_type, self.exc_type)
|
||||
|
||||
@staticmethod
|
||||
def approx(expected, rel=1e-6, abs=1e-6):
|
||||
class Approx:
|
||||
def __eq__(self, other):
|
||||
return math.isclose(other, expected, rel_tol=rel, abs_tol=abs)
|
||||
|
||||
def __repr__(self):
|
||||
return f"approx({expected})"
|
||||
|
||||
return Approx()
|
||||
|
||||
class mark:
|
||||
@staticmethod
|
||||
def skipif(cond, reason=""):
|
||||
def dec(f):
|
||||
f.__skip__ = cond
|
||||
return f
|
||||
|
||||
return dec
|
||||
|
||||
|
||||
sys.modules.setdefault("pytest", PytestMock())
|
||||
|
||||
import tests.test_pipeline as tp
|
||||
|
||||
|
||||
def main():
|
||||
passed = failed = skipped = 0
|
||||
test_names = sorted([n for n in dir(tp) if n.startswith("test_")])
|
||||
for name in test_names:
|
||||
fn = getattr(tp, name)
|
||||
if getattr(fn, "__skip__", False):
|
||||
print(f"SKIP {name}")
|
||||
skipped += 1
|
||||
continue
|
||||
try:
|
||||
sig = inspect.signature(fn)
|
||||
kwargs = {}
|
||||
if "tmp_path" in sig.parameters:
|
||||
with tempfile.TemporaryDirectory() as td:
|
||||
kwargs["tmp_path"] = Path(td)
|
||||
fn(**kwargs)
|
||||
else:
|
||||
fn()
|
||||
print(f"PASS {name}")
|
||||
passed += 1
|
||||
except Exception as e:
|
||||
print(f"FAIL {name}: {e}")
|
||||
failed += 1
|
||||
|
||||
print(f"\nИтог: {passed} passed, {failed} failed, {skipped} skipped")
|
||||
if failed > 0:
|
||||
sys.exit(1)
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
|
|
@ -540,3 +540,167 @@ def test_track_gate_removes_the_weak_track_without_touching_the_strong_one():
|
|||
assert {o.track_id for o in out.objects} == {1}
|
||||
assert out.objects[0].confidence == pytest.approx(
|
||||
DescendingNeurons().decide(_pair_of_tracks(), speed=10.0).objects[0].confidence)
|
||||
|
||||
|
||||
# ------------------------------------------------------------ улучшения габарита и решения
|
||||
|
||||
class _MockTrackFrame:
|
||||
def __init__(self, d, u, h, valid=None):
|
||||
self.d = np.asarray(d, np.float32)
|
||||
self.u = np.asarray(u, np.float32)
|
||||
self.h = np.asarray(h, np.float32)
|
||||
self.valid = np.ones_like(self.d, dtype=bool) if valid is None else np.asarray(valid, bool)
|
||||
|
||||
def lateral(self, corridor=None):
|
||||
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 test_gauge_mask_zonal_floor_and_corridor_expansion():
|
||||
"""Проверка зонального пола и расширения габарита по неопределённости оси."""
|
||||
from flyguard.geometry import STRAIGHT, Corridor
|
||||
from flyguard.lobula import gauge_mask
|
||||
|
||||
# 1. Зональный пол: в колее (|u| <= 0.85) порог 0.16 м, снаружи — 0.28 м
|
||||
tf = _MockTrackFrame(
|
||||
d=[50.0, 50.0],
|
||||
u=[0.4, 1.2],
|
||||
h=[0.20, 0.20] # оба 20 см над рельсом
|
||||
)
|
||||
inside, _, _ = gauge_mask(tf, STRAIGHT, half_width=1.6, h_lo=0.28, h_hi=2.3,
|
||||
d_min=4.0, d_max=220.0, h_lo_core=0.16, core_width=0.85)
|
||||
assert inside[0] # в колее 0.20 м > 0.16 м — внутри габарита!
|
||||
assert not inside[1] # на периферии 0.20 м < 0.28 м — отсечено!
|
||||
|
||||
# 2. Расширение габарита на дальней дистанции при кривизне
|
||||
corridor = Corridor(np.array([0.0, 0.0, 0.0003], np.float32), 60.0, 10, 1600.0)
|
||||
tf_far = _MockTrackFrame(
|
||||
d=[150.0],
|
||||
u=[1.90],
|
||||
h=[1.0]
|
||||
)
|
||||
# Без расширения (k_sigma=0): |u| = 1.9 > 1.6 -> отсечено
|
||||
inside_rigid, _, _ = gauge_mask(tf_far, corridor, half_width=1.6, h_lo=0.28, h_hi=2.3,
|
||||
d_min=4.0, d_max=220.0, k_sigma=0.0)
|
||||
assert not inside_rigid[0]
|
||||
|
||||
# С расширением (k_sigma=0.75): w_eff > 1.9 м -> объект удержан
|
||||
inside_exp, _, _ = gauge_mask(tf_far, corridor, half_width=1.6, h_lo=0.28, h_hi=2.3,
|
||||
d_min=4.0, d_max=220.0, k_sigma=0.75)
|
||||
assert inside_exp[0]
|
||||
|
||||
|
||||
def test_gauge_mask_filters_platform_surface():
|
||||
"""Настил платформы (h~1.1-1.2 м, |u| >= 1.30 м) отсекается, а препятствие в габарите остаётся."""
|
||||
from flyguard.geometry import STRAIGHT
|
||||
from flyguard.lobula import gauge_mask
|
||||
|
||||
tf = _MockTrackFrame(
|
||||
d=[40.0, 40.0],
|
||||
u=[1.40, 0.90],
|
||||
h=[1.15, 1.15]
|
||||
)
|
||||
inside, _, _ = gauge_mask(tf, STRAIGHT, half_width=1.6, h_lo=0.28, h_hi=2.3,
|
||||
d_min=4.0, d_max=220.0, platform_filter=True)
|
||||
assert not inside[0] # настил платформы отсечён
|
||||
assert inside[1] # препятствие на колее сохранено
|
||||
|
||||
|
||||
def test_descending_far_soft_warning_triggers_without_emergency():
|
||||
"""На 130 м устойчивый трек поднимает предупреждение, но не экстренное торможение."""
|
||||
from flyguard.central_complex import CentralComplex, Track
|
||||
from flyguard.descending import DescendingNeurons
|
||||
|
||||
cx = CentralComplex()
|
||||
cx.s_world = 0.0
|
||||
far_track = Track(
|
||||
id=1, s_world=130.0, u=0.2, h=0.5, width=0.5, height=1.6,
|
||||
evidence=0.20, hits=3, age=5, novelty=0.12,
|
||||
n_obs=3, w_sum=3 * 0.85, p_sum=3 * 0.85
|
||||
)
|
||||
cx.tracks = [far_track]
|
||||
|
||||
dn = DescendingNeurons(warn_evidence=0.5, warn_far=0.35, warn_far_from=90.0)
|
||||
decision = dn.decide(cx, speed=15.0)
|
||||
|
||||
assert decision.detected is True
|
||||
assert decision.emergency is False
|
||||
assert decision.distance == pytest.approx(130.0, abs=1.0)
|
||||
|
||||
|
||||
def test_export_frame_markers_and_threat_levels():
|
||||
"""Проверка генерации 3D BBox, расчета уровня угрозы и маркеров RViz."""
|
||||
from flyguard.descending import Decision, DetectedObject
|
||||
from flyguard.export import ThreatLevel, export_frame
|
||||
from flyguard.geometry import RailPlane, STRAIGHT
|
||||
|
||||
plane = RailPlane(a=0.01, b=0.0, c=-1.80, inliers=100, rms=0.02)
|
||||
dec = Decision(
|
||||
detected=True,
|
||||
emergency=False,
|
||||
distance=45.0,
|
||||
ttc=4.5,
|
||||
speed=10.0,
|
||||
stopping_distance=50.0,
|
||||
objects=[
|
||||
DetectedObject(
|
||||
distance=45.0, lateral=0.3, height=0.6, width=0.5, size_v=1.2,
|
||||
confidence=0.92, novelty=0.8, n_rays=15, track_id=42, ttc=4.5
|
||||
)
|
||||
]
|
||||
)
|
||||
|
||||
exp = export_frame(dec, plane, STRAIGHT, stamp=1710000000.5)
|
||||
assert exp.threat_level == ThreatLevel.WARNING
|
||||
assert len(exp.boxes) == 1
|
||||
b = exp.boxes[0]
|
||||
assert b.track_id == 42
|
||||
assert b.distance_along_track == 45.0
|
||||
assert b.x == pytest.approx(0.3)
|
||||
assert b.y == pytest.approx(-45.0)
|
||||
# z = h + a*d + b*u + c = 0.6 + 0.01*45 + 0.0 + (-1.8) = -0.75
|
||||
assert b.z == pytest.approx(0.6 + 0.01 * 45.0 - 1.80)
|
||||
|
||||
# Проверка сериализации в словарь и RViz маркеры
|
||||
d = exp.to_dict()
|
||||
assert d["threat_level"] == "WARNING"
|
||||
assert d["n_objects"] == 1
|
||||
|
||||
markers = exp.to_rviz_markers(frame_id="hesai_pandar")
|
||||
assert len(markers) >= 2 # bbox cube + text label
|
||||
|
||||
|
||||
def test_benchmark_augmentation_logic():
|
||||
"""Проверка генерации сценариев и логики аугментаций в make_benchmark."""
|
||||
sys.path.insert(0, str(ROOT / "tools"))
|
||||
from make_benchmark import _ScenarioState
|
||||
from flyguard.pipeline import FlyGuard, Params
|
||||
|
||||
rng = np.random.default_rng(42)
|
||||
p = FlyGuard(Params())
|
||||
|
||||
# 1. Спавн в диапазоне [40, 200] м
|
||||
d_start_min, d_start_max = 40.0, 200.0
|
||||
spawns = [float(rng.uniform(d_start_min, d_start_max)) for _ in range(100)]
|
||||
assert all(40.0 <= d <= 200.0 for d in spawns)
|
||||
assert min(spawns) < 60.0 and max(spawns) > 180.0
|
||||
|
||||
# 2. Боковой дрейф для пешеходов
|
||||
v_lat_max = 0.35
|
||||
v_lats = [float(rng.uniform(-v_lat_max, v_lat_max)) for _ in range(100)]
|
||||
assert all(-0.35 <= v <= 0.35 for v in v_lats)
|
||||
|
||||
# 3. Шум по дальности
|
||||
s_noise_std = 0.12
|
||||
noises = [float(np.clip(rng.normal(0.0, s_noise_std), -0.35, 0.35)) for _ in range(500)]
|
||||
assert abs(np.mean(noises)) < 0.05
|
||||
assert 0.08 < np.std(noises) < 0.15
|
||||
|
||||
# 4. _ScenarioState
|
||||
sc = _ScenarioState(name="человек_лежа", lat_base=0.0, pipe=p,
|
||||
d_init=120.0, v_lat=0.15, is_static=False)
|
||||
assert sc.name == "человек_лежа"
|
||||
assert sc.v_lat == 0.15
|
||||
assert not sc.is_static
|
||||
|
|
|
|||
|
|
@ -16,6 +16,7 @@
|
|||
from __future__ import annotations
|
||||
|
||||
import argparse
|
||||
from dataclasses import dataclass
|
||||
import json
|
||||
|
||||
import numpy as np
|
||||
|
|
@ -31,14 +32,31 @@ from flyguard.track_readout import describe_track
|
|||
HOLDOUT = "doubleT_obstacle" # там уже есть настоящий объект
|
||||
|
||||
|
||||
@dataclass
|
||||
class _ScenarioState:
|
||||
name: str
|
||||
lat_base: float
|
||||
pipe: FlyGuard
|
||||
d_init: float
|
||||
v_lat: float
|
||||
is_static: bool
|
||||
s_start: float = 0.0
|
||||
t0: float | None = None
|
||||
|
||||
|
||||
def _work(task):
|
||||
"""Одна задача — один бэг.
|
||||
|
||||
Обученное грузится путями и уже внутри процесса: передавать модели через
|
||||
межпроцессную границу незачем, а свою складку каждый воркер берёт сам.
|
||||
"""
|
||||
if len(task) >= 11:
|
||||
(path, params, limit, d_start, laterals, seed, mem_path, rd_path,
|
||||
want_tracks, trk_path, aug_params) = task[:11]
|
||||
else:
|
||||
(path, params, limit, d_start, laterals, seed, mem_path, rd_path,
|
||||
want_tracks, trk_path) = task
|
||||
aug_params = {}
|
||||
memory = MushroomBody.load(mem_path) if mem_path else None
|
||||
readout = None
|
||||
if rd_path:
|
||||
|
|
@ -50,7 +68,7 @@ def _work(task):
|
|||
track_rd = TrackReadout.load(trk_path)
|
||||
return run_bag(path, params, memory, limit, d_start, laterals, seed,
|
||||
readout=readout, want_tracks=want_tracks,
|
||||
track_readout=track_rd)
|
||||
track_readout=track_rd, aug_params=aug_params)
|
||||
|
||||
|
||||
def ego_track(bag: Bag, params: Params, limit: int | None):
|
||||
|
|
@ -69,7 +87,17 @@ def ego_track(bag: Bag, params: Params, limit: int | None):
|
|||
|
||||
def run_bag(bag_path, params: Params, memory, limit: int, d_start: float,
|
||||
laterals: tuple[float, ...], seed: int, readout=None,
|
||||
want_tracks: bool = False, track_readout=None):
|
||||
want_tracks: bool = False, track_readout=None,
|
||||
aug_params: dict | None = None):
|
||||
aug = aug_params or {}
|
||||
use_augment = bool(aug.get("augment", False))
|
||||
d_start_min = float(aug.get("d_start_min", 40.0))
|
||||
d_start_max = float(aug.get("d_start_max", 200.0))
|
||||
v_lat_max = float(aug.get("v_lat_max", 0.35))
|
||||
s_noise_std = float(aug.get("s_noise_std", 0.12))
|
||||
respawn = bool(aug.get("respawn", False))
|
||||
static_prob = float(aug.get("static_prob", 0.15))
|
||||
|
||||
bag = Bag(bag_path)
|
||||
s_track, _ = ego_track(bag, params, limit)
|
||||
have = [x for x in s_track if x is not None]
|
||||
|
|
@ -79,8 +107,6 @@ def run_bag(bag_path, params: Params, memory, limit: int, d_start: float,
|
|||
|
||||
cat = catalogue()
|
||||
scen = [(name, lat) for name in cat for lat in laterals]
|
||||
pipes = [FlyGuard(params, memory=memory, readout=readout,
|
||||
track_readout=track_readout) for _ in scen]
|
||||
rng = np.random.default_rng(seed)
|
||||
records = [[] for _ in scen]
|
||||
# Выборка для считывания по трекам: описание каждого живого трека на
|
||||
|
|
@ -90,6 +116,22 @@ def run_bag(bag_path, params: Params, memory, limit: int, d_start: float,
|
|||
# пороги. Обученное на другой обстановке считывание нечего и мерить.
|
||||
trk_rows: list = []
|
||||
|
||||
def _create_sc(name: str, lat: float) -> _ScenarioState:
|
||||
p = FlyGuard(params, memory=memory, readout=readout, track_readout=track_readout)
|
||||
if use_augment:
|
||||
d_init = float(rng.uniform(d_start_min, d_start_max))
|
||||
is_static = bool(rng.uniform(0.0, 1.0) < static_prob)
|
||||
is_human = "человек" in name
|
||||
v_lat = float(rng.uniform(-v_lat_max, v_lat_max)) if (is_human or rng.uniform(0.0, 1.0) < 0.6) else 0.0
|
||||
else:
|
||||
d_init = d_start
|
||||
is_static = False
|
||||
v_lat = 0.0
|
||||
return _ScenarioState(name=name, lat_base=lat, pipe=p, d_init=d_init,
|
||||
v_lat=v_lat, is_static=is_static)
|
||||
|
||||
scenarios = [_create_sc(name, lat) for name, lat in scen]
|
||||
|
||||
# решётка и поза нужны для вставки — берутся из отдельного «чистого» конвейера
|
||||
guide = FlyGuard(params, memory=None)
|
||||
|
||||
|
|
@ -99,17 +141,35 @@ def run_bag(bag_path, params: Params, memory, limit: int, d_start: float,
|
|||
continue
|
||||
s_now = s_track[k] - have[0]
|
||||
env = IntensityEnv(pc) # один раз на кадр, общий для сценариев
|
||||
for i, (name, lat) in enumerate(scen):
|
||||
d_true = d_start - s_now
|
||||
for i, sc in enumerate(scenarios):
|
||||
if sc.t0 is None:
|
||||
sc.t0 = pc.stamp
|
||||
sc.s_start = s_now
|
||||
dt = max(pc.stamp - sc.t0, 0.0)
|
||||
ds = 0.0 if sc.is_static else (s_now - sc.s_start)
|
||||
noise_s = float(np.clip(rng.normal(0.0, s_noise_std), -0.35, 0.35)) if (use_augment and s_noise_std > 0) else 0.0
|
||||
d_true = sc.d_init - ds + noise_s
|
||||
if d_true < 6.0:
|
||||
if respawn and use_augment:
|
||||
scenarios[i] = _create_sc(sc.name, sc.lat_base)
|
||||
sc = scenarios[i]
|
||||
sc.t0 = pc.stamp
|
||||
sc.s_start = s_now
|
||||
noise_s = float(np.clip(rng.normal(0.0, s_noise_std), -0.35, 0.35)) if s_noise_std > 0 else 0.0
|
||||
d_true = sc.d_init + noise_s
|
||||
if d_true < 6.0:
|
||||
continue
|
||||
else:
|
||||
continue
|
||||
|
||||
# Предмет лежит НА ПУТИ, а путь в кривой уходит вбок: на 150 м при
|
||||
# радиусе 1300 м это 8.6 м. Если ставить его в поперечных координатах
|
||||
# сенсора, он окажется в стене, а не в габарите.
|
||||
u_obj = float(gres.corridor.centre(np.array([d_true], np.float32))[0]) + lat
|
||||
pc2, lab = inject(pc, guide.layout_full, gres.plane, cat[name],
|
||||
lat_t = float(np.clip(sc.lat_base + sc.v_lat * dt, -1.25, 1.25))
|
||||
u_obj = float(gres.corridor.centre(np.array([d_true], np.float32))[0]) + lat_t
|
||||
pc2, lab = inject(pc, guide.layout_full, gres.plane, cat[sc.name],
|
||||
Placement(d=d_true, u=u_obj), rng=rng, env=env)
|
||||
res = pipes[i].process(pc2)
|
||||
res = sc.pipe.process(pc2)
|
||||
if res is None:
|
||||
continue
|
||||
tol = max(3.0, 0.12 * d_true)
|
||||
|
|
@ -121,7 +181,7 @@ def run_bag(bag_path, params: Params, memory, limit: int, d_start: float,
|
|||
# фона, нет трека — к сопоставлению по кадрам, нет решения —
|
||||
# к порогу. Без этого разбиения улучшать нечего, кроме удачи.
|
||||
cand = any(abs(c.d - d_true) < tol for c in res.candidates)
|
||||
cx = pipes[i].cx
|
||||
cx = sc.pipe.cx
|
||||
trk = any(abs(tr.distance(cx.s_world) - d_true) < tol
|
||||
for tr in cx.tracks)
|
||||
records[i].append((d_true, int(hit), fp, lab["hit_rays"],
|
||||
|
|
@ -213,6 +273,21 @@ def main() -> None:
|
|||
help="к какой дальности вклад знакомости обнуляется")
|
||||
ap.add_argument("--mbon-prior-from", type=float, default=None,
|
||||
help="с какой дальности поправлять оценку модели на распространённость предметов; 0 — не поправлять")
|
||||
ap.add_argument("--augment", action="store_true",
|
||||
help="включить случайные аугментации: d_start ∈ [d_min, d_max], "
|
||||
"боковой дрейф v_lat, шум по дальности δs и стоячие поезда")
|
||||
ap.add_argument("--d-start-min", type=float, default=40.0,
|
||||
help="минимальная дальность спавна при аугментации, м")
|
||||
ap.add_argument("--d-start-max", type=float, default=200.0,
|
||||
help="максимальная дальность спавна при аугментации, м")
|
||||
ap.add_argument("--v-lat-max", type=float, default=0.35,
|
||||
help="максимальная боковая скорость препятствия (дрейф), м/с")
|
||||
ap.add_argument("--s-noise-std", type=float, default=0.12,
|
||||
help="СКО шума по продольной координате/дальности (джиттер лучей), м")
|
||||
ap.add_argument("--respawn", action="store_true",
|
||||
help="пересоздавать препятствие впереди при приближении ближе 6 м")
|
||||
ap.add_argument("--static-prob", type=float, default=0.15,
|
||||
help="вероятность сценария со стоячим поездом (ds = 0)")
|
||||
P.add_argument(ap)
|
||||
args = ap.parse_args()
|
||||
|
||||
|
|
@ -289,8 +364,17 @@ def main() -> None:
|
|||
print(f" внимание: для {p.name} нет своей складки — пропуск")
|
||||
continue
|
||||
trk = track_paths.get(p.name, args.track) if track_paths else args.track
|
||||
aug_params = {
|
||||
"augment": args.augment,
|
||||
"d_start_min": args.d_start_min,
|
||||
"d_start_max": args.d_start_max,
|
||||
"v_lat_max": args.v_lat_max,
|
||||
"s_noise_std": args.s_noise_std,
|
||||
"respawn": args.respawn,
|
||||
"static_prob": args.static_prob,
|
||||
}
|
||||
tasks.append((p, params, args.limit, args.d_start, laterals, args.seed,
|
||||
args.memory or "", rd, bool(args.tracks_out), trk))
|
||||
args.memory or "", rd, bool(args.tracks_out), trk, aug_params))
|
||||
|
||||
# Печатается по готовности, собирается по номеру задачи: порядок сценариев
|
||||
# в файле не должен зависеть от того, какой бэг досчитался первым.
|
||||
|
|
|
|||
Loading…
Reference in a new issue