forked from Dan4ick/Lidar_Muxa
b8e95eccbe
Ретина, ламина, медулла, лобула, грибовидное тело, веерное тело, центральный комплекс, нисходящие нейроны. Обучение памяти тоннеля и считывания MBON, оценка leave-one-bag-out, полигон дальности, 24 теста. Реальный объект на 55 м — 98.9 % кадров, ложных 7.5 трека на км, кадр обрабатывается за 33 мс на CPU.
324 lines
17 KiB
Python
324 lines
17 KiB
Python
"""Система координат пути, плоскость рельсов и «ожидаемая дальность до пола».
|
||
|
||
Соответствие мухе — **жужжальца и оцеллии**. Прежде чем обрабатывать изображение,
|
||
муха стабилизирует взгляд: жужжальца дают угловые скорости, оцеллии — направление
|
||
на горизонт, и голова доворачивается так, чтобы зрительный мир не «плавал».
|
||
Здесь роль горизонта играет плоскость пути: она оценивается по самим данным
|
||
в каждом кадре, поэтому крепление сенсора не обязано быть жёстким, а качка
|
||
вагона не превращается в ложные срабатывания.
|
||
|
||
Ключевая величина дальше по конвейеру — **ожидаемая дальность до пола** для
|
||
каждого луча. Луч с отрицательной элевацией, если ему ничто не мешает, обязан
|
||
закончиться на плоскости пути на строго определённом расстоянии. Всё, что
|
||
обрывает его раньше, — предмет, стоящий на пути. Это даёт детектор, не зависящий
|
||
от абсолютного размера объекта и работающий на любой дальности.
|
||
|
||
Система координат пути (используется во всём проекте):
|
||
d — вперёд по ходу движения, м (в кадре сенсора это −y)
|
||
u — поперёк, вправо, м (в кадре сенсора это x)
|
||
h — вверх от плоскости рельсов, м
|
||
"""
|
||
from __future__ import annotations
|
||
|
||
from dataclasses import dataclass
|
||
|
||
import numpy as np
|
||
|
||
from .retina import RangeImage, ScanLayout
|
||
|
||
|
||
@dataclass(frozen=True)
|
||
class RailPlane:
|
||
"""Плоскость головок рельсов в системе сенсора: z = a·d + b·u + c."""
|
||
|
||
a: float # тангаж: подъём плоскости с расстоянием
|
||
b: float # крен: наклон плоскости поперёк
|
||
c: float # −высота сенсора над путём (c < 0)
|
||
inliers: int
|
||
rms: float
|
||
|
||
@property
|
||
def height(self) -> float:
|
||
"""Высота сенсора над головкой рельса, м."""
|
||
return -self.c
|
||
|
||
@property
|
||
def pitch_deg(self) -> float:
|
||
return float(np.degrees(np.arctan(self.a)))
|
||
|
||
@property
|
||
def roll_deg(self) -> float:
|
||
return float(np.degrees(np.arctan(self.b)))
|
||
|
||
def height_of(self, d: np.ndarray, u: np.ndarray, z: np.ndarray) -> np.ndarray:
|
||
"""Высота точек над плоскостью пути."""
|
||
return z - (self.a * d + self.b * u + self.c)
|
||
|
||
def floor_range(self, layout: ScanLayout) -> np.ndarray:
|
||
"""Дальность, на которой каждый луч упёрся бы в плоскость пути.
|
||
|
||
Луч r·(dx, dy, dz); подстановка в уравнение плоскости даёт
|
||
r = c / (dz + a·dy − b·dx). Лучи, уходящие вверх или параллельно
|
||
плоскости, получают +inf.
|
||
"""
|
||
dx = layout.dirs[..., 0]
|
||
dy = layout.dirs[..., 1]
|
||
dz = layout.dirs[..., 2]
|
||
denom = dz + self.a * dy - self.b * dx
|
||
with np.errstate(divide="ignore", invalid="ignore"):
|
||
r = self.c / denom
|
||
return np.where((denom < -1e-6) & np.isfinite(r), r, np.float32(np.inf)).astype(np.float32)
|
||
|
||
|
||
DEFAULT_PLANE = RailPlane(a=0.0, b=0.0, c=-2.19, inliers=0, rms=0.0)
|
||
|
||
|
||
def fit_rail_plane(img: RangeImage, layout: ScanLayout, *,
|
||
d_min: float = 6.0, d_max: float = 45.0,
|
||
u_max: float = 1.9, cell_d: float = 1.0, cell_u: float = 0.25,
|
||
iters: int = 4, prev: RailPlane | None = None,
|
||
smooth: float = 0.25) -> RailPlane:
|
||
"""Робастная оценка плоскости пути по ближней зоне.
|
||
|
||
В каждой ячейке сетки (d, u) остаётся только самая низкая точка — это
|
||
отсекает шпалы, кабельные лотки и всё, что стоит на полотне. Затем идут
|
||
итерации перевзвешенных наименьших квадратов с мягкой функцией Хьюбера,
|
||
после чего подгонка повторяется уже только по точкам у самой плоскости.
|
||
|
||
`smooth` задаёт постоянную времени экспоненциального сглаживания по кадрам:
|
||
плоскость пути физически не может прыгать, и сглаживание играет ту же роль,
|
||
что обратная связь от жужжалец, — гасит дрожание оценки.
|
||
"""
|
||
xyz = img.xyz(layout)
|
||
x, y, z = xyz[..., 0], xyz[..., 1], xyz[..., 2]
|
||
d = -y
|
||
sel = img.valid & (d > d_min) & (d < d_max) & (np.abs(x) < u_max)
|
||
if sel.sum() < 200:
|
||
return prev or DEFAULT_PLANE
|
||
|
||
dv = d[sel].astype(np.float64)
|
||
uv = x[sel].astype(np.float64)
|
||
zv = z[sel].astype(np.float64)
|
||
|
||
# самая низкая точка в каждой ячейке — грубое выделение полотна
|
||
ci = ((dv - d_min) / cell_d).astype(np.int64)
|
||
cj = ((uv + u_max) / cell_u).astype(np.int64)
|
||
key = ci * 10_000 + cj
|
||
order = np.lexsort((zv, key))
|
||
key_s = key[order]
|
||
first = np.ones(key_s.size, bool)
|
||
first[1:] = key_s[1:] != key_s[:-1]
|
||
idx = order[first]
|
||
if idx.size < 40:
|
||
return prev or DEFAULT_PLANE
|
||
|
||
dd, uu, zz = dv[idx], uv[idx], zv[idx]
|
||
coef = _irls_plane(dd, uu, zz, iters)
|
||
if coef is None:
|
||
return prev or DEFAULT_PLANE
|
||
|
||
# второй проход: только точки у найденной плоскости, уже без отбора минимумов
|
||
res_all = zv - (coef[0] * dv + coef[1] * uv + coef[2])
|
||
near = np.abs(res_all) < 0.18
|
||
if near.sum() > 300:
|
||
c2 = _irls_plane(dv[near], uv[near], zv[near], iters)
|
||
if c2 is not None:
|
||
coef = c2
|
||
|
||
res = zv - (coef[0] * dv + coef[1] * uv + coef[2])
|
||
keep = np.abs(res) < 0.2
|
||
plane = RailPlane(a=float(coef[0]), b=float(coef[1]), c=float(coef[2]),
|
||
inliers=int(keep.sum()),
|
||
rms=float(np.sqrt(np.mean(res[keep] ** 2))) if keep.any() else 9.9)
|
||
|
||
# защита от вырождения: высота сенсора над путём физически ограничена
|
||
if not (0.5 < plane.height < 5.0) or abs(plane.pitch_deg) > 8 or abs(plane.roll_deg) > 8:
|
||
return prev or DEFAULT_PLANE
|
||
if prev is not None and smooth > 0:
|
||
k = smooth
|
||
plane = RailPlane(a=k * plane.a + (1 - k) * prev.a,
|
||
b=k * plane.b + (1 - k) * prev.b,
|
||
c=k * plane.c + (1 - k) * prev.c,
|
||
inliers=plane.inliers, rms=plane.rms)
|
||
return plane
|
||
|
||
|
||
def _irls_plane(d: np.ndarray, u: np.ndarray, z: np.ndarray, iters: int):
|
||
"""z ≈ a·d + b·u + c с мягким Хьюбером."""
|
||
A = np.stack([d, u, np.ones_like(d)], axis=1)
|
||
w = np.ones_like(z)
|
||
coef = np.array([0.0, 0.0, float(np.median(z))])
|
||
for _ in range(iters):
|
||
try:
|
||
coef, *_ = np.linalg.lstsq(A * w[:, None], z * w, rcond=None)
|
||
except np.linalg.LinAlgError:
|
||
return None
|
||
res = z - A @ coef
|
||
s = 1.4826 * np.median(np.abs(res - np.median(res))) + 1e-3
|
||
w = 1.0 / np.sqrt(1.0 + (res / (2.0 * s)) ** 2)
|
||
return coef
|
||
|
||
|
||
@dataclass
|
||
class Corridor:
|
||
"""Осевая линия пути впереди: u_c(d) = c0 + c1·d + c2·d².
|
||
|
||
Оценивается по дрейфу центра сечения тоннеля с расстоянием. В прямом
|
||
тоннеле c1 ≈ c2 ≈ 0; в кривой радиуса R член c2 ≈ 1/(2R). Нужна, чтобы
|
||
габарит на 150 м впереди не «въезжал» в стену на повороте — иначе вся
|
||
дальняя зона кривой превращается в сплошное ложное срабатывание.
|
||
"""
|
||
|
||
coef: np.ndarray # (3,)
|
||
d_max_seen: float # дальше этого — экстраполяция
|
||
n_slices: int
|
||
radius: float # оценка радиуса кривой, м (inf для прямой)
|
||
|
||
def centre(self, d: np.ndarray) -> np.ndarray:
|
||
"""Ось пути на дальности d.
|
||
|
||
За пределами наблюдавшейся дальности парабола продолжается **линейно**,
|
||
по касательной: экстраполировать кривизну туда, где данных не было,
|
||
значит получить десятки метров ошибки на ровном месте.
|
||
"""
|
||
d = np.asarray(d, np.float32)
|
||
c0, c1, c2 = self.coef
|
||
dm = np.float32(max(self.d_max_seen, 1.0))
|
||
d_in = np.minimum(d, dm)
|
||
u = c0 + c1 * d_in + c2 * d_in * d_in
|
||
slope = c1 + 2.0 * c2 * dm
|
||
return (u + slope * np.maximum(d - dm, 0.0)).astype(np.float32)
|
||
|
||
def sigma(self, d: np.ndarray, base: float = 0.25, rate: float = 0.004) -> np.ndarray:
|
||
"""Неопределённость положения оси: растёт с дальностью и за горизонтом видимости."""
|
||
d = np.asarray(d, np.float32)
|
||
extra = np.maximum(d - np.float32(self.d_max_seen), 0.0)
|
||
return (base + rate * d + 0.02 * extra).astype(np.float32)
|
||
|
||
|
||
STRAIGHT = Corridor(np.zeros(3), 0.0, 0, float("inf"))
|
||
|
||
MIN_TRACK_RADIUS = 300.0 # м, круче на перегонах метрополитена не бывает
|
||
MAX_AXIS_RATE = 0.35 # м за кадр, предел изменения оси на дальности 100 м
|
||
|
||
|
||
def fit_corridor(tf: "TrackFrame", *, d_lo: float = 8.0, d_hi: float = 220.0,
|
||
n_slices: int = 30, h_lo: float = 0.6, h_hi: float = 3.2,
|
||
min_pts: int = 60, d_ref: float = 22.0,
|
||
prev: Corridor | None = None, smooth: float = 0.08) -> Corridor:
|
||
"""Оценить осевую линию пути по смещению центра сечения тоннеля.
|
||
|
||
Каждый срез по дальности даёт одну оценку центра свода. Веса берутся
|
||
**равными по срезам**, а не по числу точек: у ближних срезов точек в сотни
|
||
раз больше, и взвешивание по количеству полностью подавило бы дальние срезы,
|
||
в которых как раз и содержится кривизна.
|
||
"""
|
||
edges = np.geomspace(d_lo, d_hi, n_slices + 1)
|
||
ds, us = [], []
|
||
for lo, hi in zip(edges[:-1], edges[1:]):
|
||
m = tf.valid & (tf.d >= lo) & (tf.d < hi) & (tf.h > h_lo) & (tf.h < h_hi)
|
||
if int(m.sum()) < min_pts:
|
||
continue
|
||
uu = tf.u[m]
|
||
lo_u, hi_u = np.percentile(uu, (3.0, 97.0))
|
||
if hi_u - lo_u < 1.5: # видна только одна стена — центр не определить
|
||
continue
|
||
ds.append(0.5 * (lo + hi))
|
||
us.append(0.5 * (lo_u + hi_u))
|
||
if len(ds) < 5:
|
||
return prev or STRAIGHT
|
||
|
||
d = np.asarray(ds, np.float64)
|
||
u = np.asarray(us, np.float64)
|
||
|
||
# положение поезда в сечении: медиана центров ближней зоны
|
||
ref = d <= d_ref
|
||
u = u - (np.median(u[ref]) if ref.sum() >= 2 else u[0])
|
||
|
||
far = d > 12.0
|
||
if far.sum() < 4:
|
||
return prev or STRAIGHT
|
||
d_f, u_f = d[far], u[far]
|
||
|
||
# u ≈ c1·d + c2·d², равные веса по срезам, две итерации робастного отсева
|
||
A = np.stack([d_f, d_f * d_f], axis=1)
|
||
w = np.ones_like(u_f)
|
||
c = np.zeros(2)
|
||
for _ in range(3):
|
||
try:
|
||
c, *_ = np.linalg.lstsq(A * w[:, None], u_f * w, rcond=None)
|
||
except np.linalg.LinAlgError:
|
||
return prev or STRAIGHT
|
||
res = u_f - A @ c
|
||
s = 1.4826 * np.median(np.abs(res - np.median(res))) + 0.05
|
||
w = 1.0 / np.sqrt(1.0 + (res / (2.0 * s)) ** 2)
|
||
|
||
coef = np.array([0.0, c[0], c[1]])
|
||
radius = float(abs(1.0 / (2.0 * c[1]))) if abs(c[1]) > 1e-7 else float("inf")
|
||
# Радиус круче 300 м на перегоне метрополитена не встречается. Такая оценка
|
||
# означает не кривую, а испорченное сечение: на станции платформа делает свод
|
||
# резко несимметричным, центр «уезжает», и габарит вместе с ним заезжает
|
||
# прямо на платформу — источник почти всех ложных тревог у станций.
|
||
if radius < MIN_TRACK_RADIUS:
|
||
return prev or STRAIGHT
|
||
|
||
out = Corridor(coef, float(d.max()), len(ds), radius)
|
||
if prev is None or smooth <= 0:
|
||
return out
|
||
|
||
k = smooth
|
||
blended = k * out.coef + (1 - k) * prev.coef
|
||
# путь физически не может вильнуть: ограничиваем скорость изменения оси
|
||
shift_now = blended[1] * 100.0 + blended[2] * 100.0 ** 2
|
||
shift_prev = prev.coef[1] * 100.0 + prev.coef[2] * 100.0 ** 2
|
||
excess = abs(shift_now - shift_prev)
|
||
if excess > MAX_AXIS_RATE:
|
||
t = MAX_AXIS_RATE / excess
|
||
blended = prev.coef + (blended - prev.coef) * t
|
||
r2 = (float(abs(1.0 / (2.0 * blended[2]))) if abs(blended[2]) > 1e-7 else float("inf"))
|
||
return Corridor(blended, out.d_max_seen, out.n_slices, r2)
|
||
|
||
|
||
class TrackFrame:
|
||
"""Кадр в координатах пути: (d, u, h) плюс ожидаемая дальность до пола."""
|
||
|
||
__slots__ = ("d", "u", "h", "z", "r", "valid", "inten", "floor_r", "plane",
|
||
"layout", "img")
|
||
|
||
def __init__(self, img: RangeImage, layout: ScanLayout, plane: RailPlane):
|
||
xyz = img.xyz(layout)
|
||
self.layout = layout
|
||
self.img = img
|
||
self.plane = plane
|
||
self.u = xyz[..., 0]
|
||
self.d = -xyz[..., 1]
|
||
self.z = xyz[..., 2] # в системе сенсора, не над рельсом
|
||
self.h = plane.height_of(self.d, self.u, self.z)
|
||
self.r = img.r_near
|
||
self.valid = img.valid
|
||
self.inten = img.inten
|
||
self.floor_r = plane.floor_range(layout)
|
||
|
||
def lateral(self, corridor: "Corridor | None" = None) -> np.ndarray:
|
||
"""Смещение точек от осевой линии пути, м.
|
||
|
||
Берётся **меньшее по модулю** из двух: отсчёт от прямой оси и от
|
||
оценённой кривой. Это объединение двух габаритов, а не замена одного
|
||
другим, и сделано осознанно: оценка оси неизбежно неточна, а система
|
||
безопасности не имеет права **сужать** зону поиска по неуверенной
|
||
оценке. Измерено: замена (а не объединение) поднимала пропуски
|
||
реального объекта с 1 % до 25 %, экономя при этом лишь 2.5 % кадров
|
||
с ложной тревогой — размен в неверную сторону.
|
||
"""
|
||
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 in_gauge(self, half_width: float = 1.7, h_lo: float = 0.05,
|
||
h_hi: float = 2.2, d_min: float = 3.0, d_max: float = 260.0,
|
||
corridor: "Corridor | None" = None) -> np.ndarray:
|
||
"""Маска лучей, чьи точки лежат внутри габарита приближения."""
|
||
return (self.valid & (self.d > d_min) & (self.d < d_max)
|
||
& (np.abs(self.lateral(corridor)) < half_width)
|
||
& (self.h > h_lo) & (self.h < h_hi))
|