added gpr speed tracking
This commit is contained in:
Ayzen
@@ -0,0 +1,787 @@
|
||||
"""
|
||||
MIMO GPR — локализация через пересечение эллипсов
|
||||
==================================================
|
||||
|
||||
Физика в двух словах:
|
||||
Пик A-скана пары (Tx_i, Rx_j) на задержке τ означает:
|
||||
|Tx → объект| + |объект → Rx| = v · τ
|
||||
Это уравнение эллипса. Истинный отражатель лежит на
|
||||
пересечении всех 16 эллипсов (по одному на пару).
|
||||
|
||||
Алгоритм:
|
||||
1. S(f) → IFFT → 16 A-сканов
|
||||
2. Поиск пиков: SNR = пик / медиана > порог
|
||||
3. Для каждого пика → мягкий эллипс в аккумуляторе
|
||||
(с компенсацией геометрического и углового затухания)
|
||||
4. CLEAN: найти максимум → убрать его эллипсы → повторить
|
||||
|
||||
О параметре SHELL_SIGMA:
|
||||
Аккумулятор — это «мягкое голосование». Каждый эллипс добавляет
|
||||
не единицу, а гауссово-взвешенный вклад:
|
||||
w = exp(−δ²/2σ²), где δ = |R_Tx + R_Rx − v·τ|
|
||||
SHELL_SIGMA — ширина этой гауссовой оболочки.
|
||||
Слишком широко → ghost-цели не подавляются.
|
||||
Слишком узко → вклад падает до нуля из-за дискретности сетки.
|
||||
Оптимум: ~ 0.4 × δZ, где δZ = v/(2B) — разрешение по глубине.
|
||||
|
||||
О score:
|
||||
score = количество пар (из 16), чей эллипс проходит
|
||||
через данную точку с невязкой δ < 3σ.
|
||||
Принимает целые значения от 0 до 16.
|
||||
Максимальный score у истинного объекта = 16 (все пары согласны).
|
||||
Ghost-цели имеют меньший score, т.к. согласуются только
|
||||
с частью пар.
|
||||
|
||||
О компенсации затухания:
|
||||
При генерации S(f) сигнал ослаблен:
|
||||
geo(i,j) = 1/(R_Tx · R_Rx) — геометрическое ослабление
|
||||
pat(i,j) = cos²(θ_Tx)·cos²(θ_Rx) — диаграмма направленности
|
||||
Без компенсации глубокий/угловой отражатель будет недооценён.
|
||||
Компенсация: делим вес каждого пика на ожидаемое затухание
|
||||
в точке z_apparent, вычисленное для данной пары антенн.
|
||||
|
||||
Геометрия: плоскость XZ (X — вдоль антенн, Z — глубина).
|
||||
"""
|
||||
|
||||
"""
|
||||
MIMO GPR — локализация через пересечение эллипсов
|
||||
==================================================
|
||||
Версия для реальных данных
|
||||
"""
|
||||
|
||||
import numpy as np
|
||||
import matplotlib.pyplot as plt
|
||||
from matplotlib.lines import Line2D
|
||||
from scipy.signal import find_peaks
|
||||
from scipy.ndimage import gaussian_filter, label
|
||||
from pathlib import Path
|
||||
from dataclasses import dataclass, field
|
||||
from typing import Dict, List, Tuple
|
||||
|
||||
### Изменяемые параметры ======
|
||||
INPUT_IDX = [0,1,2,3]
|
||||
OUTPUT_IDX = [0,3]
|
||||
|
||||
MIN_DEPTH = 3.0 # [м] пропустить прямую волну
|
||||
MAX_DEPTH = 20.0
|
||||
COMP_POWER = 0.22 # степень компенсации затухания
|
||||
|
||||
# Обрезка по частоте
|
||||
F_START = 28*1e8 # Нижняя частота
|
||||
F_STOP = 60*1e8 # Верхняя частота
|
||||
|
||||
# Параметры скорости в Motion Config
|
||||
SPEED_M_S = 0.48 # Скорость м/с
|
||||
LOOK_ANGLE_DEG = 2.0 # Угол наклона радара отн-но горизонтали (град)
|
||||
|
||||
# Вычитание среднего фона (background removal)
|
||||
# True → вычитать среднее по всем снимкам в папке (убирает прямую волну и статичные отражения)
|
||||
# False → использовать данные как есть
|
||||
BG_SUBTRACT = True
|
||||
BG_PATH = Path('10m_cyl_motion_21sec_0.48msec_27032026_2.8-6ghz_751/preprocessed')
|
||||
|
||||
DATA_PATH = Path('10m_cyl_motion_21sec_0.48msec_27032026_2.8-6ghz_751/preprocessed/0002_id3_ns26032413993535') # <-- УКАЖИТЕ ПУТЬ
|
||||
|
||||
# ===============================
|
||||
|
||||
@dataclass
|
||||
class MotionConfig:
|
||||
"""
|
||||
Конфигурация движения для одного кадра из 8 пар.
|
||||
|
||||
speed_m_s:
|
||||
Линейная скорость движения радара.
|
||||
|
||||
look_angle_deg:
|
||||
Угол между направлением движения и осью дальности Z.
|
||||
Если движение почти "вдоль дальности", ставьте угол близкий к 0°.
|
||||
Тогда dz = v_move * dt * cos(angle) ≈ v_move * dt.
|
||||
|
||||
sweep_time_s:
|
||||
Время прохода по всем частотам для одной пары.
|
||||
|
||||
switch_time_s:
|
||||
Время переключения между соседними парами.
|
||||
|
||||
pair_order_phys:
|
||||
Реальный порядок измерения в физических индексах:
|
||||
[(tx_phys_1, rx_phys_1), (tx_phys_2, rx_phys_2), ...]
|
||||
|
||||
reference_mode:
|
||||
Относительно какого момента считаем dt:
|
||||
- 'frame_center' : середина всего цикла по 8 парам
|
||||
- 'first_pair' : центр первой пары
|
||||
|
||||
direction_sign:
|
||||
Знак движения по оси дальности.
|
||||
+1 -> более поздние пары выглядят глубже
|
||||
-1 -> более поздние пары выглядят ближе
|
||||
"""
|
||||
speed_m_s: float = 0.50
|
||||
look_angle_deg: float = 0.0
|
||||
sweep_time_s: float = 0.040
|
||||
switch_time_s: float = 0.005
|
||||
pair_order_phys: List[Tuple[int, int]] = field(default_factory=lambda: [
|
||||
(tx_phys, rx_phys)
|
||||
for tx_phys in sorted(OUTPUT_IDX)
|
||||
for rx_phys in sorted(INPUT_IDX)
|
||||
])
|
||||
reference_mode: str = 'frame_center'
|
||||
direction_sign: float = +1.0
|
||||
|
||||
# Конфигурация движения
|
||||
MOTION_CONFIG = MotionConfig(
|
||||
speed_m_s=SPEED_M_S,
|
||||
look_angle_deg=LOOK_ANGLE_DEG,
|
||||
sweep_time_s=0.15, # ref 0.15
|
||||
switch_time_s=1e-5,
|
||||
pair_order_phys=[
|
||||
(0, 0), (0, 1), (0, 2), (0, 3), # Этот порядок текущий, возможно в будущем что-то поменяется
|
||||
(3, 0), (3, 1), (3, 2), (3, 3),
|
||||
],
|
||||
reference_mode='frame_center',
|
||||
direction_sign=+1.0,
|
||||
)
|
||||
|
||||
### Список параметров и констант использующиеся в коде: ###
|
||||
|
||||
|
||||
MODE = 'point' # Один из 2х режимов `point` or `extended`
|
||||
|
||||
eps_r = 1.0 # Диэлектрическая проницаемость среды
|
||||
v = 3e8 / np.sqrt(eps_r) # скорость света в среде
|
||||
|
||||
# ══════════════════════════════════════════════════════
|
||||
# КООРДИНАТЫ АНТЕНН — УКАЖИТЕ РЕАЛЬНЫЕ ЗНАЧЕНИЯ!
|
||||
# ══════════════════════════════════════════════════════
|
||||
# Координаты вдоль оси X на поверхности (z = 0), в метрах
|
||||
|
||||
# Физические координаты антенн по их реальным индексам
|
||||
TX_POSITIONS = {0: 90.5*0.01, # Tx с индексом 0 → x = +0.9 м
|
||||
3: -90.5*0.01} # Tx с индексом 3 → x = -0.9 м
|
||||
|
||||
RX_POSITIONS = {0: -18*0.01,
|
||||
1: 48.5*0.01,
|
||||
2: -49.0*0.01,
|
||||
3: 18.5*0.01}
|
||||
|
||||
# Массивы позиций в порядке возрастания физических индексов
|
||||
# (нужны для сетки аккумулятора и графиков)
|
||||
x_tx = np.array([TX_POSITIONS[i] for i in sorted(TX_POSITIONS)])
|
||||
x_rx = np.array([RX_POSITIONS[j] for j in sorted(RX_POSITIONS)])
|
||||
|
||||
# Параметры алгоритма
|
||||
SNR_THRESH = 4.5 # минимальный SNR пика
|
||||
SNR_COMP_MAX = 25.0 # Верхний порог для компенсированного значения SNR
|
||||
|
||||
|
||||
# Параметр: максимальное число объектов для поиска
|
||||
MAX_OBJECTS = 15 # <-- настройте под вашу задачу
|
||||
|
||||
# Границы сетки аккумулятора
|
||||
x_min, x_max = x_tx.min() - 2.0, x_tx.max() + 2.0 # [м]
|
||||
z_min, z_max = 0.2, MAX_DEPTH # <-- глубина [м]
|
||||
|
||||
|
||||
|
||||
# ══════════════════════════════════════════════════════
|
||||
# 1. ЗАГРУЗКА РЕАЛЬНЫХ ДАННЫХ
|
||||
# ══════════════════════════════════════════════════════
|
||||
|
||||
def load_mimo_data(data_path, input_idx, output_idx):
|
||||
data_path = Path(data_path)
|
||||
s21_data = {}
|
||||
freq_data = {}
|
||||
|
||||
for f in data_path.glob("i*_o*_s21.npy"):
|
||||
name = f.stem
|
||||
parts = name.split('_')
|
||||
i_tx_phys = int(parts[1][1:])
|
||||
i_rx_phys = int(parts[0][1:])
|
||||
|
||||
if i_tx_phys not in output_idx or i_rx_phys not in input_idx:
|
||||
continue
|
||||
|
||||
# Переводим физический индекс → порядковый (0,1,2,...)
|
||||
i_tx = sorted(output_idx).index(i_tx_phys)
|
||||
i_rx = sorted(input_idx).index(i_rx_phys)
|
||||
|
||||
s21_data[(i_tx, i_rx)] = np.load(f)
|
||||
freq_file = data_path / f"i{i_rx_phys}_o{i_tx_phys}_freq.npy"
|
||||
freq_data[(i_tx, i_rx)] = np.load(freq_file)
|
||||
|
||||
tx_indices = sorted(set(k[0] for k in s21_data))
|
||||
rx_indices = sorted(set(k[1] for k in s21_data))
|
||||
n_tx = len(tx_indices)
|
||||
n_rx = len(rx_indices)
|
||||
|
||||
print(f"Загружено пар: {len(s21_data)}")
|
||||
print(f"Передатчиков: {n_tx}, Приёмников: {n_rx}")
|
||||
|
||||
return s21_data, freq_data, n_tx, n_rx
|
||||
|
||||
|
||||
# Загрузка данных
|
||||
s21_data, freq_data, N_tx, N_rx = load_mimo_data(DATA_PATH, INPUT_IDX, OUTPUT_IDX)
|
||||
N_pairs = len(s21_data)
|
||||
|
||||
# ══════════════════════════════════════════════════════
|
||||
# 1б. ВЫЧИСЛЕНИЕ СРЕДНЕГО ФОНА ПО ВСЕМ СНИМКАМ
|
||||
# ══════════════════════════════════════════════════════
|
||||
|
||||
def compute_background(bg_path, input_idx, output_idx):
|
||||
"""
|
||||
Для каждой пары (i_tx, i_rx) усредняем S21 по всем снимкам в папке.
|
||||
|
||||
Возвращает:
|
||||
bg : dict[(i_tx, i_rx)] → np.array (complex), усреднённый S21
|
||||
"""
|
||||
bg_path = Path(bg_path)
|
||||
snapshots = sorted(bg_path.glob("*/")) # каждый подкаталог — один снимок
|
||||
snapshots = [s for s in snapshots if s.is_dir()]
|
||||
|
||||
if len(snapshots) == 0:
|
||||
print("⚠️ Снимков для фона не найдено, BG_SUBTRACT отключён.")
|
||||
return None
|
||||
|
||||
print(f"Вычисление фона по {len(snapshots)} снимкам...", end=" ", flush=True)
|
||||
|
||||
# Накопитель: для каждой пары суммируем S21
|
||||
bg_sum = {}
|
||||
bg_count = {}
|
||||
|
||||
for snap_dir in snapshots:
|
||||
for f in snap_dir.glob("i*_o*_s21.npy"):
|
||||
name = f.stem
|
||||
parts = name.split('_')
|
||||
i_tx_phys = int(parts[1][1:])
|
||||
i_rx_phys = int(parts[0][1:])
|
||||
|
||||
if i_tx_phys not in output_idx or i_rx_phys not in input_idx:
|
||||
continue
|
||||
|
||||
i_tx = sorted(output_idx).index(i_tx_phys)
|
||||
i_rx = sorted(input_idx).index(i_rx_phys)
|
||||
key = (i_tx, i_rx)
|
||||
|
||||
s21 = np.load(f)
|
||||
if key not in bg_sum:
|
||||
bg_sum[key] = np.zeros_like(s21, dtype=complex)
|
||||
bg_count[key] = 0
|
||||
bg_sum[key] += s21
|
||||
bg_count[key] += 1
|
||||
|
||||
bg = {key: bg_sum[key] / bg_count[key] for key in bg_sum}
|
||||
print(f"готово. Пар: {len(bg)}, снимков на пару: "
|
||||
f"{list(bg_count.values())[0] if bg_count else 0}")
|
||||
return bg
|
||||
|
||||
|
||||
if BG_SUBTRACT:
|
||||
background = compute_background(BG_PATH, INPUT_IDX, OUTPUT_IDX)
|
||||
if background is None:
|
||||
BG_SUBTRACT = False # автоматически выключаем если нет данных
|
||||
else:
|
||||
background = None
|
||||
print("BG_SUBTRACT = False, вычитание фона отключено.")
|
||||
|
||||
|
||||
# Проверка частот (берём из первой пары как референс)
|
||||
first_key = list(freq_data.keys())[0]
|
||||
freqs = freq_data[first_key]
|
||||
|
||||
|
||||
|
||||
# Проверим что частоты одинаковые для всех пар
|
||||
for key, freq in freq_data.items():
|
||||
if not np.allclose(freq, freqs):
|
||||
print(f"⚠️ Частоты для пары {key} отличаются!")
|
||||
|
||||
|
||||
mask_freq = (freqs >= F_START) & (freqs <= F_STOP)
|
||||
|
||||
freqs = freqs[mask_freq]
|
||||
|
||||
f_min, f_max = freqs[0], freqs[-1]
|
||||
BW = f_max - f_min
|
||||
N_f = len(freqs)
|
||||
|
||||
|
||||
# ══════════════════════════════════════════════════════
|
||||
# 2. ПАРАМЕТРЫ СИСТЕМЫ
|
||||
# ══════════════════════════════════════════════════════
|
||||
|
||||
# Проверка соответствия координатов антенн
|
||||
assert len(x_tx) == N_tx, f"x_tx должен содержать {N_tx} элементов"
|
||||
assert len(x_rx) == N_rx, f"x_rx должен содержать {N_rx} элементов"
|
||||
|
||||
|
||||
# SHELL_SIGMA — ширина гауссовой оболочки
|
||||
SHELL_SIGMA = v / BW * 0.5 # [м]
|
||||
|
||||
|
||||
# Сетка аккумулятора
|
||||
|
||||
x_grid = np.linspace(x_min, x_max, 300)
|
||||
z_grid = np.linspace(z_min, z_max, 300)
|
||||
XX, ZZ = np.meshgrid(x_grid, z_grid)
|
||||
|
||||
# Расстояния от сетки до каждой антенны
|
||||
R_tx_grid = {i: np.sqrt((XX - x_tx[i])**2 + ZZ**2) for i in range(N_tx)}
|
||||
R_rx_grid = {j: np.sqrt((XX - x_rx[j])**2 + ZZ**2) for j in range(N_rx)}
|
||||
|
||||
|
||||
# ══════════════════════════════════════════════════════
|
||||
# 3. ВЫЧИСЛЕНИЕ A-СКАНОВ ИЗ РЕАЛЬНЫХ S21
|
||||
# ══════════════════════════════════════════════════════
|
||||
|
||||
def compute_ascan(s21, freq, f_start, f_stop, window=True):
|
||||
"""
|
||||
S21(f) → IFFT → A-скан с правильным частотным сдвигом.
|
||||
|
||||
Проблема наивного подхода (buf[:n] = s21):
|
||||
IFFT считает, что спектр начинается с 0 Гц.
|
||||
Реальные данные начинаются с f[0] > 0, поэтому
|
||||
нулевая задержка смещается и в A-скане появляются биения.
|
||||
|
||||
Правильный подход — сдвиг спектра:
|
||||
Шаг частотной сетки df вычисляется из данных.
|
||||
Индекс первой частоты: k0 = round(f[0] / df).
|
||||
Данные кладутся в H[k0 : k0+n], а не в H[0 : n].
|
||||
Тогда IFFT корректно восстанавливает временной сигнал
|
||||
с нулевой задержкой в t=0.
|
||||
|
||||
Размер FFT:
|
||||
Минимум для покрытия всего диапазона [0, f[-1]]:
|
||||
min_len = 2 * (k0 + n - 1)
|
||||
Округляем вверх до степени двойки для скорости FFT.
|
||||
"""
|
||||
mask_freq_ = (freq >= f_start) & (freq <= f_stop)
|
||||
freq = freq[mask_freq_]
|
||||
s21 = s21[mask_freq_]
|
||||
|
||||
n = len(freq)
|
||||
if n < 2:
|
||||
raise ValueError("Слишком мало частотных точек")
|
||||
|
||||
# Шаг частотной сетки
|
||||
df = (freq[-1] - freq[0]) / (n - 1)
|
||||
if df <= 0:
|
||||
raise ValueError("Частоты не возрастают")
|
||||
|
||||
# Индекс первой частоты в полной сетке от 0 до f[-1]
|
||||
k0 = int(np.round(freq[0] / df))
|
||||
|
||||
# Минимальный размер FFT, округлённый до степени двойки
|
||||
min_len = 2 * (k0 + n - 1)
|
||||
n_fft = 1 << int(np.ceil(np.log2(min_len)))
|
||||
|
||||
|
||||
# Временна́я ось — пересчитываем из нового n_fft
|
||||
dt = 1.0 / (n_fft * df)
|
||||
t_sec = np.arange(n_fft, dtype=float) * dt
|
||||
|
||||
# Оконная функция (подавление боковых лепестков IFFT)
|
||||
s = s21 * np.hanning(n) if window else s21.copy()
|
||||
|
||||
# Спектр со сдвигом: данные на своём месте в частотной сетке
|
||||
H = np.zeros(n_fft, dtype=np.complex128)
|
||||
H[k0 : k0 + n] = s
|
||||
|
||||
y = np.abs(np.fft.ifft(H))
|
||||
|
||||
|
||||
return t_sec[:y.size], y[:y.size]
|
||||
|
||||
|
||||
print("Вычисление A-сканов из реальных данных...", end=" ", flush=True)
|
||||
|
||||
A = {} # A[(i,j)] — амплитудный A-скан
|
||||
T_h = {} # T_h[(i,j)] — временна́я ось для этой пары [с]
|
||||
Z_h = {} # Z_h[(i,j)] — ось глубины [м]
|
||||
|
||||
for (i, j), s21 in s21_data.items():
|
||||
s21_proc = s21.copy()
|
||||
|
||||
# Вычитание фона в частотной области
|
||||
if BG_SUBTRACT and background is not None and (i, j) in background:
|
||||
s21_proc = s21_proc - background[(i, j)]
|
||||
# Примечание: вычитаем до обрезки по частоте и до окна —
|
||||
# фон вычисляется из полных (необрезанных) данных,
|
||||
# поэтому вычитание корректно в полном частотном диапазоне.
|
||||
|
||||
t_pair, a_pair = compute_ascan(s21_proc, freq_data[(i, j)],
|
||||
f_start=F_START, f_stop=F_STOP)
|
||||
T_h[(i, j)] = t_pair
|
||||
Z_h[(i, j)] = t_pair * v / 2
|
||||
A[(i, j)] = a_pair
|
||||
|
||||
bg_label = "с вычитанием фона" if BG_SUBTRACT else "без вычитания фона"
|
||||
print(f"готово ({bg_label}).")
|
||||
|
||||
|
||||
# Общая ось z для визуализации и поиска пиков
|
||||
# (берём максимальный диапазон по всем парам)
|
||||
z_h = Z_h[list(Z_h.keys())[0]] # все пары дают одинаковую ось, если freq совпадают
|
||||
t_h = T_h[list(T_h.keys())[0]]
|
||||
|
||||
|
||||
# ══════════════════════════════════════════════════════
|
||||
# 4. ДЕТЕКТИРОВАНИЕ ПИКОВ
|
||||
# ══════════════════════════════════════════════════════
|
||||
|
||||
def attenuation_at_depth(i_tx, i_rx, z_app):
|
||||
"""
|
||||
Ожидаемое ослабление geo·pattern для точки прямо под виртуальным
|
||||
центром пары на глубине z_app.
|
||||
|
||||
Используется для компенсации: реальный SNR пика делится на это
|
||||
значение, чтобы вес глубокого/углового объекта не занижался.
|
||||
"""
|
||||
xc = (x_tx[i_tx] + x_rx[i_rx]) / 2.0 # виртуальный центр
|
||||
Rtx = np.sqrt((xc - x_tx[i_tx])**2 + z_app**2)
|
||||
Rrx = np.sqrt((xc - x_rx[i_rx])**2 + z_app**2)
|
||||
geo = 1.0 / (Rtx * Rrx + 1e-12)
|
||||
pat = (z_app / (Rtx + 1e-12))**2 * (z_app / (Rrx + 1e-12))**2
|
||||
return geo * pat + 1e-30 # +ε чтобы не делить на ноль
|
||||
|
||||
|
||||
def find_peaks_snr(i_tx, i_rx, SNR_COMP_MAX = SNR_COMP_MAX):
|
||||
"""
|
||||
Поиск пиков A-скана.
|
||||
Возвращает список dict:
|
||||
z_app — кажущаяся глубина [м]
|
||||
tau — задержка [с]
|
||||
snr_raw — SNR без компенсации = пик / медиана
|
||||
snr_comp— SNR с компенсацией ослабления (используется в аккумуляторе)
|
||||
"""
|
||||
ascan = A[(i_tx, i_rx)]
|
||||
z_h_ij = Z_h[(i_tx, i_rx)]
|
||||
t_h_ij = T_h[(i_tx, i_rx)]
|
||||
i_min = np.searchsorted(z_h_ij, MIN_DEPTH)
|
||||
i_max = np.searchsorted(z_h_ij, MAX_DEPTH)
|
||||
|
||||
noise = np.median(ascan[i_min:i_max]) # Добавил чтобы было удобно считать SNR отнсительно выбранной области
|
||||
min_dist = max(4, int(v / (2*BW) / (z_h_ij[1] - z_h_ij[0]) * 0.7))
|
||||
idx, _ = find_peaks(ascan[i_min:i_max],
|
||||
height=noise * SNR_THRESH,
|
||||
distance=min_dist)
|
||||
idx += i_min
|
||||
result = []
|
||||
for p in idx:
|
||||
z_app = float(z_h_ij[p])
|
||||
snr_raw = float(ascan[p] / noise)
|
||||
atten = attenuation_at_depth(i_tx, i_rx, z_app)
|
||||
atten_norm = atten / attenuation_at_depth(i_tx, i_rx, 3.0) # Референсная глубина - 3м
|
||||
snr_comp = snr_raw / (atten_norm ** COMP_POWER + 1e-12)
|
||||
snr_comp = min(snr_comp, SNR_COMP_MAX) # ← clipping
|
||||
result.append({'z_app': z_app,
|
||||
'tau': float(t_h_ij[p]),
|
||||
'snr_raw': snr_raw,
|
||||
'snr_comp': snr_comp})
|
||||
return result
|
||||
|
||||
|
||||
peaks = {(i, j): find_peaks_snr(i, j)
|
||||
for i in range(N_tx) for j in range(N_rx)}
|
||||
|
||||
# ══════════════════════════════════════════════════════
|
||||
# 6. ПОИСК ОБЪЕКТОВ
|
||||
# ══════════════════════════════════════════════════════
|
||||
|
||||
def find_centroid(acc_s, iz, ix, rpz, rpx):
|
||||
"""
|
||||
Взвешенный центроид аккумулятора в окрестности (iz, ix).
|
||||
"""
|
||||
NZ, NX = acc_s.shape
|
||||
iz0 = max(0, iz - rpz); iz1 = min(NZ, iz + rpz)
|
||||
ix0 = max(0, ix - rpx); ix1 = min(NX, ix + rpx)
|
||||
patch = acc_s[iz0:iz1, ix0:ix1].copy()
|
||||
W = patch.sum()
|
||||
if W <= 0:
|
||||
return x_grid[ix], z_grid[iz]
|
||||
rows = np.arange(iz0, iz1)[:, None] * np.ones(patch.shape)
|
||||
cols = np.ones(patch.shape) * np.arange(ix0, ix1)[None, :]
|
||||
iz_c = int(round(np.clip((rows * patch).sum() / W, 0, NZ-1)))
|
||||
ix_c = int(round(np.clip((cols * patch).sum() / W, 0, NX-1)))
|
||||
return x_grid[ix_c], z_grid[iz_c]
|
||||
|
||||
|
||||
# ══════════════════════════════════════════════════════
|
||||
# 8. MOTION-AWARE FIRST-ORDER CORRECTION
|
||||
# ══════════════════════════════════════════════════════
|
||||
|
||||
"""
|
||||
Первый блок для движения без изменения продакшн-пайплайна выше.
|
||||
|
||||
Идея:
|
||||
1. Для каждой пары (Tx, Rx) задаём время центра её измерения.
|
||||
2. По известной скорости и углу получаем сдвиг по дальности dz.
|
||||
3. Переводим dz в поправку по задержке dtau.
|
||||
4. Для уже найденных пиков формируем motion-corrected версию:
|
||||
tau_corr, z_corr
|
||||
|
||||
Это first-order модель: считаем, что вся пара измерена в момент времени t_center.
|
||||
Если позже окажется, что смещение за один sweep пары уже заметно,
|
||||
следующим шагом надо будет делать per-frequency коррекцию до IFFT.
|
||||
"""
|
||||
|
||||
|
||||
def _build_phys_to_logical_maps(output_idx, input_idx):
|
||||
tx_map = {phys: k for k, phys in enumerate(sorted(output_idx))}
|
||||
rx_map = {phys: k for k, phys in enumerate(sorted(input_idx))}
|
||||
return tx_map, rx_map
|
||||
|
||||
|
||||
|
||||
def compute_pair_timestamps(config: MotionConfig,
|
||||
output_idx=OUTPUT_IDX,
|
||||
input_idx=INPUT_IDX) -> Tuple[Dict[Tuple[int, int], Dict], List[Dict]]:
|
||||
"""
|
||||
Для каждой пары возвращает:
|
||||
t_start, t_center, dt_ref, dz_motion, dtau_motion
|
||||
|
||||
Ключи словаря pair_timestamps — логические индексы (i_tx, i_rx),
|
||||
совместимые с существующим словарём peaks.
|
||||
"""
|
||||
tx_phys_to_log, rx_phys_to_log = _build_phys_to_logical_maps(output_idx, input_idx)
|
||||
|
||||
rows: List[Dict] = []
|
||||
t_cursor = 0.0
|
||||
|
||||
for order_idx, (tx_phys, rx_phys) in enumerate(config.pair_order_phys):
|
||||
if tx_phys not in tx_phys_to_log:
|
||||
raise ValueError(f"Tx {tx_phys} отсутствует в OUTPUT_IDX={output_idx}")
|
||||
if rx_phys not in rx_phys_to_log:
|
||||
raise ValueError(f"Rx {rx_phys} отсутствует в INPUT_IDX={input_idx}")
|
||||
|
||||
i_tx = tx_phys_to_log[tx_phys]
|
||||
i_rx = rx_phys_to_log[rx_phys]
|
||||
|
||||
t_start = t_cursor
|
||||
t_center = t_start + 0.5 * config.sweep_time_s
|
||||
t_stop = t_start + config.sweep_time_s
|
||||
|
||||
rows.append({
|
||||
'order_idx': order_idx,
|
||||
'tx_phys': tx_phys,
|
||||
'rx_phys': rx_phys,
|
||||
'i_tx': i_tx,
|
||||
'i_rx': i_rx,
|
||||
't_start_s': t_start,
|
||||
't_center_s': t_center,
|
||||
't_stop_s': t_stop,
|
||||
})
|
||||
|
||||
t_cursor = t_stop + config.switch_time_s
|
||||
|
||||
if not rows:
|
||||
return {}, []
|
||||
|
||||
if config.reference_mode == 'frame_center':
|
||||
t_ref = 0.5 * (rows[0]['t_center_s'] + rows[-1]['t_center_s'])
|
||||
elif config.reference_mode == 'first_pair':
|
||||
t_ref = rows[0]['t_center_s']
|
||||
else:
|
||||
raise ValueError("reference_mode must be 'frame_center' or 'first_pair'")
|
||||
|
||||
cos_theta = np.cos(np.radians(config.look_angle_deg))
|
||||
pair_timestamps: Dict[Tuple[int, int], Dict] = {}
|
||||
|
||||
for row in rows:
|
||||
dt_ref = row['t_center_s'] - t_ref
|
||||
dz_motion = config.direction_sign * config.speed_m_s * dt_ref * cos_theta
|
||||
dtau_motion = 2.0 * dz_motion / v
|
||||
|
||||
row['dt_ref_s'] = dt_ref
|
||||
row['dz_motion_m'] = dz_motion
|
||||
row['dtau_motion_s'] = dtau_motion
|
||||
|
||||
pair_timestamps[(row['i_tx'], row['i_rx'])] = row.copy()
|
||||
|
||||
return pair_timestamps, rows
|
||||
|
||||
|
||||
|
||||
def build_corrected_peaks(peaks_in: Dict[Tuple[int, int], List[Dict]],
|
||||
pair_timestamps: Dict[Tuple[int, int], Dict]) -> Dict[Tuple[int, int], List[Dict]]:
|
||||
"""
|
||||
Формирует словарь corrected_peaks с motion-aware поправками.
|
||||
|
||||
Для каждого пика добавляет:
|
||||
tau_raw, z_app_raw
|
||||
tau_corr, z_corr
|
||||
dz_motion, dtau_motion
|
||||
|
||||
Важно:
|
||||
z_corr = v * tau_corr / 2
|
||||
потому что ось z_app в текущем пайплайне — это apparent depth.
|
||||
"""
|
||||
corrected = {}
|
||||
|
||||
for key, peak_list in peaks_in.items():
|
||||
if key not in pair_timestamps:
|
||||
raise KeyError(f"Нет временной информации для пары {key}")
|
||||
|
||||
info = pair_timestamps[key]
|
||||
dz_motion = info['dz_motion_m']
|
||||
dtau_motion = info['dtau_motion_s']
|
||||
|
||||
corrected_list = []
|
||||
for pk in peak_list:
|
||||
tau_raw = float(pk['tau'])
|
||||
z_raw = float(pk['z_app'])
|
||||
# dz_motion here is defined as a correction to the apparent depth itself:
|
||||
# z_corr = z_raw + dz_motion.
|
||||
# Therefore tau must be corrected with the same sign.
|
||||
tau_corr = tau_raw + dtau_motion
|
||||
z_corr = 0.5 * v * tau_corr
|
||||
|
||||
pk_corr = dict(pk)
|
||||
pk_corr.update({
|
||||
'tau_raw': tau_raw,
|
||||
'z_app_raw': z_raw,
|
||||
'tau_corr': tau_corr,
|
||||
'z_corr': z_corr,
|
||||
'dz_motion': dz_motion,
|
||||
'dtau_motion': dtau_motion,
|
||||
})
|
||||
corrected_list.append(pk_corr)
|
||||
|
||||
corrected[key] = corrected_list
|
||||
|
||||
return corrected
|
||||
|
||||
|
||||
|
||||
pair_timestamps, pair_timing_rows = compute_pair_timestamps(MOTION_CONFIG)
|
||||
corrected_peaks = build_corrected_peaks(peaks, pair_timestamps)
|
||||
|
||||
|
||||
|
||||
# ══════════════════════════════════════════════════════
|
||||
# 9. MOTION-AWARE IMAGE BUILD FROM CORRECTED PEAKS
|
||||
# ══════════════════════════════════════════════════════
|
||||
|
||||
"""
|
||||
Эта ячейка строит motion-aware картинку, используя corrected_peaks из блока выше.
|
||||
|
||||
Что меняется относительно статического продакшн-пайплайна:
|
||||
- в аккумуляторе используется tau_corr вместо tau
|
||||
- в apparent-depth логике CLEAN используется z_corr вместо z_app
|
||||
- score считается по corrected пикам
|
||||
|
||||
Исходные A-сканы остаются теми же, но на графике ниже можно показывать уже
|
||||
motion-corrected положения пиков.
|
||||
"""
|
||||
|
||||
|
||||
def _peak_in_work_depth(pk):
|
||||
return MIN_DEPTH <= pk['z_corr'] <= MAX_DEPTH
|
||||
|
||||
|
||||
|
||||
def build_accumulator_motion(corrected_peaks_in, exclude_z_ranges):
|
||||
acc = np.zeros_like(XX)
|
||||
for i in range(N_tx):
|
||||
for j in range(N_rx):
|
||||
for pk in corrected_peaks_in[(i, j)]:
|
||||
if not _peak_in_work_depth(pk):
|
||||
continue
|
||||
if any(lo <= pk['z_corr'] <= hi for lo, hi in exclude_z_ranges):
|
||||
continue
|
||||
r_total = v * pk['tau_corr']
|
||||
residual = R_tx_grid[i] + R_rx_grid[j] - r_total
|
||||
shell = np.exp(-0.5 * (residual / SHELL_SIGMA)**2)
|
||||
acc += shell * pk['snr_comp']
|
||||
return acc
|
||||
|
||||
|
||||
|
||||
def count_agreeing_ellipses_motion(x_est, z_est, corrected_peaks_in, exclude_z_ranges):
|
||||
count = 0
|
||||
for i in range(N_tx):
|
||||
for j in range(N_rx):
|
||||
for pk in corrected_peaks_in[(i, j)]:
|
||||
if not _peak_in_work_depth(pk):
|
||||
continue
|
||||
if any(lo <= pk['z_corr'] <= hi for lo, hi in exclude_z_ranges):
|
||||
continue
|
||||
Rt = np.sqrt((x_est - x_tx[i])**2 + z_est**2)
|
||||
Rr = np.sqrt((x_est - x_rx[j])**2 + z_est**2)
|
||||
if abs(Rt + Rr - v * pk['tau_corr']) < SHELL_SIGMA * 6:
|
||||
count += 1
|
||||
break
|
||||
return count
|
||||
|
||||
|
||||
|
||||
def clean_find_motion(corrected_peaks_in, n_search=10, suppress_r_cm=7, thresh_frac=0.05):
|
||||
dx = x_grid[1] - x_grid[0]
|
||||
dz = z_grid[1] - z_grid[0]
|
||||
rpx = int(suppress_r_cm / 100 / dx)
|
||||
rpz = int(suppress_r_cm / 100 / dz)
|
||||
|
||||
excl_z = []
|
||||
found_motion = []
|
||||
acc_initial = build_accumulator_motion(corrected_peaks_in, [])
|
||||
|
||||
for step in range(n_search):
|
||||
acc = build_accumulator_motion(corrected_peaks_in, excl_z)
|
||||
acc_s = gaussian_filter(acc, sigma=3)
|
||||
|
||||
if acc_s.max() < thresh_frac * acc_initial.max():
|
||||
break
|
||||
|
||||
iz, ix = np.unravel_index(acc_s.argmax(), acc_s.shape)
|
||||
x_est, z_est = find_centroid(acc_s, iz, ix, rpz, rpx)
|
||||
score = count_agreeing_ellipses_motion(x_est, z_est, corrected_peaks_in, excl_z)
|
||||
found_motion.append({'x': x_est, 'z': z_est, 'score': score})
|
||||
|
||||
matched = [pk['z_corr']
|
||||
for i in range(N_tx) for j in range(N_rx)
|
||||
for pk in corrected_peaks_in[(i, j)]
|
||||
if _peak_in_work_depth(pk)
|
||||
and not any(lo <= pk['z_corr'] <= hi for lo, hi in excl_z)
|
||||
and abs(np.sqrt((x_est - x_tx[i])**2 + z_est**2) +
|
||||
np.sqrt((x_est - x_rx[j])**2 + z_est**2) -
|
||||
v * pk['tau_corr']) < SHELL_SIGMA * 3]
|
||||
|
||||
if matched:
|
||||
margin = SHELL_SIGMA * 1.0
|
||||
excl_z.append((min(matched) - margin, max(matched) + margin))
|
||||
|
||||
return found_motion, acc_initial
|
||||
|
||||
|
||||
if MODE == 'point':
|
||||
found_motion, accum_motion = clean_find_motion(corrected_peaks, n_search=MAX_OBJECTS)
|
||||
|
||||
|
||||
# ─── График 2: motion-aware карта накопления ───────────────────────
|
||||
fig, ax = plt.subplots(figsize=(12, 7))
|
||||
acc_motion_s = gaussian_filter(accum_motion, sigma=3)
|
||||
im = ax.imshow(
|
||||
acc_motion_s,
|
||||
extent=[x_grid[0]*100, x_grid[-1]*100, z_grid[-1]*100, z_grid[0]*100],
|
||||
aspect='auto', origin='upper', cmap='hot',
|
||||
vmin=acc_motion_s.max()*0.45, vmax=acc_motion_s.max()*0.95,
|
||||
)
|
||||
plt.colorbar(im, ax=ax, label='Накопленный вес (motion-aware)')
|
||||
ax.plot(x_tx*100, np.zeros(N_tx), 'r^', ms=10, label='Tx', zorder=5)
|
||||
ax.plot(x_rx*100, np.zeros(N_rx), 'bv', ms=10, label='Rx', zorder=5)
|
||||
for obj in found_motion:
|
||||
lbl = f"score={obj['score']}/{N_pairs}"
|
||||
ax.plot(obj['x']*100, obj['z']*100, 'wD', ms=9, zorder=11, markeredgecolor='black', mew=1.2)
|
||||
ax.annotate(lbl, (obj['x']*100, obj['z']*100), textcoords='offset points', xytext=(6, 4),
|
||||
fontsize=8, color='white', bbox=dict(boxstyle='round,pad=0.2', fc='black', alpha=0.5))
|
||||
ax.plot([], [], 'wD', ms=9, markeredgecolor='k', mew=1.2, label='Найденные объекты')
|
||||
ax.set_xlabel('X [см]')
|
||||
ax.set_ylabel('Глубина Z [см]')
|
||||
ax.set_title('Motion-aware карта накопления эллипсов')
|
||||
ax.set_xlim(x_grid[0]*100, x_grid[-1]*100)
|
||||
ax.set_ylim(z_grid[-1]*100, z_grid[0]*100)
|
||||
ax.legend(loc='lower right', fontsize=9)
|
||||
ax.grid(alpha=0.25)
|
||||
ax.invert_yaxis()
|
||||
plt.tight_layout()
|
||||
Reference in New Issue
Block a user