forked from Dan4ick/Lidar_Muxa
443 lines
25 KiB
Python
443 lines
25 KiB
Python
"""LOBULA — колонковые детекторы объектов (аналог LC11) и сборка кандидатов.
|
||
|
||
В лобуле мухи сидят колонковые нейроны LC: каждый смотрит в свой участок поля
|
||
зрения и реагирует на **признак**, а не на картинку целиком. LC11 — детектор
|
||
мелкого объекта: он возбуждается компактным пятном, выделяющимся из фона, и
|
||
подавляется широкопольным движением, то есть собственным перемещением мухи.
|
||
Выход LC-популяции — не изображение, а короткий список «где что-то есть».
|
||
|
||
Здесь тем же занимается `find_candidates`: из ретинотопических масок улик
|
||
собираются связные пятна, для каждого считаются геометрические признаки, и
|
||
дальше по конвейеру идут уже десятки кандидатов вместо сотен тысяч лучей.
|
||
|
||
Решение о том, препятствие это или штатная конструкция тоннеля, здесь НЕ
|
||
принимается — этим занимаются грибовидное тело (знакомость) и центральный
|
||
комплекс (накопление улик по кадрам).
|
||
"""
|
||
from __future__ import annotations
|
||
|
||
from dataclasses import dataclass, field
|
||
|
||
import numpy as np
|
||
from scipy import ndimage
|
||
|
||
from .geometry import Corridor, TrackFrame
|
||
from .lamina import LaminaOutput, contrast_to_depth_gap
|
||
|
||
_NEIGHBOURS = np.ones((3, 3), bool)
|
||
|
||
|
||
@dataclass
|
||
class Candidate:
|
||
"""Кандидат в препятствия — компактная группа лучей."""
|
||
|
||
d: float # расстояние вперёд, м
|
||
u: float # смещение от оси пути, м
|
||
h: float # высота над головкой рельса, м
|
||
d_min: float # ближняя граница, м
|
||
h_min: float # нижняя точка над рельсом, м — есть ли опора снизу
|
||
width: float # ширина, м
|
||
height: float # высота, м
|
||
depth: float # протяжённость вдоль пути, м
|
||
containment: float # доля лучей пятна, оставшихся внутри габарита, 0…1
|
||
n_rays: int # число лучей
|
||
n_rings: int
|
||
n_cols: int
|
||
gap: float # насколько ближе фона, м
|
||
on: float # ON-контраст, 1/м
|
||
floor_deficit: float # насколько раньше оборвался луч до полотна, м
|
||
shadow: float # доля тени (нет эха) сразу за объектом
|
||
inten: float # медианная интенсивность
|
||
az_deg: float
|
||
el_deg: float
|
||
bbox: tuple[int, int, int, int]
|
||
score: float = 0.0
|
||
novelty: float = 1.0
|
||
extra: dict = field(default_factory=dict)
|
||
|
||
@property
|
||
def area(self) -> float:
|
||
return self.width * self.height
|
||
|
||
|
||
def cluster_by_depth(mask: np.ndarray, r: np.ndarray, *,
|
||
rel_tol: float = 0.06, abs_tol: float = 0.35,
|
||
col_reach: int = 3, row_reach: int = 2):
|
||
"""Связные компоненты с учётом разрыва по глубине.
|
||
|
||
Обычная связность по соседству склеивает предмет со стеной, которая
|
||
оказалась в том же месте изображения, но на сто метров дальше. Поэтому два
|
||
соседних луча объединяются только если их дальности близки — допуск растёт
|
||
с расстоянием, потому что и разрешение, и шум растут так же.
|
||
|
||
Соседство берётся с запасом по азимуту (`col_reach`), чтобы пропуски эха
|
||
внутри предмета не дробили его на части.
|
||
|
||
Возвращает (labels, n_labels); метка 0 — фон.
|
||
"""
|
||
idx = np.flatnonzero(mask.ravel())
|
||
if idx.size == 0:
|
||
return np.zeros(mask.shape, np.int32), 0
|
||
|
||
h, w = mask.shape
|
||
lut = np.full(mask.size, -1, np.int64)
|
||
lut[idx] = np.arange(idx.size)
|
||
ri = r.ravel()[idx]
|
||
rows, cols = np.divmod(idx, w)
|
||
|
||
src, dst = [], []
|
||
for dr in range(0, row_reach + 1):
|
||
for dc in range(-col_reach, col_reach + 1):
|
||
if dr == 0 and dc <= 0:
|
||
continue
|
||
nr, nc = rows + dr, cols + dc
|
||
ok = (nr < h) & (nc >= 0) & (nc < w)
|
||
nb = lut[np.where(ok, nr * w + nc, 0)]
|
||
ok &= nb >= 0
|
||
if not ok.any():
|
||
continue
|
||
a = np.flatnonzero(ok)
|
||
b = nb[ok]
|
||
close = np.abs(ri[a] - ri[b]) <= (abs_tol + rel_tol * np.minimum(ri[a], ri[b]))
|
||
if close.any():
|
||
src.append(a[close])
|
||
dst.append(b[close])
|
||
|
||
n_nodes = idx.size
|
||
if src:
|
||
from scipy.sparse import coo_matrix
|
||
from scipy.sparse.csgraph import connected_components
|
||
s = np.concatenate(src)
|
||
t = np.concatenate(dst)
|
||
g = coo_matrix((np.ones(s.size, np.int8), (s, t)), shape=(n_nodes, n_nodes))
|
||
n_comp, comp = connected_components(g, directed=False)
|
||
else:
|
||
n_comp = n_nodes
|
||
comp = np.arange(n_nodes)
|
||
|
||
labels = np.zeros(mask.size, np.int32)
|
||
labels[idx] = comp + 1
|
||
return labels.reshape(mask.shape), n_comp
|
||
|
||
|
||
def split_by_figure(labels: np.ndarray, n: int, r: np.ndarray,
|
||
figure: np.ndarray, *, max_depth: float, thr: float,
|
||
col_reach: int = 3, row_reach: int = 2, top: int = 0):
|
||
"""Вырезать фигуру из компоненты, растёкшейся по фону.
|
||
|
||
Гладкая стена тоннеля связна по глубине от ближнего поля до горизонта:
|
||
соседние лучи отличаются на сантиметры, и допуск сшивает их в одну
|
||
компоненту длиной сто метров. Предмет у такой стены попадает в неё же
|
||
и вместе с ней отбрасывается правилом `max_depth` — система слепнет
|
||
именно там, где смотрит вдоль стены, а это опаснее лишней тревоги.
|
||
|
||
Разорвать такую компоненту по дальности нельзя: предмет и стена рядом с
|
||
ним стоят на одной дальности. Зато они по-разному **приближаются**. Вдоль
|
||
фиксированного луча стена, параллельная движению, не приближается вовсе:
|
||
поезд едет, точка пересечения скользит по стене, дальность не меняется.
|
||
Предмет, обращённый к поезду, приближается ровно на пройденный путь.
|
||
Отсюда `advance` ≈ 0 у фона и ≈ 1 у фигуры.
|
||
|
||
Это и есть разделение фигуры и фона, которым у мухи занят канал
|
||
T4/T5 → LPTC: широкопольный поток задаёт ожидание, а то, что движется
|
||
иначе, становится фигурой.
|
||
|
||
Второй, более сильный источник того же признака — **контраст ламины**.
|
||
Гладкая стена по построению даёт нулевой центр-окружение: сколько бы ни
|
||
менялась дальность вдоль стены, она меняется плавно, и центр равен своему
|
||
окружению. Предмет на стене — это ступенька, и она видна. Замер на
|
||
вставленном человеке (`roundT_pressureGate_roundT` и `roundT_doubleT`):
|
||
при пороге 6 м порог проходят 85 % и 75 % лучей предмета против 0.13 % и
|
||
0.22 % лучей фона, а сам фон распадается всего на 5–9 компонент в кадре
|
||
вместо 1259 у разреза по допуску.
|
||
|
||
Поэтому `figure` — любой покадровый признак «это не фон»: скорость
|
||
сближения вдоль луча или контраст к окружению в метрах. Пересобираются
|
||
только переглубокие компоненты и только по лучам с `figure > thr`;
|
||
остальные метки не трогаются.
|
||
|
||
`top` ограничивает число фигур, выносимых из одной компоненты, оставляя
|
||
сильнейшие по признаку. Это не косметика, а необходимость: разрез отрезает
|
||
фон, и протяжённость кандидата вдоль пути падает с 6.9 до 0.8 м — вместе с
|
||
ней исчезает множитель компактности в весе улики, который до того давил
|
||
конструкции в десять раз. Каждая лишняя фигура поэтому стоит дорого.
|
||
Ограничение — тот же приём глобального торможения, которым APL оставляет
|
||
активными считанные проценты клеток Кеньона. 0 — без ограничения.
|
||
"""
|
||
if n == 0 or figure is None:
|
||
return labels, n
|
||
flat = labels.ravel()
|
||
rr = r.ravel()
|
||
adv = figure.ravel()
|
||
order = np.argsort(flat, kind="stable")
|
||
starts = np.searchsorted(flat[order], np.arange(n + 2))
|
||
|
||
out, next_id, copied = labels, n, False
|
||
for lbl in range(1, n + 1):
|
||
idx = order[starts[lbl]:starts[lbl + 1]]
|
||
if idx.size < 2:
|
||
continue
|
||
v = rr[idx]
|
||
v = v[np.isfinite(v)]
|
||
if v.size < 2 or float(v.max() - v.min()) <= max_depth:
|
||
continue
|
||
|
||
keep = idx[adv[idx] > thr]
|
||
if keep.size < 4 or keep.size == idx.size:
|
||
continue
|
||
sub = np.zeros(labels.shape, bool)
|
||
sub.ravel()[keep] = True
|
||
parts, n2 = cluster_by_depth(sub, r, col_reach=col_reach, row_reach=row_reach)
|
||
if n2 == 0:
|
||
continue
|
||
lbl_of = parts.ravel()[keep]
|
||
if top > 0 and n2 > top:
|
||
# сильнейшие по признаку: сумма превышения порога по всем лучам
|
||
w = np.bincount(lbl_of, weights=adv[keep] - thr, minlength=n2 + 1)
|
||
best = np.argsort(w[1:])[::-1][:top] + 1
|
||
alive = np.isin(lbl_of, best)
|
||
keep, lbl_of = keep[alive], lbl_of[alive]
|
||
if keep.size < 4:
|
||
continue
|
||
# перенумеровать оставшиеся подряд
|
||
_, lbl_of = np.unique(lbl_of, return_inverse=True)
|
||
lbl_of += 1
|
||
n2 = int(lbl_of.max())
|
||
if not copied:
|
||
out, copied = labels.copy(), True
|
||
# фон этой компоненты уходит из рассмотрения: он и был бы отброшен
|
||
out.ravel()[idx] = 0
|
||
out.ravel()[keep] = next_id + lbl_of
|
||
next_id += n2
|
||
return out, next_id
|
||
|
||
|
||
def _core_zone(abs_lat: np.ndarray, d: np.ndarray, core_width: float,
|
||
core_from: float) -> np.ndarray:
|
||
"""Где действует пониженный пол: в колее и не ближе core_from."""
|
||
zone = abs_lat <= core_width
|
||
if core_from > 0.0:
|
||
with np.errstate(invalid="ignore"):
|
||
zone = zone & (d >= core_from)
|
||
return zone
|
||
|
||
|
||
def gauge_mask(tf: TrackFrame, corridor: Corridor, *, half_width: float,
|
||
h_lo: float, h_hi: float, d_min: float, d_max: float,
|
||
k_sigma: float = 0.0, h_lo_core: float = 1e9,
|
||
core_width: float = 0.85, core_from: float = 0.0,
|
||
platform_filter: bool = False,
|
||
platform_u: float = 1.30, platform_h_lo: float = 1.05,
|
||
platform_h_hi: float = 1.25):
|
||
"""Лучи, попавшие в габарит приближения. Считается один раз на кадр.
|
||
|
||
Нужны и для выделения кандидатов, и для накопителя веерного тела, поэтому
|
||
вынесены отдельно: пересчитывать один и тот же набор масок дважды незачем.
|
||
"""
|
||
lat = tf.lateral(corridor)
|
||
abs_lat = np.abs(lat)
|
||
in_range = tf.valid & (tf.d > d_min) & (tf.d < d_max)
|
||
|
||
if k_sigma > 0.0:
|
||
sig = corridor.sigma(tf.d)
|
||
w_eff = half_width + k_sigma * np.maximum(sig - 0.25, 0.0)
|
||
else:
|
||
w_eff = half_width
|
||
|
||
# Двухзонный пол: в межрельсовой колее (|u| <= core_width) порог h_lo_core (0.16 м),
|
||
# чтобы не терять лежащего человека или низкие предметы. Опускается он
|
||
# только дальше core_from: вблизи в полосу 0.16…0.28 м попадают головки
|
||
# рельсов, и предмет теряется вместе с ними (EXPERIMENTS п. 16).
|
||
floor_h = np.where(_core_zone(abs_lat, tf.d, core_width, core_from),
|
||
min(h_lo_core, h_lo), h_lo)
|
||
|
||
inside = in_range & (abs_lat < w_eff) & (tf.h > floor_h) & (tf.h < h_hi)
|
||
|
||
if platform_filter:
|
||
platform = (abs_lat >= platform_u) & (tf.h >= platform_h_lo) & (tf.h <= platform_h_hi)
|
||
inside = inside & ~platform
|
||
|
||
return inside, lat, in_range
|
||
|
||
|
||
def find_candidates(tf: TrackFrame, lam: LaminaOutput, corridor: Corridor, *,
|
||
half_width: float = 1.7, h_lo: float = 0.28, h_hi: float = 2.3,
|
||
d_min: float = 4.0, d_max: float = 220.0,
|
||
min_rays: int = 4, min_rays_far: int = 0,
|
||
min_rays_far_from: float = 90.0,
|
||
max_candidates: int = 64,
|
||
dilate_cols: int = 2, max_depth: float = 15.0,
|
||
ctx_up: float = 4.0, split_adv: float = 0.0,
|
||
split_gap: float = 0.0, split_near: float = 0.0,
|
||
near_long: float = 0.0,
|
||
split_top: int = 0,
|
||
advance: np.ndarray | None = None,
|
||
masks: tuple | None = None,
|
||
k_sigma: float = 0.0, h_lo_core: float = 1e9,
|
||
core_width: float = 0.85, core_from: float = 0.0,
|
||
platform_filter: bool = False,
|
||
platform_u: float = 1.30, platform_h_lo: float = 1.05,
|
||
platform_h_hi: float = 1.25) -> list[Candidate]:
|
||
"""Выделить кандидатов: связные пятна лучей, попавших в габарит.
|
||
|
||
Кластеризация ведётся по **расширенной** области, а членство в габарите
|
||
проверяется уже для готового пятна. Без этого вертикальный лоскут стены,
|
||
срезанный границей коридора, неотличим от предмета: обрезка сама создаёт
|
||
компактное пятно нужного размера. Доля лучей, оставшихся внутри габарита
|
||
(`containment`), сразу показывает, предмет это целиком или край стены.
|
||
"""
|
||
if masks is None:
|
||
masks = gauge_mask(tf, corridor, half_width=half_width, h_lo=h_lo,
|
||
h_hi=h_hi, d_min=d_min, d_max=d_max,
|
||
k_sigma=k_sigma, h_lo_core=h_lo_core,
|
||
core_width=core_width, core_from=core_from,
|
||
platform_filter=platform_filter,
|
||
platform_u=platform_u, platform_h_lo=platform_h_lo,
|
||
platform_h_hi=platform_h_hi)
|
||
inside, lat, in_range = masks
|
||
if not inside.any():
|
||
return []
|
||
|
||
# Контекст расширяется в стороны и вверх, но НЕ вниз: полотно пути проходит
|
||
# прямо под каждым предметом и на большой дальности попадает в тот же допуск
|
||
# по глубине, так что расширение вниз склеило бы любой объект с полом.
|
||
# Вверх, наоборот, нужно доставать до свода: колонна и рама крепи идут от
|
||
# полотна до потолка, и если обрезать контекст на полпути, срез сам выглядит
|
||
# компактным предметом ростом с габарит.
|
||
abs_lat = np.abs(lat)
|
||
if k_sigma > 0.0:
|
||
sig = corridor.sigma(tf.d)
|
||
w_ctx = half_width + 1.6 + k_sigma * np.maximum(sig - 0.25, 0.0)
|
||
else:
|
||
w_ctx = half_width + 1.6
|
||
|
||
floor_h = np.where(_core_zone(abs_lat, tf.d, core_width, core_from),
|
||
min(h_lo_core, h_lo), h_lo)
|
||
context = (in_range & (abs_lat < w_ctx)
|
||
& (tf.h > floor_h) & (tf.h < h_hi + ctx_up))
|
||
|
||
if platform_filter:
|
||
platform = (abs_lat >= platform_u) & (tf.h >= platform_h_lo) & (tf.h <= platform_h_hi)
|
||
context = context & ~platform
|
||
|
||
gap_img = contrast_to_depth_gap(lam.on, tf.r)
|
||
|
||
labels, n = cluster_by_depth(context, tf.r, col_reach=dilate_cols + 1)
|
||
if n == 0:
|
||
return []
|
||
if split_gap > 0:
|
||
# Контраст к окружению как признак фигуры. Если задан и порог движения,
|
||
# выполняются оба условия сразу: стена должна и приближаться как предмет,
|
||
# и торчать из своего окружения.
|
||
fig = gap_img if (split_adv <= 0 or advance is None) else np.where(advance > split_adv, gap_img, 0.0)
|
||
if split_near > 0:
|
||
# Ближе этой дальности разрез был выключен целиком, и причина
|
||
# стояла в комментарии неверная: считалось, что вблизи предмет со
|
||
# стеной не слипается. Замер показал обратное — на 6…30 м предмет,
|
||
# смещённый от оси на 0.9 м, теряется в 34 % наблюдений против 1 %
|
||
# на самой оси, и хуже всего там, где есть платформа. Он уходит
|
||
# вместе с её кромкой.
|
||
#
|
||
# Резать вблизи всё подряд дорого: обстановки, дающей контраст, в
|
||
# ближнем поле на порядок больше. Поэтому режем только компоненты,
|
||
# СЛИШКОМ РАСТЯНУТЫЕ по глубине, чтобы быть предметом: человек
|
||
# занимает по дальности десятки сантиметров, кромка платформы и
|
||
# стена — десятки метров. Мерой служит разброс дальности внутри
|
||
# компоненты (две свёртки по меткам, доли миллисекунды).
|
||
near = tf.d <= split_near
|
||
if near_long > 0.0 and n > 0:
|
||
lab = labels.ravel()
|
||
rr = np.where(np.isfinite(tf.r), tf.r, 0.0).ravel()
|
||
cnt = np.bincount(lab, minlength=n + 1).astype(np.float64)
|
||
s1 = np.bincount(lab, weights=rr, minlength=n + 1)
|
||
s2 = np.bincount(lab, weights=rr * rr, minlength=n + 1)
|
||
c = np.maximum(cnt, 1.0)
|
||
var = np.maximum(s2 / c - (s1 / c) ** 2, 0.0)
|
||
stretched = np.sqrt(var) > near_long
|
||
stretched[0] = False
|
||
near = near & ~stretched[labels]
|
||
fig = np.where(near, 0.0, fig)
|
||
labels, n = split_by_figure(labels, n, tf.r, fig, max_depth=max_depth,
|
||
thr=split_gap, col_reach=dilate_cols + 1,
|
||
top=split_top)
|
||
elif split_adv > 0 and advance is not None:
|
||
labels, n = split_by_figure(labels, n, tf.r, advance, max_depth=max_depth,
|
||
thr=split_adv, col_reach=dilate_cols + 1)
|
||
|
||
flat = labels.ravel()
|
||
counts = np.bincount(flat, minlength=n + 1)
|
||
counts[0] = 0
|
||
inner = np.bincount(flat, weights=inside.ravel().astype(np.float64), minlength=n + 1)
|
||
inner[0] = 0
|
||
# Сколько лучей достаточно, чтобы считать компоненту кандидатом. Вблизи
|
||
# обрывок из трёх лучей — это кусок чего-то большего, и он только
|
||
# дробит настоящий предмет: замер даёт P@50 у человека 0.70 → 0.64,
|
||
# если разрешить тройки везде. Вдали три луча — всё, что вообще есть
|
||
# (медиана на 150 м — шесть), и запрет отсекает предмет целиком.
|
||
# Поэтому порог зависит от дальности, как и нормировка числа лучей в
|
||
# `_quality`. 0 — не различать (EXPERIMENTS п. 12.5).
|
||
need = np.full(inner.shape, float(min_rays))
|
||
if min_rays_far > 0:
|
||
din = np.where(inside, np.nan_to_num(tf.d, nan=0.0, posinf=0.0), 0.0)
|
||
dsum = np.bincount(flat, weights=din.ravel().astype(np.float64),
|
||
minlength=n + 1)
|
||
dmean = dsum / np.maximum(inner, 1.0)
|
||
need = np.where(dmean >= min_rays_far_from, float(min_rays_far),
|
||
float(min_rays))
|
||
keep = np.flatnonzero(inner >= need)
|
||
if keep.size == 0:
|
||
return []
|
||
if keep.size > max_candidates:
|
||
keep = keep[np.argsort(inner[keep])[::-1][:max_candidates]]
|
||
|
||
with np.errstate(invalid="ignore"):
|
||
deficit = np.where(np.isfinite(tf.floor_r), tf.floor_r - tf.r, 0.0)
|
||
deficit = np.where(tf.valid & (deficit > 0), deficit, 0.0).astype(np.float32)
|
||
|
||
order = np.argsort(labels.ravel(), kind="stable")
|
||
sorted_lbl = labels.ravel()[order]
|
||
starts = np.searchsorted(sorted_lbl, np.arange(n + 2))
|
||
|
||
inside_flat = inside.ravel()
|
||
out: list[Candidate] = []
|
||
for lbl in keep:
|
||
ctx_idx = order[starts[lbl]:starts[lbl + 1]]
|
||
sel = ctx_idx[inside_flat[ctx_idx]] # признаки считаем по ядру в габарите
|
||
if sel.size < need[lbl]:
|
||
continue
|
||
ii, jj = np.divmod(sel, labels.shape[1])
|
||
|
||
d = tf.d[ii, jj]
|
||
u = lat[ii, jj]
|
||
h = tf.h[ii, jj]
|
||
nn = d.size
|
||
|
||
depth = float(d.max() - d.min()) if nn > 1 else 0.0
|
||
if depth > max_depth:
|
||
# ни один посторонний предмет не тянется на десятки метров вдоль пути:
|
||
# это полотно, стена или кабельный лоток, взятые под скользящим углом
|
||
continue
|
||
|
||
cand = Candidate(
|
||
d=float(np.median(d)), u=float(np.median(u)), h=float(np.median(h)),
|
||
d_min=float(d.min()), h_min=float(h.min()),
|
||
width=float(u.max() - u.min()) if nn > 1 else 0.0,
|
||
height=float(h.max() - h.min()) if nn > 1 else 0.0,
|
||
depth=depth,
|
||
containment=float(sel.size / max(ctx_idx.size, 1)),
|
||
n_rays=int(nn), n_rings=int(np.unique(ii).size), n_cols=int(np.unique(jj).size),
|
||
gap=float(np.median(gap_img[ii, jj])),
|
||
on=float(np.median(lam.on[ii, jj])),
|
||
floor_deficit=float(np.median(deficit[ii, jj])),
|
||
shadow=float(np.median(lam.hole[ii, jj])),
|
||
inten=float(np.median(tf.inten[ii, jj])),
|
||
az_deg=float(np.mean(tf.layout.az_grid_deg[jj])),
|
||
el_deg=float(np.mean(tf.layout.el_deg[ii])),
|
||
bbox=(int(ii.min()), int(ii.max()), int(jj.min()), int(jj.max())),
|
||
)
|
||
# индексы лучей нужны только для отладочной визуализации; они небольшие
|
||
cand.extra["rays"] = (ii, jj)
|
||
out.append(cand)
|
||
|
||
out.sort(key=lambda c: c.d)
|
||
return out
|