forked from Dan4ick/Lidar_Muxa
828 lines
36 KiB
Python
828 lines
36 KiB
Python
"""Тесты конвейера, не требующие ROS.
|
||
|
||
Проверяется то, что легко сломать незаметно: разбор CDR, порядок точек,
|
||
выпрямление скоса каналов, геометрия плоскости пути, связность с учётом
|
||
глубины, кодирование грибовидного тела и вставка синтетического предмета.
|
||
|
||
pytest ros2_ws/src/flyguard/test
|
||
"""
|
||
from __future__ import annotations
|
||
|
||
import sys
|
||
from pathlib import Path
|
||
|
||
import numpy as np
|
||
import pytest
|
||
|
||
ROOT = Path(__file__).resolve().parents[1]
|
||
if str(ROOT) not in sys.path:
|
||
sys.path.insert(0, str(ROOT))
|
||
|
||
from flyguard.cdr import point_dtype # noqa: E402
|
||
from flyguard.geometry import RailPlane # noqa: E402
|
||
from flyguard.lobula import cluster_by_depth # noqa: E402
|
||
from flyguard.mushroom_body import MushroomBody, MushroomBodyConfig # noqa: E402
|
||
from flyguard.retina import ScanLayout # noqa: E402
|
||
|
||
DATA = ROOT / "data" / "for_hackathon"
|
||
if not DATA.exists():
|
||
DATA = ROOT.parent / "data" / "for_hackathon"
|
||
|
||
|
||
# --------------------------------------------------------------------------- CDR
|
||
|
||
def test_point_dtype_handles_unaligned_timestamp():
|
||
"""У лидара поле timestamp (float64) лежит по смещению 18 — без выравнивания."""
|
||
fields = [("x", 0, 7, 1), ("y", 4, 7, 1), ("z", 8, 7, 1), ("intensity", 12, 7, 1),
|
||
("ring", 16, 4, 1), ("timestamp", 18, 8, 1)]
|
||
dt = point_dtype(fields, 26)
|
||
assert dt.itemsize == 26
|
||
assert dt["timestamp"].itemsize == 8
|
||
|
||
|
||
def test_point_dtype_pads_trailing_gap():
|
||
"""Хвост до point_step добивается паддингом — так устроено само сообщение."""
|
||
assert point_dtype([("x", 0, 7, 1)], 26).itemsize == 26
|
||
|
||
|
||
def test_point_dtype_rejects_overlapping_fields():
|
||
with pytest.raises(ValueError, match="перекрыва"):
|
||
point_dtype([("x", 0, 8, 1), ("y", 4, 7, 1)], 26)
|
||
|
||
|
||
# --------------------------------------------------------------------------- решётка
|
||
|
||
def _layout(n_rings=8, n_az=40, n_echo=2, shift=None):
|
||
el = np.linspace(6.0, -6.0, n_rings)
|
||
shift = np.zeros(n_rings, np.int64) if shift is None else shift
|
||
return ScanLayout(el, -0.1, 2.0, shift, np.zeros(n_rings), n_az, n_echo)
|
||
|
||
|
||
def test_rectification_undoes_channel_skew():
|
||
"""Сдвиг строк должен ровно компенсировать азимутальный сдвиг канала."""
|
||
shift = np.array([-2, -1, 0, 1, 2, 0, -1, 1], np.int64)
|
||
lay = _layout(shift=shift)
|
||
j = np.arange(lay.n_az)
|
||
for h in range(lay.n_rings):
|
||
expected = np.clip(j + shift[h], 0, lay.n_az - 1)
|
||
assert np.array_equal(lay.gather[h], expected)
|
||
assert lay.gather_ok[2].all() # нулевой сдвиг — потерь нет
|
||
|
||
|
||
def test_azimuth_grid_is_monotonic_and_centred():
|
||
lay = _layout(n_az=41)
|
||
d = np.diff(lay.az_grid_deg)
|
||
assert np.allclose(d, -0.1)
|
||
assert lay.dirs.shape == (lay.n_rings, lay.n_az, 3)
|
||
assert np.allclose(np.linalg.norm(lay.dirs, axis=-1), 1.0, atol=1e-5)
|
||
|
||
|
||
def test_forward_direction_is_minus_y():
|
||
"""Азимут 0 смотрит вперёд, а вперёд в системе сенсора — это −Y."""
|
||
lay = ScanLayout(np.array([0.0]), -0.1, 0.0, np.zeros(1, np.int64),
|
||
np.zeros(1), 1, 1)
|
||
assert lay.dirs[0, 0, 1] == pytest.approx(-1.0, abs=1e-6)
|
||
|
||
|
||
# --------------------------------------------------------------------------- геометрия
|
||
|
||
def test_floor_range_matches_flat_plane():
|
||
"""Луч вниз под углом θ проходит до полотна H/sin θ — это дальность вдоль
|
||
луча, а не горизонтальное расстояние: именно её сравнивают с измеренной."""
|
||
height = 2.0
|
||
plane = RailPlane(a=0.0, b=0.0, c=-height, inliers=0, rms=0.0)
|
||
for el in (-1.0, -3.0, -10.0):
|
||
lay = ScanLayout(np.array([el]), -0.1, 0.0, np.zeros(1, np.int64),
|
||
np.zeros(1), 1, 1)
|
||
got = plane.floor_range(lay)[0, 0]
|
||
assert got == pytest.approx(height / np.sin(np.radians(-el)), rel=1e-3)
|
||
|
||
|
||
def test_upward_rays_never_hit_floor():
|
||
plane = RailPlane(a=0.0, b=0.0, c=-2.0, inliers=0, rms=0.0)
|
||
lay = ScanLayout(np.array([5.0]), -0.1, 0.0, np.zeros(1, np.int64), np.zeros(1), 1, 1)
|
||
assert not np.isfinite(plane.floor_range(lay)[0, 0])
|
||
|
||
|
||
def test_height_above_plane_accounts_for_tilt():
|
||
plane = RailPlane(a=0.01, b=-0.02, c=-1.5, inliers=0, rms=0.0)
|
||
d = np.array([10.0]); u = np.array([2.0]); z = np.array([0.0])
|
||
expected = 0.0 - (0.01 * 10.0 + (-0.02) * 2.0 + (-1.5))
|
||
assert plane.height_of(d, u, z)[0] == pytest.approx(expected)
|
||
|
||
|
||
# --------------------------------------------------------------------------- кластеризация
|
||
|
||
def test_depth_aware_clustering_splits_on_range_gap():
|
||
"""Предмет перед далёкой стеной не должен слипнуться с ней в одно пятно."""
|
||
mask = np.zeros((4, 10), bool)
|
||
mask[:, :] = True
|
||
r = np.full((4, 10), 150.0, np.float32)
|
||
r[:, 3:6] = 55.0 # предмет на 55 м на фоне 150 м
|
||
labels, n = cluster_by_depth(mask, r, col_reach=1, row_reach=1)
|
||
assert n >= 2
|
||
assert len(set(labels[:, 3:6].ravel())) == 1
|
||
assert labels[0, 0] != labels[0, 4]
|
||
|
||
|
||
def test_clustering_tolerance_grows_with_range():
|
||
"""Допуск относительный: разрыв 1 м слитен на 150 м и разделим на 5 м."""
|
||
mask = np.ones((1, 6), bool)
|
||
far = np.array([[150.0, 150.0, 151.0, 151.0, 150.0, 150.0]], np.float32)
|
||
near = np.array([[5.0, 5.0, 6.0, 6.0, 5.0, 5.0]], np.float32)
|
||
_, n_far = cluster_by_depth(mask, far, col_reach=1, row_reach=0)
|
||
_, n_near = cluster_by_depth(mask, near, col_reach=1, row_reach=0)
|
||
assert n_far == 1
|
||
assert n_near >= 2
|
||
|
||
|
||
def test_empty_mask_is_handled():
|
||
labels, n = cluster_by_depth(np.zeros((3, 3), bool), np.zeros((3, 3), np.float32))
|
||
assert n == 0 and labels.sum() == 0
|
||
|
||
|
||
# --------------------------------------------------------------------------- память
|
||
|
||
def test_mushroom_body_code_is_sparse_and_deterministic():
|
||
mb = MushroomBody(MushroomBodyConfig(n_kc=2000, sparsity=0.01, seed=1))
|
||
X = np.random.default_rng(0).normal(size=(5, mb.n_pn)).astype(np.float32)
|
||
mb.fit_normalizer(X)
|
||
a, b = mb.encode(X), mb.encode(X)
|
||
assert a.shape == (5, mb.n_active)
|
||
assert mb.n_active == 20
|
||
assert np.array_equal(a, b)
|
||
assert len(set(a[0].tolist())) == mb.n_active # без повторов
|
||
|
||
|
||
def test_learning_suppresses_only_what_was_shown():
|
||
mb = MushroomBody(MushroomBodyConfig(n_kc=4000, sparsity=0.01, seed=2))
|
||
rng = np.random.default_rng(3)
|
||
familiar = rng.normal(size=(200, mb.n_pn)).astype(np.float32)
|
||
novel = (rng.normal(size=(50, mb.n_pn)) + 8.0).astype(np.float32)
|
||
mb.fit_normalizer(familiar)
|
||
mb.learn(familiar, rate=0.3)
|
||
assert mb.novelty(familiar).mean() < mb.novelty(novel).mean()
|
||
|
||
|
||
def test_auto_rate_shrinks_with_sample_size():
|
||
mb = MushroomBody(MushroomBodyConfig(n_kc=10_000, sparsity=0.001))
|
||
assert mb.auto_rate(1_000) > mb.auto_rate(100_000)
|
||
assert 0 < mb.auto_rate(10 ** 7) <= 0.5
|
||
|
||
|
||
def test_feature_count_mismatch_is_explicit():
|
||
mb = MushroomBody()
|
||
with pytest.raises(ValueError, match="признак"):
|
||
mb.encode(np.zeros((1, mb.n_pn + 1), np.float32))
|
||
|
||
|
||
# --------------------------------------------------------- фигура и фон, привыкание
|
||
|
||
def _cand(**kw):
|
||
from flyguard.lobula import Candidate
|
||
|
||
base = dict(d=60.0, u=0.1, h=1.0, d_min=59.7, h_min=0.3, width=0.4, height=1.0,
|
||
depth=0.5, containment=0.9, n_rays=30, n_rings=6, n_cols=5,
|
||
gap=8.0, on=0.001, floor_deficit=0.2, shadow=0.2, inten=40.0,
|
||
az_deg=0.5, el_deg=-1.0, bbox=(10, 16, 20, 25))
|
||
base.update(kw)
|
||
return Candidate(**base)
|
||
|
||
|
||
def test_contrast_split_keeps_step_and_drops_smooth_wall():
|
||
"""Гладкая стена не даёт фигуры, ступенька на ней — даёт."""
|
||
from flyguard.lobula import cluster_by_depth, split_by_figure
|
||
|
||
# стена: дальность плавно растёт вдоль строки от 20 до 120 м
|
||
r = np.tile(np.linspace(20.0, 120.0, 200, dtype=np.float32), (12, 1))
|
||
mask = np.ones(r.shape, bool)
|
||
labels, n = cluster_by_depth(mask, r, col_reach=3)
|
||
assert n == 1 # стена связна целиком
|
||
|
||
# контраст гладкой стены равен нулю — резать нечего
|
||
flat = np.zeros_like(r)
|
||
out, n_out = split_by_figure(labels, n, r, flat, max_depth=15.0, thr=6.0)
|
||
assert n_out == n and out is labels
|
||
|
||
# предмет: ступенька, торчащая из стены на 10 м
|
||
gap = np.zeros_like(r)
|
||
gap[4:8, 90:98] = 10.0
|
||
out, n_out = split_by_figure(labels, n, r, gap, max_depth=15.0, thr=6.0)
|
||
kept = out[out > 0]
|
||
assert kept.size == 32 # ровно лучи ступеньки
|
||
assert np.unique(kept).size == 1 # и это одна компонента
|
||
|
||
|
||
def test_habituation_suppresses_shape_repeating_at_different_places():
|
||
from flyguard.mushroom_body import Habituation, HabituationConfig
|
||
|
||
hab = Habituation(HabituationConfig(warmup=0, norm_n=1))
|
||
seen = []
|
||
s = 0.0
|
||
for _ in range(14):
|
||
s += 25.0
|
||
c = _cand(d=60.0)
|
||
hab.update([c], s, 25.0)
|
||
seen.append(c.novelty)
|
||
assert seen[0] > 0.9 and seen[-1] < 0.25
|
||
assert hab.places >= 10
|
||
|
||
|
||
def test_habituation_keeps_object_standing_in_one_place_novel():
|
||
"""Подъезд к неподвижному предмету — это одно место, а не сто повторов."""
|
||
from flyguard.mushroom_body import Habituation, HabituationConfig
|
||
|
||
hab = Habituation(HabituationConfig(warmup=0, norm_n=1))
|
||
s = 0.0
|
||
nov = []
|
||
for k in range(50):
|
||
s += 3.0
|
||
nov.append(_cand(d=160.0 - 3.0 * k))
|
||
hab.update([nov[-1]], s, 3.0)
|
||
assert hab.places <= 2 # одно место (плюс дрейф оценки)
|
||
assert nov[-1].novelty > 0.7
|
||
|
||
|
||
def test_habituation_forgets_after_a_long_run():
|
||
from flyguard.mushroom_body import Habituation, HabituationConfig
|
||
|
||
hab = Habituation(HabituationConfig(warmup=0, norm_n=1, recover_m=100.0))
|
||
s = 0.0
|
||
for _ in range(14):
|
||
s += 25.0
|
||
hab.update([_cand(d=60.0)], s, 25.0)
|
||
low = hab.level
|
||
hab.advance(600.0)
|
||
assert hab.level < low * 0.2
|
||
|
||
|
||
# --------------------------------------------------------------- считывание MBON
|
||
|
||
def test_mbon_readout_learns_and_round_trips(tmp_path):
|
||
"""Обучается, сохраняется без потерь и даёт калиброванную вероятность."""
|
||
from flyguard.mbon_readout import MbonConfig, MbonReadout
|
||
|
||
rng = np.random.default_rng(3)
|
||
X = rng.standard_normal((2000, 23)).astype(np.float32)
|
||
y = ((X[:, 0] + 0.5 * X[:, 3]) > 0.3).astype(np.float32)
|
||
m = MbonReadout(MbonConfig(n_kc=4000, sparsity=0.02), n_pn=23)
|
||
m.fit_normalizer(X)
|
||
m.learn(X, y, epochs=60, lr=4.0)
|
||
|
||
s = m.score(X)
|
||
assert s.min() >= 0.0 and s.max() <= 1.0
|
||
assert ((s > 0.5) == (y > 0.5)).mean() > 0.8 # калибровка, не только порядок
|
||
|
||
path = tmp_path / "mbon.npz"
|
||
m.save(path)
|
||
again = MbonReadout.load(path)
|
||
assert np.allclose(again.score(X[:64]), s[:64], atol=1e-6)
|
||
|
||
|
||
def test_mbon_replaces_hand_formula_and_blend_interpolates():
|
||
"""Модель входит в вес улики, а смешивание даёт обе крайности."""
|
||
from flyguard.central_complex import _quality
|
||
|
||
c = _cand(gap=0.0, containment=0.3, depth=9.0, h_min=1.4, n_rays=5)
|
||
hand = _quality(c) # без модели
|
||
c.extra["mbon"] = 0.99
|
||
assert _quality(c, mbon_blend=1.0) > hand * 3 # модель вытягивает
|
||
assert _quality(c, mbon_blend=0.0) == hand # ручная формула нетронута
|
||
mid = _quality(c, mbon_blend=0.5)
|
||
assert hand < mid < _quality(c, mbon_blend=1.0)
|
||
|
||
# уверенное «это обстановка» гасит даже хорошую геометрию
|
||
good = _cand(gap=12.0, containment=1.0, depth=0.4, h_min=0.3, n_rays=80)
|
||
strong = _quality(good)
|
||
good.extra["mbon"] = 0.01
|
||
assert _quality(good, mbon_blend=1.0) < strong * 0.2
|
||
|
||
|
||
def test_mbon_absent_leaves_pipeline_unchanged():
|
||
"""Без модели вес улики считается ровно как раньше."""
|
||
from flyguard.central_complex import _quality
|
||
|
||
c = _cand()
|
||
assert "mbon" not in c.extra
|
||
assert _quality(c, mbon_blend=1.0) == _quality(c, mbon_blend=0.0)
|
||
|
||
|
||
def test_far_warning_threshold_only_relaxes_beyond_its_distance():
|
||
"""Послабление далёким трекам не должно трогать ближнюю зону."""
|
||
from flyguard.descending import DescendingNeurons
|
||
|
||
flat = DescendingNeurons()
|
||
assert flat._warn_at(10.0) == flat._warn_at(199.0) == flat.warn_evidence
|
||
|
||
dn = DescendingNeurons(warn_evidence=0.5, warn_far=0.3,
|
||
warn_far_from=90.0, max_range=200.0)
|
||
assert dn._warn_at(50.0) == 0.5 # ближняя зона не тронута
|
||
assert dn._warn_at(90.0) == 0.5 # ровно на границе — тоже
|
||
assert abs(dn._warn_at(145.0) - 0.4) < 1e-6 # середина
|
||
assert abs(dn._warn_at(200.0) - 0.3) < 1e-6 # дальний край
|
||
assert abs(dn._warn_at(400.0) - 0.3) < 1e-6 # и не ниже него
|
||
|
||
|
||
def test_novelty_fades_with_range_and_is_intact_up_close():
|
||
"""Знакомость должна работать вблизи и не мешать вдали."""
|
||
from flyguard.central_complex import _quality
|
||
|
||
near, far = _cand(), _cand()
|
||
near.d, far.d = 40.0, 160.0
|
||
near.novelty = far.novelty = 0.15 # «знакомо», как далёкий предмет
|
||
|
||
# вблизи гашения нет: знакомое остаётся придавленным
|
||
a = _quality(near, nov_fade_from=0.0)
|
||
b = _quality(near, nov_fade_from=90.0)
|
||
assert abs(a - b) < 1e-9
|
||
|
||
# вдали вклад знакомости снят целиком
|
||
c = _quality(far, nov_fade_from=0.0)
|
||
d = _quality(far, nov_fade_from=90.0, nov_fade_to=160.0)
|
||
assert d > c
|
||
|
||
# и в промежутке — частично
|
||
mid = _cand(); mid.d, mid.novelty = 125.0, 0.15
|
||
m = _quality(mid, nov_fade_from=90.0, nov_fade_to=160.0)
|
||
assert _quality(mid, nov_fade_from=0.0) < m < d
|
||
|
||
|
||
# ------------------------------------------------------- модель сенсора: яркость
|
||
|
||
def test_injected_intensity_copies_the_surroundings():
|
||
"""Яркость вставки берётся из записи, а не назначается.
|
||
|
||
Абсолютной шкалы интенсивности в данных нет: медиана кандидатов по бэгам
|
||
3…7, а в записи с настоящим предметом 23.5. Любое назначенное число делает
|
||
вставку опознаваемой по одной яркости — сначала как «тускло = предмет»
|
||
(ламбертова ρ·cosθ/r²), потом как «ярко = предмет» (постоянные 35).
|
||
Поэтому у функции нет аргументов ни дальности, ни отражения.
|
||
"""
|
||
import inspect
|
||
|
||
from flyguard.synth import local_intensity
|
||
|
||
# отражения среди аргументов нет; дальность есть, но только как выбор
|
||
# полосы реальных возвратов для лучей, ушедших в пустоту
|
||
args = set(inspect.signature(local_intensity).parameters)
|
||
assert not args & {"reflectivity", "rho", "cos_inc"}
|
||
|
||
rng = np.random.default_rng(0)
|
||
prev = np.array([10.0, 20.0, 0.0, 0.0], np.float32) # два луча в пустоту
|
||
pool = np.full(50, 30.0, np.float32)
|
||
frame = np.full(10_000, 7.0, np.float32)
|
||
v = local_intensity(prev, pool, frame, rng)
|
||
|
||
assert 8.0 < v[0] < 13.0 and 16.0 < v[1] < 25.0 # взято с тех же лучей
|
||
assert 24.0 < v[2] < 38.0 and 24.0 < v[3] < 38.0 # взято из запаса
|
||
|
||
# запас пуст — остаётся кадр целиком
|
||
v2 = local_intensity(np.zeros(200, np.float32), np.zeros(0, np.float32),
|
||
frame, rng)
|
||
assert 5.5 < float(np.median(v2)) < 9.0
|
||
|
||
# без разложения окружения по дальности сама дальность ничего не меняет
|
||
a = local_intensity(prev, pool, frame, np.random.default_rng(3), d=20.0)
|
||
b = local_intensity(prev, pool, frame, np.random.default_rng(3), d=200.0)
|
||
assert np.allclose(a, b)
|
||
|
||
|
||
# --------------------------------------------------------------------------- данные
|
||
|
||
@pytest.mark.skipif(not DATA.exists(), reason="датасет не распакован")
|
||
def test_real_bag_projects_without_angular_error():
|
||
"""На реальном бэге выпрямленная решётка обязана описывать лучи точно."""
|
||
from flyguard.bag import Bag
|
||
|
||
bag = Bag(next(p for p in DATA.iterdir() if p.is_dir()))
|
||
clouds = [pc for _, pc in bag.frames(start=2, stop=8)]
|
||
lay = ScanLayout.calibrate(clouds)
|
||
img = lay.project(clouds[-1])
|
||
|
||
assert img.shape == (lay.n_rings, lay.n_az)
|
||
assert 0.2 < img.valid.mean() < 0.9
|
||
r = img.r_near[img.valid]
|
||
assert r.min() > 0 and r.max() < 250
|
||
assert np.all(img.r_far[img.valid] >= img.r_near[img.valid] - 1e-3)
|
||
|
||
|
||
# ------------------------------------------------------------ раскладка по бэгам
|
||
|
||
_TOOLS = ROOT / "tools" # выгрузка: tools лежит рядом с тестами
|
||
if not _TOOLS.exists():
|
||
_TOOLS = ROOT.parents[2] / "tools" # основной проект: ros2_ws/src/flyguard
|
||
if str(_TOOLS) not in sys.path:
|
||
sys.path.insert(0, str(_TOOLS))
|
||
|
||
import _parallel as _P # noqa: E402
|
||
|
||
|
||
def _twice(x):
|
||
"""Задача для проверки. Верхнего уровня: иначе её не передать в процесс."""
|
||
return x * 2
|
||
|
||
|
||
def test_parallel_keeps_task_order_when_results_arrive_out_of_order():
|
||
"""Считается по готовности, складывается по номеру задачи.
|
||
|
||
Ломается это незаметно и опасно: цифры остаются правдоподобными, просто
|
||
приписываются не тому бэгу. Поэтому проверяется не «столько же строк», а
|
||
что результат каждой задачи лёг на своё место.
|
||
"""
|
||
tasks = list(range(7))
|
||
want = [(t, t * 2) for t in tasks]
|
||
|
||
seq = [None] * len(tasks)
|
||
for i, t, r, _ in _P.run(_twice, tasks, jobs=1):
|
||
seq[i] = (t, r)
|
||
|
||
par = [None] * len(tasks)
|
||
for i, t, r, _ in _P.run(_twice, tasks, jobs=4):
|
||
par[i] = (t, r)
|
||
|
||
assert seq == want
|
||
assert par == want
|
||
|
||
|
||
def test_parallel_never_starts_more_processes_than_there_are_bags():
|
||
"""Бэгов пять, и шестой процесс занять нечем."""
|
||
assert _P.resolve(0, 1) == 1 # одна задача — без пула вовсе
|
||
assert _P.resolve(8, 3) == 3 # просили больше, чем есть работы
|
||
assert _P.resolve(1, 5) == 1 # явная последовательная отладка
|
||
assert 1 <= _P.resolve(0, 5) <= 5
|
||
|
||
|
||
# ------------------------------------------------------------ решение по треку
|
||
|
||
def _pair_of_tracks():
|
||
"""Два трека с ОДИНАКОВОЙ насыщенной уликой и разным средним весом."""
|
||
from flyguard.central_complex import CentralComplex, Track
|
||
|
||
cx = CentralComplex()
|
||
cx.s_world = 0.0
|
||
good = Track(id=1, s_world=60.0, u=0.0, h=0.3, width=0.5, height=1.7,
|
||
evidence=1.0, hits=9, age=9, novelty=0.6,
|
||
n_obs=9, w_sum=9 * 0.95, p_sum=9 * 0.95)
|
||
junk = Track(id=2, s_world=70.0, u=0.5, h=0.3, width=0.5, height=1.7,
|
||
evidence=1.0, hits=9, age=9, novelty=0.6,
|
||
n_obs=9, w_sum=9 * 0.27, p_sum=9 * 0.27)
|
||
cx.tracks = [good, junk]
|
||
return cx
|
||
|
||
|
||
def test_saturated_evidence_stops_separating_and_the_mean_weight_does_not():
|
||
"""Улика обрезана единицей — и перестаёт различать.
|
||
|
||
На реальном объекте медиана улики 1.000 и у предмета, и у ложных треков
|
||
(AUC 0.624), потому что сумма упёрлась в потолок у обоих. Средний вес
|
||
наблюдения при этом 0.998 против 0.269. Здесь это проверяется на паре
|
||
треков, у которых улика одинакова НАРОЧНО.
|
||
"""
|
||
from flyguard.descending import DescendingNeurons
|
||
|
||
plain = DescendingNeurons().decide(_pair_of_tracks(), speed=10.0)
|
||
assert {o.track_id for o in plain.objects} == {1, 2}
|
||
|
||
picked = DescendingNeurons(track_blend=0.7, track_score="w_mean").decide(
|
||
_pair_of_tracks(), speed=10.0)
|
||
assert {o.track_id for o in picked.objects} == {1}
|
||
|
||
|
||
def test_track_score_without_a_model_falls_back_to_evidence():
|
||
"""`model` без файла модели обязан вести себя как раньше, а не как ноль.
|
||
|
||
Отсчёт модели по умолчанию нулевой, и молчаливое его использование
|
||
погасило бы вообще все треки.
|
||
"""
|
||
from flyguard.descending import DescendingNeurons
|
||
|
||
dn = DescendingNeurons(track_blend=0.7, track_score="model",
|
||
track_readout=None)
|
||
out = dn.decide(_pair_of_tracks(), speed=10.0)
|
||
assert {o.track_id for o in out.objects} == {1, 2}
|
||
|
||
|
||
def test_track_accumulators_do_not_touch_the_evidence():
|
||
"""Сводка по треку копится отдельно и в улику не вмешивается."""
|
||
from flyguard.central_complex import CentralComplex
|
||
from flyguard.lobula import Candidate
|
||
|
||
cx = CentralComplex()
|
||
c = Candidate(d=60.0, u=0.0, h=0.5, d_min=59.8, h_min=0.1, width=0.4,
|
||
height=1.6, depth=0.3, containment=0.9, n_rays=40,
|
||
n_rings=8, n_cols=6, gap=4.0, on=0.2, floor_deficit=0.0,
|
||
shadow=0.3, inten=12.0, az_deg=0.0, el_deg=-1.0,
|
||
bbox=(0, 0, 8, 6), novelty=0.5)
|
||
cx.update([c], ds=0.0, dt=0.1)
|
||
t = cx.tracks[0]
|
||
assert t.n_obs == 1 and t.w_mean > 0.0
|
||
assert abs(t.evidence - cx.gain * t.w_mean) < 1e-6
|
||
|
||
|
||
def test_track_p_mean_falls_back_when_there_is_no_readout():
|
||
"""Без модели покадрового считывания отсчёта нет, и ноль тут погасил бы всё."""
|
||
from flyguard.central_complex import Track
|
||
|
||
t = Track(id=1, s_world=60.0, u=0.0, h=0.3, width=0.5, height=1.7,
|
||
n_obs=4, w_sum=4 * 0.8)
|
||
assert t.p_n == 0
|
||
assert abs(t.p_mean - 0.8) < 1e-9
|
||
t.p_n, t.p_sum = 4, 4 * 0.3
|
||
assert abs(t.p_mean - 0.3) < 1e-9
|
||
|
||
|
||
def test_track_gate_removes_the_weak_track_without_touching_the_strong_one():
|
||
"""Порог ничего не отнимает у прошедшего — в отличие от смешивания."""
|
||
from flyguard.descending import DescendingNeurons
|
||
|
||
out = DescendingNeurons(track_gate=0.5, track_score="w_mean").decide(
|
||
_pair_of_tracks(), speed=10.0)
|
||
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_core_floor_is_not_lowered_closer_than_core_from():
|
||
"""Вблизи опущенный пол открывает головки рельсов — там он не действует."""
|
||
from flyguard.geometry import STRAIGHT
|
||
from flyguard.lobula import gauge_mask
|
||
|
||
tf = _MockTrackFrame(d=[20.0, 60.0, 60.0], u=[0.4, 0.4, 1.2], h=[0.20, 0.20, 0.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, core_from=55.0)
|
||
assert not inside[0] # 20 м — ближе порога, пол прежний
|
||
assert inside[1] # 60 м в колее — пол опущен
|
||
assert not inside[2] # вне колеи пол не опускается нигде
|
||
|
||
|
||
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,
|
||
far_channel=True)
|
||
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."""
|
||
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
|
||
|
||
|
||
def test_device_detection_and_fallback():
|
||
"""Проверка автоматического определения GPU устройства и fallback на CPU."""
|
||
from flyguard.device import get_device, get_device_info, is_cuda_available, to_numpy
|
||
|
||
dev = get_device("auto")
|
||
assert dev in ("cpu", "cuda") or dev.startswith("cuda:")
|
||
|
||
if not is_cuda_available():
|
||
assert get_device("cuda") == "cpu"
|
||
assert get_device("cpu") == "cpu"
|
||
|
||
info = get_device_info()
|
||
assert "device" in info
|
||
assert "torch_available" in info
|
||
|
||
arr = np.array([1.0, 2.0, 3.0], dtype=np.float32)
|
||
assert np.array_equal(to_numpy(arr), arr)
|
||
|
||
|
||
def test_lamina_device_routing():
|
||
"""Проверка работы Lamina с указанием устройства (cpu / auto)."""
|
||
from flyguard import lamina
|
||
|
||
r = np.full((16, 32), 20.0, dtype=np.float32)
|
||
r[8, 16] = 5.0 # препятствие
|
||
valid = np.ones((16, 32), dtype=bool)
|
||
|
||
out_cpu = lamina.process(r, valid, device="cpu")
|
||
out_auto = lamina.process(r, valid, device="auto")
|
||
|
||
assert out_cpu.on.shape == (16, 32)
|
||
assert out_auto.on.shape == (16, 32)
|
||
assert out_cpu.on[8, 16] > 0.0
|
||
assert np.allclose(out_cpu.on, out_auto.on, atol=1e-5)
|
||
|
||
|
||
def test_device_runtime_failure_and_fallback():
|
||
"""Проверка динамического перехода на CPU при сбое/отвале GPU в рантайме."""
|
||
from flyguard.device import is_cuda_available, get_device, notify_cuda_error, reset_device_cache
|
||
from flyguard import lamina
|
||
|
||
# Симуляция критического сбоя GPU
|
||
notify_cuda_error(RuntimeError("Simulated CUDA device disconnect / OOM"))
|
||
try:
|
||
assert not is_cuda_available()
|
||
assert get_device("cuda") == "cpu"
|
||
assert get_device("auto") == "cpu"
|
||
|
||
r = np.full((16, 32), 20.0, dtype=np.float32)
|
||
r[8, 16] = 4.0
|
||
valid = np.ones((16, 32), dtype=bool)
|
||
|
||
# Даже при явном указании device="cuda", Lamina должна успешно отработать на CPU
|
||
out = lamina.process(r, valid, device="cuda")
|
||
assert out.on.shape == (16, 32)
|
||
assert out.on[8, 16] > 0.0
|
||
finally:
|
||
reset_device_cache()
|
||
|
||
|
||
|
||
def test_scenario_stream_does_not_depend_on_the_rest_of_the_catalogue():
|
||
"""Новый предмет в каталоге не сдвигает случайность остальных сценариев.
|
||
|
||
Раньше генератор был один на запись, и сценарии тянули из него по очереди:
|
||
добавили человека лёжа — поменялись цифры по ящику. Теперь поток сценария
|
||
определяется только зерном, именем предмета и поперечным смещением.
|
||
"""
|
||
from make_benchmark import scenario_rng
|
||
|
||
a = scenario_rng(12345, "ящик", 0.9).random(5)
|
||
assert np.array_equal(a, scenario_rng(12345, "ящик", 0.9).random(5))
|
||
assert not np.array_equal(a, scenario_rng(12345, "ящик", 0.0).random(5))
|
||
assert not np.array_equal(a, scenario_rng(12345, "чемодан", 0.9).random(5))
|
||
assert not np.array_equal(a, scenario_rng(777, "ящик", 0.9).random(5))
|
||
|
||
|
||
def test_export_box_follows_a_curved_track():
|
||
"""В кривой рамка стоит на пути и повёрнута вдоль него.
|
||
|
||
Боковое смещение трека отсчитано от оси пути, и без поправки на её
|
||
положение рамка на 150 м при радиусе 1300 м стояла в 8.6 м от пути, а
|
||
поворот был зеркальным — 13° мимо направления пути.
|
||
"""
|
||
import math
|
||
from flyguard.descending import Decision, DetectedObject
|
||
from flyguard.export import export_frame
|
||
from flyguard.geometry import Corridor, RailPlane
|
||
|
||
R = 1300.0
|
||
cor = Corridor(np.array([0.0, 0.0, 1.0 / (2 * R)], np.float32), 200.0, 10, 1600.0)
|
||
plane = RailPlane(a=0.0, b=0.0, c=-1.8, inliers=100, rms=0.02)
|
||
for d in (55.0, 100.0, 150.0):
|
||
dec = Decision(detected=True, distance=d, speed=10.0, stopping_distance=30.0,
|
||
objects=[DetectedObject(distance=d, lateral=0.3, height=0.8,
|
||
width=0.5, size_v=1.7, confidence=0.9,
|
||
novelty=0.6, n_rays=10, track_id=1,
|
||
ttc=d / 10)])
|
||
box = export_frame(dec, plane, cor).boxes[0]
|
||
on_track = float(cor.centre(np.array([d], np.float32))[0]) + 0.3
|
||
assert box.x == pytest.approx(on_track, abs=1e-3)
|
||
|
||
slope = d / R
|
||
long_axis = np.array([-math.sin(box.yaw), math.cos(box.yaw)])
|
||
tangent = np.array([slope, -1.0]) / math.hypot(slope, 1.0)
|
||
assert abs(float(long_axis @ tangent)) == pytest.approx(1.0, abs=1e-6)
|