655 lines
36 KiB
Python
655 lines
36 KiB
Python
"""RETINA — омматидиальная решётка.
|
||
|
||
Фасеточный глаз дрозофилы — регулярная решётка омматидиев, каждый смотрит в свою
|
||
фиксированную сторону. Вращающийся лидар устроен так же: пара (кольцо, столбец)
|
||
задаёт направление луча. Поэтому облако точек сразу переводится в *ретинотопический*
|
||
дальностный образ `(кольцо, азимут)`, и вся дальнейшая обработка идёт в этой
|
||
решётке — как в зрительной системе мухи, а не в неупорядоченном облаке.
|
||
|
||
Три особенности конкретного сенсора, измеренные по данным (см. `docs/ALGORITHM.md`):
|
||
|
||
1. **Раскладка различается между бэгами**: 3600 азимутов на 360° против 1200 на
|
||
100°. Решётка калибруется по самим данным, ничего не захардкожено.
|
||
2. **Двойное эхо**: соседние столбцы делят один азимут. Когда эхо одно, оба слота
|
||
содержат одно значение; когда два — ближнее несёт объект, дальнее фон за ним.
|
||
Реально различаются ~3 % лучей, и это именно тонкие предметы и кромки.
|
||
3. **Скос решётки**: у каждого лазерного канала свой постоянный азимутальный сдвиг,
|
||
разброс достигает **15.5°** (≈155 столбцов). В сыром виде «столбец» не является
|
||
направлением: соседние кольца одного столбца смотрят в стороны, разнесённые на
|
||
градусы. Поэтому образ **выпрямляется** целочисленным сдвигом строк; остаточная
|
||
ошибка < половины шага азимута и учитывается в таблице направлений.
|
||
|
||
Быстрый путь опирается на порядок точек (столбец · эхо · кольцо). В синтетическом
|
||
бэге организаторов он соблюдается не везде: у облака нет поля `ring`, а там, где
|
||
вставлен предмет, заслонённые им точки удалены, а точки предмета вписаны в
|
||
середину массива — всё, что дальше, сдвинуто (так в половине кадров). Поэтому
|
||
порядок проверяется в каждом кадре: у кадра с целым порядком элевация каждой
|
||
точки совпадает с элевацией её кольца до 0.0001°. Кадр, где это не так,
|
||
раскладывается в ту же решётку **по углам каждой точки** — медленнее, зато без
|
||
допущений о порядке. Калибровка без поля `ring` находит кольца по гистограмме
|
||
элевации и берёт только кадры с целым порядком, а если таких нет — строит
|
||
решётку целиком по углам.
|
||
"""
|
||
from __future__ import annotations
|
||
|
||
import warnings
|
||
from contextlib import contextmanager
|
||
from dataclasses import dataclass
|
||
from pathlib import Path
|
||
|
||
import numpy as np
|
||
|
||
from .cdr import PointCloud2
|
||
|
||
DEG = np.pi / 180.0
|
||
|
||
# Допуск проверки порядка точек: в целом кадре элевация точки совпадает с
|
||
# элевацией её кольца до 0.0001° (замерено на всех бэгах и на синтетике), а
|
||
# ближайшие кольца Pandar128 разнесены на 0.086°. Вставленные точки предмета
|
||
# уходят от колец на 0.03° и больше, сдвинутые — на целое кольцо.
|
||
ORDER_TOL_DEG = 0.01
|
||
|
||
|
||
@dataclass
|
||
class RangeImage:
|
||
"""Выпрямленный дальностный образ в координатах `(кольцо, азимут)`."""
|
||
|
||
stamp: float
|
||
r_near: np.ndarray # (H, W) float32 — ближнее эхо, 0 = нет эха
|
||
r_far: np.ndarray # (H, W) float32 — дальнее эхо, 0 = нет эха
|
||
inten: np.ndarray # (H, W) float32 — интенсивность ближнего эха
|
||
valid: np.ndarray # (H, W) bool
|
||
|
||
@property
|
||
def shape(self) -> tuple[int, int]:
|
||
return self.r_near.shape
|
||
|
||
def xyz(self, layout: "ScanLayout") -> np.ndarray:
|
||
"""Декартовы координаты ближнего эха: (H, W, 3)."""
|
||
return layout.dirs * self.r_near[..., None]
|
||
|
||
def crop(self, cols: slice) -> "RangeImage":
|
||
return RangeImage(self.stamp, self.r_near[:, cols], self.r_far[:, cols],
|
||
self.inten[:, cols], self.valid[:, cols])
|
||
|
||
|
||
class ScanLayout:
|
||
"""Калиброванная и выпрямленная решётка лучей.
|
||
|
||
Азимут отсчитывается от направления движения (вперёд = −Y в системе сенсора),
|
||
положительный вправо; элевация — вверх от горизонтали сенсора.
|
||
"""
|
||
|
||
def __init__(self, el_deg: np.ndarray, az_step_deg: float, az0_deg: float,
|
||
col_shift: np.ndarray, az_resid_deg: np.ndarray,
|
||
n_az: int, n_echo: int, wrap: bool = False, indexed: bool = True):
|
||
self.el_deg = np.asarray(el_deg, np.float64) # (H,)
|
||
self.az_step_deg = float(az_step_deg) # шаг на азимутальный индекс
|
||
self.az0_deg = float(az0_deg)
|
||
self.col_shift = np.asarray(col_shift, np.int64) # (H,) выпрямление
|
||
self.az_resid_deg = np.asarray(az_resid_deg, np.float64) # (H,) остаток < шага/2
|
||
self.n_az = int(n_az)
|
||
self.n_echo = int(n_echo)
|
||
self.wrap = bool(wrap) # круговой скан: края смыкаются
|
||
# False — порядок точек неизвестен (решётка построена по углам), и
|
||
# каждый кадр раскладывается по углам точек
|
||
self.indexed = bool(indexed)
|
||
self.n_rings = self.el_deg.size
|
||
self.n_points = self.n_az * self.n_echo * self.n_rings
|
||
self.n_geometric = 0 # сколько кадров пришлось раскладывать по углам
|
||
self._init_lookup()
|
||
|
||
self.az_grid_deg = self.az0_deg + self.az_step_deg * np.arange(self.n_az)
|
||
|
||
# карта выборки для выпрямления: out[h, j] = raw[h, j + shift[h]]
|
||
j = np.arange(self.n_az)[None, :]
|
||
src = j + self.col_shift[:, None]
|
||
if self.wrap:
|
||
# у кругового скана «выехавшие» столбцы приходят с другого края
|
||
self.gather = np.mod(src, self.n_az).astype(np.intp)
|
||
self.gather_ok = np.ones_like(self.gather, bool)
|
||
else:
|
||
self.gather_ok = (src >= 0) & (src < self.n_az)
|
||
self.gather = np.clip(src, 0, self.n_az - 1).astype(np.intp)
|
||
|
||
self.az_full_deg = self.az_grid_deg[None, :] + self.az_resid_deg[:, None]
|
||
self.dirs = self._unit_dirs().astype(np.float32) # (H, W, 3)
|
||
# угловой шаг по кольцам: у решётки из одного кольца градиента нет
|
||
self.el_step_deg = (np.abs(np.gradient(self.el_deg)) if self.n_rings > 1
|
||
else np.full(self.n_rings, 0.125))
|
||
|
||
# ------------------------------------------------------------------ геометрия
|
||
|
||
def _unit_dirs(self) -> np.ndarray:
|
||
az = self.az_full_deg * DEG
|
||
el = (self.el_deg[:, None] * DEG) * np.ones_like(az)
|
||
c = np.cos(el)
|
||
return np.stack([c * np.sin(az), -c * np.cos(az), np.sin(el)], axis=-1)
|
||
|
||
def _init_lookup(self) -> None:
|
||
"""Таблицы для проверки порядка точек и поиска кольца по элевации."""
|
||
self._sin_el = np.sin(self.el_deg * DEG).astype(np.float32)
|
||
self._el_order = np.argsort(self.el_deg)
|
||
self._el_asc = self.el_deg[self._el_order]
|
||
|
||
def column_slice(self, half_fov_deg: float) -> slice:
|
||
"""Непрерывный диапазон столбцов внутри ±half_fov по азимуту."""
|
||
inside = np.flatnonzero(np.abs(self.az_grid_deg) <= half_fov_deg)
|
||
if inside.size == 0:
|
||
return slice(0, self.n_az)
|
||
return slice(int(inside[0]), int(inside[-1]) + 1)
|
||
|
||
def sub(self, cols: slice) -> "ScanLayout":
|
||
"""Урезанная по азимуту копия решётки (для обработки только переднего сектора)."""
|
||
start = cols.start or 0
|
||
stop = cols.stop if cols.stop is not None else self.n_az
|
||
out = ScanLayout.__new__(ScanLayout)
|
||
out.el_deg = self.el_deg
|
||
out.az_step_deg = self.az_step_deg
|
||
out.az0_deg = float(self.az_grid_deg[start])
|
||
out.col_shift = self.col_shift
|
||
out.az_resid_deg = self.az_resid_deg
|
||
out.n_az = stop - start
|
||
out.n_echo = self.n_echo
|
||
out.wrap = False # вырезанный сектор больше не смыкается
|
||
out.indexed = self.indexed
|
||
out.n_rings = self.n_rings
|
||
out.n_points = self.n_points
|
||
out.n_geometric = 0
|
||
out._init_lookup()
|
||
out.az_grid_deg = self.az_grid_deg[start:stop]
|
||
out.gather_ok = self.gather_ok[:, start:stop]
|
||
out.gather = self.gather[:, start:stop]
|
||
out.az_full_deg = self.az_full_deg[:, start:stop]
|
||
out.dirs = np.ascontiguousarray(self.dirs[:, start:stop])
|
||
out.el_step_deg = self.el_step_deg
|
||
return out
|
||
|
||
# ------------------------------------------------------------------ проекция
|
||
|
||
def project(self, pc: PointCloud2, cols: slice | None = None) -> RangeImage:
|
||
"""Облако точек → выпрямленный дальностный образ.
|
||
|
||
`cols` задаёт нужный диапазон **выходных** столбцов. Тяжёлая
|
||
арифметика (корень по 900 тыс. точек) выполняется только над теми
|
||
сырыми столбцами, которые в этот диапазон попадут с учётом скоса
|
||
каналов, — на круговом скане это экономит почти всё время стадии.
|
||
|
||
Кадр с нарушенным порядком точек (см. докстроку модуля) раскладывается
|
||
по углам точек: результат тот же, что дал бы целый кадр.
|
||
"""
|
||
w = self.n_az
|
||
start = 0 if cols is None else (cols.start or 0)
|
||
stop = w if cols is None else (cols.stop if cols.stop is not None else w)
|
||
if self.indexed and pc.n_points == self.n_points:
|
||
img = self._project_indexed(pc, start, stop)
|
||
if img is not None:
|
||
return img
|
||
self.n_geometric += 1
|
||
return self._project_geometric(pc, start, stop)
|
||
|
||
def _project_indexed(self, pc: PointCloud2, start: int, stop: int) -> RangeImage | None:
|
||
"""Быстрый путь по порядку точек. None — порядок в кадре нарушен."""
|
||
n, w, e = self.n_rings, self.n_az, self.n_echo
|
||
lo = start + int(self.col_shift.min())
|
||
hi = stop + int(self.col_shift.max())
|
||
if self.wrap:
|
||
raw_cols = np.arange(lo, hi) % w
|
||
else:
|
||
lo = max(lo, 0)
|
||
hi = min(hi, w)
|
||
raw_cols = None
|
||
|
||
pts = pc.points
|
||
|
||
def cube(name: str) -> np.ndarray:
|
||
a = pts[name].reshape(w, e, n)
|
||
a = a[raw_cols] if raw_cols is not None else a[lo:hi]
|
||
return a.transpose(2, 0, 1)
|
||
|
||
x, y, z = cube("x"), cube("y"), cube("z")
|
||
good = (x != 0) | (y != 0) | (z != 0)
|
||
r = np.sqrt(x * x + y * y + z * z, dtype=np.float32)
|
||
good &= np.isfinite(r) # драйверы, отдающие «нет эха» как NaN
|
||
r = np.where(good, r, np.float32(0.0))
|
||
|
||
# Порядок: элевация каждой точки должна совпасть с элевацией её кольца.
|
||
# У «нет эха» x = y = z = r = 0, и отклонение тоже ноль.
|
||
dev = np.abs(z - r * self._sin_el[:, None, None])
|
||
if np.any(dev > r * np.float32(ORDER_TOL_DEG * DEG) + np.float32(1e-3)):
|
||
return None
|
||
|
||
# Яркость — не обязательное поле: у драйвера без неё облако всё равно
|
||
# раскладывается, а яркость считается нулевой (как на медленном пути)
|
||
has_i = "intensity" in pts.dtype.names
|
||
if e == 1:
|
||
r_near = r[..., 0]
|
||
r_far = r[..., 0]
|
||
it = cube("intensity")[..., 0] if has_i else np.zeros_like(r_near)
|
||
valid = good[..., 0]
|
||
else:
|
||
inten = cube("intensity") if has_i else np.zeros_like(r)
|
||
near_i = np.argmin(np.where(good, r, np.float32(np.inf)), axis=-1)[..., None]
|
||
far_i = np.argmax(r, axis=-1)[..., None]
|
||
r_near = np.take_along_axis(r, near_i, -1)[..., 0]
|
||
r_far = np.take_along_axis(r, far_i, -1)[..., 0]
|
||
it = np.take_along_axis(inten, near_i, -1)[..., 0]
|
||
valid = good.any(axis=-1)
|
||
|
||
# выпрямление скоса каналов, с поправкой на смещение окна
|
||
g = self.gather[:, start:stop]
|
||
ok = self.gather_ok[:, start:stop]
|
||
if raw_cols is not None:
|
||
g = (g - lo) % w
|
||
else:
|
||
g = g - lo
|
||
ok = ok & (g >= 0) & (g < (hi - lo))
|
||
np.clip(g, 0, hi - lo - 1, out=g)
|
||
|
||
r_near = np.take_along_axis(r_near, g, 1)
|
||
r_far = np.take_along_axis(r_far, g, 1)
|
||
it = np.take_along_axis(it, g, 1)
|
||
valid = np.take_along_axis(valid, g, 1) & ok
|
||
|
||
r_near = np.where(valid, r_near, np.float32(0.0))
|
||
r_far = np.where(valid, r_far, np.float32(0.0))
|
||
return RangeImage(pc.stamp, np.ascontiguousarray(r_near),
|
||
np.ascontiguousarray(r_far), np.ascontiguousarray(it),
|
||
np.ascontiguousarray(valid))
|
||
|
||
def _project_geometric(self, pc: PointCloud2, start: int, stop: int) -> RangeImage:
|
||
"""Раскладка по углам: кольцо — по элевации точки, столбец — по азимуту.
|
||
|
||
Не опирается на порядок точек вовсе. Если в ячейку попало несколько
|
||
точек (двойное эхо, вставленный предмет поверх фона), ближняя идёт в
|
||
`r_near`, дальняя — в `r_far`, как у целого кадра.
|
||
"""
|
||
n, wid = self.n_rings, stop - start
|
||
pts = pc.points
|
||
x = np.asarray(pts["x"], np.float32)
|
||
y = np.asarray(pts["y"], np.float32)
|
||
z = np.asarray(pts["z"], np.float32)
|
||
good = ((x != 0) | (y != 0) | (z != 0)) & np.isfinite(x) & np.isfinite(y) \
|
||
& np.isfinite(z)
|
||
idx = np.flatnonzero(good)
|
||
x, y, z = x[idx], y[idx], z[idx]
|
||
|
||
# грубый отбор сектора по азимуту, без поправки кольца
|
||
step = self.az_step_deg
|
||
az = np.degrees(np.arctan2(x, -y))
|
||
jf = (az - np.float32(self.az0_deg)) / np.float32(step)
|
||
if self.wrap:
|
||
jf = np.mod(jf, self.n_az)
|
||
margin = float(np.abs(self.az_resid_deg).max()) / abs(step) + 1.0
|
||
sel = np.flatnonzero((jf > start - margin) & (jf < stop - 1 + margin))
|
||
idx, x, y, z, az = idx[sel], x[sel], y[sel], z[sel], az[sel]
|
||
|
||
r = np.sqrt(x * x + y * y + z * z)
|
||
el = np.degrees(np.arcsin(np.clip(z / np.maximum(r, np.float32(1e-6)), -1.0, 1.0)))
|
||
# ближайшее кольцо по элевации
|
||
k = np.clip(np.searchsorted(self._el_asc, el), 1, n - 1)
|
||
k -= (el - self._el_asc[k - 1]) < (self._el_asc[k] - el)
|
||
h = self._el_order[k]
|
||
ok = np.abs(el - self.el_deg[h]) <= np.maximum(self.el_step_deg[h], 0.2)
|
||
j = np.rint((az - self.az_resid_deg[h] - self.az0_deg) / step).astype(np.int64)
|
||
if self.wrap:
|
||
j %= self.n_az
|
||
ok &= (j >= start) & (j < stop)
|
||
sel = np.flatnonzero(ok)
|
||
|
||
r_near = np.zeros(n * wid, np.float32)
|
||
r_far = np.zeros(n * wid, np.float32)
|
||
it = np.zeros(n * wid, np.float32)
|
||
valid = np.zeros(n * wid, bool)
|
||
if sel.size:
|
||
cell = h[sel] * wid + (j[sel] - start)
|
||
rr = r[sel]
|
||
# сортировка по (ячейка, дальность): первая в ячейке — ближняя
|
||
mm = np.minimum(rr * 1000.0, (1 << 20) - 1).astype(np.int64)
|
||
order = np.argsort(cell * (1 << 20) + mm)
|
||
cs, rs = cell[order], rr[order]
|
||
brk = np.flatnonzero(cs[1:] != cs[:-1])
|
||
first = np.concatenate(([0], brk + 1))
|
||
last = np.concatenate((brk, [cs.size - 1]))
|
||
r_near[cs[first]] = rs[first]
|
||
r_far[cs[last]] = rs[last]
|
||
valid[cs[first]] = True
|
||
if "intensity" in pts.dtype.names:
|
||
src = np.asarray(pts["intensity"], np.float32)[idx[sel]][order]
|
||
it[cs[first]] = src[first]
|
||
return RangeImage(pc.stamp, r_near.reshape(n, wid), r_far.reshape(n, wid),
|
||
it.reshape(n, wid), valid.reshape(n, wid))
|
||
|
||
# ------------------------------------------------------------------ калибровка
|
||
|
||
@staticmethod
|
||
def calibrate(clouds: list[PointCloud2], n_rings: int | None = None) -> "ScanLayout":
|
||
"""Восстановить решётку по нескольким кадрам.
|
||
|
||
Определяются: число колец и эх, элевация каждого кольца, шаг развёртки,
|
||
азимутальный сдвиг каждого канала и целочисленное выпрямление образа.
|
||
"""
|
||
if not clouds:
|
||
raise ValueError("нужен хотя бы один кадр для калибровки")
|
||
pc0 = clouds[0]
|
||
if not _ring_ordered(pc0):
|
||
# Без поля `ring` (синтетика организаторов) или с ним, но не
|
||
# «столбец за столбцом»: драйвер отдаёт только точки с эхом,
|
||
# раскладывает кадр по кольцам или перемешивает. Кольца — по
|
||
# гистограмме элевации, а в калибровку по порядку точек идут только
|
||
# кадры, где этот порядок цел. Нет таких — решётка строится целиком
|
||
# по углам.
|
||
el_ring = _ring_elevations(clouds)
|
||
if n_rings is None:
|
||
n_rings = el_ring.size
|
||
whole = [pc for pc in clouds if _is_organized(pc, el_ring)]
|
||
whole = [pc for pc in whole if pc.n_points == whole[0].n_points] if whole else []
|
||
if not whole:
|
||
return ScanLayout._calibrate_geometric(clouds, el_ring)
|
||
clouds, pc0 = whole, whole[0]
|
||
if n_rings is None:
|
||
n_rings = int(pc0.points["ring"].max()) + 1
|
||
n_cols, rem = divmod(pc0.n_points, n_rings)
|
||
if rem:
|
||
raise ValueError(f"{pc0.n_points} точек не делится на {n_rings} колец")
|
||
|
||
# Направление луча задано сенсором и в каждом кадре одно и то же:
|
||
# кадры нужны только чтобы закрыть лучи, не вернувшие эхо. Поэтому
|
||
# берётся первое конечное значение, а не медиана по стопке кадров —
|
||
# та стоила 1.4 с из 2.8 с всей калибровки и ничего не уточняла:
|
||
# ниже и шаг развёртки, и сдвиг канала берутся медианой по тысячам
|
||
# столбцов, где шум одного отсчёта всё равно усредняется.
|
||
az = el = None
|
||
for pc in clouds:
|
||
x = pc.points["x"].reshape(n_cols, n_rings).T.astype(np.float32)
|
||
y = pc.points["y"].reshape(n_cols, n_rings).T.astype(np.float32)
|
||
z = pc.points["z"].reshape(n_cols, n_rings).T.astype(np.float32)
|
||
ok = (x != 0) | (y != 0) | (z != 0)
|
||
with _quiet():
|
||
a = np.where(ok, np.degrees(np.arctan2(x, -y)), np.nan)
|
||
e = np.where(ok, np.degrees(np.arctan2(z, np.hypot(x, y))), np.nan)
|
||
if az is None:
|
||
az, el = a, e
|
||
continue
|
||
gap = np.isnan(az)
|
||
if not gap.any():
|
||
break
|
||
az[gap] = a[gap]
|
||
el[gap] = e[gap]
|
||
az = az.astype(np.float64)
|
||
el = el.astype(np.float64)
|
||
|
||
n_echo = _detect_echoes(az)
|
||
n_az = n_cols // n_echo
|
||
if n_echo > 1:
|
||
with _quiet():
|
||
az = np.nanmean(az.reshape(n_rings, n_az, n_echo), axis=2)
|
||
el = np.nanmean(el.reshape(n_rings, n_az, n_echo), axis=2)
|
||
|
||
# общий шаг развёртки: медиана по кольцам от робастной оценки наклона
|
||
slopes = []
|
||
for h in range(n_rings):
|
||
row = az[h]
|
||
idx = np.flatnonzero(np.isfinite(row))
|
||
if idx.size < 50:
|
||
continue
|
||
d = np.diff(np.unwrap(np.radians(row[idx]))) / np.diff(idx)
|
||
slopes.append(np.median(np.degrees(d)))
|
||
if not slopes:
|
||
raise ValueError("недостаточно валидных лучей для калибровки развёртки")
|
||
step = float(np.median(slopes))
|
||
|
||
# смещение каждого канала относительно общей развёртки
|
||
j = np.arange(n_az, dtype=np.float64)
|
||
base = step * j
|
||
with _quiet():
|
||
c_ring = np.nanmedian(_wrap180(az - base[None, :]), axis=1) # (H,)
|
||
c_ring = _fill_linear(c_ring)
|
||
c0 = float(np.median(c_ring))
|
||
|
||
# круговой скан: развёртка покрывает полные 360°
|
||
wrap = abs(step) * n_az > 350.0
|
||
|
||
# начало отсчёта выбирается так, чтобы «вперёд» (азимут 0) был в середине
|
||
# образа — иначе шов ±180° разрезал бы рабочий сектор пополам
|
||
if wrap:
|
||
k = int(np.rint(-c0 / step)) - n_az // 2
|
||
c0 = _wrap180(c0 + step * k)
|
||
# разница берётся по кратчайшей дуге: иначе шов ±180° даёт сдвиг в пол-оборота
|
||
shift = np.rint(_wrap180(c0 - c_ring) / step).astype(np.int64)
|
||
resid = _wrap180(c_ring + step * shift - c0)
|
||
|
||
with _quiet():
|
||
el_ring = _fill_linear(np.nanmedian(el, axis=1))
|
||
return ScanLayout(el_ring, step, float(c0), shift, resid, n_az, n_echo, wrap)
|
||
|
||
@staticmethod
|
||
def _calibrate_geometric(clouds: list[PointCloud2], el_ring: np.ndarray) -> "ScanLayout":
|
||
"""Решётка только по углам точек, когда ни в одном кадре нет целого порядка.
|
||
|
||
Кольцо — ближайшая элевация из `el_ring`. Шаг развёртки — самая частая
|
||
разность соседних азимутов внутри кольца, сдвиг кольца внутри шага —
|
||
круговое среднее фазы его азимутов. Столбец растёт, азимут убывает — как
|
||
у Pandar128 в наших бэгах, чтобы образ не отразился зеркально.
|
||
"""
|
||
n = el_ring.size
|
||
el_desc = np.sort(el_ring)[::-1]
|
||
order = np.argsort(el_desc)
|
||
asc = el_desc[order]
|
||
azs, hs = [], []
|
||
for pc in clouds[:6]:
|
||
x, y, z, ok = _xyz64(pc)
|
||
x, y, z = x[ok], y[ok], z[ok]
|
||
el = np.degrees(np.arctan2(z, np.hypot(x, y)))
|
||
k = np.clip(np.searchsorted(asc, el), 1, n - 1)
|
||
k -= (el - asc[k - 1]) < (asc[k] - el)
|
||
h = order[k]
|
||
on = np.abs(el - el_desc[h]) < ORDER_TOL_DEG
|
||
azs.append(np.degrees(np.arctan2(x[on], -y[on])))
|
||
hs.append(h[on])
|
||
az = np.concatenate(azs)
|
||
h = np.concatenate(hs)
|
||
|
||
diffs = []
|
||
for ring in range(n):
|
||
a = np.unique(np.round(az[h == ring], 4))
|
||
if a.size > 20:
|
||
diffs.append(np.diff(a))
|
||
if not diffs:
|
||
raise ValueError("недостаточно валидных лучей для калибровки развёртки")
|
||
d = np.concatenate(diffs)
|
||
d = d[d > 1e-3]
|
||
vals, cnt = np.unique(np.round(d, 3), return_counts=True)
|
||
step = float(vals[np.argmax(cnt)])
|
||
step = float(np.median(d[np.abs(d - step) < 0.1 * step]))
|
||
|
||
ph = np.exp(2j * np.pi * az / step)
|
||
s = (np.bincount(h, weights=ph.real, minlength=n)
|
||
+ 1j * np.bincount(h, weights=ph.imag, minlength=n))
|
||
ref = float(np.angle(s.sum()) / (2 * np.pi) * step)
|
||
frac = np.where(np.abs(s) > 0, np.angle(s) / (2 * np.pi) * step, ref)
|
||
resid = (frac - ref + step / 2) % step - step / 2
|
||
|
||
lo, hi = np.percentile(az, [0.05, 99.95])
|
||
wrap = bool(hi - lo > 350.0)
|
||
if wrap:
|
||
n_az = int(round(360.0 / step))
|
||
az0 = ref + step * round((180.0 - ref) / step)
|
||
else:
|
||
az0 = ref + step * round((hi - ref) / step)
|
||
n_az = int(round((az0 - lo) / step)) + 1
|
||
return ScanLayout(el_desc, -step, float(az0), np.zeros(n, np.int64), resid,
|
||
n_az, 1, wrap, indexed=False)
|
||
|
||
# ------------------------------------------------------------------ сериализация
|
||
|
||
def save(self, path: str | Path) -> None:
|
||
np.savez_compressed(path, el_deg=self.el_deg, az_step_deg=self.az_step_deg,
|
||
az0_deg=self.az0_deg, col_shift=self.col_shift,
|
||
az_resid_deg=self.az_resid_deg, n_az=self.n_az,
|
||
n_echo=self.n_echo, wrap=self.wrap, indexed=self.indexed)
|
||
|
||
@staticmethod
|
||
def load(path: str | Path) -> "ScanLayout":
|
||
d = np.load(path)
|
||
return ScanLayout(d["el_deg"], float(d["az_step_deg"]), float(d["az0_deg"]),
|
||
d["col_shift"], d["az_resid_deg"], int(d["n_az"]),
|
||
int(d["n_echo"]), bool(d["wrap"]) if "wrap" in d else False,
|
||
bool(d["indexed"]) if "indexed" in d else True)
|
||
|
||
def __repr__(self) -> str:
|
||
kind = "" if self.indexed else ", по углам точек"
|
||
return (f"ScanLayout(колец={self.n_rings}, азимутов={self.n_az}, эх={self.n_echo}, "
|
||
f"сектор {self.az_grid_deg.min():.1f}°…{self.az_grid_deg.max():.1f}°, "
|
||
f"шаг {abs(self.az_step_deg):.3f}°, "
|
||
f"элевация {self.el_deg.min():.1f}°…{self.el_deg.max():.1f}°, "
|
||
f"скос каналов {np.ptp(self.col_shift)} стлб{kind})")
|
||
|
||
|
||
# ---------------------------------------------------------------------- вспомогательное
|
||
|
||
@contextmanager
|
||
def _quiet():
|
||
"""Пустые срезы и деление на ноль при калибровке — норма, а не ошибка."""
|
||
with warnings.catch_warnings(), np.errstate(all="ignore"):
|
||
warnings.simplefilter("ignore", RuntimeWarning)
|
||
yield
|
||
|
||
|
||
def _wrap180(a):
|
||
"""Привести угол(ы) в градусах к полуинтервалу (−180, 180]."""
|
||
return -((-np.asarray(a, np.float64) + 180.0) % 360.0 - 180.0)
|
||
|
||
|
||
def _xyz64(pc: PointCloud2):
|
||
"""Координаты в float64 и маска точек с эхом (не ноль и не NaN)."""
|
||
p = pc.points
|
||
x = p["x"].astype(np.float64)
|
||
y = p["y"].astype(np.float64)
|
||
z = p["z"].astype(np.float64)
|
||
ok = ((x != 0) | (y != 0) | (z != 0)) & np.isfinite(x) & np.isfinite(y) & np.isfinite(z)
|
||
return x, y, z, ok
|
||
|
||
|
||
def _ring_elevations(clouds: list[PointCloud2], max_frames: int = 6) -> np.ndarray:
|
||
"""Элевации колец по гистограмме, без поля `ring` и без опоры на порядок.
|
||
|
||
У лазерного канала элевация постоянна до 0.0001°, поэтому точки кольца
|
||
ложатся в один бин в 0.002°; соседние кольца Pandar128 разнесены на 0.086°
|
||
и больше. Разброс float32 может расщепить кольцо на соседние бины — они
|
||
сливаются. Вставленные точки (синтетика) рассыпаны по элевации и дают
|
||
мелкие группы, которые отсекает порог по весу. Порядок — сверху вниз,
|
||
как нумерует каналы Hesai.
|
||
"""
|
||
parts = []
|
||
for pc in clouds[:max_frames]:
|
||
x, y, z, ok = _xyz64(pc)
|
||
parts.append(np.degrees(np.arctan2(z[ok], np.hypot(x[ok], y[ok]))))
|
||
el = np.concatenate(parts)
|
||
if el.size == 0:
|
||
raise ValueError("нет ни одной точки с эхом для калибровки колец")
|
||
q, cnt = np.unique(np.rint(el / 0.002).astype(np.int64), return_counts=True)
|
||
grp = np.concatenate(([0], np.cumsum(np.diff(q) > 5)))
|
||
w = np.bincount(grp, weights=cnt)
|
||
c = np.bincount(grp, weights=cnt * q * 0.002) / w
|
||
top = np.sort(w)[-min(64, w.size):]
|
||
keep = w >= 0.25 * np.median(top)
|
||
if keep.sum() < 2:
|
||
raise ValueError("не удалось выделить кольца по элевации")
|
||
return np.sort(c[keep])[::-1]
|
||
|
||
|
||
def _ring_ordered(pc: PointCloud2) -> bool:
|
||
"""Идут ли точки столбец за столбцом с одним порядком колец (по полю `ring`).
|
||
|
||
Калибровка по полю `ring` раскладывает кадр на столбцы по n_rings точек и
|
||
без этой проверки молча ошибалась на любом другом порядке: драйвер, который
|
||
отдаёт только точки с эхом, падал на «не делится на 128 колец», кадр,
|
||
разложенный по кольцам, — на каждом кадре, а перемешанный давал неверные
|
||
дальности (`tools/check_formats.py`).
|
||
"""
|
||
if "ring" not in pc.points.dtype.names or pc.n_points == 0:
|
||
return False
|
||
ring = np.asarray(pc.points["ring"]).astype(np.int64)
|
||
n = int(ring.max()) + 1
|
||
if n < 2 or ring.size % n:
|
||
return False
|
||
cols = ring.reshape(-1, n)
|
||
first = cols[0]
|
||
return bool(np.array_equal(np.sort(first), np.arange(n)) and np.all(cols == first))
|
||
|
||
|
||
def forward_azimuth(clouds: list[PointCloud2], far: float = 30.0, half: float = 30.0,
|
||
min_points: int = 2000) -> float:
|
||
"""Куда у облака смотрит «вперёд», по дальним эхам: 0 — как у выданных записей.
|
||
|
||
У выданных записей вперёд −Y. Другой драйвер может повернуть систему
|
||
координат (по REP-103 вперёд +X), и тогда рабочий сектор смотрит в стену —
|
||
узел молча не видит ничего. Далеко лидар видит только вдоль тоннеля, а назад
|
||
мешает сам поезд, поэтому направление с большинством дальних эх — вперёд.
|
||
Поворот признаётся только явный: в переднем секторе дальних эх почти нет, а
|
||
в одном из трёх других — больше половины. Иначе 0, и облако не трогается.
|
||
"""
|
||
parts = []
|
||
for pc in clouds[:6]:
|
||
x, y, z, ok = _xyz64(pc)
|
||
m = ok & (np.hypot(x, y) > far)
|
||
parts.append(np.degrees(np.arctan2(x[m], -y[m])))
|
||
az = np.concatenate(parts) if parts else np.zeros(0)
|
||
if az.size < min_points:
|
||
return 0.0
|
||
share = {c: float(np.mean(np.abs(_wrap180(az - c)) <= half))
|
||
for c in (0.0, 90.0, 180.0, -90.0)}
|
||
if share[0.0] >= 0.02:
|
||
return 0.0
|
||
best = max((90.0, 180.0, -90.0), key=lambda c: share[c])
|
||
return best if share[best] >= 0.5 else 0.0
|
||
|
||
|
||
def rotate_cloud(pc: PointCloud2, az_deg: float) -> PointCloud2:
|
||
"""Повернуть облако вокруг вертикали так, чтобы азимут `az_deg` стал «вперёд»."""
|
||
pts = np.array(pc.points) # копия: буфер кадра только для чтения
|
||
f, r = -pts["y"].astype(np.float64), pts["x"].astype(np.float64)
|
||
c, s = np.cos(np.radians(az_deg)), np.sin(np.radians(az_deg))
|
||
pts["x"] = -f * s + r * c
|
||
pts["y"] = -(f * c + r * s)
|
||
return 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)
|
||
|
||
|
||
def _is_organized(pc: PointCloud2, el_ring: np.ndarray) -> bool:
|
||
"""Цел ли порядок точек: элевация каждой точки = элевация её кольца."""
|
||
n = el_ring.size
|
||
if pc.n_points == 0 or pc.n_points % n:
|
||
return False
|
||
x, y, z, ok = _xyz64(pc)
|
||
el = np.degrees(np.arctan2(z, np.hypot(x, y))).reshape(-1, n)
|
||
ok = ok.reshape(-1, n)
|
||
for table in (el_ring, el_ring[::-1]):
|
||
if np.all(np.abs(el - table[None, :])[ok] < ORDER_TOL_DEG):
|
||
return True
|
||
return False
|
||
|
||
|
||
def _detect_echoes(az: np.ndarray) -> int:
|
||
"""Двойное эхо: соседние столбцы делят азимут (проверяется по каждому кольцу)."""
|
||
if az.shape[1] < 4:
|
||
return 1
|
||
a, b = az[:, 0::2], az[:, 1::2]
|
||
m = np.isfinite(a) & np.isfinite(b)
|
||
if m.sum() < 100:
|
||
return 1
|
||
return 2 if np.mean(np.abs(a[m] - b[m]) < 1e-3) > 0.8 else 1
|
||
|
||
|
||
def _fill_linear(a: np.ndarray) -> np.ndarray:
|
||
"""Линейно достроить NaN-пропуски по индексу (сетка равномерная)."""
|
||
a = np.asarray(a, np.float64).copy()
|
||
bad = ~np.isfinite(a)
|
||
if bad.all():
|
||
raise ValueError("нет ни одного валидного угла для калибровки")
|
||
if bad.any():
|
||
idx = np.arange(a.size)
|
||
a[bad] = np.interp(idx[bad], idx[~bad], a[~bad])
|
||
return a
|