forked from Dan4ick/Lidar_Muxa
b8e95eccbe
Ретина, ламина, медулла, лобула, грибовидное тело, веерное тело, центральный комплекс, нисходящие нейроны. Обучение памяти тоннеля и считывания MBON, оценка leave-one-bag-out, полигон дальности, 24 теста. Реальный объект на 55 м — 98.9 % кадров, ложных 7.5 трека на км, кадр обрабатывается за 33 мс на CPU.
180 lines
11 KiB
Python
180 lines
11 KiB
Python
"""FAN-SHAPED BODY — накопление слабых улик в координатах пути.
|
||
|
||
Покадровое решение проваливается там, где предмет даёт единицы лучей. На 170 м
|
||
человек освещён **в каждом** кадре, но всего пятью лучами: формы из них не
|
||
построить, а локальный контраст на такой дальности обнуляется — кольцо
|
||
окружения ламины упирается в стену тоннеля, которая на той же дальности.
|
||
Зато предмет неподвижен в мире, а тоннель проплывает мимо: за проход одни и те
|
||
же пять лучей попадают в одну и ту же точку пространства семнадцать раз подряд.
|
||
|
||
Именно так устроено веерное тело центрального комплекса мухи: оно копит
|
||
вектор к цели в координатах, привязанных к миру, а не к текущему кадру, и
|
||
достаёт из слабого повторяющегося сигнала то, чего нет ни в одном отдельном
|
||
наблюдении.
|
||
|
||
Здесь то же самое буквально: сетка, привязанная к пройденному пути, в которую
|
||
кадр за кадром складываются лучи из габарита. Сетка сдвигается на пройденное
|
||
расстояние, поэтому неподвижный предмет всегда попадает в одну ячейку, а шум
|
||
и случайные отражения размазываются.
|
||
|
||
Своих кандидатов накопитель не порождает — это проверено и отвергнуто
|
||
измерением: по геометрии накопленное скопление предмета и накопленный кусок
|
||
конструкции тоннеля неразличимы (совпадают все десять признаков, от дальности
|
||
до протяжённости). Разделяет их только память тоннеля, а ей нужны признаки
|
||
кадра, которых у скопления нет.
|
||
|
||
Роль накопителя другая и точная: он отвечает про **уже найденного покадрового
|
||
кандидата**, возвращались ли лучи из этой точки мира кадр за кадром. На
|
||
больших дальностях это единственная доступная улика — локальный контраст там
|
||
структурно равен нулю, потому что кольцо окружения ламины упирается в стену
|
||
тоннеля, стоящую на той же дальности.
|
||
|
||
Стоимость — доли миллисекунды: пара тысяч лучей в `bincount` по сетке из
|
||
пятнадцати тысяч ячеек.
|
||
"""
|
||
from __future__ import annotations
|
||
|
||
from dataclasses import dataclass
|
||
|
||
import numpy as np
|
||
from scipy import ndimage
|
||
|
||
|
||
# Шаг сетки вдоль пути крупный намеренно: оценка собственного движения копит
|
||
# ошибку около 3 %, за семнадцать кадров это уже 0.7 м. Мелкая ячейка размазала
|
||
# бы предмет по соседям и убила весь смысл накопления. Для отчёта о дистанции
|
||
# 2 м всё равно на порядок точнее допуска (12 % дальности).
|
||
DS_BIN = 2.0 # м вдоль пути
|
||
DU_BIN = 0.20 # м поперёк
|
||
DH_BIN = 0.25 # м по высоте
|
||
|
||
DECAY = 0.97 # забывание: полураспад около 23 кадров
|
||
MIN_FRAMES = 6 # меньше — это вспышка, а не предмет
|
||
MAX_SPAN_S = 8.0 # м: длиннее — это стена или лоток, а не предмет
|
||
MAX_WIDTH = 1.5 # м: шире — край габарита, а не предмет
|
||
BG_WIN_M = 24.0 # м: окно оценки фона вдоль пути
|
||
|
||
|
||
@dataclass
|
||
class Accumulated:
|
||
"""Скопление попаданий в одной точке пути."""
|
||
|
||
d: float
|
||
u: float
|
||
h: float
|
||
h_min: float
|
||
width: float
|
||
height: float
|
||
span_s: float
|
||
hits: float
|
||
frames: float
|
||
support: float # попаданий относительно ожидаемого для этой дальности
|
||
|
||
|
||
class FanBody:
|
||
"""Сетка в координатах пути, копящая лучи из габарита."""
|
||
|
||
def __init__(self, *, d_max: float = 220.0, half_width: float = 1.6,
|
||
h_lo: float = 0.28, h_hi: float = 2.3,
|
||
d_near: float = 55.0, min_support: float = 0.8,
|
||
ref_rays: float = 1.4, ref_d: float = 175.0):
|
||
self.ds, self.du, self.dh = DS_BIN, DU_BIN, DH_BIN
|
||
self.h_lo, self.h_hi = h_lo, h_hi
|
||
self.half_width = half_width
|
||
self.d_near = d_near # ближе этого покадровый тракт и так справляется
|
||
self.min_support = min_support
|
||
# Сколько попаданий ждать от настоящего предмета на данной дальности.
|
||
# Калибровано по замеру: человек, вставленный в реальный проезд, даёт
|
||
# после вычитания фона и разброса по ячейкам эквивалент 1.4 луча в
|
||
# кадре на 175 м (пять лучей приходят, но часть уходит в фон и в
|
||
# соседние ячейки). Число лучей падает как 1/R², накопление держит
|
||
# около 1/(1−DECAY) кадров.
|
||
self.k_expect = ref_rays * ref_d ** 2 / (1.0 - DECAY)
|
||
|
||
self.n_s = int(np.ceil(d_max / self.ds)) + 1
|
||
self.n_u = int(np.ceil(2 * half_width / self.du)) + 1
|
||
self.n_h = int(np.ceil((h_hi - h_lo) / self.dh)) + 1
|
||
self.hits = np.zeros((self.n_s, self.n_u, self.n_h), np.float32)
|
||
self.seen = np.zeros((self.n_s, self.n_u), np.float32)
|
||
self._off = 0.0 # смещение начала сетки внутри ячейки, м
|
||
|
||
# ------------------------------------------------------------------ такт
|
||
|
||
def update(self, d: np.ndarray, u: np.ndarray, h: np.ndarray, ds: float) -> None:
|
||
"""Сдвинуть сетку на пройденное `ds` и досыпать лучи текущего кадра."""
|
||
self._advance(ds)
|
||
if d.size == 0:
|
||
return
|
||
si = ((self._off + d) / self.ds).astype(np.int32)
|
||
ui = ((u + self.half_width) / self.du).astype(np.int32)
|
||
hi = ((h - self.h_lo) / self.dh).astype(np.int32)
|
||
ok = ((si >= 0) & (si < self.n_s) & (ui >= 0) & (ui < self.n_u)
|
||
& (hi >= 0) & (hi < self.n_h))
|
||
if not ok.any():
|
||
return
|
||
flat = (si[ok] * self.n_u + ui[ok]) * self.n_h + hi[ok]
|
||
self.hits += np.bincount(flat, minlength=self.hits.size).reshape(self.hits.shape)
|
||
# «Кадров подряд» считается по столбцу, а не по ячейке высоты: предмет
|
||
# может качнуться на четверть метра, и это не повод обнулять счёт.
|
||
col = np.unique(si[ok] * self.n_u + ui[ok])
|
||
self.seen.ravel()[col] += 1.0
|
||
|
||
def _advance(self, ds: float) -> None:
|
||
self.hits *= DECAY
|
||
self.seen *= DECAY
|
||
if ds <= 0:
|
||
return
|
||
self._off += ds
|
||
k = int(self._off // self.ds)
|
||
if k <= 0:
|
||
return
|
||
self._off -= k * self.ds
|
||
if k >= self.n_s:
|
||
self.hits[:] = 0.0
|
||
self.seen[:] = 0.0
|
||
return
|
||
self.hits[:-k] = self.hits[k:]
|
||
self.hits[-k:] = 0.0
|
||
self.seen[:-k] = self.seen[k:]
|
||
self.seen[-k:] = 0.0
|
||
|
||
# ------------------------------------------------------------------ съём
|
||
|
||
def support_at(self, d: np.ndarray, u: np.ndarray) -> np.ndarray:
|
||
"""Опора накопителя в точках пути: во сколько раз попаданий больше фона.
|
||
|
||
Своих кандидатов накопитель не порождает намеренно. Замер показал, что
|
||
по геометрии накопленное скопление предмета и накопленный кусок
|
||
конструкции тоннеля неразличимы: совпадают дальность, смещение, высота,
|
||
ширина, протяжённость — всё. Разделяет их только память тоннеля, а она
|
||
работает с признаками кадра (контраст, интенсивность, тень), которых у
|
||
скопления нет.
|
||
|
||
Поэтому накопитель отвечает на один вопрос про уже найденного
|
||
покадрового кандидата: **возвращались ли лучи из этой точки мира кадр
|
||
за кадром**. На больших дальностях это единственная доступная улика:
|
||
локальный контраст там структурно равен нулю, потому что кольцо
|
||
окружения упирается в стену тоннеля на той же дальности.
|
||
"""
|
||
if d.size == 0:
|
||
return np.zeros(0, np.float32)
|
||
col = self.hits.sum(axis=2)
|
||
win = max(int(BG_WIN_M / self.ds) | 1, 3)
|
||
base = ndimage.median_filter(col, size=(win, 1), mode="nearest")
|
||
resid = np.maximum(col - base, 0.0)
|
||
|
||
si = np.clip(((self._off + d) / self.ds).astype(np.int32), 0, self.n_s - 1)
|
||
ui = np.clip(((u + self.half_width) / self.du).astype(np.int32), 0, self.n_u - 1)
|
||
# окно ±1 ячейка: предмет шире одной ячейки, а оценка пути слегка плывёт
|
||
tot = np.zeros(d.size, np.float32)
|
||
frames = np.zeros(d.size, np.float32)
|
||
for ds_ in (-1, 0, 1):
|
||
for du_ in (-1, 0, 1):
|
||
a = np.clip(si + ds_, 0, self.n_s - 1)
|
||
b = np.clip(ui + du_, 0, self.n_u - 1)
|
||
tot += resid[a, b]
|
||
frames = np.maximum(frames, self.seen[a, b])
|
||
expect = self.k_expect / np.maximum(d, 1.0) ** 2
|
||
sup = tot / np.maximum(expect, 1e-6)
|
||
# без нескольких кадров подряд это не улика, а вспышка
|
||
return np.where(frames > MIN_FRAMES, sup, 0.0).astype(np.float32)
|