forked from Dan4ick/Lidar_Muxa
аугментации бенчмарка, зонный пол в колее, фильтр платформы, экспорт 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: юнит-тесты и автономный раннер.
This commit is contained in:
Данил Омелечко
parent
d39038184b
commit
3c59e1f87a
10 changed files with 945 additions and 28 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`.
|
||||||
|
|
@ -310,9 +310,14 @@ def _quality(c: Candidate, use_shape: bool = True, mbon_power: float = 1.0,
|
||||||
contrast = max(contrast, float(np.clip(acc, 0.2, 1.0)))
|
contrast = max(contrast, float(np.clip(acc, 0.2, 1.0)))
|
||||||
if use_shape:
|
if use_shape:
|
||||||
whole = float(np.clip((c.containment - 0.25) / 0.45, 0.05, 1.0))
|
whole = float(np.clip((c.containment - 0.25) / 0.45, 0.05, 1.0))
|
||||||
# протяжённость вдоль пути сверх собственного размера — признак конструкции
|
|
||||||
span = max(c.width, c.height, 0.2)
|
span = max(c.width, c.height, 0.2)
|
||||||
compact = float(np.clip(1.5 - c.depth / (3.0 * span), 0.1, 1.0))
|
# Предмет или человек, лежащий в колее вдоль пути (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))
|
||||||
# опора снизу: упавший предмет, человек, камень стоят на полотне, а знак,
|
# опора снизу: упавший предмет, человек, камень стоят на полотне, а знак,
|
||||||
# лоток или кронштейн висят на стене, и под ними пусто
|
# лоток или кронштейн висят на стене, и под ними пусто
|
||||||
grounded = float(np.clip(1.25 - c.h_min / 1.2, 0.15, 1.0))
|
grounded = float(np.clip(1.25 - c.h_min / 1.2, 0.15, 1.0))
|
||||||
|
|
|
||||||
|
|
@ -113,8 +113,9 @@ class DescendingNeurons:
|
||||||
ready: list[tuple[Track, float]] = []
|
ready: list[tuple[Track, float]] = []
|
||||||
for t in cx.tracks:
|
for t in cx.tracks:
|
||||||
d = t.distance(cx.s_world)
|
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
|
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)
|
self._latched.discard(t.id)
|
||||||
continue
|
continue
|
||||||
ready.append((t, d))
|
ready.append((t, d))
|
||||||
|
|
@ -141,7 +142,19 @@ class DescendingNeurons:
|
||||||
b = min(max(self.track_blend, 0.0), 1.0)
|
b = min(max(self.track_blend, 0.0), 1.0)
|
||||||
score = (max(q, 1e-4) ** b
|
score = (max(q, 1e-4) ** b
|
||||||
* max(t.evidence, 1e-4) ** (1.0 - 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)
|
self._latched.discard(t.id)
|
||||||
continue
|
continue
|
||||||
self._latched.add(t.id)
|
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,
|
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)
|
lat = tf.lateral(corridor)
|
||||||
|
abs_lat = np.abs(lat)
|
||||||
in_range = tf.valid & (tf.d > d_min) & (tf.d < d_max)
|
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
|
return inside, lat, in_range
|
||||||
|
|
||||||
|
|
||||||
|
|
@ -237,7 +258,11 @@ def find_candidates(tf: TrackFrame, lam: LaminaOutput, corridor: Corridor, *,
|
||||||
near_long: float = 0.0,
|
near_long: float = 0.0,
|
||||||
split_top: int = 0,
|
split_top: int = 0,
|
||||||
advance: np.ndarray | None = None,
|
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:
|
if masks is None:
|
||||||
masks = gauge_mask(tf, corridor, half_width=half_width, h_lo=h_lo,
|
masks = gauge_mask(tf, corridor, half_width=half_width, h_lo=h_lo,
|
||||||
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
|
inside, lat, in_range = masks
|
||||||
if not inside.any():
|
if not inside.any():
|
||||||
return []
|
return []
|
||||||
|
|
@ -259,8 +288,20 @@ def find_candidates(tf: TrackFrame, lam: LaminaOutput, corridor: Corridor, *,
|
||||||
# Вверх, наоборот, нужно доставать до свода: колонна и рама крепи идут от
|
# Вверх, наоборот, нужно доставать до свода: колонна и рама крепи идут от
|
||||||
# полотна до потолка, и если обрезать контекст на полпути, срез сам выглядит
|
# полотна до потолка, и если обрезать контекст на полпути, срез сам выглядит
|
||||||
# компактным предметом ростом с габарит.
|
# компактным предметом ростом с габарит.
|
||||||
context = (in_range & (np.abs(lat) < half_width + 1.6)
|
abs_lat = np.abs(lat)
|
||||||
& (tf.h > h_lo) & (tf.h < h_hi + ctx_up))
|
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)
|
gap_img = contrast_to_depth_gap(lam.on, tf.r)
|
||||||
|
|
||||||
|
|
|
||||||
|
|
@ -36,6 +36,10 @@ class Params:
|
||||||
fov_deg: float = 30.0 # полусектор обработки по азимуту
|
fov_deg: float = 30.0 # полусектор обработки по азимуту
|
||||||
half_width: float = 1.6 # полуширина габарита, м
|
half_width: float = 1.6 # полуширина габарита, м
|
||||||
h_lo: float = 0.28 # нижняя граница габарита над рельсом, м
|
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 # верхняя граница, м
|
h_hi: float = 2.3 # верхняя граница, м
|
||||||
ctx_up: float = 4.0 # насколько контекст лезет выше габарита, м
|
ctx_up: float = 4.0 # насколько контекст лезет выше габарита, м
|
||||||
# Разделение фигуры и фона по движению (см. lobula.split_by_figure):
|
# Разделение фигуры и фона по движению (см. lobula.split_by_figure):
|
||||||
|
|
@ -124,9 +128,9 @@ class Params:
|
||||||
# (3.3 на км в обоих случаях). Замер — EXPERIMENTS п. 11.3.
|
# (3.3 на км в обоих случаях). Замер — EXPERIMENTS п. 11.3.
|
||||||
min_hits: int = 2
|
min_hits: int = 2
|
||||||
novelty_floor: float = 0.10
|
novelty_floor: float = 0.10
|
||||||
warn_far: float = 0.5 # порог тревоги на дальнем краю
|
warn_far: float = 0.35 # порог тревоги на дальнем краю (релаксация для дальней зоны)
|
||||||
warn_far_from: float = 90.0 # с какой дальности он начинает падать
|
warn_far_from: float = 90.0 # с какой дальности он начинает падать
|
||||||
leak_far: float = 0.12 # утечка улики на дальнем краю
|
leak_far: float = 0.06 # утечка улики на дальнем краю (компенсирует прореживание лучей)
|
||||||
leak_far_from: float = 90.0 # с какой дальности она начинает падать
|
leak_far_from: float = 90.0 # с какой дальности она начинает падать
|
||||||
d_min: float = 4.0
|
d_min: float = 4.0
|
||||||
d_max: float = 220.0
|
d_max: float = 220.0
|
||||||
|
|
@ -328,7 +332,11 @@ class FlyGuard:
|
||||||
with t("lobula"):
|
with t("lobula"):
|
||||||
masks = gauge_mask(tf, self.corridor, half_width=self.p.half_width,
|
masks = gauge_mask(tf, self.corridor, half_width=self.p.half_width,
|
||||||
h_lo=self.p.h_lo, h_hi=self.p.h_hi,
|
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,
|
cands = find_candidates(tf, lam, self.corridor, masks=masks,
|
||||||
half_width=self.p.half_width, h_lo=self.p.h_lo,
|
half_width=self.p.half_width, h_lo=self.p.h_lo,
|
||||||
h_hi=self.p.h_hi, ctx_up=self.p.ctx_up,
|
h_hi=self.p.h_hi, ctx_up=self.p.ctx_up,
|
||||||
|
|
@ -340,7 +348,11 @@ class FlyGuard:
|
||||||
d_min=self.p.d_min,
|
d_min=self.p.d_min,
|
||||||
d_max=self.p.d_max, min_rays=self.p.min_rays,
|
d_max=self.p.d_max, min_rays=self.p.min_rays,
|
||||||
min_rays_far=self.p.min_rays_far,
|
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"):
|
with t("mushroom"):
|
||||||
cands = self.memory.annotate(cands)
|
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),
|
Sphere(radius=0.11, h_centre=1.60)], reflectivity=0.35),
|
||||||
"человек_сидя": ObjectModel("человек_сидя", [
|
"человек_сидя": ObjectModel("человек_сидя", [
|
||||||
Box(0.45, 0.50, 0.85)], reflectivity=0.35),
|
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.60, 0.60, 0.60)], reflectivity=0.40),
|
||||||
"чемодан": ObjectModel("чемодан", [Box(0.25, 0.45, 0.55)], reflectivity=0.30),
|
"чемодан": ObjectModel("чемодан", [Box(0.25, 0.45, 0.55)], reflectivity=0.30),
|
||||||
"ведро": ObjectModel("ведро", [Cylinder(radius=0.15, height=0.35)], reflectivity=0.45),
|
"ведро": 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()
|
||||||
|
|
@ -517,3 +517,168 @@ def test_track_accumulators_do_not_touch_the_evidence():
|
||||||
t = cx.tracks[0]
|
t = cx.tracks[0]
|
||||||
assert t.n_obs == 1 and t.w_mean > 0.0
|
assert t.n_obs == 1 and t.w_mean > 0.0
|
||||||
assert abs(t.evidence - cx.gain * t.w_mean) < 1e-6
|
assert abs(t.evidence - cx.gain * t.w_mean) < 1e-6
|
||||||
|
|
||||||
|
|
||||||
|
# ------------------------------------------------------------ улучшения габарита и решения
|
||||||
|
|
||||||
|
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
|
from __future__ import annotations
|
||||||
|
|
||||||
import argparse
|
import argparse
|
||||||
|
from dataclasses import dataclass
|
||||||
import json
|
import json
|
||||||
|
|
||||||
import numpy as np
|
import numpy as np
|
||||||
|
|
@ -31,14 +32,31 @@ from flyguard.track_readout import describe_track
|
||||||
HOLDOUT = "doubleT_obstacle" # там уже есть настоящий объект
|
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):
|
def _work(task):
|
||||||
"""Одна задача — один бэг.
|
"""Одна задача — один бэг.
|
||||||
|
|
||||||
Обученное грузится путями и уже внутри процесса: передавать модели через
|
Обученное грузится путями и уже внутри процесса: передавать модели через
|
||||||
межпроцессную границу незачем, а свою складку каждый воркер берёт сам.
|
межпроцессную границу незачем, а свою складку каждый воркер берёт сам.
|
||||||
"""
|
"""
|
||||||
(path, params, limit, d_start, laterals, seed, mem_path, rd_path,
|
if len(task) >= 11:
|
||||||
want_tracks, trk_path) = task
|
(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
|
memory = MushroomBody.load(mem_path) if mem_path else None
|
||||||
readout = None
|
readout = None
|
||||||
if rd_path:
|
if rd_path:
|
||||||
|
|
@ -50,7 +68,7 @@ def _work(task):
|
||||||
track_rd = TrackReadout.load(trk_path)
|
track_rd = TrackReadout.load(trk_path)
|
||||||
return run_bag(path, params, memory, limit, d_start, laterals, seed,
|
return run_bag(path, params, memory, limit, d_start, laterals, seed,
|
||||||
readout=readout, want_tracks=want_tracks,
|
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):
|
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,
|
def run_bag(bag_path, params: Params, memory, limit: int, d_start: float,
|
||||||
laterals: tuple[float, ...], seed: int, readout=None,
|
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)
|
bag = Bag(bag_path)
|
||||||
s_track, _ = ego_track(bag, params, limit)
|
s_track, _ = ego_track(bag, params, limit)
|
||||||
have = [x for x in s_track if x is not None]
|
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()
|
cat = catalogue()
|
||||||
scen = [(name, lat) for name in cat for lat in laterals]
|
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)
|
rng = np.random.default_rng(seed)
|
||||||
records = [[] for _ in scen]
|
records = [[] for _ in scen]
|
||||||
# Выборка для считывания по трекам: описание каждого живого трека на
|
# Выборка для считывания по трекам: описание каждого живого трека на
|
||||||
|
|
@ -90,6 +116,22 @@ def run_bag(bag_path, params: Params, memory, limit: int, d_start: float,
|
||||||
# пороги. Обученное на другой обстановке считывание нечего и мерить.
|
# пороги. Обученное на другой обстановке считывание нечего и мерить.
|
||||||
trk_rows: list = []
|
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)
|
guide = FlyGuard(params, memory=None)
|
||||||
|
|
||||||
|
|
@ -99,17 +141,35 @@ def run_bag(bag_path, params: Params, memory, limit: int, d_start: float,
|
||||||
continue
|
continue
|
||||||
s_now = s_track[k] - have[0]
|
s_now = s_track[k] - have[0]
|
||||||
env = IntensityEnv(pc) # один раз на кадр, общий для сценариев
|
env = IntensityEnv(pc) # один раз на кадр, общий для сценариев
|
||||||
for i, (name, lat) in enumerate(scen):
|
for i, sc in enumerate(scenarios):
|
||||||
d_true = d_start - s_now
|
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 d_true < 6.0:
|
||||||
continue
|
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 м при
|
# Предмет лежит НА ПУТИ, а путь в кривой уходит вбок: на 150 м при
|
||||||
# радиусе 1300 м это 8.6 м. Если ставить его в поперечных координатах
|
# радиусе 1300 м это 8.6 м. Если ставить его в поперечных координатах
|
||||||
# сенсора, он окажется в стене, а не в габарите.
|
# сенсора, он окажется в стене, а не в габарите.
|
||||||
u_obj = float(gres.corridor.centre(np.array([d_true], np.float32))[0]) + lat
|
lat_t = float(np.clip(sc.lat_base + sc.v_lat * dt, -1.25, 1.25))
|
||||||
pc2, lab = inject(pc, guide.layout_full, gres.plane, cat[name],
|
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)
|
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:
|
if res is None:
|
||||||
continue
|
continue
|
||||||
tol = max(3.0, 0.12 * d_true)
|
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)
|
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
|
trk = any(abs(tr.distance(cx.s_world) - d_true) < tol
|
||||||
for tr in cx.tracks)
|
for tr in cx.tracks)
|
||||||
records[i].append((d_true, int(hit), fp, lab["hit_rays"],
|
records[i].append((d_true, int(hit), fp, lab["hit_rays"],
|
||||||
|
|
@ -211,6 +271,21 @@ def main() -> None:
|
||||||
help="к какой дальности вклад знакомости обнуляется")
|
help="к какой дальности вклад знакомости обнуляется")
|
||||||
ap.add_argument("--mbon-prior-from", type=float, default=None,
|
ap.add_argument("--mbon-prior-from", type=float, default=None,
|
||||||
help="с какой дальности поправлять оценку модели на распространённость предметов; 0 — не поправлять")
|
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)
|
P.add_argument(ap)
|
||||||
args = ap.parse_args()
|
args = ap.parse_args()
|
||||||
|
|
||||||
|
|
@ -285,8 +360,17 @@ def main() -> None:
|
||||||
print(f" внимание: для {p.name} нет своей складки — пропуск")
|
print(f" внимание: для {p.name} нет своей складки — пропуск")
|
||||||
continue
|
continue
|
||||||
trk = track_paths.get(p.name, args.track) if track_paths else args.track
|
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,
|
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