diff --git a/README.md b/README.md index 798c14c..526af91 100644 --- a/README.md +++ b/README.md @@ -97,7 +97,7 @@ flyguard/ ядро: стадии обработки, память, сч export.py 3D-рамки, время до столкновения, маркеры RViz synth.py вставка предметов трассировкой лучей tools/ обучение, оценка, разбор, полигон с аугментациями -tests/ 45 тестов, запускаются без данных и без ROS +tests/ 47 тестов, запускаются без данных и без ROS docs/ методика и результаты artifacts/ обученные модели ``` diff --git a/docs/EXPERIMENTS.md b/docs/EXPERIMENTS.md index f9b4446..56abcf5 100644 --- a/docs/EXPERIMENTS.md +++ b/docs/EXPERIMENTS.md @@ -3139,3 +3139,87 @@ MBON, новизна; у каждого трека ход улики) на си * **Дальние фантомы** — п. 19.4. * **Фантом у края на 16 м**: настоящая конструкция, заходящая в габарит на 0.03–0.19 м; с габаритом 1.2 м он граничный. + +## 20. Габарит на кривой и край габарита: ось против эталона, рамки в RViz, № 6 (25.09, ночь) + +Вопрос пользователя: соответствует ли габарит, нарисованный в RViz, тому, что +проверяется, если поезд поворачивает, — и поможет ли усиленная проверка у края +габарита найти № 6 (2×2, заходит в габарит на 0.06–0.28 м). + +### 20.1. Ось пути против эталона организаторов + +Предметы синтетики неподвижны в мире и стоят на пути, поэтому по ним ось пути +впервые проверяется не нашей же оценкой (как на полигоне, п. 13), а чужой +разметкой. № 2 стоит на левой кривой радиусом около 760 м: на 118 м он в 9.1 м +левее оси лидара. Центр предмета против нашей оси c(d) в том же кадре: + +| дальность, м | 22 | 31 | 39 | 48 | 64 | 87 | 118 | +|---|---:|---:|---:|---:|---:|---:|---:| +| эталон, м | −0.32 | −0.60 | −1.02 | −1.49 | −2.77 | −4.89 | −9.10 | +| наша ось, м | −0.36 | −0.72 | −1.17 | −1.70 | −3.04 | −5.11 | −8.14 | +| ошибка, м | 0.04 | 0.12 | 0.15 | 0.21 | 0.27 | 0.22 | 0.96 | + +На прямых участках (№ 1, 8, 9) ось иногда изгибается сама: медиана ошибки +дальше 20 м 0.05–0.42 м, до 1.1 м на 57 м (ящик 2×2 посередине сам сдвигает +центр сечения) и до 1.2 м на 110–130 м. Поэтому проверяется объединение +прямого и изогнутого габаритов (`TrackFrame.lateral`): неточная ось не сужает +зону поиска. + +### 20.2. Рисование в RViz + +Облако обзора выровнено по плоскости рельсов, но в кривой не выпрямлено, а +габарит рисовался прямым коробом. Рамки препятствий ставились по смещению от +оси пути, то есть в другой системе, чем точки: на кривой синтетики рамка № 2 на +39 м стояла на +0.13 м при точках на −1.05 м, на 31 м — на +0.12 при −0.59. + +Теперь кандидат несёт медиану смещения своих лучей в системе лидара +(`extra["u_raw"]`), трек сглаживает его так же, как `u`, и рамка рисуется по +нему (`DetectedObject.sensor_x`); в сообщении `lateral` по-прежнему от оси +пути. Контур габарита в кривой — два: яркий вдоль оценённой оси, бледный +прямой; проверяется объединение обоих. На прямом пути они совпадают, и +рисуется один (`export.gauge_outline`, тесты +`test_track_carries_the_sensor_lateral_for_drawing`, +`test_gauge_outline_bends_with_the_track`). Решения не менялись: на синтетике +тот же список находок и фантомов с теми же дальностями. Кадр узла в +контейнере на этой кривой — `docs/figures/curve_gauge.png`. + +### 20.3. № 6: усиленная проверка у края — отвергнуто по замеру + +№ 6 даёт кандидатов с 72 до 18.7 м: полоса в габарите шириной 0.1–0.33 м и +высотой 0.8–1.96 м, перепад дальности к фону 7.5–12.6 м, доля лучей в габарите +0.00–0.01 (с 50 м), новизна 0.13–0.57, отсчёт MBON 0.001–0.046. По перепаду +он резко отличается от фантома у края на 16 м (0–1.9 м), и правило +«полоса у края, высокая, с сильным перепадом» его бы нашло. Но на записях без +препятствий та же подпись встречается постоянно. Перепись кандидатов с +перепадом от 5 м, высотой от 0.7 м, |u| от 0.7 м и дальностью до 80 м +(`scratchpad/edge_census.py` по прогону принятой конфигурации): + +| запись | мест, где подпись держится 3+ кадра | +|---|---:| +| `roundT_doubleT` | 8 | +| `squareT_platform_squareT_switch` | 5, одно — 451 кадр | +| `doubleT_platform` | 2 | +| `doubleT_obstacle` (кроме самого объекта) | 1 | + +Место на `squareT_…` — u +1.08 м, высота 2.0 м, ширина 0.27 м, перепад 11 м, +новизна 0.23, доля в габарите 0.34 — по цифрам тот же № 6 (u +1.06, 1.43, +0.26, 9.6, 0.27, 0.29). Даже ближе 45 м и выше 1 м остаётся одно место на +`roundT_doubleT`: 40→22 м, u +1.09…+1.17, перепад 5.2 м. + +Отличает № 6 от этих конструкций не форма, а поперечное положение: настоящая +конструкция стоит не ближе 1.35 м от оси пути, иначе в неё врежется вагон, а у +нас оказывается на 1.0–1.1 м, потому что ошибка оси на 40–80 м — 0.2–0.4 м +(п. 20.1), и лидар стоит не точно над серединой колеи. Чтобы взять № 6 без +тревог на конструкциях, нужна точность поперечной координаты у края около +0.1 м на этих дальностях. Опереть ось на рельсы там не на что: с 50 м полотно +пути даёт десятки точек за кадр на всю ширину тоннеля (сечения по дальности на +синтетике). Не взято. + +### 20.4. Итог + +| что | было | стало | +|---|---|---| +| рамка № 2 на кривой, 39 м | в 1.2 м от точек | у точек (−1.11 против −1.05) | +| контур габарита в кривой | прямой короб, изогнутой части не видно | оба контура | +| решения узла | — | без изменений (синтетика: те же находки, фантомы, дальности и число кадров) | +| тесты | 48 | 50 | diff --git a/flyguard/central_complex.py b/flyguard/central_complex.py index 38c4a22..d1b3df0 100644 --- a/flyguard/central_complex.py +++ b/flyguard/central_complex.py @@ -46,6 +46,9 @@ class Track: novelty: float = 1.0 speed_lat: float = 0.0 # поперечная скорость, м/с p_track: float = 0.0 # отсчёт считывания по треку, если оно подключено + # То же смещение в системе лидара, без поправки на кривую. В решении не + # участвует: по нему рамка рисуется у своих точек, а не у оси пути. + u_raw: float = 0.0 history: list = field(default_factory=list) # Сводка по всем наблюдениям трека, накапливаемая на лету. Считать её из @@ -239,6 +242,7 @@ class CentralComplex: a = float(np.clip(0.5 * (60.0 / max(c.d, 20.0)), 0.15, 0.6)) t.s_world = (1 - a) * t.s_world + a * (self.s_world + c.d) t.u = (1 - a) * t.u + a * c.u + t.u_raw = (1 - a) * t.u_raw + a * float(c.extra.get("u_raw", c.u)) t.h = (1 - a) * t.h + a * c.h t.width = max(t.width * 0.7, c.width) t.height = max(t.height * 0.7, c.height) @@ -255,7 +259,8 @@ class CentralComplex: continue t = Track(id=next(self._ids), s_world=self.s_world + c.d, u=c.u, h=c.h, width=c.width, height=c.height, first_d=c.d, last_d=c.d, - last_n_rays=c.n_rays, novelty=c.novelty) + last_n_rays=c.n_rays, novelty=c.novelty, + u_raw=float(c.extra.get("u_raw", c.u))) w = _quality(c, self.use_shape, self.mbon_power, self.mbon_blend, self.nov_fade_from, self.nov_fade_to, self.lying_exempt, self.top_from, self.flat) diff --git a/flyguard/descending.py b/flyguard/descending.py index f2ea056..79707a2 100644 --- a/flyguard/descending.py +++ b/flyguard/descending.py @@ -36,6 +36,9 @@ class DetectedObject: n_rays: int track_id: int ttc: float + # Смещение в системе лидара: `lateral` отсчитан от оси пути, и в кривой + # рамка по нему стоит в стороне от точек. NaN — неизвестно. + sensor_x: float = float("nan") @dataclass @@ -202,7 +205,8 @@ class DescendingNeurons: out.objects.append(DetectedObject( distance=d, lateral=t.u, height=t.h, width=t.width, size_v=t.height, confidence=min(1.0, t.evidence * t.novelty + 0.0), - novelty=t.novelty, n_rays=t.last_n_rays, track_id=t.id, ttc=ttc)) + novelty=t.novelty, n_rays=t.last_n_rays, track_id=t.id, ttc=ttc, + sensor_x=t.u_raw)) nearest, d0 = live[0] out.detected = True diff --git a/flyguard/export.py b/flyguard/export.py index f8d253e..f6d7e7c 100644 --- a/flyguard/export.py +++ b/flyguard/export.py @@ -196,15 +196,21 @@ def export_frame(decision: Decision, plane: RailPlane | None, corridor: Corridor # пути в кадре — (наклон, −1), и длинная ось рамки (её локальная y) # совпадает с ним при повороте на +atan(наклон). Проверка обоих — # test_export_box_follows_a_curved_track. + # + # Если трек знает своё смещение в системе лидара (`sensor_x`), берётся + # оно: габарит — объединение прямого и изогнутого, и у предмета, + # попавшего в прямой, `u` отсчитан не от кривой, а от оси лидара. + sx = float(getattr(obj, "sensor_x", float("nan"))) if corridor is not None and corridor.n_slices > 0: - x_sensor = float(u + corridor.centre(np.array([d], np.float32))[0]) + x_sensor = (sx if math.isfinite(sx) + else float(u + corridor.centre(np.array([d], np.float32))[0])) c0, c1, c2 = corridor.coef dm = max(corridor.d_max_seen, 1.0) d_in = min(d, dm) slope = c1 + 2.0 * c2 * d_in yaw = float(math.atan(slope)) else: - x_sensor = float(u) + x_sensor = sx if math.isfinite(sx) else float(u) yaw = 0.0 y_sensor = float(-d) z_sensor = float(h + a * d + b * x_sensor + c) @@ -264,3 +270,34 @@ def export_frame(decision: Decision, plane: RailPlane | None, corridor: Corridor boxes=boxes, corridor_points_xyz=corridor_pts ) + + +def gauge_outline(boxes, centre=None, step: float = 2.0, + frame_every: float = 20.0) -> list[tuple[float, float, float]]: + """Отрезки контура габарита (пары точек для LINE_LIST) в координатах облака обзора. + + `boxes` — секции `(полуширина, низ, верх, от, до)`; `centre(d)` — ось пути + (`Corridor.centre`), None — прямой короб вдоль x = 0. Облако обзора в кривой + не выпрямлено, поэтому габарит вдоль изогнутой оси рисуется изогнутым — + там, где узел его и проверяет. Вперёд = −y, вправо = +x, вверх = +z над + головкой рельса, как у облака обзора. Рамки поперёк — через `frame_every`. + """ + pts: list[tuple[float, float, float]] = [] + for hw, z0, z1, d0, d1 in boxes: + if d1 <= d0: + continue + n = 1 if centre is None else max(int(np.ceil((d1 - d0) / step)), 1) + ds = np.linspace(d0, d1, n + 1) + cx = np.zeros_like(ds) if centre is None else np.asarray(centre(ds), np.float64) + corners = [(-hw, z0), (hw, z0), (hw, z1), (-hw, z1)] + for x, z in corners: # вдоль пути + for a in range(n): + pts.append((float(cx[a] + x), float(-ds[a]), float(z))) + pts.append((float(cx[a + 1] + x), float(-ds[a + 1]), float(z))) + fd = np.arange(d0, d1 + 1e-6, frame_every) # поперёк + fc = np.zeros_like(fd) if centre is None else np.asarray(centre(fd), np.float64) + for dd, c in zip(fd, fc): + for (xa, za), (xb, zb) in zip(corners, corners[1:] + corners[:1]): + pts.append((float(c + xa), float(-dd), float(za))) + pts.append((float(c + xb), float(-dd), float(zb))) + return pts diff --git a/flyguard/lobula.py b/flyguard/lobula.py index 9c20621..d6f7198 100644 --- a/flyguard/lobula.py +++ b/flyguard/lobula.py @@ -446,6 +446,10 @@ def find_candidates(tf: TrackFrame, lam: LaminaOutput, corridor: Corridor, *, ) # индексы лучей нужны только для отладочной визуализации; они небольшие cand.extra["rays"] = (ii, jj) + # Смещение в системе лидара, без поправки на кривую: `u` отсчитан от + # оси пути, и в кривой рамка по нему встала бы в стороне от своих + # точек (на синтетике — в 1.2 м на 39 м). Нужно только для рисования. + cand.extra["u_raw"] = float(np.median(tf.u[ii, jj])) return cand use_top = top_detect and h_top > h_hi and half_width_top > 0.0 diff --git a/tests/test_pipeline.py b/tests/test_pipeline.py index c9aa849..5f82dd1 100644 --- a/tests/test_pipeline.py +++ b/tests/test_pipeline.py @@ -826,3 +826,49 @@ def test_export_box_follows_a_curved_track(): 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) + + +def test_track_carries_the_sensor_lateral_for_drawing(): + """Рамка в RViz стоит у своих точек, а не у оси пути. + + На кривой синтетики организаторов (радиус около 760 м) предмет на оси + пути на 39 м лежит в 1.05 м левее оси лидара, а рамка рисовалась по + смещению от оси пути — на +0.13 м, в 1.2 м от своих точек. + """ + from flyguard.central_complex import CentralComplex + from flyguard.descending import DescendingNeurons + from flyguard.lobula import Candidate + + cx = CentralComplex() + c = Candidate(d=39.0, u=0.02, h=0.3, d_min=38.8, h_min=0.1, width=0.3, + height=0.3, depth=0.2, containment=1.0, n_rays=20, + n_rings=5, n_cols=4, gap=6.0, on=0.2, floor_deficit=0.0, + shadow=0.3, inten=12.0, az_deg=-1.5, el_deg=-1.0, + bbox=(0, 0, 5, 4), novelty=0.6, extra={"u_raw": -1.05}) + cx.update([c], ds=0.0, dt=0.1) + t = cx.tracks[0] + assert t.u == pytest.approx(0.02) and t.u_raw == pytest.approx(-1.05) + + t.evidence, t.hits, t.age = 1.0, 9, 9 + obj = DescendingNeurons().decide(cx, speed=10.0).objects[0] + assert obj.lateral == pytest.approx(0.02) # смещение от оси пути — в сообщении + assert obj.sensor_x == pytest.approx(-1.05) # а рисуется рамка здесь + + +def test_gauge_outline_bends_with_the_track(): + """Контур габарита в RViz идёт вдоль оценённой оси, а не прямо.""" + from flyguard.export import gauge_outline + from flyguard.geometry import Corridor + + boxes = [(1.2, 0.28, 2.3, 4.0, 200.0)] + straight = np.array(gauge_outline(boxes)) + assert set(np.round(np.abs(straight[:, 0]), 6)) == {1.2} + + R = 760.0 + cor = Corridor(np.array([0.0, 0.0, -1.0 / (2 * R)], np.float32), 200.0, 10, R) + bent = np.array(gauge_outline(boxes, cor.centre)) + at100 = bent[np.isclose(bent[:, 1], -100.0)] + c100 = float(cor.centre(np.array([100.0], np.float32))[0]) + assert c100 == pytest.approx(-100.0 ** 2 / (2 * R), rel=1e-3) + assert at100[:, 0].min() == pytest.approx(c100 - 1.2, abs=1e-4) + assert at100[:, 0].max() == pytest.approx(c100 + 1.2, abs=1e-4)