1327 lines
53 KiB
Python
1327 lines
53 KiB
Python
"""
|
||
MIMO GPR — coherent time-domain BackProjection
|
||
=================================================
|
||
|
||
Эта ячейка полностью независима от верхнего эллипсного алгоритма:
|
||
1. загружает те же S21-данные;
|
||
2. строит oversampled A-сканы через тот же частотный сдвиг перед IFFT;
|
||
3. для каждой точки (x,z) вычисляет tau_ij = (Rtx + Rrx) / v;
|
||
4. интерполирует комплексный A-скан в этой задержке;
|
||
5. когерентно суммирует комплексные вклады всех Tx/Rx-пар с компенсацией geo·pattern.
|
||
|
||
Это coherent BP: суммируются комплексные h_ij(tau), затем строится |sum h_ij|.
|
||
"""
|
||
|
||
import builtins
|
||
import json
|
||
import numpy as np
|
||
import matplotlib.pyplot as plt
|
||
from scipy.ndimage import gaussian_filter, label
|
||
from pathlib import Path
|
||
from dataclasses import dataclass, field
|
||
from typing import Dict, List, Tuple
|
||
|
||
_print_raw = builtins.print
|
||
PRINT_DIAGNOSTICS = False
|
||
|
||
|
||
def print(*args, **kwargs):
|
||
if PRINT_DIAGNOSTICS:
|
||
_print_raw(*args, **kwargs)
|
||
|
||
|
||
# ══════════════════════════════════════════════════════
|
||
# 0.1 ИЗМЕНЯЕМЫЕ ПАРАМЕТРЫ
|
||
# ══════════════════════════════════════════════════════
|
||
INPUT_IDX = [0, 1, 2, 3]
|
||
OUTPUT_IDX = [0, 1]
|
||
|
||
# Частотный диапазон и глубинный gate.
|
||
F_START = 38 * 1e8
|
||
F_STOP = 6.0 * 1e9
|
||
MIN_DEPTH = 3.0
|
||
MAX_DEPTH = 12.0
|
||
|
||
# Движение радара во время кадра.
|
||
# Подтвержденная схема измерения: Tx0(f0), Tx1(f0), Tx0(f1), Tx1(f1), ...
|
||
# 'int_minus' — полная intra-frequency correction к центру interleaved sweep;
|
||
# 'int_focus' — focus-like correction без чистого Z-сдвига.
|
||
SPEED_M_S = 1.75 # скорость в м/с
|
||
LOOK_ANGLE_DEG = 7.5 # угол наклона радара, град
|
||
TX_SWEEP_TIME_S = 0.0714 # Половинное время свипа, с
|
||
MOTION_CORRECTION_MODE = 'int_minus' # 'int_minus' или 'int_focus'
|
||
|
||
# Данные и вычитание среднего фона.
|
||
BG_SUBTRACT = True
|
||
DATA_PATH = Path('/Users/ivan_root/Downloads/Telegram_dwnld/20260608_moving/20260608_3.5-6_751_10k_-3p_2nd/preprocessed/0015_id16_ns8216344309213')
|
||
BG_PATH = DATA_PATH.parent
|
||
|
||
# Убрать паразитные боковые лепестки с 2D карты.
|
||
BP_REMOVE_SIDELOBE_OBJECTS = True # True: убрать SL-кандидаты из финальной таблицы и разметки
|
||
BP_MAX_DETECTED_OBJECTS_TO_DRAW = 8 # N: если найдено больше объектов, цели на карте не рисуются
|
||
BP_DRAW_TOP_M_OBJECTS = 4 # M: если найдено <= N, рисуются только первые M по текущему score
|
||
BP_OBJECT_MIN_FRAC = 0.7 # остановка: пик ниже этой доли от глобального максимума
|
||
|
||
# Компенсация затухания разделена на 2 части:
|
||
# range: геометрическое расхождение 1/(Rtx*Rrx)
|
||
# angle: диаграмма направленности cos_tx^2 * cos_rx^2
|
||
COMP_RANGE_POWER = 0.1
|
||
|
||
# Метрика для ранжирования целей.
|
||
# 'peak' — порядок по максимумам coherent BP;
|
||
# 'combined' — coherent peak + coherence factor + prominence + contrast.
|
||
BP_SCORE_MODE = 'combined'
|
||
|
||
# ══════════════════════════════════════════════════════
|
||
# 0.2 КОНФИГИ АНТЕНН И ДВИЖЕНИЯ
|
||
# ══════════════════════════════════════════════════════
|
||
|
||
# Физические координаты антенн по их реальным индексам, [м].
|
||
# Формат: (x, y, z), где X - поперечная ось, Z - дальность вдоль оси радара,
|
||
# Y - нормаль к плоскости XOZ. BP-карта строится в плоскости y=BP_PLANE_Y.
|
||
# Геометрию лучше переносить из config_profile.json текущего эксперимента.
|
||
BP_PLANE_Y = 0.0
|
||
|
||
TX_POSITIONS = {
|
||
0: (-75.0 * 0.01, 0.0, 0.0),
|
||
1: ( 75.0 * 0.01, 0.0, 0.0),
|
||
}
|
||
|
||
RX_POSITIONS = {
|
||
0: ( 19.0 * 0.01, 0.0, 0.0),
|
||
1: ( 45.0 * 0.01, 0.0, 0.0),
|
||
2: (-45.0 * 0.01, 0.0, 0.0),
|
||
3: (-19.0 * 0.01, 0.0, 0.0),
|
||
}
|
||
|
||
|
||
@dataclass
|
||
class MotionConfig:
|
||
"""
|
||
Конфигурация speed-correction для interleaved Tx-by-frequency режима.
|
||
|
||
Реальная схема измерения:
|
||
Tx0(f0), Tx1(f0), Tx0(f1), Tx1(f1), ...
|
||
|
||
Поэтому меж-Tx сдвиг A-сканов не используется. Коррекция движения делается
|
||
только в частотной области до IFFT: для каждой частоты учитывается время,
|
||
в которое эта частота была измерена данной Tx-группой.
|
||
|
||
speed_m_s:
|
||
Линейная скорость радара во время кадра.
|
||
|
||
look_angle_deg:
|
||
Угол между направлением движения и осью дальности Z.
|
||
|
||
tx_sweep_time_s:
|
||
Базовое время одной VNA-линейки частот. Для одной Tx-группы соседние
|
||
частоты измеряются через N_tx таких временных шагов, поэтому effective
|
||
frequency sweep этой Tx-группы равен N_tx * tx_sweep_time_s.
|
||
|
||
motion_mode:
|
||
'int_minus' — полная intra-frequency correction к центру interleaved sweep;
|
||
'int_focus' — удалить аффинную по частоте часть phase(f), оставив
|
||
focus-like остаток без чистого сдвига Z.
|
||
|
||
pair_order_phys:
|
||
Порядок физических Tx/Rx-пар в данных. Пары с одинаковым tx_phys имеют
|
||
один interleave-slot и отличаются только Rx.
|
||
|
||
direction_sign:
|
||
Знак движения вдоль оси дальности. +1 означает, что при росте времени
|
||
радар смещается в сторону увеличения Z.
|
||
"""
|
||
speed_m_s: float = 0.0
|
||
look_angle_deg: float = 0.0
|
||
tx_sweep_time_s: float = 0.15
|
||
motion_mode: str = 'int_minus'
|
||
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)
|
||
])
|
||
direction_sign: float = +1.0
|
||
|
||
|
||
MOTION_CONFIG = MotionConfig(
|
||
speed_m_s=SPEED_M_S,
|
||
look_angle_deg=LOOK_ANGLE_DEG,
|
||
tx_sweep_time_s=TX_SWEEP_TIME_S,
|
||
motion_mode=MOTION_CORRECTION_MODE,
|
||
pair_order_phys=[
|
||
(tx_phys, rx_phys)
|
||
for tx_phys in sorted(OUTPUT_IDX)
|
||
for rx_phys in sorted(INPUT_IDX)
|
||
],
|
||
direction_sign=+1.0,
|
||
)
|
||
|
||
# ══════════════════════════════════════════════════════
|
||
# 0.3 НЕИЗМЕНЯЕМЫЕ ПАРАМЕТРЫ (ЛУЧШЕ НЕ ТРОГАТЬ)
|
||
# ══════════════════════════════════════════════════════
|
||
|
||
eps_r = 1.0
|
||
v = 3e8 / np.sqrt(eps_r)
|
||
|
||
|
||
def positions_to_xyz(position_dict):
|
||
coords = []
|
||
for idx in sorted(position_dict):
|
||
pos = np.asarray(position_dict[idx], dtype=float)
|
||
if pos.ndim == 0:
|
||
pos = np.array([float(pos), 0.0, 0.0], dtype=float)
|
||
elif pos.shape == (2,):
|
||
pos = np.array([pos[0], pos[1], 0.0], dtype=float)
|
||
elif pos.shape != (3,):
|
||
raise ValueError(f'Позиция антенны {idx} должна быть x, (x,y) или (x,y,z), получено {pos}')
|
||
coords.append(pos)
|
||
return np.vstack(coords)
|
||
|
||
|
||
tx_xyz = positions_to_xyz(TX_POSITIONS)
|
||
rx_xyz = positions_to_xyz(RX_POSITIONS)
|
||
x_tx, y_tx, z_tx = tx_xyz.T
|
||
x_rx, y_rx, z_rx = rx_xyz.T
|
||
|
||
# Сетка BP-карты.
|
||
x_ant = np.concatenate([x_tx, x_rx])
|
||
x_min, x_max = x_ant.min() - 2.0, x_ant.max() + 2.0
|
||
z_min, z_max = 0.1, MAX_DEPTH
|
||
NX_BP = 300
|
||
NZ_BP = 300
|
||
|
||
# Oversampling A-сканов: повышает плотность точек по t, но не физическое разрешение.
|
||
BP_OVERSAMPLE = 8
|
||
BP_WINDOW = True
|
||
|
||
# Нормировка каналов Tx/Rx перед когерентным сложением.
|
||
# Масштаб считается по |complex A-scan| в выбранном диапазоне глубин.
|
||
PAIR_NORMALIZE = True
|
||
PAIR_NORM_PERCENTILE = 50.0
|
||
PAIR_NORM_DEPTH_MIN = MIN_DEPTH
|
||
PAIR_NORM_DEPTH_MAX = MAX_DEPTH
|
||
PAIR_NORM_EPS = 1e-15
|
||
|
||
# Компенсация нормируется на точку под виртуальным центром пары на глубине COMP_REF_DEPTH.
|
||
COMP_ANGLE_POWER = 0.0
|
||
COMP_RANGE_WEIGHT_MAX = 5.0
|
||
COMP_ANGLE_WEIGHT_MAX = 2.0
|
||
COMP_WEIGHT_MAX = 8.0
|
||
COMP_REF_DEPTH = 5.0
|
||
BP_VALIDATE_COMPENSATION = False # clean mode: не считаем отдельную карту без compensation
|
||
|
||
# Сглаживание только для удобства поиска/визуализации максимума.
|
||
BP_SMOOTH_SIGMA = 1.5
|
||
|
||
# Параметры поиска объектов на BP-карте.
|
||
MAX_OBJECTS = 10
|
||
BP_REGION_THRESH_FRAC = 0.75 # область объекта: связная область выше этой доли от локального пика
|
||
BP_SUPPRESS_THRESH_FRAC = 0.2 # подавление: более широкая связная область вокруг найденного пика
|
||
BP_SUPPRESS_USE_WINDOW = True # False: подавлять всю связанную область; True: ограничить окно вокруг пика
|
||
BP_SUPPRESS_RX_CM = 80.0 # используется только если BP_SUPPRESS_USE_WINDOW=True
|
||
BP_SUPPRESS_RZ_CM = 40.0 # используется только если BP_SUPPRESS_USE_WINDOW=True
|
||
BP_MIN_REGION_AREA_CM2 = 10.0 # отсечение совсем мелких шумовых пятен
|
||
|
||
# Компактный центроид вокруг локального максимума, устойчивее центроида всей вытянутой области.
|
||
BP_CENTER_USE_COMPACT = True
|
||
BP_CENTER_RX_CM = 60.0
|
||
BP_CENTER_RZ_CM = 25.0
|
||
BP_CENTER_THRESH_FRAC = 0.88
|
||
BP_CENTER_WEIGHT_POWER = 2.0
|
||
|
||
# Диагностика боковых лепестков coherent BP.
|
||
BP_SIDELOBE_DETECT = True
|
||
BP_SIDELOBE_RANGE_RMS_TOL_CM = 20.0 # RMS-разница бистатических глубин по всем парам
|
||
BP_SIDELOBE_MIN_DX_CM = 35.0 # боковой лепесток должен быть заметно смещен по X
|
||
BP_SIDELOBE_MAX_DZ_CM = 70.0 # но находиться примерно на той же глубине
|
||
BP_SIDELOBE_MAX_REL_PEAK = 0.85 # кандидат должен быть слабее родительского максимума
|
||
|
||
# Фазовая метрика внутри compact-области объекта.
|
||
BP_PHASE_WEIGHT_POWER = 1.0
|
||
|
||
# Локальная выраженность объекта над окружающим фоном.
|
||
BP_LOCAL_BG_RX_CM = 120.0
|
||
BP_LOCAL_BG_RZ_CM = 80.0
|
||
BP_LOCAL_BG_PERCENTILE = 50.0
|
||
BP_LOCAL_CONTRAST_EPS = 1e-12
|
||
|
||
# Экспериментальный score для ранжирования найденных объектов.
|
||
BP_SCORE_COMPUTE_INCOHERENT = True
|
||
BP_SCORE_COH_PEAK_WEIGHT = 0.45
|
||
BP_SCORE_COHERENCE_FACTOR_WEIGHT = 0.25
|
||
BP_SCORE_PROMINENCE_WEIGHT = 0.20
|
||
BP_SCORE_CONTRAST_WEIGHT = 0.10
|
||
BP_SCORE_CONTRAST_CAP = 6.0
|
||
BP_SCORE_CF_EPS = 1e-12
|
||
|
||
|
||
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 _tx_event_order_from_pairs(pair_order_phys):
|
||
tx_events = []
|
||
for tx_phys, _ in pair_order_phys:
|
||
if tx_phys not in tx_events:
|
||
tx_events.append(tx_phys)
|
||
return tx_events
|
||
|
||
|
||
def compute_pair_timestamps(config: MotionConfig,
|
||
output_idx=OUTPUT_IDX,
|
||
input_idx=INPUT_IDX) -> Tuple[Dict[Tuple[int, int], Dict], List[Dict]]:
|
||
"""Возвращает interleave-slot для каждой логической Tx/Rx-пары."""
|
||
tx_phys_to_log, rx_phys_to_log = _build_phys_to_logical_maps(output_idx, input_idx)
|
||
expected_pairs = {(tx, rx) for tx in output_idx for rx in input_idx}
|
||
observed_pairs = set(config.pair_order_phys)
|
||
missing_pairs = expected_pairs - observed_pairs
|
||
extra_pairs = observed_pairs - expected_pairs
|
||
if missing_pairs:
|
||
raise ValueError(f'pair_order_phys не содержит пары: {sorted(missing_pairs)}')
|
||
if extra_pairs:
|
||
raise ValueError(f'pair_order_phys содержит лишние пары: {sorted(extra_pairs)}')
|
||
if len(config.pair_order_phys) != len(observed_pairs):
|
||
raise ValueError('pair_order_phys содержит повторяющиеся пары')
|
||
|
||
tx_event_order = _tx_event_order_from_pairs(config.pair_order_phys)
|
||
if not tx_event_order:
|
||
return {}, []
|
||
|
||
n_tx_events = len(tx_event_order)
|
||
pair_timestamps: Dict[Tuple[int, int], Dict] = {}
|
||
rows: List[Dict] = []
|
||
|
||
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}')
|
||
|
||
event_idx = tx_event_order.index(tx_phys)
|
||
row = {
|
||
'order_idx': order_idx,
|
||
'tx_event_idx': event_idx,
|
||
'tx_interleave_slot': event_idx,
|
||
'tx_interleave_slots': n_tx_events,
|
||
'tx_phys': tx_phys,
|
||
'rx_phys': rx_phys,
|
||
'i_tx': tx_phys_to_log[tx_phys],
|
||
'i_rx': rx_phys_to_log[rx_phys],
|
||
'base_sweep_time_s': config.tx_sweep_time_s,
|
||
}
|
||
rows.append(row)
|
||
pair_timestamps[(row['i_tx'], row['i_rx'])] = row.copy()
|
||
|
||
return pair_timestamps, rows
|
||
|
||
|
||
def print_motion_summary(rows: List[Dict], config: MotionConfig):
|
||
if config.motion_mode not in ('int_minus', 'int_focus'):
|
||
raise ValueError("motion_mode must be 'int_minus' or 'int_focus'")
|
||
|
||
n_slots = max((r['tx_interleave_slots'] for r in rows), default=len(OUTPUT_IDX))
|
||
print(f'Motion correction: {config.motion_mode}; speed={config.speed_m_s:.3f} м/с, '
|
||
f'angle={config.look_angle_deg:.2f}°, tx_sweep={config.tx_sweep_time_s*1e3:.2f} мс')
|
||
print(f'Frequency timing: interleaved Tx-by-frequency; Tx slots={n_slots}; '
|
||
f'effective Tx frequency span≈{n_slots*config.tx_sweep_time_s*1e3:.2f} мс')
|
||
print(f" {'pair':<8} {'slot':>4} {'tx_phys':>7} {'rx_phys':>7}")
|
||
for r in rows:
|
||
pair = f"Tx{r['i_tx']}-Rx{r['i_rx']}"
|
||
print(f" {pair:<8} {r['tx_interleave_slot']:>4d} {r['tx_phys']:>7d} {r['rx_phys']:>7d}")
|
||
|
||
|
||
def compute_frequency_sample_times(freq_full: np.ndarray,
|
||
f_start: float,
|
||
f_stop: float,
|
||
pair_info: Dict,
|
||
sweep_time_s: float) -> Tuple[np.ndarray, np.ndarray, float]:
|
||
"""Возвращает индексы частот, абсолютное время каждой точки и центр Tx-sweep."""
|
||
mask = (freq_full >= f_start) & (freq_full <= f_stop)
|
||
idx = np.flatnonzero(mask)
|
||
if idx.size == 0:
|
||
raise ValueError('После частотной обрезки не осталось точек для phase correction')
|
||
|
||
n_full = len(freq_full)
|
||
if n_full < 2:
|
||
return idx, np.zeros(idx.size, dtype=float), 0.0
|
||
|
||
n_slots = int(pair_info.get('tx_interleave_slots', len(OUTPUT_IDX)))
|
||
slot = int(pair_info.get('tx_interleave_slot', pair_info.get('tx_event_idx', 0)))
|
||
dt_base = sweep_time_s / (n_full - 1)
|
||
|
||
full_indices = np.arange(n_full, dtype=float)
|
||
t_all = (n_slots * full_indices + slot) * dt_base
|
||
t_abs = t_all[idx]
|
||
t_center = 0.5 * (float(t_all[0]) + float(t_all[-1]))
|
||
return idx, t_abs, t_center
|
||
|
||
|
||
def apply_intra_sweep_phase_correction(s21: np.ndarray,
|
||
freq_full: np.ndarray,
|
||
pair_info: Dict,
|
||
config: MotionConfig,
|
||
wave_speed: float,
|
||
f_start: float,
|
||
f_stop: float):
|
||
"""Frequency-domain speed correction до IFFT для interleaved Tx-by-frequency."""
|
||
if config.motion_mode not in ('int_minus', 'int_focus'):
|
||
raise ValueError("motion_mode must be 'int_minus' or 'int_focus'")
|
||
|
||
s21_corr = np.array(s21, dtype=np.complex128, copy=True)
|
||
idx, t_abs, t_center_s = compute_frequency_sample_times(
|
||
freq_full=freq_full,
|
||
f_start=f_start,
|
||
f_stop=f_stop,
|
||
pair_info=pair_info,
|
||
sweep_time_s=config.tx_sweep_time_s,
|
||
)
|
||
|
||
dt_intra = t_abs - t_center_s
|
||
theta = np.radians(config.look_angle_deg)
|
||
delta_range_intra = config.direction_sign * config.speed_m_s * np.cos(theta) * dt_intra
|
||
delta_path_intra = 2.0 * delta_range_intra
|
||
dtau_intra = delta_path_intra / wave_speed
|
||
|
||
phi = 2.0 * np.pi * freq_full[idx] * dtau_intra
|
||
|
||
# int_focus убирает постоянную + линейную по частоте часть phase(f).
|
||
# Оставшийся нелинейный остаток улучшает фокус, почти не сдвигая Z.
|
||
if config.motion_mode == 'int_focus' and np.any(dtau_intra != 0.0):
|
||
f_rel = freq_full[idx].astype(float) - float(np.mean(freq_full[idx]))
|
||
x_fit = np.column_stack([np.ones_like(f_rel), f_rel])
|
||
beta_fit, *_ = np.linalg.lstsq(x_fit, phi, rcond=None)
|
||
phi = phi - x_fit @ beta_fit
|
||
|
||
if np.any(dtau_intra != 0.0):
|
||
s21_corr[idx] *= np.exp(-1j * phi)
|
||
|
||
meta = {
|
||
'enabled': bool(np.any(dtau_intra != 0.0)),
|
||
'motion_mode': config.motion_mode,
|
||
'freq_idx': idx,
|
||
't_abs_s': t_abs,
|
||
'dt_intra_s': dt_intra,
|
||
't_center_s': t_center_s,
|
||
'intra_dtau_s': dtau_intra,
|
||
'total_dtau_s': dtau_intra,
|
||
'delta_range_m': delta_range_intra,
|
||
'delta_path_m': delta_path_intra,
|
||
'phi_rad': phi,
|
||
}
|
||
return s21_corr, meta
|
||
|
||
|
||
pair_timestamps, pair_timing_rows = compute_pair_timestamps(MOTION_CONFIG)
|
||
print_motion_summary(pair_timing_rows, MOTION_CONFIG)
|
||
|
||
# ══════════════════════════════════════════════════════
|
||
# 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
|
||
|
||
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)
|
||
|
||
return s21_data, freq_data
|
||
|
||
|
||
def compute_background(bg_path, input_idx, output_idx):
|
||
bg_path = Path(bg_path)
|
||
snapshots = [s for s in sorted(bg_path.glob('*/')) if s.is_dir()]
|
||
|
||
if len(snapshots) == 0:
|
||
print('Фоновые снимки не найдены, BG_SUBTRACT отключен.')
|
||
return None
|
||
|
||
print(f'Вычисление фона по {len(snapshots)} снимкам...', end=' ', flush=True)
|
||
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=np.complex128)
|
||
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)}')
|
||
return bg
|
||
|
||
|
||
s21_data, freq_data = load_mimo_data(DATA_PATH, INPUT_IDX, OUTPUT_IDX)
|
||
N_tx = len(OUTPUT_IDX)
|
||
N_rx = len(INPUT_IDX)
|
||
N_pairs = len(s21_data)
|
||
|
||
assert len(x_tx) == N_tx, f'x_tx должен содержать {N_tx} элементов'
|
||
assert len(x_rx) == N_rx, f'x_rx должен содержать {N_rx} элементов'
|
||
assert N_pairs > 0, 'Не найдено ни одной Tx/Rx-пары. Проверьте DATA_PATH.'
|
||
|
||
print('Геометрия антенн, [м]:')
|
||
for local_i, phys_i in enumerate(sorted(TX_POSITIONS)):
|
||
print(f' Tx{local_i} / o{phys_i}: x={x_tx[local_i]:+.3f}, y={y_tx[local_i]:+.3f}, z={z_tx[local_i]:+.3f}')
|
||
for local_j, phys_j in enumerate(sorted(RX_POSITIONS)):
|
||
print(f' Rx{local_j} / i{phys_j}: x={x_rx[local_j]:+.3f}, y={y_rx[local_j]:+.3f}, z={z_rx[local_j]:+.3f}')
|
||
|
||
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]
|
||
freq_mask = (freqs >= F_START) & (freqs <= F_STOP)
|
||
freqs_bp = freqs[freq_mask]
|
||
|
||
if len(freqs_bp) < 2:
|
||
raise ValueError('В выбранном частотном диапазоне меньше двух точек.')
|
||
|
||
f_min = float(freqs_bp[0])
|
||
f_max = float(freqs_bp[-1])
|
||
BW = f_max - f_min
|
||
df_values = np.diff(freqs_bp)
|
||
df_median = float(np.median(df_values))
|
||
df_min = float(np.min(df_values))
|
||
df_max = float(np.max(df_values))
|
||
df_rel_spread = (df_max - df_min) / (df_median + 1e-30)
|
||
range_resolution = v / (2 * BW)
|
||
unambiguous_depth = v / (2 * df_median)
|
||
unambiguous_total_path = v / df_median
|
||
|
||
print(f'Загружено пар Tx/Rx: {N_pairs}')
|
||
print(f'Частотный диапазон BP: {f_min/1e9:.3f} - {f_max/1e9:.3f} ГГц')
|
||
print(f'Точек частоты в BP-диапазоне: {len(freqs_bp)}')
|
||
print(f'Шаг частоты df: median={df_median/1e6:.3f} МГц, '
|
||
f'min={df_min/1e6:.3f} МГц, max={df_max/1e6:.3f} МГц')
|
||
print(f'Неравномерность df: {(df_rel_spread*100):.3f}% от median')
|
||
print(f'Полоса B: {BW/1e9:.3f} ГГц')
|
||
print(f'Теоретический предел разрешения по глубине deltaZ = {range_resolution*100:.2f} см')
|
||
print(f'Unambiguous range по глубине = {unambiguous_depth:.2f} м '
|
||
f'(max total path = {unambiguous_total_path:.2f} м)')
|
||
print(f'BP_OVERSAMPLE = {BP_OVERSAMPLE}')
|
||
print(f'MOTION_CORRECTION_MODE = {MOTION_CORRECTION_MODE}')
|
||
print('FREQUENCY_TIMING = interleaved_tx_by_frequency')
|
||
|
||
|
||
# ══════════════════════════════════════════════════════
|
||
# 2. OVERSAMPLED A-СКАНЫ
|
||
# ══════════════════════════════════════════════════════
|
||
|
||
def compute_ascan_bp(s21, freq, f_start, f_stop, window=True, oversample=8):
|
||
"""
|
||
S21(f) -> A-скан с правильным частотным сдвигом и oversampling.
|
||
|
||
Частотный шаг df остается тем же, а n_fft увеличивается в oversample раз.
|
||
Поэтому временная сетка становится плотнее: dt = 1 / (n_fft * df).
|
||
"""
|
||
mask = (freq >= f_start) & (freq <= f_stop)
|
||
freq_cut = freq[mask]
|
||
s21_cut = s21[mask]
|
||
|
||
if len(freq_cut) < 2:
|
||
raise ValueError('После обрезки по частоте осталось меньше двух точек.')
|
||
|
||
df = float(np.median(np.diff(freq_cut)))
|
||
n = len(freq_cut)
|
||
k0 = int(round(freq_cut[0] / df))
|
||
|
||
min_len = 2 * (k0 + n - 1)
|
||
n_fft_base = 1 << int(np.ceil(np.log2(min_len)))
|
||
n_fft = int(n_fft_base * oversample)
|
||
|
||
dt = 1.0 / (n_fft * df)
|
||
t_sec = np.arange(n_fft, dtype=float) * dt
|
||
|
||
s = s21_cut * np.hanning(n) if window else s21_cut.copy()
|
||
|
||
H = np.zeros(n_fft, dtype=np.complex128)
|
||
H[k0:k0 + n] = s
|
||
|
||
h_complex = np.fft.ifft(H)
|
||
a_abs = np.abs(h_complex)
|
||
|
||
return t_sec, a_abs, h_complex, n_fft_base, n_fft
|
||
|
||
|
||
|
||
print('Вычисление oversampled A-сканов...', end=' ', flush=True)
|
||
A_bp = {}
|
||
H_bp = {}
|
||
T_bp = {}
|
||
Z_bp = {}
|
||
n_fft_info = {}
|
||
pair_norm_info = {}
|
||
phase_meta = {}
|
||
|
||
for (i, j), s21 in s21_data.items():
|
||
s21_proc = np.array(s21, dtype=np.complex128, copy=True)
|
||
if BG_SUBTRACT and background is not None and (i, j) in background:
|
||
s21_proc = s21_proc - background[(i, j)]
|
||
|
||
if (i, j) not in pair_timestamps:
|
||
raise KeyError(f'Нет временной информации для пары {(i, j)}')
|
||
|
||
s21_corr, meta = apply_intra_sweep_phase_correction(
|
||
s21=s21_proc,
|
||
freq_full=freq_data[(i, j)],
|
||
pair_info=pair_timestamps[(i, j)],
|
||
config=MOTION_CONFIG,
|
||
wave_speed=v,
|
||
f_start=F_START,
|
||
f_stop=F_STOP,
|
||
)
|
||
phase_meta[(i, j)] = meta
|
||
|
||
t_pair, a_pair, h_pair, n_fft_base, n_fft = compute_ascan_bp(
|
||
s21_corr,
|
||
freq_data[(i, j)],
|
||
f_start=F_START,
|
||
f_stop=F_STOP,
|
||
window=BP_WINDOW,
|
||
oversample=BP_OVERSAMPLE,
|
||
)
|
||
T_bp[(i, j)] = t_pair
|
||
Z_bp[(i, j)] = t_pair * v / 2
|
||
A_bp[(i, j)] = a_pair
|
||
H_bp[(i, j)] = h_pair
|
||
n_fft_info[(i, j)] = (n_fft_base, n_fft)
|
||
|
||
# Robust per-pair amplitude normalization. It equalizes channel scale, not phase.
|
||
for key in sorted(H_bp.keys()):
|
||
z_axis = Z_bp[key]
|
||
h_abs = np.abs(H_bp[key])
|
||
norm_mask = (z_axis >= PAIR_NORM_DEPTH_MIN) & (z_axis <= PAIR_NORM_DEPTH_MAX)
|
||
if not np.any(norm_mask):
|
||
norm_mask = np.ones_like(z_axis, dtype=bool)
|
||
|
||
median_val = float(np.median(h_abs[norm_mask]))
|
||
p75_val = float(np.percentile(h_abs[norm_mask], 75))
|
||
p95_val = float(np.percentile(h_abs[norm_mask], 95))
|
||
scale = float(np.percentile(h_abs[norm_mask], PAIR_NORM_PERCENTILE))
|
||
if not np.isfinite(scale) or scale <= PAIR_NORM_EPS:
|
||
scale = 1.0
|
||
|
||
pair_norm_info[key] = {
|
||
'median': median_val,
|
||
'p75': p75_val,
|
||
'p95': p95_val,
|
||
'scale': scale,
|
||
}
|
||
|
||
if PAIR_NORMALIZE:
|
||
H_bp[key] = H_bp[key] / (scale + PAIR_NORM_EPS)
|
||
A_bp[key] = np.abs(H_bp[key])
|
||
|
||
|
||
z_h_bp = Z_bp[first_key]
|
||
t_h_bp = T_bp[first_key]
|
||
print('готово.')
|
||
print(f'dt = {(t_h_bp[1] - t_h_bp[0])*1e12:.2f} пс')
|
||
print(f'dz_sample = {(z_h_bp[1] - z_h_bp[0])*100:.3f} см')
|
||
if phase_meta:
|
||
delta_ranges = np.concatenate([m['delta_range_m'] for m in phase_meta.values()])
|
||
total_dtau_vals = np.concatenate([m['total_dtau_s'] for m in phase_meta.values()])
|
||
phis = np.concatenate([m['phi_rad'] for m in phase_meta.values()])
|
||
print(f'Motion mode до IFFT: {MOTION_CONFIG.motion_mode}')
|
||
print('Frequency timing: interleaved_tx_by_frequency')
|
||
print(f'Total dtau(f) range = {total_dtau_vals.min()*1e9:+.3f}..{total_dtau_vals.max()*1e9:+.3f} нс')
|
||
print(f'Intra-sweep dz range = {delta_ranges.min()*100:+.3f}..{delta_ranges.max()*100:+.3f} см')
|
||
print(f'Motion phi range = {phis.min():+.3e}..{phis.max():+.3e} рад')
|
||
|
||
print('\n' + '=' * 86)
|
||
print(f' НОРМИРОВКА КАНАЛОВ Tx/Rx: enabled={PAIR_NORMALIZE}, '
|
||
f'percentile={PAIR_NORM_PERCENTILE:.1f}, '
|
||
f'z=[{PAIR_NORM_DEPTH_MIN:.2f}, {PAIR_NORM_DEPTH_MAX:.2f}] м')
|
||
print('=' * 86)
|
||
print(f" {'Pair':<8} {'median':>12} {'p75':>12} {'p95':>12} {'scale':>12} {'rel_scale':>12}")
|
||
print('-' * 86)
|
||
scales = np.array([v['scale'] for v in pair_norm_info.values()], dtype=float)
|
||
scale_ref = float(np.median(scales)) if len(scales) else 1.0
|
||
for key in sorted(pair_norm_info.keys()):
|
||
info = pair_norm_info[key]
|
||
rel_scale = info['scale'] / (scale_ref + PAIR_NORM_EPS)
|
||
print(f" Tx{key[0]}-Rx{key[1]:<3} {info['median']:>12.4e} {info['p75']:>12.4e} "
|
||
f"{info['p95']:>12.4e} {info['scale']:>12.4e} {rel_scale:>12.3f}")
|
||
print('=' * 86)
|
||
|
||
|
||
# ══════════════════════════════════════════════════════
|
||
# 3. TIME-DOMAIN COHERENT BACKPROJECTION
|
||
# ══════════════════════════════════════════════════════
|
||
|
||
x_grid_bp = np.linspace(x_min, x_max, NX_BP)
|
||
z_grid_bp = np.linspace(z_min, z_max, NZ_BP)
|
||
XX_bp, ZZ_bp = np.meshgrid(x_grid_bp, z_grid_bp)
|
||
depth_gate = (ZZ_bp >= MIN_DEPTH) & (ZZ_bp <= MAX_DEPTH)
|
||
|
||
|
||
def bistatic_ranges(i_tx, i_rx, XX, ZZ, yy=BP_PLANE_Y):
|
||
Rtx = np.sqrt((XX - x_tx[i_tx])**2 + (yy - y_tx[i_tx])**2 + (ZZ - z_tx[i_tx])**2)
|
||
Rrx = np.sqrt((XX - x_rx[i_rx])**2 + (yy - y_rx[i_rx])**2 + (ZZ - z_rx[i_rx])**2)
|
||
return Rtx, Rrx
|
||
|
||
|
||
def antenna_boresight_cos_z(R, z_ant, ZZ):
|
||
# В локальных координатах радара все антенны смотрят вдоль +Z.
|
||
return (ZZ - z_ant) / (R + 1e-12)
|
||
|
||
|
||
def attenuation_components_map(i_tx, i_rx, XX, ZZ):
|
||
Rtx, Rrx = bistatic_ranges(i_tx, i_rx, XX, ZZ)
|
||
geo = 1.0 / (Rtx * Rrx + 1e-12)
|
||
cos_tx = antenna_boresight_cos_z(Rtx, z_tx[i_tx], ZZ)
|
||
cos_rx = antenna_boresight_cos_z(Rrx, z_rx[i_rx], ZZ)
|
||
angle = cos_tx**2 * cos_rx**2
|
||
return geo + 1e-30, angle + 1e-30
|
||
|
||
|
||
def attenuation_components_at_ref_depth(i_tx, i_rx, z_ref):
|
||
xc = (x_tx[i_tx] + x_rx[i_rx]) / 2.0
|
||
Rtx, Rrx = bistatic_ranges(i_tx, i_rx, xc, z_ref)
|
||
geo = 1.0 / (Rtx * Rrx + 1e-12)
|
||
cos_tx = antenna_boresight_cos_z(Rtx, z_tx[i_tx], z_ref)
|
||
cos_rx = antenna_boresight_cos_z(Rrx, z_rx[i_rx], z_ref)
|
||
angle = cos_tx**2 * cos_rx**2
|
||
return geo + 1e-30, angle + 1e-30
|
||
|
||
|
||
def bp_compensation_weight(i_tx, i_rx, XX, ZZ):
|
||
geo, angle = attenuation_components_map(i_tx, i_rx, XX, ZZ)
|
||
geo_ref, angle_ref = attenuation_components_at_ref_depth(i_tx, i_rx, COMP_REF_DEPTH)
|
||
|
||
geo_norm = geo / geo_ref
|
||
angle_norm = angle / angle_ref
|
||
|
||
range_weight = 1.0 / (geo_norm ** COMP_RANGE_POWER + 1e-12)
|
||
angle_weight = 1.0 / (angle_norm ** COMP_ANGLE_POWER + 1e-12)
|
||
|
||
range_weight = np.clip(range_weight, 0.0, COMP_RANGE_WEIGHT_MAX)
|
||
angle_weight = np.clip(angle_weight, 0.0, COMP_ANGLE_WEIGHT_MAX)
|
||
|
||
weight = range_weight * angle_weight
|
||
return np.clip(weight, 0.0, COMP_WEIGHT_MAX)
|
||
|
||
|
||
def interpolate_ascan_amplitude(tau, t_axis, a_axis):
|
||
return np.interp(tau.ravel(), t_axis, a_axis, left=0.0, right=0.0).reshape(tau.shape)
|
||
|
||
|
||
def interpolate_ascan_complex(tau, t_axis, h_axis):
|
||
h_real = np.interp(tau.ravel(), t_axis, h_axis.real, left=0.0, right=0.0)
|
||
h_imag = np.interp(tau.ravel(), t_axis, h_axis.imag, left=0.0, right=0.0)
|
||
return (h_real + 1j * h_imag).reshape(tau.shape)
|
||
|
||
|
||
def backproject_coherent(H, T, compensate=True):
|
||
bp_complex = np.zeros_like(XX_bp, dtype=np.complex128)
|
||
contribution_count = np.zeros_like(XX_bp, dtype=float)
|
||
|
||
for i in range(N_tx):
|
||
for j in range(N_rx):
|
||
key = (i, j)
|
||
if key not in H:
|
||
continue
|
||
|
||
Rtx, Rrx = bistatic_ranges(i, j, XX_bp, ZZ_bp)
|
||
tau_ref = (Rtx + Rrx) / v
|
||
tau = tau_ref
|
||
valid = depth_gate & (tau >= T[key][0]) & (tau <= T[key][-1])
|
||
|
||
h_tau = interpolate_ascan_complex(tau, T[key], H[key])
|
||
h_tau = np.where(valid, h_tau, 0.0 + 0.0j)
|
||
|
||
if compensate:
|
||
w = bp_compensation_weight(i, j, XX_bp, ZZ_bp)
|
||
w = np.where(valid, w, 0.0)
|
||
else:
|
||
w = np.where(valid, 1.0, 0.0)
|
||
|
||
bp_complex += h_tau * w
|
||
contribution_count += valid.astype(float)
|
||
|
||
bp_complex = bp_complex / (contribution_count + 1e-12)
|
||
bp_complex = np.where(depth_gate, bp_complex, 0.0 + 0.0j)
|
||
bp_abs = np.abs(bp_complex)
|
||
return bp_abs, bp_complex
|
||
|
||
|
||
def backproject_coherent_and_incoherent(H, T, compensate=True):
|
||
"""Одним проходом строит coherent |sum h| и incoherent sum |h| BP-карты."""
|
||
bp_complex = np.zeros_like(XX_bp, dtype=np.complex128)
|
||
bp_incoherent = np.zeros_like(XX_bp, dtype=float)
|
||
contribution_count = np.zeros_like(XX_bp, dtype=float)
|
||
|
||
for i in range(N_tx):
|
||
for j in range(N_rx):
|
||
key = (i, j)
|
||
if key not in H:
|
||
continue
|
||
|
||
Rtx, Rrx = bistatic_ranges(i, j, XX_bp, ZZ_bp)
|
||
tau_ref = (Rtx + Rrx) / v
|
||
tau = tau_ref
|
||
valid = depth_gate & (tau >= T[key][0]) & (tau <= T[key][-1])
|
||
|
||
h_tau = interpolate_ascan_complex(tau, T[key], H[key])
|
||
h_tau = np.where(valid, h_tau, 0.0 + 0.0j)
|
||
|
||
if compensate:
|
||
w = bp_compensation_weight(i, j, XX_bp, ZZ_bp)
|
||
w = np.where(valid, w, 0.0)
|
||
else:
|
||
w = np.where(valid, 1.0, 0.0)
|
||
|
||
bp_complex += h_tau * w
|
||
bp_incoherent += np.abs(h_tau) * w
|
||
contribution_count += valid.astype(float)
|
||
|
||
bp_complex = bp_complex / (contribution_count + 1e-12)
|
||
bp_complex = np.where(depth_gate, bp_complex, 0.0 + 0.0j)
|
||
bp_abs = np.abs(bp_complex)
|
||
|
||
bp_incoherent = bp_incoherent / (contribution_count + 1e-12)
|
||
bp_incoherent = np.where(depth_gate, bp_incoherent, 0.0)
|
||
|
||
bp_cf = bp_abs / (bp_incoherent + BP_SCORE_CF_EPS)
|
||
bp_cf = np.where(depth_gate, bp_cf, 0.0)
|
||
bp_cf = np.clip(bp_cf, 0.0, 1.0)
|
||
return bp_abs, bp_complex, bp_incoherent, bp_cf
|
||
|
||
|
||
def normalize_bp_map(bp_raw, smooth_sigma=BP_SMOOTH_SIGMA):
|
||
bp_norm = bp_raw / (bp_raw.max() + 1e-12)
|
||
bp_norm = gaussian_filter(bp_norm, sigma=smooth_sigma)
|
||
bp_norm = np.where(depth_gate, bp_norm, 0.0)
|
||
return bp_norm / (bp_norm.max() + 1e-12)
|
||
|
||
def component_containing_peak(image, iz, ix, threshold, window_mask=None):
|
||
mask = image >= threshold
|
||
if window_mask is not None:
|
||
mask &= window_mask
|
||
|
||
labels, n_labels = label(mask, structure=np.ones((3, 3), dtype=int))
|
||
if n_labels == 0 or labels[iz, ix] == 0:
|
||
fallback = np.zeros_like(image, dtype=bool)
|
||
fallback[iz, ix] = True
|
||
return fallback
|
||
|
||
return labels == labels[iz, ix]
|
||
|
||
|
||
def weighted_centroid(image, region_mask, threshold=0.0):
|
||
values = image[region_mask]
|
||
weights = np.clip(values - threshold, 0.0, None)
|
||
if weights.sum() <= 1e-15:
|
||
weights = values.copy()
|
||
if weights.sum() <= 1e-15:
|
||
iz, ix = np.argwhere(region_mask)[0]
|
||
return x_grid_bp[ix], z_grid_bp[iz]
|
||
|
||
x_vals = XX_bp[region_mask]
|
||
z_vals = ZZ_bp[region_mask]
|
||
return (x_vals * weights).sum() / weights.sum(), (z_vals * weights).sum() / weights.sum()
|
||
|
||
|
||
|
||
def compact_peak_centroid(image, iz, ix, peak):
|
||
x0 = x_grid_bp[ix]
|
||
z0 = z_grid_bp[iz]
|
||
window_mask = (
|
||
(np.abs(XX_bp - x0) <= BP_CENTER_RX_CM / 100.0) &
|
||
(np.abs(ZZ_bp - z0) <= BP_CENTER_RZ_CM / 100.0)
|
||
)
|
||
threshold = BP_CENTER_THRESH_FRAC * peak
|
||
center_mask = window_mask & (image >= threshold)
|
||
|
||
if center_mask.sum() == 0:
|
||
center_mask = window_mask.copy()
|
||
center_mask[iz, ix] = True
|
||
|
||
values = image[center_mask]
|
||
weights = np.clip(values - threshold, 0.0, None) ** BP_CENTER_WEIGHT_POWER
|
||
if weights.sum() <= 1e-15:
|
||
weights = values.copy()
|
||
if weights.sum() <= 1e-15:
|
||
return x0, z0, center_mask
|
||
|
||
x_vals = XX_bp[center_mask]
|
||
z_vals = ZZ_bp[center_mask]
|
||
x_c = (x_vals * weights).sum() / weights.sum()
|
||
z_c = (z_vals * weights).sum() / weights.sum()
|
||
return x_c, z_c, center_mask
|
||
|
||
def find_bp_objects(bp_image):
|
||
"""
|
||
CLEAN-подобный поиск объектов на BP-карте.
|
||
|
||
Для каждого шага берется максимум рабочей карты, вокруг него выделяется
|
||
связная область выше BP_REGION_THRESH_FRAC от локального пика, затем
|
||
считается взвешенный центроид этой области. После этого более широкая
|
||
область вокруг той же цели подавляется на рабочей карте.
|
||
"""
|
||
work = bp_image.copy()
|
||
objects = []
|
||
global_peak = float(work.max())
|
||
stop_level = BP_OBJECT_MIN_FRAC * global_peak
|
||
dx_cm = abs(x_grid_bp[1] - x_grid_bp[0]) * 100
|
||
dz_cm = abs(z_grid_bp[1] - z_grid_bp[0]) * 100
|
||
pixel_area_cm2 = dx_cm * dz_cm
|
||
|
||
for step in range(MAX_OBJECTS):
|
||
peak = float(work.max())
|
||
if peak <= stop_level or peak <= 0:
|
||
break
|
||
|
||
iz, ix = np.unravel_index(np.argmax(work), work.shape)
|
||
x_peak_local = x_grid_bp[ix]
|
||
z_peak_local = z_grid_bp[iz]
|
||
|
||
region_threshold = BP_REGION_THRESH_FRAC * peak
|
||
region_mask = component_containing_peak(work, iz, ix, region_threshold)
|
||
region_area_cm2 = float(region_mask.sum() * pixel_area_cm2)
|
||
|
||
if region_area_cm2 < BP_MIN_REGION_AREA_CM2:
|
||
work[iz, ix] = 0.0
|
||
continue
|
||
|
||
x_region_centroid, z_region_centroid = weighted_centroid(work, region_mask, threshold=region_threshold)
|
||
if BP_CENTER_USE_COMPACT:
|
||
x_centroid, z_centroid, center_mask = compact_peak_centroid(work, iz, ix, peak)
|
||
else:
|
||
x_centroid, z_centroid = x_region_centroid, z_region_centroid
|
||
center_mask = region_mask.copy()
|
||
|
||
center_area_cm2 = float(center_mask.sum() * pixel_area_cm2)
|
||
region_values = work[region_mask]
|
||
objects.append({
|
||
'index': len(objects) + 1,
|
||
'ix_peak': int(ix),
|
||
'iz_peak': int(iz),
|
||
'x_peak': float(x_peak_local),
|
||
'z_peak': float(z_peak_local),
|
||
'x': float(x_centroid),
|
||
'z': float(z_centroid),
|
||
'x_region': float(x_region_centroid),
|
||
'z_region': float(z_region_centroid),
|
||
'peak': peak,
|
||
'area_cm2': region_area_cm2,
|
||
'center_area_cm2': center_area_cm2,
|
||
'region_mask': region_mask.copy(),
|
||
'center_mask': center_mask.copy(),
|
||
'mean_value': float(region_values.mean()),
|
||
'sum_value': float(region_values.sum()),
|
||
})
|
||
|
||
if BP_SUPPRESS_USE_WINDOW:
|
||
suppress_window = (
|
||
(np.abs(XX_bp - x_peak_local) <= BP_SUPPRESS_RX_CM / 100.0) &
|
||
(np.abs(ZZ_bp - z_peak_local) <= BP_SUPPRESS_RZ_CM / 100.0)
|
||
)
|
||
else:
|
||
suppress_window = None
|
||
|
||
suppress_threshold = BP_SUPPRESS_THRESH_FRAC * peak
|
||
suppress_mask = component_containing_peak(
|
||
work, iz, ix, suppress_threshold, window_mask=suppress_window
|
||
)
|
||
|
||
# Если широкий порог дал слишком маленькую область, подавляем хотя бы область детекции.
|
||
if suppress_mask.sum() < region_mask.sum():
|
||
suppress_mask = region_mask
|
||
|
||
work[suppress_mask] = 0.0
|
||
|
||
return objects, work
|
||
|
||
|
||
def circular_phase_stats(bp_complex_map, mask):
|
||
if mask.sum() == 0:
|
||
return np.nan, np.nan, np.nan
|
||
|
||
h = bp_complex_map[mask]
|
||
amp = np.abs(h)
|
||
valid = amp > 0
|
||
if not np.any(valid):
|
||
return np.nan, np.nan, np.nan
|
||
|
||
phase = np.angle(h[valid])
|
||
weights = amp[valid] ** BP_PHASE_WEIGHT_POWER
|
||
if weights.sum() <= 1e-15:
|
||
weights = np.ones_like(phase)
|
||
|
||
vec = np.sum(weights * np.exp(1j * phase)) / (np.sum(weights) + 1e-15)
|
||
phase_mean = float(np.angle(vec))
|
||
phase_coherence = float(np.abs(vec))
|
||
phase_circular_variance = float(1.0 - phase_coherence)
|
||
return phase_mean, phase_coherence, phase_circular_variance
|
||
|
||
|
||
def add_phase_metrics(objects, bp_complex_map):
|
||
for obj in objects:
|
||
mask = obj.get('center_mask', obj['region_mask'])
|
||
phase_mean, phase_coh, phase_var = circular_phase_stats(bp_complex_map, mask)
|
||
obj['phase_mean_rad'] = phase_mean
|
||
obj['phase_coherence'] = phase_coh
|
||
obj['phase_circular_variance'] = phase_var
|
||
return objects
|
||
|
||
|
||
def add_local_prominence_metrics(objects, bp_image):
|
||
for obj in objects:
|
||
x0 = obj['x_peak']
|
||
z0 = obj['z_peak']
|
||
outer_mask = (
|
||
(np.abs(XX_bp - x0) <= BP_LOCAL_BG_RX_CM / 100.0) &
|
||
(np.abs(ZZ_bp - z0) <= BP_LOCAL_BG_RZ_CM / 100.0) &
|
||
depth_gate
|
||
)
|
||
bg_mask = outer_mask & (~obj['region_mask'])
|
||
if bg_mask.sum() < 10:
|
||
bg_mask = depth_gate & (~obj['region_mask'])
|
||
|
||
if bg_mask.sum() == 0:
|
||
bg_level = 0.0
|
||
bg_p75 = 0.0
|
||
else:
|
||
bg_values = bp_image[bg_mask]
|
||
bg_level = float(np.percentile(bg_values, BP_LOCAL_BG_PERCENTILE))
|
||
bg_p75 = float(np.percentile(bg_values, 75))
|
||
|
||
peak = float(obj['peak'])
|
||
obj['local_bg'] = bg_level
|
||
obj['local_bg_p75'] = bg_p75
|
||
obj['prominence'] = peak - bg_level
|
||
obj['contrast'] = peak / (bg_level + BP_LOCAL_CONTRAST_EPS)
|
||
return objects
|
||
|
||
|
||
def add_incoherent_support_metrics(objects, bp_incoherent_image, bp_cf_image=None):
|
||
for obj in objects:
|
||
if bp_incoherent_image is None:
|
||
obj['incoh_peak'] = 0.0
|
||
obj['incoh_mean'] = 0.0
|
||
obj['incoh_center_mean'] = 0.0
|
||
obj['coherence_factor_peak_raw'] = 0.0
|
||
obj['coherence_factor_center_raw'] = 0.0
|
||
obj['coherence_factor_peak'] = 0.0
|
||
obj['coherence_factor_center'] = 0.0
|
||
continue
|
||
|
||
region_mask = obj['region_mask']
|
||
center_mask = obj.get('center_mask', region_mask)
|
||
if region_mask.sum() == 0:
|
||
obj['incoh_peak'] = 0.0
|
||
obj['incoh_mean'] = 0.0
|
||
else:
|
||
region_values = bp_incoherent_image[region_mask]
|
||
obj['incoh_peak'] = float(region_values.max())
|
||
obj['incoh_mean'] = float(region_values.mean())
|
||
|
||
if center_mask.sum() == 0:
|
||
obj['incoh_center_mean'] = obj['incoh_mean']
|
||
else:
|
||
obj['incoh_center_mean'] = float(bp_incoherent_image[center_mask].mean())
|
||
|
||
if bp_cf_image is None:
|
||
obj['coherence_factor_peak_raw'] = float(
|
||
obj.get('peak', 0.0) / (obj['incoh_peak'] + BP_SCORE_CF_EPS)
|
||
)
|
||
obj['coherence_factor_center_raw'] = float(
|
||
obj.get('mean_value', 0.0) / (obj['incoh_center_mean'] + BP_SCORE_CF_EPS)
|
||
)
|
||
else:
|
||
iz_peak = int(obj.get('iz_peak', 0))
|
||
ix_peak = int(obj.get('ix_peak', 0))
|
||
obj['coherence_factor_peak_raw'] = float(bp_cf_image[iz_peak, ix_peak])
|
||
if center_mask.sum() == 0:
|
||
obj['coherence_factor_center_raw'] = obj['coherence_factor_peak_raw']
|
||
else:
|
||
obj['coherence_factor_center_raw'] = float(bp_cf_image[center_mask].mean())
|
||
|
||
obj['coherence_factor_peak'] = float(np.clip(obj['coherence_factor_peak_raw'], 0.0, 1.0))
|
||
obj['coherence_factor_center'] = float(np.clip(obj['coherence_factor_center_raw'], 0.0, 1.0))
|
||
return objects
|
||
|
||
|
||
def _contrast_score_unit(contrast):
|
||
if not np.isfinite(contrast):
|
||
return 0.0
|
||
if BP_SCORE_CONTRAST_CAP <= 1.0:
|
||
return 0.0
|
||
return float(np.clip((contrast - 1.0) / (BP_SCORE_CONTRAST_CAP - 1.0), 0.0, 1.0))
|
||
|
||
|
||
def add_bp_score_metrics(objects):
|
||
total_weight = (
|
||
BP_SCORE_COH_PEAK_WEIGHT +
|
||
BP_SCORE_COHERENCE_FACTOR_WEIGHT +
|
||
BP_SCORE_PROMINENCE_WEIGHT +
|
||
BP_SCORE_CONTRAST_WEIGHT
|
||
)
|
||
if total_weight <= 0:
|
||
total_weight = 1.0
|
||
|
||
for obj in objects:
|
||
coh_peak_score = float(np.clip(obj.get('peak', 0.0), 0.0, 1.0))
|
||
coherence_factor_score = float(np.clip(obj.get('coherence_factor_peak', 0.0), 0.0, 1.0))
|
||
prominence_score = float(np.clip(obj.get('prominence', 0.0), 0.0, 1.0))
|
||
contrast_score = _contrast_score_unit(obj.get('contrast', np.nan))
|
||
|
||
score_combined = (
|
||
BP_SCORE_COH_PEAK_WEIGHT * coh_peak_score +
|
||
BP_SCORE_COHERENCE_FACTOR_WEIGHT * coherence_factor_score +
|
||
BP_SCORE_PROMINENCE_WEIGHT * prominence_score +
|
||
BP_SCORE_CONTRAST_WEIGHT * contrast_score
|
||
) / total_weight
|
||
|
||
obj['score_old'] = coh_peak_score
|
||
obj['score_new'] = float(score_combined)
|
||
obj['score_coh_peak_part'] = coh_peak_score
|
||
obj['score_cf_part'] = coherence_factor_score
|
||
obj['score_prominence_part'] = prominence_score
|
||
obj['score_contrast_part'] = contrast_score
|
||
obj['score_selected'] = obj['score_new'] if BP_SCORE_MODE == 'combined' else obj['score_old']
|
||
return objects
|
||
|
||
|
||
def prepare_bp_objects_for_display(objects, remove_sidelobes=True):
|
||
if remove_sidelobes:
|
||
visible = [obj for obj in objects if not obj.get('sidelobe_candidate', False)]
|
||
else:
|
||
visible = list(objects)
|
||
|
||
if BP_SCORE_MODE == 'combined':
|
||
visible = sorted(visible, key=lambda obj: obj.get('score_new', 0.0), reverse=True)
|
||
else:
|
||
visible = sorted(visible, key=lambda obj: obj.get('score_old', obj.get('peak', 0.0)), reverse=True)
|
||
|
||
for rank, obj in enumerate(visible, start=1):
|
||
obj['display_index'] = rank
|
||
return visible
|
||
|
||
|
||
def bistatic_depth_signature(x_obj, z_obj):
|
||
signature = []
|
||
for i in range(N_tx):
|
||
for j in range(N_rx):
|
||
Rtx, Rrx = bistatic_ranges(i, j, x_obj, z_obj)
|
||
signature.append(0.5 * (Rtx + Rrx))
|
||
return np.array(signature, dtype=float)
|
||
|
||
|
||
def mark_sidelobe_candidates(objects):
|
||
for obj in objects:
|
||
obj['sidelobe_candidate'] = False
|
||
obj['sidelobe_parent'] = None
|
||
obj['sidelobe_range_rms_cm'] = np.nan
|
||
obj['sidelobe_dx_cm'] = np.nan
|
||
obj['sidelobe_dz_cm'] = np.nan
|
||
|
||
if not BP_SIDELOBE_DETECT:
|
||
return objects
|
||
|
||
signatures = [bistatic_depth_signature(obj['x'], obj['z']) for obj in objects]
|
||
|
||
for k, obj in enumerate(objects):
|
||
best_parent = None
|
||
best_rms_cm = np.inf
|
||
best_dx_cm = np.nan
|
||
best_dz_cm = np.nan
|
||
|
||
for p in range(k):
|
||
parent = objects[p]
|
||
rel_peak = obj['peak'] / (parent['peak'] + 1e-12)
|
||
dx_cm = abs(obj['x'] - parent['x']) * 100.0
|
||
dz_cm = abs(obj['z'] - parent['z']) * 100.0
|
||
rms_cm = float(np.sqrt(np.mean((signatures[k] - signatures[p])**2)) * 100.0)
|
||
|
||
is_candidate = (
|
||
rel_peak <= BP_SIDELOBE_MAX_REL_PEAK and
|
||
dx_cm >= BP_SIDELOBE_MIN_DX_CM and
|
||
dz_cm <= BP_SIDELOBE_MAX_DZ_CM and
|
||
rms_cm <= BP_SIDELOBE_RANGE_RMS_TOL_CM
|
||
)
|
||
|
||
if is_candidate and rms_cm < best_rms_cm:
|
||
best_parent = parent
|
||
best_rms_cm = rms_cm
|
||
best_dx_cm = dx_cm
|
||
best_dz_cm = dz_cm
|
||
|
||
if best_parent is not None:
|
||
obj['sidelobe_candidate'] = True
|
||
obj['sidelobe_parent'] = best_parent['index']
|
||
obj['sidelobe_range_rms_cm'] = best_rms_cm
|
||
obj['sidelobe_dx_cm'] = best_dx_cm
|
||
obj['sidelobe_dz_cm'] = best_dz_cm
|
||
|
||
return objects
|
||
|
||
|
||
if BP_SCORE_COMPUTE_INCOHERENT:
|
||
print('Расчет coherent + incoherent time-domain BP...', end=' ', flush=True)
|
||
bp_raw, bp_complex, bp_incoh_raw, bp_cf_map = backproject_coherent_and_incoherent(H_bp, T_bp, compensate=True)
|
||
bp_incoh_map_s = normalize_bp_map(bp_incoh_raw)
|
||
else:
|
||
print('Расчет coherent time-domain BP...', end=' ', flush=True)
|
||
bp_raw, bp_complex = backproject_coherent(H_bp, T_bp, compensate=True)
|
||
bp_incoh_raw = None
|
||
bp_incoh_map_s = None
|
||
bp_cf_map = None
|
||
bp_map_s = normalize_bp_map(bp_raw)
|
||
print('готово.')
|
||
print(f'BP score mode: {BP_SCORE_MODE} (peak=старый, combined=экспериментальный)')
|
||
|
||
if BP_VALIDATE_COMPENSATION:
|
||
print('Расчет coherent BP без compensation для валидации...', end=' ', flush=True)
|
||
bp_raw_nocomp, bp_complex_nocomp = backproject_coherent(H_bp, T_bp, compensate=False)
|
||
bp_map_nocomp_s = normalize_bp_map(bp_raw_nocomp)
|
||
print('готово.')
|
||
else:
|
||
bp_raw_nocomp = None
|
||
bp_complex_nocomp = None
|
||
bp_map_nocomp_s = None
|
||
|
||
bp_objects_all, bp_residual = find_bp_objects(bp_map_s)
|
||
bp_objects_all = add_phase_metrics(bp_objects_all, bp_complex)
|
||
bp_objects_all = add_local_prominence_metrics(bp_objects_all, bp_map_s)
|
||
bp_objects_all = add_incoherent_support_metrics(bp_objects_all, bp_incoh_map_s, bp_cf_map)
|
||
bp_objects_all = mark_sidelobe_candidates(bp_objects_all)
|
||
bp_objects_all = add_bp_score_metrics(bp_objects_all)
|
||
|
||
bp_map_display = bp_map_s.copy()
|
||
if BP_REMOVE_SIDELOBE_OBJECTS:
|
||
for obj in bp_objects_all:
|
||
if obj['sidelobe_candidate']:
|
||
bp_map_display[obj['region_mask']] = 0.0
|
||
bp_objects = prepare_bp_objects_for_display(bp_objects_all, remove_sidelobes=BP_REMOVE_SIDELOBE_OBJECTS)
|
||
bp_detected_object_count = len(bp_objects)
|
||
if bp_detected_object_count > BP_MAX_DETECTED_OBJECTS_TO_DRAW:
|
||
bp_objects_to_plot = []
|
||
else:
|
||
bp_objects_to_plot = bp_objects[:BP_DRAW_TOP_M_OBJECTS]
|
||
|
||
iz_max, ix_max = np.unravel_index(np.argmax(bp_map_s), bp_map_s.shape)
|
||
x_peak = x_grid_bp[ix_max]
|
||
z_peak = z_grid_bp[iz_max]
|
||
peak_value = bp_map_s[iz_max, ix_max]
|
||
main_obj = bp_objects[0] if bp_objects else None
|
||
|
||
target_results = [
|
||
{
|
||
'rank': int(rank),
|
||
'x_cm': round(float(obj['x'] * 100.0), 1),
|
||
'z_cm': round(float(obj['z'] * 100.0), 1),
|
||
'combined_score': round(float(obj.get('score_new', np.nan)), 4),
|
||
}
|
||
for rank, obj in enumerate(bp_objects_to_plot, start=1)
|
||
]
|
||
_print_raw(json.dumps({'targets': target_results}, ensure_ascii=False, indent=2))
|
||
|
||
|
||
# ══════════════════════════════════════════════════════
|
||
# 4. ТОЛЬКО BP-КАРТА
|
||
# ══════════════════════════════════════════════════════
|
||
|
||
fig, ax = plt.subplots(figsize=(12, 7))
|
||
im = ax.imshow(
|
||
bp_map_display,
|
||
extent=[x_grid_bp[0]*100, x_grid_bp[-1]*100, z_grid_bp[-1]*100, z_grid_bp[0]*100],
|
||
aspect='auto',
|
||
cmap='jet',
|
||
vmin=0.25,
|
||
vmax=0.95,
|
||
)
|
||
plt.colorbar(im, ax=ax, label='Нормированная |coherent BP|')
|
||
ax.plot(x_tx * 100, z_tx * 100, 'r^', ms=12, label='Tx', zorder=5)
|
||
ax.plot(x_rx * 100, z_rx * 100, 'bv', ms=12, label='Rx', zorder=5)
|
||
|
||
for obj in bp_objects_to_plot:
|
||
ax.contour(
|
||
x_grid_bp * 100,
|
||
z_grid_bp * 100,
|
||
obj['region_mask'].astype(float),
|
||
levels=[0.5],
|
||
colors='white',
|
||
linewidths=0.9,
|
||
alpha=0.75,
|
||
)
|
||
ax.contour(
|
||
x_grid_bp * 100,
|
||
z_grid_bp * 100,
|
||
obj['center_mask'].astype(float),
|
||
levels=[0.5],
|
||
colors='cyan',
|
||
linewidths=0.8,
|
||
alpha=0.85,
|
||
)
|
||
if obj['sidelobe_candidate']:
|
||
ax.plot(obj['x'] * 100, obj['z'] * 100, 'x', color='yellow', ms=9,
|
||
mew=2.0, zorder=7)
|
||
label_text = f"{obj['index']} SL"
|
||
text_color = 'yellow'
|
||
else:
|
||
ax.plot(obj['x'] * 100, obj['z'] * 100, 'wo', ms=7,
|
||
markeredgecolor='k', mew=0.8, zorder=6)
|
||
label_text = str(obj.get('display_index', obj['index']))
|
||
text_color = 'white'
|
||
ax.text(obj['x'] * 100 + 3, obj['z'] * 100, label_text,
|
||
color=text_color, fontsize=9, weight='bold', zorder=7)
|
||
|
||
if len(bp_objects_to_plot) > 0:
|
||
ax.plot([], [], 'wo', ms=7, markeredgecolor='k', mew=0.8, label='Центроид области')
|
||
if not BP_REMOVE_SIDELOBE_OBJECTS:
|
||
ax.plot([], [], 'x', color='yellow', ms=9, mew=2.0, label='Sidelobe candidate')
|
||
|
||
ax.axhline(MIN_DEPTH * 100, color='white', lw=1.0, ls='--', alpha=0.75)
|
||
ax.set_xlabel('X [см]')
|
||
ax.set_ylabel('Глубина Z [см]')
|
||
map_title = f'Time-domain coherent BackProjection | motion={MOTION_CORRECTION_MODE}, score={BP_SCORE_MODE}, shown={len(bp_objects_to_plot)}/{bp_detected_object_count}'
|
||
if BP_REMOVE_SIDELOBE_OBJECTS:
|
||
map_title += ' (SL области скрыты на карте)'
|
||
ax.set_title(map_title)
|
||
ax.set_xlim(x_grid_bp[0] * 100, x_grid_bp[-1] * 100)
|
||
ax.set_ylim(z_grid_bp[-1] * 100, 0)
|
||
ax.legend(loc='lower right', fontsize=9)
|
||
ax.grid(alpha=0.22)
|
||
ax.invert_yaxis()
|
||
plt.tight_layout()
|
||
plt.show()
|