diff --git a/Horns_motion_3libre.py b/Horns_motion_3libre.py new file mode 100644 index 0000000..23233b6 --- /dev/null +++ b/Horns_motion_3libre.py @@ -0,0 +1,1326 @@ +""" +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() diff --git a/data_acq_and_processing/processing/data_processor/include/processing_live_config.hpp b/data_acq_and_processing/processing/data_processor/include/processing_live_config.hpp index 0d4714d..8f64985 100644 --- a/data_acq_and_processing/processing/data_processor/include/processing_live_config.hpp +++ b/data_acq_and_processing/processing/data_processor/include/processing_live_config.hpp @@ -44,10 +44,14 @@ struct ProcessingLiveConfig { std::vector gpr_output_positions{}; float gpr_min_depth_m = 2.0F; float gpr_max_depth_m = 14.0F; - float gpr_range_comp_power = 0.28F; - float gpr_angle_comp_power = 0.10F; + float gpr_range_comp_power = 0.1F; + float gpr_angle_comp_power = 0.0F; float gpr_comp_power = 0.2F; std::string gpr_score_mode = "combined"; + // Backprojection intra-sweep speed-correction mode: "int_minus" (full + // correction) or "int_focus" (focusing residual only). Mirrors the Python + // Horns_motion_3libre.py MOTION_CORRECTION_MODE selector. + std::string gpr_motion_mode = "int_minus"; float gpr_speed_m_s = 0.0F; float gpr_look_angle_deg = 0.0F; // Motion model parameters for the legacy GPR pipeline. The direction sign diff --git a/data_acq_and_processing/processing/data_processor/src/processing_live_config.cpp b/data_acq_and_processing/processing/data_processor/src/processing_live_config.cpp index f1b1a8a..b54e582 100644 --- a/data_acq_and_processing/processing/data_processor/src/processing_live_config.cpp +++ b/data_acq_and_processing/processing/data_processor/src/processing_live_config.cpp @@ -60,6 +60,13 @@ using Json = nlohmann::json; throw std::runtime_error(field_name + " must be one of: peak, combined"); } +[[nodiscard]] auto parse_gpr_motion_mode(const std::string& value, const std::string& field_name) -> std::string { + if (value == "int_minus" || value == "int_focus") { + return value; + } + throw std::runtime_error(field_name + " must be one of: int_minus, int_focus"); +} + void apply_legacy_gpr_algorithm_alias(ProcessingLiveConfig& config, const std::string& value) { if (value == "backprojection") { return; @@ -266,6 +273,12 @@ void apply_legacy_gpr_algorithm_alias(ProcessingLiveConfig& config, const std::s } config.gpr_score_mode = parse_gpr_score_mode(found->get(), "processing.gpr_score_mode"); } + if (const auto found = root.find("gpr_motion_mode"); found != root.end()) { + if (!found->is_string()) { + throw std::runtime_error("processing.gpr_motion_mode must be string"); + } + config.gpr_motion_mode = parse_gpr_motion_mode(found->get(), "processing.gpr_motion_mode"); + } if (const auto found = root.find("gpr_speed_m_s"); found != root.end()) { if (!found->is_number()) { throw std::runtime_error("processing.gpr_speed_m_s must be number"); diff --git a/data_acq_and_processing/processing/processors/src/gpr_backprojection_processor.ipp b/data_acq_and_processing/processing/processors/src/gpr_backprojection_processor.ipp index 3bef4e9..61fcccb 100644 --- a/data_acq_and_processing/processing/processors/src/gpr_backprojection_processor.ipp +++ b/data_acq_and_processing/processing/processors/src/gpr_backprojection_processor.ipp @@ -16,8 +16,12 @@ constexpr double kPairNormPercentile = 50.0; constexpr double kPairNormEps = 1e-15; constexpr double kSmoothSigma = 1.5; +// Gaussian-kernel half-width in sigmas, matching scipy.ndimage.gaussian_filter's +// default `truncate=4.0` (radius = int(truncate*sigma + 0.5)). Boundaries use the +// same default 'reflect' (half-sample symmetric) extension — see reflect_index. +constexpr double kGaussianTruncate = 4.0; constexpr std::size_t kMaxObjects = 10U; -constexpr double kObjectMinFrac = 0.35; +constexpr double kObjectMinFrac = 0.7; constexpr double kRegionThresholdFrac = 0.75; constexpr double kSuppressThresholdFrac = 0.20; constexpr double kSuppressRadiusXM = 0.80; @@ -48,6 +52,16 @@ constexpr double kScoreCfEps = 1e-12; using PairKey = std::uint64_t; +// Frequency-domain speed correction mode (Horns_motion_3libre.py MOTION_CONFIG): +// IntMinus — full intra-frequency correction to the interleaved-sweep centre. +// IntFocus — remove the constant + linear-in-frequency part of phi(f), leaving +// the focusing residual without a net Z shift. +enum class MotionMode { IntMinus, IntFocus }; + +[[nodiscard]] auto parse_motion_mode(const std::string& value) -> MotionMode { + return value == "int_focus" ? MotionMode::IntFocus : MotionMode::IntMinus; +} + struct GeometrySelection { // Per-local-index Tx/Rx antenna coordinates in metres. y_/z_ default to 0 // for legacy configs so the imaging plane coincides with the antennas. @@ -269,6 +283,25 @@ void fft_inplace(std::vector>& values, bool inverse) { return static_cast(value); } +// scipy 'reflect' boundary (half-sample symmetric): the signal is mirrored about +// the outer edge of the first/last sample, so index -1 maps to 0, -2 to 1, n to +// n-1, and so on. Matches scipy.ndimage.gaussian_filter's default mode. +[[nodiscard]] auto reflect_index(std::ptrdiff_t value, std::size_t limit) -> std::size_t { + if (limit <= 1U) { + return 0U; + } + const auto extent = static_cast(limit); + const std::ptrdiff_t period = 2 * extent; + std::ptrdiff_t wrapped = value % period; + if (wrapped < 0) { + wrapped += period; + } + if (wrapped >= extent) { + wrapped = (period - 1) - wrapped; + } + return static_cast(wrapped); +} + [[nodiscard]] auto max_value(const std::vector& values) -> double { if (values.empty()) { return 0.0; @@ -291,7 +324,7 @@ void normalize_in_place(std::vector& values) { return {1.0}; } - const auto radius = static_cast(std::ceil(sigma * 3.0)); + const auto radius = static_cast((kGaussianTruncate * sigma) + 0.5); std::vector kernel(static_cast((radius * 2) + 1), 0.0); double sum = 0.0; for (std::ptrdiff_t offset = -radius; offset <= radius; ++offset) { @@ -326,7 +359,7 @@ void normalize_in_place(std::vector& values) { for (std::size_t col = 0U; col < width; ++col) { double sum = 0.0; for (std::ptrdiff_t offset = -radius; offset <= radius; ++offset) { - const auto sample_col = clamp_index(static_cast(col) + offset, width); + const auto sample_col = reflect_index(static_cast(col) + offset, width); sum += values[(row * width) + sample_col] * kernel[static_cast(offset + radius)]; } temp[(row * width) + col] = sum; @@ -337,7 +370,7 @@ void normalize_in_place(std::vector& values) { for (std::size_t col = 0U; col < width; ++col) { double sum = 0.0; for (std::ptrdiff_t offset = -radius; offset <= radius; ++offset) { - const auto sample_row = clamp_index(static_cast(row) + offset, height); + const auto sample_row = reflect_index(static_cast(row) + offset, height); sum += temp[(sample_row * width) + col] * kernel[static_cast(offset + radius)]; } output[(row * width) + col] = sum; @@ -580,6 +613,114 @@ void validate_collection_trace_order( return traces; } +// Frequency-domain speed correction applied to S21 *before* the IFFT, ported +// from Horns_motion_3libre.py (apply_intra_sweep_phase_correction + +// compute_frequency_sample_times). The measurement is interleaved Tx-by-frequency +// — Tx0(f0), Tx1(f0), Tx0(f1), ... — so each Tx occupies one interleave slot and +// its frequency k is sampled at global step (slot_count*k + slot). Each in-band +// frequency's phase is rotated back to the centre of the (full) interleaved sweep. +// +// Unlike the offline Python script, every physical input is taken from the live +// system: `speed_mps` is the socket-fed velocity, `look_angle_deg`/`direction_sign` +// are config, and `tx_sweep_time_s` is derived per-collection from the acquisition +// timestamps (capture_span / Tx count). A zero speed or non-positive sweep time +// makes this a no-op. +void apply_intra_sweep_motion_correction( + SelectedTrace& trace, + std::size_t slot, + std::size_t slot_count, + double start_hz, + double stop_hz, + double velocity_mps, + double speed_mps, + double look_angle_deg, + double direction_sign, + double tx_sweep_time_s, + MotionMode motion_mode +) { + const std::size_t n_full = trace.frequency_hz.size(); + if (n_full < 2U || trace.s21.size() != n_full || slot_count == 0U + || !(velocity_mps > 0.0) || !(tx_sweep_time_s > 0.0)) { + return; + } + + const double low_hz = std::min(start_hz, stop_hz); + const double high_hz = std::max(start_hz, stop_hz); + + // Absolute sample time of full-sweep frequency index k for this Tx slot. + const double dt_base = tx_sweep_time_s / static_cast(n_full - 1U); + const auto sample_time_s = [&](std::size_t index) { + return ((static_cast(slot_count) * static_cast(index)) + static_cast(slot)) * dt_base; + }; + // t_center is the midpoint of the *full* interleaved sweep for this slot, + // i.e. between the first and last full-array frequencies (not the cut band). + const double t_center_s = 0.5 * (sample_time_s(0U) + sample_time_s(n_full - 1U)); + + const double theta_rad = (look_angle_deg * kPi) / 180.0; + const double motion_factor = direction_sign * speed_mps * std::cos(theta_rad); + + std::vector band_indices{}; + std::vector band_frequency_hz{}; + std::vector phi{}; + band_indices.reserve(n_full); + band_frequency_hz.reserve(n_full); + phi.reserve(n_full); + + bool any_nonzero = false; + for (std::size_t index = 0U; index < n_full; ++index) { + const double frequency = trace.frequency_hz[index]; + if (frequency < low_hz || frequency > high_hz) { + continue; + } + const double dt_intra_s = sample_time_s(index) - t_center_s; + const double delta_range_m = motion_factor * dt_intra_s; + const double delta_path_m = 2.0 * delta_range_m; + const double dtau_s = delta_path_m / velocity_mps; + if (dtau_s != 0.0) { + any_nonzero = true; + } + band_indices.push_back(index); + band_frequency_hz.push_back(frequency); + phi.push_back(2.0 * kPi * frequency * dtau_s); + } + + if (!any_nonzero || band_indices.empty()) { + return; + } + + // int_focus removes the constant + linear-in-frequency component of phi(f). + // With f_rel = f - mean(f) the design columns [1, f_rel] are orthogonal, so + // the least-squares fit is intercept = mean(phi), slope = /. + if (motion_mode == MotionMode::IntFocus) { + const auto count = static_cast(band_frequency_hz.size()); + double mean_frequency = 0.0; + double mean_phi = 0.0; + for (std::size_t i = 0U; i < band_frequency_hz.size(); ++i) { + mean_frequency += band_frequency_hz[i]; + mean_phi += phi[i]; + } + mean_frequency /= count; + mean_phi /= count; + + double cross = 0.0; + double f_rel_sq = 0.0; + for (std::size_t i = 0U; i < band_frequency_hz.size(); ++i) { + const double f_rel = band_frequency_hz[i] - mean_frequency; + cross += f_rel * phi[i]; + f_rel_sq += f_rel * f_rel; + } + const double slope = (f_rel_sq > 0.0) ? (cross / f_rel_sq) : 0.0; + for (std::size_t i = 0U; i < phi.size(); ++i) { + const double f_rel = band_frequency_hz[i] - mean_frequency; + phi[i] -= mean_phi + (slope * f_rel); + } + } + + for (std::size_t i = 0U; i < band_indices.size(); ++i) { + trace.s21[band_indices[i]] *= std::polar(1.0, -phi[i]); + } +} + [[nodiscard]] auto compute_ascan( const SelectedTrace& trace, double start_hz, @@ -841,6 +982,38 @@ void normalize_pair_ascans( return std::clamp(range_weight * angle_weight, 0.0, kTotalWeightMax); } +// Run `body(row_begin, row_end)` over a partition of [0, row_count) across the +// available hardware threads. Each call owns a disjoint, contiguous row range, so +// a body that writes only its own rows needs no synchronization. The calling +// thread runs the first chunk while spawned workers handle the rest. Falls back to +// a single serial call when there is one row or no concurrency is reported. +template +void parallel_for_rows(std::size_t row_count, const Body& body) { + if (row_count == 0U) { + return; + } + + const unsigned int detected = std::thread::hardware_concurrency(); + const std::size_t worker_count = std::clamp( + detected == 0U ? 1U : static_cast(detected), 1U, row_count + ); + if (worker_count == 1U) { + body(0U, row_count); + return; + } + + const std::size_t chunk = (row_count + worker_count - 1U) / worker_count; + std::vector workers; + workers.reserve(worker_count - 1U); + for (std::size_t begin = chunk; begin < row_count; begin += chunk) { + workers.emplace_back(body, begin, std::min(begin + chunk, row_count)); + } + body(0U, std::min(chunk, row_count)); + for (auto& worker : workers) { + worker.join(); + } +} + [[nodiscard]] auto backproject_coherent( const std::vector& selected_traces, const std::unordered_map& ascans_by_pair, @@ -862,77 +1035,102 @@ void normalize_pair_ascans( result.coherent.assign(cell_count, std::complex(0.0, 0.0)); result.incoherent.assign(cell_count, 0.0); result.coherence_factor.assign(cell_count, 0.0); - std::vector contribution_count(cell_count, 0.0); + // Resolve each contributing pair once, in selected-trace order. Iterating + // these per cell reproduces the serial accumulation order exactly, so the + // parallel result is bit-for-bit identical to a single-threaded sweep. + struct PairContribution { + const std::vector* tx_distances; + const std::vector* rx_distances; + const AscanResult* ascan; + double geo_ref; + double angle_ref; + double z_tx_ant; + double z_rx_ant; + }; + + std::vector contributions; + contributions.reserve(selected_traces.size()); for (const auto& trace : selected_traces) { const auto key = make_pair_key(trace.tx_local_index, trace.rx_local_index); const auto ascan_it = ascans_by_pair.find(key); - if (ascan_it == ascans_by_pair.end()) { + if (ascan_it == ascans_by_pair.end() || ascan_it->second.time_s.empty()) { continue; } - const auto& ascan = ascan_it->second; - if (ascan.time_s.empty()) { - continue; - } - const auto [geo_ref, angle_ref] = attenuation_components_at_ref_depth( trace.tx_local_index, trace.rx_local_index, selection, imaging_plane_y_m ); - const auto& tx_distances = grid.tx_distance_grids[trace.tx_local_index]; - const auto& rx_distances = grid.rx_distance_grids[trace.rx_local_index]; - const double z_tx_ant = selection.z_tx[trace.tx_local_index]; - const double z_rx_ant = selection.z_rx[trace.rx_local_index]; + contributions.push_back(PairContribution{ + &grid.tx_distance_grids[trace.tx_local_index], + &grid.rx_distance_grids[trace.rx_local_index], + &ascan_it->second, + geo_ref, + angle_ref, + selection.z_tx[trace.tx_local_index], + selection.z_rx[trace.rx_local_index], + }); + } - for (std::size_t row = 0U; row < height; ++row) { + // Each grid row writes only its own cells, so rows partition cleanly across + // threads with no shared mutable state. Within a cell the contributions are + // summed in pair order and then averaged — the same arithmetic, in the same + // order, as the original serial pair-outer/cell-inner loop. + const auto accumulate_rows = [&](std::size_t row_begin, std::size_t row_end) { + for (std::size_t row = row_begin; row < row_end; ++row) { const double z_m = grid.z_grid[row]; - const bool in_depth_gate = z_m >= min_depth_m && z_m <= max_depth_m; - if (!in_depth_gate) { - continue; + if (z_m < min_depth_m || z_m > max_depth_m) { + continue; // Depth-gated rows stay zero, as in the serial version. } - const double dz_tx = z_m - z_tx_ant; - const double dz_rx = z_m - z_rx_ant; for (std::size_t col = 0U; col < width; ++col) { const auto cell_index = (row * width) + col; - const double r_tx = tx_distances[cell_index]; - const double r_rx = rx_distances[cell_index]; - const double tau_s = (r_tx + r_rx) / velocity_mps; - if (tau_s < ascan.time_s.front() || tau_s > ascan.time_s.back()) { + std::complex coherent_sum(0.0, 0.0); + double incoherent_sum = 0.0; + double contribution_count = 0.0; + + for (const auto& contribution : contributions) { + const double r_tx = (*contribution.tx_distances)[cell_index]; + const double r_rx = (*contribution.rx_distances)[cell_index]; + const double tau_s = (r_tx + r_rx) / velocity_mps; + const auto& ascan = *contribution.ascan; + if (tau_s < ascan.time_s.front() || tau_s > ascan.time_s.back()) { + continue; + } + + const auto sample = interpolate_complex(ascan, tau_s); + const double weight = compensation_weight( + r_tx, + r_rx, + z_m - contribution.z_tx_ant, + z_m - contribution.z_rx_ant, + contribution.geo_ref, + contribution.angle_ref, + range_power, + angle_power + ); + coherent_sum += sample * weight; + incoherent_sum += std::abs(sample) * weight; + contribution_count += 1.0; + } + + if (!(contribution_count > 0.0)) { continue; } - const auto sample = interpolate_complex(ascan, tau_s); - - const double weight = compensation_weight( - r_tx, - r_rx, - dz_tx, - dz_rx, - geo_ref, - angle_ref, - range_power, - angle_power - ); - result.coherent[cell_index] += sample * weight; - result.incoherent[cell_index] += std::abs(sample) * weight; - contribution_count[cell_index] += 1.0; + coherent_sum /= contribution_count; + incoherent_sum /= contribution_count; + result.coherent[cell_index] = coherent_sum; + result.incoherent[cell_index] = incoherent_sum; + result.image[cell_index] = std::abs(coherent_sum); + result.coherence_factor[cell_index] = + std::clamp(result.image[cell_index] / (incoherent_sum + kScoreCfEps), 0.0, 1.0); } } - } - - for (std::size_t index = 0U; index < cell_count; ++index) { - if (!(contribution_count[index] > 0.0)) { - continue; - } - result.coherent[index] /= contribution_count[index]; - result.incoherent[index] /= contribution_count[index]; - result.image[index] = std::abs(result.coherent[index]); - result.coherence_factor[index] = - std::clamp(result.image[index] / (result.incoherent[index] + kScoreCfEps), 0.0, 1.0); - } + }; + parallel_for_rows(height, accumulate_rows); return result; } @@ -1595,7 +1793,7 @@ void add_bp_score_metrics( validate_collection_trace_order(run_config, collection); const auto background_mean = build_background_mean(previous_collections, selection, live_config); - const auto selected_traces = collect_selected_traces(collection, selection, background_mean); + auto selected_traces = collect_selected_traces(collection, selection, background_mean); if (selected_traces.empty()) { return results; } @@ -1610,6 +1808,38 @@ void add_bp_score_metrics( return results; } + // Intra-sweep speed correction before the IFFT. The Tx interleave slot is the + // Tx local index (outputs are sorted), with slot_count = number of selected Tx. + // tx_sweep_time is derived per-collection from the acquisition timestamps: + // one Tx's interleaved sweep spans the whole frame, so capture_span / N_tx. + // Speed comes from the socket-fed live config; look-angle/direction from config. + const auto motion_mode = parse_motion_mode(live_config.gpr_motion_mode); + const std::size_t motion_slot_count = selection.output_positions.size(); + const double capture_span_s = + collection.capture_end_ns > collection.capture_start_ns + ? static_cast(collection.capture_end_ns - collection.capture_start_ns) * 1e-9 + : 0.0; + const double tx_sweep_time_s = + motion_slot_count > 0U ? capture_span_s / static_cast(motion_slot_count) : 0.0; + const double motion_speed_mps = static_cast(live_config.gpr_speed_m_s); + const double motion_look_angle_deg = static_cast(live_config.gpr_look_angle_deg); + const double motion_direction_sign = static_cast(live_config.gpr_direction_sign); + for (auto& trace : selected_traces) { + apply_intra_sweep_motion_correction( + trace, + trace.tx_local_index, + motion_slot_count, + start_hz, + stop_hz, + velocity_mps, + motion_speed_mps, + motion_look_angle_deg, + motion_direction_sign, + tx_sweep_time_s, + motion_mode + ); + } + std::unordered_map ascans_by_pair{}; for (const auto& trace : selected_traces) { auto ascan = compute_ascan(trace, start_hz, stop_hz); diff --git a/data_acq_and_processing/processing/processors/src/gpr_processor.cpp b/data_acq_and_processing/processing/processors/src/gpr_processor.cpp index 5c3ea80..3374a0c 100644 --- a/data_acq_and_processing/processing/processors/src/gpr_processor.cpp +++ b/data_acq_and_processing/processing/processors/src/gpr_processor.cpp @@ -10,6 +10,7 @@ #include #include #include +#include #include #include #include diff --git a/locator_test_client.py b/locator_test_client.py index fd13381..ad284c5 100644 --- a/locator_test_client.py +++ b/locator_test_client.py @@ -6,7 +6,7 @@ import threading import time from typing import Any, Dict, Tuple -HOST = "192.168.2.6" +HOST = "127.0.0.1" PORT = 8888 CLIENT_DEVICE_ID = 0 MIN_TEST_VLC = 5.0 diff --git a/python_app/gui/controllers/app_window_config/live_processing_mixin.py b/python_app/gui/controllers/app_window_config/live_processing_mixin.py index 56caaa8..2d4c3b3 100644 --- a/python_app/gui/controllers/app_window_config/live_processing_mixin.py +++ b/python_app/gui/controllers/app_window_config/live_processing_mixin.py @@ -74,11 +74,13 @@ _WEB_LIVE_SCHEMA = [ ("gpr_visible_x_max_m", "Visible window", _GPR_MODES, _dual("_gpr_visible_x_max_m", "_legacy_gpr_visible_x_max_m")), ("gpr_visible_z_min_m", "Visible window", _GPR_MODES, _dual("_gpr_visible_z_min_m", "_legacy_gpr_visible_z_min_m")), ("gpr_visible_z_max_m", "Visible window", _GPR_MODES, _dual("_gpr_visible_z_max_m", "_legacy_gpr_visible_z_max_m")), - ("gpr_look_angle_deg", "Motion", ("legacy_gpr",), _attr("_legacy_gpr_look_angle_deg")), + ("gpr_motion_mode", "Motion", ("gpr",), _attr("_gpr_motion_mode")), + ("gpr_look_angle_deg", "Motion", _GPR_MODES, _dual("_gpr_look_angle_deg", "_legacy_gpr_look_angle_deg")), + ("gpr_direction_sign", "Motion", ("gpr",), _attr("_gpr_direction_sign")), ("gpr_apply_freq_phase_correction", "Motion", ("legacy_gpr",), _attr("_legacy_gpr_apply_freq_phase_correction")), ("gpr_reference_mode", "Motion", ("legacy_gpr",), _attr("_legacy_gpr_reference_mode")), - ("ignore_socket_speed", "Motion", ("legacy_gpr",), _attr("_legacy_gpr_ignore_socket_speed_enabled")), - ("gpr_speed_m_s", "Motion", ("legacy_gpr",), _attr("_legacy_gpr_speed_m_s")), + ("ignore_socket_speed", "Motion", _GPR_MODES, _dual("_gpr_ignore_socket_speed_enabled", "_legacy_gpr_ignore_socket_speed_enabled")), + ("gpr_speed_m_s", "Motion", _GPR_MODES, _dual("_gpr_speed_m_s", "_legacy_gpr_speed_m_s")), ] _WEB_LIVE_GETTERS = {field: getter for field, _group, _modes, getter in _WEB_LIVE_SCHEMA} @@ -93,7 +95,6 @@ _NON_WIDGET_LIVE_FIELDS = frozenset({ "reprocess_current_result", "history_command", "history_command_seq", - "gpr_direction_sign", }) # Display toggles: GUI-only rendering choices (kept in the GUI profile, not the live diff --git a/python_app/gui/controllers/app_window_config/profile_io_mixin.py b/python_app/gui/controllers/app_window_config/profile_io_mixin.py index da398f9..9571c9e 100644 --- a/python_app/gui/controllers/app_window_config/profile_io_mixin.py +++ b/python_app/gui/controllers/app_window_config/profile_io_mixin.py @@ -293,6 +293,11 @@ class AppWindowConfigProfileIOMixin: self._gpr_range_comp_power, self._gpr_angle_comp_power, self._gpr_score_mode, + self._gpr_motion_mode, + self._gpr_look_angle_deg, + self._gpr_direction_sign, + self._gpr_ignore_socket_speed_enabled, + self._gpr_speed_m_s, self._gpr_max_detected_objects_to_draw, self._gpr_draw_top_m_objects, self._gpr_start_freq_mhz, @@ -453,6 +458,13 @@ class AppWindowConfigProfileIOMixin: self._gpr_range_comp_power.setValue(float(gui_state.processing.gpr.range_comp_power)) self._gpr_angle_comp_power.setValue(float(gui_state.processing.gpr.angle_comp_power)) self._set_combo_current_text(self._gpr_score_mode, gui_state.processing.gpr.score_mode) + self._set_combo_current_text(self._gpr_motion_mode, gui_state.processing.gpr.motion_mode) + self._gpr_look_angle_deg.setValue(float(gui_state.processing.gpr.look_angle_deg)) + self._gpr_direction_sign.setValue(float(gui_state.processing.gpr.direction_sign)) + self._gpr_ignore_socket_speed_enabled.setChecked( + bool(gui_state.processing.gpr.ignore_socket_speed_enabled) + ) + self._gpr_speed_m_s.setValue(float(gui_state.processing.gpr.speed_m_s)) self._gpr_max_detected_objects_to_draw.setValue( int(gui_state.processing.gpr.max_detected_objects_to_draw) ) diff --git a/python_app/gui/controllers/app_window_config/state_builders.py b/python_app/gui/controllers/app_window_config/state_builders.py index b3e7c4a..8690ef8 100644 --- a/python_app/gui/controllers/app_window_config/state_builders.py +++ b/python_app/gui/controllers/app_window_config/state_builders.py @@ -223,9 +223,14 @@ class AppWindowConfigStateBuildersMixin: output_positions=self._default_gpr_output_positions_from_config(config), min_depth_m=2.0, max_depth_m=14.0, - range_comp_power=0.28, - angle_comp_power=0.10, + range_comp_power=0.1, + angle_comp_power=0.0, score_mode="combined", + motion_mode="int_minus", + look_angle_deg=0.0, + direction_sign=1.0, + speed_m_s=0.0, + ignore_socket_speed_enabled=False, max_detected_objects_to_draw=5, draw_top_m_objects=2, start_freq_mhz=3000.0, @@ -347,6 +352,11 @@ class AppWindowConfigStateBuildersMixin: range_comp_power=float(self._gpr_range_comp_power.value()), angle_comp_power=float(self._gpr_angle_comp_power.value()), score_mode=self._gpr_score_mode.currentText(), + motion_mode=self._gpr_motion_mode.currentText(), + look_angle_deg=float(self._gpr_look_angle_deg.value()), + direction_sign=float(self._gpr_direction_sign.value()), + speed_m_s=float(self._gpr_speed_m_s.value()), + ignore_socket_speed_enabled=bool(self._gpr_ignore_socket_speed_enabled.isChecked()), max_detected_objects_to_draw=int(self._gpr_max_detected_objects_to_draw.value()), draw_top_m_objects=int(self._gpr_draw_top_m_objects.value()), start_freq_mhz=float(self._gpr_start_freq_mhz.value()), diff --git a/python_app/gui/controllers/sections/processing_section.py b/python_app/gui/controllers/sections/processing_section.py index 19f1da5..0be01f0 100644 --- a/python_app/gui/controllers/sections/processing_section.py +++ b/python_app/gui/controllers/sections/processing_section.py @@ -234,6 +234,48 @@ def build_processing_group(owner) -> QGroupBox: owner._gpr_score_mode.addItems(["peak", "combined"]) owner._set_combo_current_text(owner._gpr_score_mode, gpr_live_defaults.score_mode) + owner._gpr_motion_mode = QComboBox() + owner._gpr_motion_mode.addItems(["int_minus", "int_focus"]) + owner._set_combo_current_text(owner._gpr_motion_mode, gpr_live_defaults.motion_mode) + owner._gpr_motion_mode.setToolTip( + "Intra-sweep speed correction before the IFFT. int_minus: full correction; " + "int_focus: focusing residual only (no net Z shift)." + ) + + owner._gpr_look_angle_deg = QDoubleSpinBox() + owner._gpr_look_angle_deg.setDecimals(2) + owner._gpr_look_angle_deg.setRange(-90.0, 90.0) + owner._gpr_look_angle_deg.setSingleStep(0.5) + owner._gpr_look_angle_deg.setValue(float(gpr_live_defaults.look_angle_deg)) + owner._gpr_look_angle_deg.setToolTip( + "Radar look angle vs the range axis (deg). Used by intra-sweep motion correction." + ) + + owner._gpr_direction_sign = QDoubleSpinBox() + owner._gpr_direction_sign.setDecimals(0) + owner._gpr_direction_sign.setRange(-1.0, 1.0) + owner._gpr_direction_sign.setSingleStep(2.0) + owner._gpr_direction_sign.setValue(float(gpr_live_defaults.direction_sign)) + owner._gpr_direction_sign.setToolTip( + "Motion direction along range: +1 (later frequencies deeper) or -1 (shallower)." + ) + + owner._gpr_ignore_socket_speed_enabled = QCheckBox("Ignore socket speed") + owner._gpr_ignore_socket_speed_enabled.setChecked(bool(gpr_live_defaults.ignore_socket_speed_enabled)) + owner._gpr_ignore_socket_speed_enabled.setToolTip( + "When checked, use the speed below for motion correction instead of the " + "value streamed over the locator socket." + ) + + owner._gpr_speed_m_s = QDoubleSpinBox() + owner._gpr_speed_m_s.setDecimals(3) + owner._gpr_speed_m_s.setRange(-100.0, 100.0) + owner._gpr_speed_m_s.setSingleStep(0.05) + owner._gpr_speed_m_s.setValue(float(gpr_live_defaults.speed_m_s)) + owner._gpr_speed_m_s.setToolTip( + "Radar speed (m/s) used by intra-sweep motion correction when 'Ignore socket speed' is on." + ) + owner._gpr_max_detected_objects_to_draw = QSpinBox() owner._gpr_max_detected_objects_to_draw.setRange(0, 10_000) owner._gpr_max_detected_objects_to_draw.setValue(int(gpr_live_defaults.max_detected_objects_to_draw)) @@ -317,6 +359,11 @@ def build_processing_group(owner) -> QGroupBox: ("Range comp power", owner._gpr_range_comp_power), ("Angle comp power", owner._gpr_angle_comp_power), ("Score mode", owner._gpr_score_mode), + ("Motion mode", owner._gpr_motion_mode), + ("Look angle deg", owner._gpr_look_angle_deg), + ("Direction sign", owner._gpr_direction_sign), + owner._gpr_ignore_socket_speed_enabled, + ("Speed m/s", owner._gpr_speed_m_s), ("Render mode", owner._gpr_render_mode), ("Min visible score", owner._gpr_min_visible_score), ("Max detected objects", owner._gpr_max_detected_objects_to_draw), @@ -524,6 +571,11 @@ def build_processing_group(owner) -> QGroupBox: owner._gpr_range_comp_power.valueChanged.connect(owner._on_processing_live_settings_changed) owner._gpr_angle_comp_power.valueChanged.connect(owner._on_processing_live_settings_changed) owner._gpr_score_mode.currentTextChanged.connect(owner._on_processing_live_settings_changed) + owner._gpr_motion_mode.currentTextChanged.connect(owner._on_processing_live_settings_changed) + owner._gpr_look_angle_deg.valueChanged.connect(owner._on_processing_live_settings_changed) + owner._gpr_direction_sign.valueChanged.connect(owner._on_processing_live_settings_changed) + owner._gpr_ignore_socket_speed_enabled.toggled.connect(owner._on_processing_live_settings_changed) + owner._gpr_speed_m_s.valueChanged.connect(owner._on_processing_live_settings_changed) owner._gpr_start_freq_mhz.valueChanged.connect(owner._on_processing_live_settings_changed) owner._gpr_stop_freq_mhz.valueChanged.connect(owner._on_processing_live_settings_changed) owner._gpr_background_subtract_enabled.toggled.connect(owner._on_processing_live_settings_changed) diff --git a/python_app/models/gui_profile_codec.py b/python_app/models/gui_profile_codec.py index 84c20ff..afe0c53 100644 --- a/python_app/models/gui_profile_codec.py +++ b/python_app/models/gui_profile_codec.py @@ -288,6 +288,36 @@ def gui_profile_from_dict(payload: dict[str, Any]) -> GuiProfileModel: gui.processing.gpr.score_mode, "gui.processing.gpr", ), + motion_mode=_optional_string( + gpr_object, + "motion_mode", + gui.processing.gpr.motion_mode, + "gui.processing.gpr", + ), + look_angle_deg=_optional_float( + gpr_object, + "look_angle_deg", + gui.processing.gpr.look_angle_deg, + "gui.processing.gpr", + ), + direction_sign=_optional_float( + gpr_object, + "direction_sign", + gui.processing.gpr.direction_sign, + "gui.processing.gpr", + ), + speed_m_s=_optional_float( + gpr_object, + "speed_m_s", + gui.processing.gpr.speed_m_s, + "gui.processing.gpr", + ), + ignore_socket_speed_enabled=_optional_bool( + gpr_object, + "ignore_socket_speed_enabled", + gui.processing.gpr.ignore_socket_speed_enabled, + "gui.processing.gpr", + ), max_detected_objects_to_draw=_optional_int( gpr_object, "max_detected_objects_to_draw", @@ -425,6 +455,8 @@ def gui_profile_from_dict(payload: dict[str, Any]) -> GuiProfileModel: raise ValueError("gui.processing.gpr.render_mode must be one of: heatmap, objects_only") if gui.processing.gpr.score_mode not in {"peak", "combined"}: raise ValueError("gui.processing.gpr.score_mode must be one of: peak, combined") + if gui.processing.gpr.motion_mode not in {"int_minus", "int_focus"}: + raise ValueError("gui.processing.gpr.motion_mode must be one of: int_minus, int_focus") if gui.processing.legacy_gpr.mode not in {"point", "extended"}: raise ValueError("gui.processing.legacy_gpr.mode must be one of: point, extended") if gui.processing.legacy_gpr.render_mode not in {"heatmap", "objects_only"}: @@ -550,6 +582,11 @@ def gui_profile_to_dict(model: GuiProfileModel) -> dict[str, Any]: "range_comp_power": gui.processing.gpr.range_comp_power, "angle_comp_power": gui.processing.gpr.angle_comp_power, "score_mode": gui.processing.gpr.score_mode, + "motion_mode": gui.processing.gpr.motion_mode, + "look_angle_deg": gui.processing.gpr.look_angle_deg, + "direction_sign": gui.processing.gpr.direction_sign, + "speed_m_s": gui.processing.gpr.speed_m_s, + "ignore_socket_speed_enabled": gui.processing.gpr.ignore_socket_speed_enabled, "max_detected_objects_to_draw": gui.processing.gpr.max_detected_objects_to_draw, "draw_top_m_objects": gui.processing.gpr.draw_top_m_objects, "start_freq_mhz": gui.processing.gpr.start_freq_mhz, diff --git a/python_app/models/gui_profile_schema.py b/python_app/models/gui_profile_schema.py index 91afb85..cf328dc 100644 --- a/python_app/models/gui_profile_schema.py +++ b/python_app/models/gui_profile_schema.py @@ -58,9 +58,17 @@ class GuiGprStateModel: output_positions: str = "" min_depth_m: float = 2.0 max_depth_m: float = 14.0 - range_comp_power: float = 0.28 - angle_comp_power: float = 0.10 + range_comp_power: float = 0.1 + angle_comp_power: float = 0.0 score_mode: str = "combined" + motion_mode: str = "int_minus" + # Intra-sweep motion-correction inputs. Sweep time is derived from acquisition + # metadata; speed comes from the socket unless `ignore_socket_speed_enabled`, + # in which case `speed_m_s` from here is used. + look_angle_deg: float = 0.0 + direction_sign: float = 1.0 + speed_m_s: float = 0.0 + ignore_socket_speed_enabled: bool = False max_detected_objects_to_draw: int = 5 draw_top_m_objects: int = 2 start_freq_mhz: float = 3000.0 diff --git a/python_app/orchestration/live_processing_config.py b/python_app/orchestration/live_processing_config.py index 55b3bb0..0ab6a89 100644 --- a/python_app/orchestration/live_processing_config.py +++ b/python_app/orchestration/live_processing_config.py @@ -31,10 +31,14 @@ class ProcessingLiveConfig: gpr_output_positions: list[int] | None = None gpr_min_depth_m: float = 2.0 gpr_max_depth_m: float = 14.0 - gpr_range_comp_power: float = 0.28 - gpr_angle_comp_power: float = 0.10 + gpr_range_comp_power: float = 0.1 + gpr_angle_comp_power: float = 0.0 gpr_comp_power: float = 0.2 gpr_score_mode: str = "combined" + # Backprojection intra-sweep speed-correction mode: "int_minus" (full + # correction) or "int_focus" (focusing residual only). Mirrors Python + # Horns_motion_3libre.py MOTION_CORRECTION_MODE. + gpr_motion_mode: str = "int_minus" gpr_max_detected_objects_to_draw: int = 5 gpr_draw_top_m_objects: int = 2 gpr_speed_m_s: float = 0.0 @@ -108,6 +112,7 @@ class ProcessingLiveConfig: "gpr_angle_comp_power": float(self.gpr_angle_comp_power), "gpr_comp_power": float(self.gpr_comp_power), "gpr_score_mode": str(self.gpr_score_mode), + "gpr_motion_mode": str(self.gpr_motion_mode), "gpr_max_detected_objects_to_draw": int(self.gpr_max_detected_objects_to_draw), "gpr_draw_top_m_objects": int(self.gpr_draw_top_m_objects), "gpr_speed_m_s": float(self.gpr_speed_m_s), diff --git a/run_config.json b/run_config.json index cb96765..fa3edaa 100644 --- a/run_config.json +++ b/run_config.json @@ -281,9 +281,14 @@ "output_positions": "0,1", "min_depth_m": 2.0, "max_depth_m": 14.0, - "range_comp_power": 0.28, - "angle_comp_power": 0.1, + "range_comp_power": 0.1, + "angle_comp_power": 0.0, "score_mode": "combined", + "motion_mode": "int_minus", + "look_angle_deg": 0.0, + "direction_sign": 1.0, + "speed_m_s": 0.0, + "ignore_socket_speed_enabled": false, "max_detected_objects_to_draw": 5, "draw_top_m_objects": 2, "start_freq_mhz": 3000.0, diff --git a/run_config_examples/run_config_simulator.example.json b/run_config_examples/run_config_simulator.example.json index cb96765..fa3edaa 100644 --- a/run_config_examples/run_config_simulator.example.json +++ b/run_config_examples/run_config_simulator.example.json @@ -281,9 +281,14 @@ "output_positions": "0,1", "min_depth_m": 2.0, "max_depth_m": 14.0, - "range_comp_power": 0.28, - "angle_comp_power": 0.1, + "range_comp_power": 0.1, + "angle_comp_power": 0.0, "score_mode": "combined", + "motion_mode": "int_minus", + "look_angle_deg": 0.0, + "direction_sign": 1.0, + "speed_m_s": 0.0, + "ignore_socket_speed_enabled": false, "max_detected_objects_to_draw": 5, "draw_top_m_objects": 2, "start_freq_mhz": 3000.0, diff --git a/run_configs/run_config.json b/run_configs/run_config.json index cb96765..fa3edaa 100644 --- a/run_configs/run_config.json +++ b/run_configs/run_config.json @@ -281,9 +281,14 @@ "output_positions": "0,1", "min_depth_m": 2.0, "max_depth_m": 14.0, - "range_comp_power": 0.28, - "angle_comp_power": 0.1, + "range_comp_power": 0.1, + "angle_comp_power": 0.0, "score_mode": "combined", + "motion_mode": "int_minus", + "look_angle_deg": 0.0, + "direction_sign": 1.0, + "speed_m_s": 0.0, + "ignore_socket_speed_enabled": false, "max_detected_objects_to_draw": 5, "draw_top_m_objects": 2, "start_freq_mhz": 3000.0,