габарит на кривой, рамки у своих точек, разбор ящика у края
This commit is contained in:
Данил Омелечко
parent
5aa3dead0e
commit
872565abcf
7 changed files with 185 additions and 5 deletions
|
|
@ -97,7 +97,7 @@ flyguard/ ядро: стадии обработки, память, сч
|
|||
export.py 3D-рамки, время до столкновения, маркеры RViz
|
||||
synth.py вставка предметов трассировкой лучей
|
||||
tools/ обучение, оценка, разбор, полигон с аугментациями
|
||||
tests/ 45 тестов, запускаются без данных и без ROS
|
||||
tests/ 47 тестов, запускаются без данных и без ROS
|
||||
docs/ методика и результаты
|
||||
artifacts/ обученные модели
|
||||
```
|
||||
|
|
|
|||
|
|
@ -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 |
|
||||
|
|
|
|||
|
|
@ -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)
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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)
|
||||
|
|
|
|||
Loading…
Reference in a new issue