"""Система координат пути, плоскость рельсов и «ожидаемая дальность до пола». Соответствие мухе — **жужжальца и оцеллии**. Прежде чем обрабатывать изображение, муха стабилизирует взгляд: жужжальца дают угловые скорости, оцеллии — направление на горизонт, и голова доворачивается так, чтобы зрительный мир не «плавал». Здесь роль горизонта играет плоскость пути: она оценивается по самим данным в каждом кадре, поэтому крепление сенсора не обязано быть жёстким, а качка вагона не превращается в ложные срабатывания. Ключевая величина дальше по конвейеру — **ожидаемая дальность до пола** для каждого луча. Луч с отрицательной элевацией, если ему ничто не мешает, обязан закончиться на плоскости пути на строго определённом расстоянии. Всё, что обрывает его раньше, — предмет, стоящий на пути. Это даёт детектор, не зависящий от абсолютного размера объекта и работающий на любой дальности. Система координат пути (используется во всём проекте): 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))