"""Синтетические препятствия: трассировка лучей в реальные кадры. Разметки в датасете нет, а организаторы прямо предупредили, что приватный тест собран добавлением синтезированных препятствий в новые проезды. Поэтому свой полигон строится тем же способом: берётся настоящий кадр пустого тоннеля, в него трассировкой лучей вставляется предмет заданного размера на заданной дистанции, и получается **размеченный** пример с точно известным ответом. Вставка идёт в исходное облако точек, а не в готовый дальностный образ, поэтому через конвейер проходит ровно тот же путь, что и настоящие данные, начиная с ретины. Модель сенсора намеренно пессимистична: добавляется шум дальности, а вероятность несостоявшегося эха растёт с расстоянием и с углом падения. Лучше недооценить свой детектор, чем на защите обнаружить, что полигон был слишком добрым. Яркость вставки берётся из самой записи — из возвратов на тех же лучах: абсолютной шкалы интенсивности в этих данных нет, и любое назначенное число делает вставку узнаваемой (см. длинный комментарий ниже). """ from __future__ import annotations from dataclasses import dataclass, field import numpy as np from .cdr import PointCloud2 from .geometry import RailPlane from .retina import ScanLayout INF = np.float32(np.inf) # --------------------------------------------------------------------------- тела @dataclass class Box: """Параллелепипед в координатах пути, стоящий на плоскости рельсов.""" length: float # вдоль пути, м width: float # поперёк, м height: float # вверх от рельса, м h_base: float = 0.0 def intersect(self, od, ou, oh, dd, du, dh) -> np.ndarray: lo = np.array([-self.length / 2, -self.width / 2, self.h_base], np.float32) hi = np.array([self.length / 2, self.width / 2, self.h_base + self.height], np.float32) t0 = np.full(od.shape, -INF, np.float32) t1 = np.full(od.shape, INF, np.float32) for o, d, a, b in ((od, dd, lo[0], hi[0]), (ou, du, lo[1], hi[1]), (oh, dh, lo[2], hi[2])): with np.errstate(divide="ignore", invalid="ignore"): ta = (a - o) / d tb = (b - o) / d lo_t = np.where(d != 0, np.minimum(ta, tb), np.where((o >= a) & (o <= b), -INF, INF)) hi_t = np.where(d != 0, np.maximum(ta, tb), np.where((o >= a) & (o <= b), INF, -INF)) t0 = np.maximum(t0, lo_t) t1 = np.minimum(t1, hi_t) hit = (t1 >= np.maximum(t0, 0.0)) & np.isfinite(t0) return np.where(hit, np.maximum(t0, 0.0), INF) @dataclass class Cylinder: """Вертикальный цилиндр — человек, столб, ведро.""" radius: float height: float h_base: float = 0.0 def intersect(self, od, ou, oh, dd, du, dh) -> np.ndarray: a = dd * dd + du * du b = 2.0 * (od * dd + ou * du) c = od * od + ou * ou - self.radius ** 2 disc = b * b - 4 * a * c ok = (disc > 0) & (a > 1e-9) sq = np.sqrt(np.where(ok, disc, 0.0)) with np.errstate(divide="ignore", invalid="ignore"): t = (-b - sq) / (2 * a) t2 = (-b + sq) / (2 * a) t = np.where(t > 0, t, t2) h = oh + t * dh ok &= (t > 0) & (h >= self.h_base) & (h <= self.h_base + self.height) return np.where(ok, t, INF) @dataclass class Sphere: radius: float h_centre: float def intersect(self, od, ou, oh, dd, du, dh) -> np.ndarray: oz = oh - self.h_centre a = dd * dd + du * du + dh * dh b = 2.0 * (od * dd + ou * du + oz * dh) c = od * od + ou * ou + oz * oz - self.radius ** 2 disc = b * b - 4 * a * c ok = disc > 0 sq = np.sqrt(np.where(ok, disc, 0.0)) with np.errstate(divide="ignore", invalid="ignore"): t = (-b - sq) / (2 * a) return np.where(ok & (t > 0), t, INF) @dataclass class ObjectModel: """Предмет: набор тел плюс отражательные свойства.""" name: str parts: list = field(default_factory=list) reflectivity: float = 0.3 # 0…1, доля отражённого света def intersect(self, od, ou, oh, dd, du, dh) -> np.ndarray: t = np.full(od.shape, INF, np.float32) for p in self.parts: t = np.minimum(t, p.intersect(od, ou, oh, dd, du, dh)) return t @property def size(self) -> tuple[float, float]: """Грубые габариты (ширина, высота) для отчётов.""" w = h = 0.0 for p in self.parts: if isinstance(p, Box): w = max(w, p.width); h = max(h, p.h_base + p.height) elif isinstance(p, Cylinder): w = max(w, 2 * p.radius); h = max(h, p.h_base + p.height) elif isinstance(p, Sphere): w = max(w, 2 * p.radius); h = max(h, p.h_centre + p.radius) return w, h def catalogue() -> dict[str, ObjectModel]: """Набор предметов, встречающихся в тоннеле, от крупных к мелким.""" return { "человек_стоя": ObjectModel("человек_стоя", [ Cylinder(radius=0.22, height=1.45), Sphere(radius=0.11, h_centre=1.60)], reflectivity=0.35), "человек_сидя": ObjectModel("человек_сидя", [ Box(0.45, 0.50, 0.85)], reflectivity=0.35), "человек_лежа": ObjectModel("человек_лежа", [ Box(length=1.80, width=0.50, height=0.30, h_base=0.0)], reflectivity=0.35), "ящик": ObjectModel("ящик", [Box(0.60, 0.60, 0.60)], reflectivity=0.40), "чемодан": ObjectModel("чемодан", [Box(0.25, 0.45, 0.55)], reflectivity=0.30), "ведро": ObjectModel("ведро", [Cylinder(radius=0.15, height=0.35)], reflectivity=0.45), "камень": ObjectModel("камень", [Sphere(radius=0.11, h_centre=0.11)], reflectivity=0.20), "бутылка": ObjectModel("бутылка", [Cylinder(radius=0.045, height=0.30)], reflectivity=0.25), "кабель": ObjectModel("кабель", [Box(2.20, 0.06, 0.06)], reflectivity=0.15), "каска": ObjectModel("каска", [Sphere(radius=0.14, h_centre=0.10)], reflectivity=0.55), } # --------------------------------------------------------------------------- вставка @dataclass class Placement: """Где стоит предмет.""" d: float # вперёд от сенсора, м u: float = 0.0 # поперёк от оси пути, м yaw_deg: float = 0.0 # поворот вокруг вертикали (для вытянутых тел) def ray_dirs_track(layout: ScanLayout, plane: RailPlane): """Направления лучей в координатах пути: (dd, du, dh) и начало (0, 0, H).""" dirs = layout.dirs dd = -dirs[..., 1].astype(np.float32) du = dirs[..., 0].astype(np.float32) dh = (dirs[..., 2] - plane.a * dd - plane.b * du).astype(np.float32) return dd, du, dh, np.float32(plane.height) # Паспортные данные Pandar128E3X (руководство v4p5, п. 1.4 и Приложение A): # дальность 0.3…200 м при отражательной способности 10 %, вероятность # обнаружения на паспортной дальности PoD = 70 %, точность ±2 см на 1…200 м. SPEC_REFLECTIVITY = 0.10 SPEC_POD = 0.70 HARD_MAX_RANGE = 230.0 # дальше в датасете возвратов не встречается # Расходимость луча в руководстве не указана; принята равной угловому шагу # решётки (0.1° по азимуту, 0.125° по элевации в полосе высокого разрешения) — # это верхняя оценка, дающая консервативный результат для мелких предметов. BEAM_DIV_H = 1.75e-3 # рад BEAM_DIV_V = 2.18e-3 # рад _CHANNEL_RANGE: np.ndarray | None = None # Интенсивность вставки. Здесь нельзя придумать ни формулы, ни числа. # # Сначала стояла ламбертова ρ·cosθ/r². Прибор, однако, отдаёт не принятую # энергию, а отражательную способность с компенсацией дальности: медиана по # облаку держится 8…9 от 5 до 50 м и как 1/r² не падает. Вставка получалась на # 55 м в двадцать раз тусклее настоящего предмета, а за 110 м упиралась в # нижний срез шкалы — и «тускло» становилось безошибочным признаком предмета. # # Замена на постоянную яркость 100·ρ·√cosθ, привязанную к настоящему предмету # (34.5 на 55.9 м), просто перевернула артефакт: 35 против 3…7 у обстановки на # каждой полосе дальности, AUC по одной интенсивности 0.96…0.97. # # Причина в том, что абсолютной шкалы тут нет. Медиана яркости кандидатов # обстановки по бэгам: 3.0, 4.0, 4.0, 4.0, 7.0 — а в `doubleT_obstacle`, где # лежит настоящий предмет, 23.5 при 34.5 у самого предмета. Разница между # записями впятеро больше, чем контраст предмета к фону внутри записи. Любое # абсолютное число, назначенное вставке, оказывается подарком детектору — в ту # или в другую сторону. # # Поэтому вставка берёт яркость **реальных возвратов с тех же самых лучей** — # того, что предмет заслонил; где эха не было, из возвратов вдоль остальных его # лучей. Признак становится неинформативным, и полигон меряет геометрию и # движение, то есть то, что мы моделируем честно. Оценка заниженная: настоящий # предмет в записи был в 1.47 раза ярче окружения. На линии с откалиброванной # яркостью этот запас можно вернуть — замером, а не верой. # # Отражательная способность никуда не делась: она определяет, вернётся ли эхо # вообще (`dropout_probability`), а это и есть её настоящая роль. INTEN_SPREAD = 0.12 # разброс отсчёта, логнормальный, ≈ ±12 % class IntensityEnv: """Яркость реальных возвратов кадра, разложенная по дальности. Нужна, чтобы луч предмета, ушедший в пустоту, получил яркость такую же, как у настоящих возвратов С ТОЙ ЖЕ дальности, а не с ближних и ярких. Считается один раз на кадр и переиспользуется всеми сценариями: в сборе выборки их 135 на кадр, и пересчитывать корни по полутора миллионам точек для каждого незачем. """ EDGES = np.array([0, 20, 40, 60, 90, 130, 180, 260], np.float32) def __init__(self, pc: PointCloud2): q = pc.points it = np.asarray(q["intensity"], np.float32) x, y, z = q["x"], q["y"], q["z"] r2 = x * x + y * y + z * z ok = (it > 0) & (r2 > 1.0) & np.isfinite(r2) r = np.sqrt(r2[ok], dtype=np.float32) v = it[ok] b = np.clip(np.searchsorted(self.EDGES, r, side="right") - 1, 0, self.EDGES.size - 2) order = np.argsort(b, kind="stable") b_s, v_s = b[order], v[order] cut = np.searchsorted(b_s, np.arange(self.EDGES.size - 1), side="left") cut = np.append(cut, b_s.size) self._pools = [v_s[cut[i]:cut[i + 1]] for i in range(self.EDGES.size - 1)] self._all = v_s def sample(self, d: float, n: int, rng: np.random.Generator) -> np.ndarray: i = int(np.clip(np.searchsorted(self.EDGES, d, side="right") - 1, 0, self.EDGES.size - 2)) pool = self._pools[i] if pool.size < 32: # на этой дальности возвратов нет pool = self._all if pool.size == 0: return np.full(n, 5.0, np.float32) return rng.choice(pool, size=n).astype(np.float32) def local_intensity(prev: np.ndarray, pool: np.ndarray, frame: np.ndarray, rng: np.random.Generator, env: "IntensityEnv | None" = None, d: float = 0.0) -> np.ndarray: """Яркость вставки по окружению. Отражения среди аргументов нет намеренно. Дальность участвует ровно в одном качестве — какую полосу реальных возвратов брать для лучей, ушедших в пустоту. Никакого закона яркости от дальности здесь не задаётся. """ v = np.asarray(prev, np.float32).copy() miss = ~(v > 0) if miss.any(): n = int(miss.sum()) if env is not None: v[miss] = env.sample(d, n, rng) else: src = pool[pool > 0] if pool.size else np.empty(0, np.float32) if src.size == 0: # все лучи предмета в пустоту wide = frame[::997] src = wide[wide > 0] v[miss] = (rng.choice(src, size=n) if src.size else np.float32(5.0)) v = v * rng.lognormal(0.0, INTEN_SPREAD, v.shape) return np.clip(v, 1.0, 255.0).astype(np.float32) def channel_max_range() -> np.ndarray: """Паспортная дальность каждого канала при 10 % отражения, (128,). Каналы сильно неравноправны: 34–65 «дальнобойные» и берут 200 м, а 98–128 смотрят в землю и рассчитаны только на ближнее и среднее поле. Без учёта этого синтетический полигон завышал бы дальность обнаружения для предметов, попадающих в нижние каналы. """ global _CHANNEL_RANGE if _CHANNEL_RANGE is None: import csv from pathlib import Path path = Path(__file__).with_name("data") / "pandar128_channels.csv" try: rows = sorted(csv.DictReader(open(path, encoding="utf-8")), key=lambda r: int(r["channel"])) _CHANNEL_RANGE = np.array([float(r["max_range_10pct_m"]) for r in rows], np.float32) except (OSError, KeyError, ValueError): _CHANNEL_RANGE = np.full(128, 200.0, np.float32) return _CHANNEL_RANGE def dropout_probability(r: np.ndarray, reflectivity: float, ang_w: np.ndarray | float = 1.0, ang_h: np.ndarray | float = 1.0, max_range: np.ndarray | float = 200.0) -> np.ndarray: """Вероятность, что эхо не вернётся. Принятая мощность падает как ρ·A/r², где A — доля пятна луча, закрытая предметом. Отсюда «эффективная дальность» r·√(ρ_паспорт/(ρ·A)), которую остаётся сравнить с паспортной дальностью **этого канала**. Переход сделан логистическим и смещён так, чтобы на паспортной дальности вероятность обнаружения равнялась заявленным 70 %. Заполнение пятна важно именно для мелких предметов: на 200 м луч шириной 1.75 мрад покрывает 35 см, и бутылка диаметром 9 см отражает лишь четверть его энергии — поэтому она пропадает намного раньше человека, хотя по геометрии в неё ещё попадают лучи. """ fill = np.clip(ang_w / BEAM_DIV_H, 0.05, 1.0) * np.clip(ang_h / BEAM_DIV_V, 0.05, 1.0) rho = max(reflectivity, 0.02) * fill eff = r * np.sqrt(SPEC_REFLECTIVITY / rho) width = 0.12 * np.asarray(max_range, np.float32) r50 = np.asarray(max_range, np.float32) + width * np.log(SPEC_POD / (1 - SPEC_POD)) p_detect = 1.0 / (1.0 + np.exp((eff - r50) / np.maximum(width, 1e-3))) p_detect = np.where(r > HARD_MAX_RANGE, 0.0, p_detect) return np.clip(1.0 - p_detect, 0.0, 1.0).astype(np.float32) def inject(pc: PointCloud2, layout: ScanLayout, plane: RailPlane, obj: ObjectModel, place: Placement, *, rng: np.random.Generator | None = None, range_noise: float = 0.02, cols: slice | None = None, env: "IntensityEnv | None" = None) -> tuple[PointCloud2, dict]: """Вставить предмет в облако точек. Возвращает (новое облако, разметка).""" rng = rng or np.random.default_rng() n_rings, n_az, n_echo = layout.n_rings, layout.n_az, layout.n_echo dd, du, dh, H = ray_dirs_track(layout, plane) # начало луча в системе предмета: сенсор в (0,0,H), предмет в (d, u, 0) od = np.full(dd.shape, -np.float32(place.d), np.float32) ou = np.full(dd.shape, -np.float32(place.u), np.float32) oh = np.full(dd.shape, H, np.float32) if place.yaw_deg: c, s = np.cos(np.radians(place.yaw_deg)), np.sin(np.radians(place.yaw_deg)) od, ou = c * od + s * ou, -s * od + c * ou dd, du = c * dd + s * du, -s * dd + c * du t = obj.intersect(od, ou, oh, dd, du, dh) hit = np.isfinite(t) & (t > 1.0) if not hit.any(): return pc, dict(hit_rays=0, d=place.d, u=place.u, name=obj.name) # шум дальности и пропуски эха t = t + rng.normal(0.0, range_noise, t.shape).astype(np.float32) w_obj, h_obj = obj.size ang_w = w_obj / np.maximum(t, 1.0) ang_h = h_obj / np.maximum(t, 1.0) ch_range = channel_max_range() per_ray_range = (ch_range[:n_rings, None] if ch_range.size >= n_rings else np.float32(200.0)) p_drop = dropout_probability(t, obj.reflectivity, ang_w, ang_h, per_ray_range) hit &= rng.random(t.shape) > p_drop pts = pc.points.copy() ring_i, col_i = np.nonzero(hit) raw_col = layout.gather[ring_i, col_i] # выпрямленный столбец → сырой new_r = t[ring_i, col_i] # плоский индекс точки: (сырой столбец · число эх + эхо) · число колец + кольцо base = (raw_col * n_echo) * n_rings + ring_i fx, fy, fz = pts["x"], pts["y"], pts["z"] # виды на поля, запись идёт в pts # предмет виден, только если он ближе уже зарегистрированного эха r_exist = np.full(base.shape, INF, np.float32) for e in range(n_echo): idx = base + e * n_rings ex, ey, ez = fx[idx], fy[idx], fz[idx] have = (ex != 0) | (ey != 0) | (ez != 0) r_e = np.where(have, np.sqrt(ex * ex + ey * ey + ez * ez), INF) r_exist = np.minimum(r_exist, r_e) closer = new_r < r_exist # яркость того, что было на лучах предмета: на выбранных — то, что он # заслонил, остальные идут в запасной набор для лучей без эха prev_sel = pts["intensity"][base[closer]] pool = pts["intensity"][base] ring_i, col_i, base, new_r = ring_i[closer], col_i[closer], base[closer], new_r[closer] n_written = int(base.size) if n_written: dir_sel = layout.dirs[ring_i, col_i] nx = (dir_sel[:, 0] * new_r).astype(np.float32) ny = (dir_sel[:, 1] * new_r).astype(np.float32) nz = (dir_sel[:, 2] * new_r).astype(np.float32) inten = local_intensity(prev_sel, pool, pts["intensity"], rng, env=env, d=place.d) for e in range(n_echo): idx = base + e * n_rings fx[idx] = nx fy[idx] = ny fz[idx] = nz pts["intensity"][idx] = inten.astype(np.float32) out = PointCloud2(stamp=pc.stamp, frame_id=pc.frame_id, height=pc.height, width=pc.width, point_step=pc.point_step, is_dense=pc.is_dense, points=pts) w, hgt = obj.size return out, dict(hit_rays=int(n_written), d=float(place.d), u=float(place.u), name=obj.name, width=w, height=hgt, reflectivity=obj.reflectivity, # индексы лучей, в которые предмет реально записан: по ним # разметка кандидата точная, а не «по дальности примерно» rays=(ring_i, col_i))